78 lines
2.5 KiB
C++
78 lines
2.5 KiB
C++
// 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
|