initial commit, 4.5 stable

thirdparty/jolt_physics/Jolt/Geometry/Ellipse.h

@@ -0,0 +1,77 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Math/Float2.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Ellipse centered around the origin
+/// @see https://en.wikipedia.org/wiki/Ellipse
+class Ellipse
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Construct ellipse with radius A along the X-axis and B along the Y-axis
+					Ellipse(float inA, float inB) : mA(inA), mB(inB) { JPH_ASSERT(inA > 0.0f); JPH_ASSERT(inB > 0.0f); }
+
+	/// Check if inPoint is inside the ellipse
+	bool			IsInside(const Float2 &inPoint) const
+	{
+		return Square(inPoint.x / mA) + Square(inPoint.y / mB) <= 1.0f;
+	}
+
+	/// Get the closest point on the ellipse to inPoint
+	/// Assumes inPoint is outside the ellipse
+	/// @see Rotation Joint Limits in Quaternion Space by Gino van den Bergen, section 10.1 in Game Engine Gems 3.
+	Float2			GetClosestPoint(const Float2 &inPoint) const
+	{
+		float a_sq = Square(mA);
+		float b_sq = Square(mB);
+
+		// Equation of ellipse: f(x, y) = (x/a)^2 + (y/b)^2 - 1 = 0											[1]
+		// Normal on surface: (df/dx, df/dy) = (2 x / a^2, 2 y / b^2)
+		// Closest point (x', y') on ellipse to point (x, y): (x', y') + t (x / a^2, y / b^2) = (x, y)
+		// <=> (x', y') = (a^2 x / (t + a^2), b^2 y / (t + b^2))
+		// Requiring point to be on ellipse (substituting into [1]): g(t) = (a x / (t + a^2))^2 + (b y / (t + b^2))^2 - 1 = 0
+
+		// Newton Raphson iteration, starting at t = 0
+		float t = 0.0f;
+		for (;;)
+		{
+			// Calculate g(t)
+			float t_plus_a_sq = t + a_sq;
+			float t_plus_b_sq = t + b_sq;
+			float gt = Square(mA * inPoint.x / t_plus_a_sq) + Square(mB * inPoint.y / t_plus_b_sq) - 1.0f;
+
+			// Check if g(t) it is close enough to zero
+			if (abs(gt) < 1.0e-6f)
+				return Float2(a_sq * inPoint.x / t_plus_a_sq, b_sq * inPoint.y / t_plus_b_sq);
+
+			// Get derivative dg/dt = g'(t) = -2 (b^2 y^2 / (t + b^2)^3 + a^2 x^2 / (t + a^2)^3)
+			float gt_accent = -2.0f *
+				(a_sq * Square(inPoint.x) / Cubed(t_plus_a_sq)
+				+ b_sq * Square(inPoint.y) / Cubed(t_plus_b_sq));
+
+			// Calculate t for next iteration: tn+1 = tn - g(t) / g'(t)
+			float tn = t - gt / gt_accent;
+			t = tn;
+		}
+	}
+
+	/// Get normal at point inPoint (non-normalized vector)
+	Float2			GetNormal(const Float2 &inPoint) const
+	{
+		// Calculated by [d/dx f(x, y), d/dy f(x, y)], where f(x, y) is the ellipse equation from above
+		return Float2(inPoint.x / Square(mA), inPoint.y / Square(mB));
+	}
+
+private:
+	float			mA;				///< Radius along X-axis
+	float			mB;				///< Radius along Y-axis
+};
+
+JPH_NAMESPACE_END