From f9dbc9138fe9840124905b4b1c709cdda15d5c89 Mon Sep 17 00:00:00 2001
From: lisyarus <lisyarus@gmail.com>
Date: Wed, 4 Nov 2020 10:31:02 +0300
Subject: [PATCH] Add a quadratic solver that assumes real roots

---
 libs/geom/include/psemek/geom/math.hpp | 21 +++++++++++++++++++++
 1 file changed, 21 insertions(+)

diff --git a/libs/geom/include/psemek/geom/math.hpp b/libs/geom/include/psemek/geom/math.hpp
index 6b5a69f6..85b900a5 100644
--- a/libs/geom/include/psemek/geom/math.hpp
+++ b/libs/geom/include/psemek/geom/math.hpp
@@ -48,4 +48,25 @@ namespace psemek::geom
 		return std::pair{x1, x2};
 	}
 
+	// roots of ax^2 + bx + c = 0
+	// in increasing order
+	// the quadratic is assumed to have real roots
+	template <typename T>
+	std::pair<T, T> solve_quadratic_positive(T const & a, T const & b, T const & c)
+	{
+		auto const D = b * b - 4 * a * c;
+		if (D <= 0)
+		{
+			auto x = - b / (2 * a);
+			return std::pair{x, x};
+		}
+
+		auto t = - (b + (b > 0 ? 1 : -1) * std::sqrt(D)) / 2;
+		auto x1 = t / a;
+		auto x2 = c / t;
+
+		if (x1 > x2) std::swap(x1, x2);
+		return std::pair{x1, x2};
+	}
+
 }