@bloomengine/engine 0.3.1

package/native/third_party/JoltPhysics/Jolt/Physics/Constraints/PathConstraint.h

@@ -0,0 +1,191 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Constraints/TwoBodyConstraint.h>
+#include <Jolt/Physics/Constraints/PathConstraintPath.h>
+#include <Jolt/Physics/Constraints/MotorSettings.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/AxisConstraintPart.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/DualAxisConstraintPart.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/HingeRotationConstraintPart.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/RotationEulerConstraintPart.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_SUPPRESS_WARNING_PUSH
+JPH_GCC_SUPPRESS_WARNING("-Wshadow") // GCC complains about the 'Free' value conflicting with the 'Free' method
+
+/// How to constrain the rotation of the body to a PathConstraint
+enum class EPathRotationConstraintType
+{
+	Free,							///< Do not constrain the rotation of the body at all
+	ConstrainAroundTangent,			///< Only allow rotation around the tangent vector (following the path)
+	ConstrainAroundNormal,			///< Only allow rotation around the normal vector (perpendicular to the path)
+	ConstrainAroundBinormal,		///< Only allow rotation around the binormal vector (perpendicular to the path)
+	ConstrainToPath,				///< Fully constrain the rotation of body 2 to the path (following the tangent and normal of the path)
+	FullyConstrained,				///< Fully constrain the rotation of the body 2 to the rotation of body 1
+};
+
+JPH_SUPPRESS_WARNING_POP
+
+/// Path constraint settings, used to constrain the degrees of freedom between two bodies to a path
+///
+/// The requirements of the path are that:
+/// * Tangent, normal and bi-normal form an orthonormal basis with: tangent cross bi-normal = normal
+/// * The path points along the tangent vector
+/// * The path is continuous so doesn't contain any sharp corners
+///
+/// The reason for all this is that the constraint acts like a slider constraint with the sliding axis being the tangent vector (the assumption here is that delta time will be small enough so that the path is linear for that delta time).
+class JPH_EXPORT PathConstraintSettings final : public TwoBodyConstraintSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, PathConstraintSettings)
+
+public:
+	// See: ConstraintSettings::SaveBinaryState
+	virtual void					SaveBinaryState(StreamOut &inStream) const override;
+
+	/// Create an instance of this constraint
+	virtual TwoBodyConstraint *		Create(Body &inBody1, Body &inBody2) const override;
+
+	/// The path that constrains the two bodies
+	RefConst<PathConstraintPath>	mPath;
+
+	/// The position of the path start relative to world transform of body 1
+	Vec3							mPathPosition = Vec3::sZero();
+
+	/// The rotation of the path start relative to world transform of body 1
+	Quat							mPathRotation = Quat::sIdentity();
+
+	/// The fraction along the path that corresponds to the initial position of body 2. Usually this is 0, the beginning of the path. But if you want to start an object halfway the path you can calculate this with mPath->GetClosestPoint(point on path to attach body to).
+	float							mPathFraction = 0.0f;
+
+	/// Maximum amount of friction force to apply (N) when not driven by a motor.
+	float							mMaxFrictionForce = 0.0f;
+
+	/// In case the constraint is powered, this determines the motor settings along the path
+	MotorSettings					mPositionMotorSettings;
+
+	/// How to constrain the rotation of the body to the path
+	EPathRotationConstraintType		mRotationConstraintType = EPathRotationConstraintType::Free;
+
+protected:
+	// See: ConstraintSettings::RestoreBinaryState
+	virtual void					RestoreBinaryState(StreamIn &inStream) override;
+};
+
+/// Path constraint, used to constrain the degrees of freedom between two bodies to a path
+class JPH_EXPORT PathConstraint final : public TwoBodyConstraint
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Construct point constraint
+									PathConstraint(Body &inBody1, Body &inBody2, const PathConstraintSettings &inSettings);
+
+	// Generic interface of a constraint
+	virtual EConstraintSubType		GetSubType() const override								{ return EConstraintSubType::Path; }
+	virtual void					NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override;
+	virtual void					SetupVelocityConstraint(float inDeltaTime) override;
+	virtual void					ResetWarmStart() override;
+	virtual void					WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
+	virtual bool					SolveVelocityConstraint(float inDeltaTime) override;
+	virtual bool					SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override;
+#ifdef JPH_DEBUG_RENDERER
+	virtual void					DrawConstraint(DebugRenderer *inRenderer) const override;
+#endif // JPH_DEBUG_RENDERER
+	virtual void					SaveState(StateRecorder &inStream) const override;
+	virtual void					RestoreState(StateRecorder &inStream) override;
+	virtual bool					IsActive() const override								{ return TwoBodyConstraint::IsActive() && mPath != nullptr; }
+	virtual Ref<ConstraintSettings> GetConstraintSettings() const override;
+
+	// See: TwoBodyConstraint
+	virtual Mat44					GetConstraintToBody1Matrix() const override				{ return mPathToBody1; }
+	virtual Mat44					GetConstraintToBody2Matrix() const override				{ return mPathToBody2; }
+
+	/// Update the path for this constraint
+	void							SetPath(const PathConstraintPath *inPath, float inPathFraction);
+
+	/// Access to the current path
+	const PathConstraintPath *		GetPath() const											{ return mPath; }
+
+	/// Access to the current fraction along the path e [0, GetPath()->GetMaxPathFraction()]
+	float							GetPathFraction() const									{ return mPathFraction; }
+
+	/// Friction control
+	void							SetMaxFrictionForce(float inFrictionForce)				{ mMaxFrictionForce = inFrictionForce; }
+	float							GetMaxFrictionForce() const								{ return mMaxFrictionForce; }
+
+	/// Position motor settings
+	MotorSettings &					GetPositionMotorSettings()								{ return mPositionMotorSettings; }
+	const MotorSettings &			GetPositionMotorSettings() const						{ return mPositionMotorSettings; }
+
+	// Position motor controls (drives body 2 along the path)
+	void							SetPositionMotorState(EMotorState inState)				{ JPH_ASSERT(inState == EMotorState::Off || mPositionMotorSettings.IsValid()); mPositionMotorState = inState; }
+	EMotorState						GetPositionMotorState() const							{ return mPositionMotorState; }
+	void							SetTargetVelocity(float inVelocity)						{ mTargetVelocity = inVelocity; }
+	float							GetTargetVelocity() const								{ return mTargetVelocity; }
+	void							SetTargetPathFraction(float inFraction)					{ JPH_ASSERT(mPath->IsLooping() || (inFraction >= 0.0f && inFraction <= mPath->GetPathMaxFraction())); mTargetPathFraction = inFraction; }
+	float							GetTargetPathFraction() const							{ return mTargetPathFraction; }
+
+	///@name Get Lagrange multiplier from last physics update (the linear/angular impulse applied to satisfy the constraint)
+	inline Vector<2>				GetTotalLambdaPosition() const							{ return mPositionConstraintPart.GetTotalLambda(); }
+	inline float					GetTotalLambdaPositionLimits() const					{ return mPositionLimitsConstraintPart.GetTotalLambda(); }
+	inline float					GetTotalLambdaMotor() const								{ return mPositionMotorConstraintPart.GetTotalLambda(); }
+	inline Vector<2>				GetTotalLambdaRotationHinge() const						{ return mHingeConstraintPart.GetTotalLambda(); }
+	inline Vec3						GetTotalLambdaRotation() const							{ return mRotationConstraintPart.GetTotalLambda(); }
+
+private:
+	// Internal helper function to calculate the values below
+	void							CalculateConstraintProperties(float inDeltaTime);
+
+	// CONFIGURATION PROPERTIES FOLLOW
+
+	RefConst<PathConstraintPath>	mPath;													///< The path that attaches the two bodies
+	Mat44							mPathToBody1;											///< Transform that takes a quantity from path space to body 1 center of mass space
+	Mat44							mPathToBody2;											///< Transform that takes a quantity from path space to body 2 center of mass space
+	EPathRotationConstraintType		mRotationConstraintType;								///< How to constrain the rotation of the path
+
+	// Friction
+	float							mMaxFrictionForce;
+
+	// Motor controls
+	MotorSettings					mPositionMotorSettings;
+	EMotorState						mPositionMotorState = EMotorState::Off;
+	float							mTargetVelocity = 0.0f;
+	float							mTargetPathFraction = 0.0f;
+
+	// RUN TIME PROPERTIES FOLLOW
+
+	// Positions where the point constraint acts on in world space
+	Vec3							mR1;
+	Vec3							mR2;
+
+	// X2 + R2 - X1 - R1
+	Vec3							mU;
+
+	// World space path tangent
+	Vec3							mPathTangent;
+
+	// Normals to the path tangent
+	Vec3							mPathNormal;
+	Vec3							mPathBinormal;
+
+	// Inverse of initial rotation from body 1 to body 2 in body 1 space (only used when rotation constraint type is FullyConstrained)
+	Quat							mInvInitialOrientation;
+
+	// Current fraction along the path where body 2 is attached
+	float							mPathFraction = 0.0f;
+
+	// Translation constraint parts
+	DualAxisConstraintPart			mPositionConstraintPart;								///< Constraint part that keeps the movement along the tangent of the path
+	AxisConstraintPart				mPositionLimitsConstraintPart;							///< Constraint part that prevents movement beyond the beginning and end of the path
+	AxisConstraintPart				mPositionMotorConstraintPart;							///< Constraint to drive the object along the path or to apply friction
+
+	// Rotation constraint parts
+	HingeRotationConstraintPart		mHingeConstraintPart;									///< Constraint part that removes 2 degrees of rotation freedom
+	RotationEulerConstraintPart		mRotationConstraintPart;								///< Constraint part that removes all rotational freedom
+};
+
+JPH_NAMESPACE_END

package/native/third_party/JoltPhysics/Jolt/Physics/Constraints/PathConstraintPath.cpp

@@ -0,0 +1,85 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Constraints/PathConstraintPath.h>
+#include <Jolt/Core/StreamUtils.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT(PathConstraintPath)
+{
+	JPH_ADD_BASE_CLASS(PathConstraintPath, SerializableObject)
+}
+
+#ifdef JPH_DEBUG_RENDERER
+// Helper function to transform the results of GetPointOnPath to world space
+static inline void sTransformPathPoint(RMat44Arg inTransform, Vec3Arg inPosition, RVec3 &outPosition, Vec3 &ioNormal, Vec3 &ioBinormal)
+{
+	outPosition = inTransform * inPosition;
+	ioNormal = inTransform.Multiply3x3(ioNormal);
+	ioBinormal = inTransform.Multiply3x3(ioBinormal);
+}
+
+// Helper function to draw a path segment
+static inline void sDrawPathSegment(DebugRenderer *inRenderer, RVec3Arg inPrevPosition, RVec3Arg inPosition, Vec3Arg inNormal, Vec3Arg inBinormal)
+{
+	inRenderer->DrawLine(inPrevPosition, inPosition, Color::sWhite);
+	inRenderer->DrawArrow(inPosition, inPosition + 0.1f * inNormal, Color::sRed, 0.02f);
+	inRenderer->DrawArrow(inPosition, inPosition + 0.1f * inBinormal, Color::sGreen, 0.02f);
+}
+
+void PathConstraintPath::DrawPath(DebugRenderer *inRenderer, RMat44Arg inBaseTransform) const
+{
+	// Calculate first point
+	Vec3 lfirst_pos, first_tangent, first_normal, first_binormal;
+	GetPointOnPath(0.0f, lfirst_pos, first_tangent, first_normal, first_binormal);
+	RVec3 first_pos;
+	sTransformPathPoint(inBaseTransform, lfirst_pos, first_pos, first_normal, first_binormal);
+
+	float t_max = GetPathMaxFraction();
+
+	// Draw the segments
+	RVec3 prev_pos = first_pos;
+	for (float t = 0.1f; t < t_max; t += 0.1f)
+	{
+		Vec3 lpos, tangent, normal, binormal;
+		GetPointOnPath(t, lpos, tangent, normal, binormal);
+		RVec3 pos;
+		sTransformPathPoint(inBaseTransform, lpos, pos, normal, binormal);
+		sDrawPathSegment(inRenderer, prev_pos, pos, normal, binormal);
+		prev_pos = pos;
+	}
+
+	// Draw last point
+	Vec3 lpos, tangent, normal, binormal;
+	GetPointOnPath(t_max, lpos, tangent, normal, binormal);
+	RVec3 pos;
+	sTransformPathPoint(inBaseTransform, lpos, pos, normal, binormal);
+	sDrawPathSegment(inRenderer, prev_pos, pos, normal, binormal);
+}
+#endif // JPH_DEBUG_RENDERER
+
+void PathConstraintPath::SaveBinaryState(StreamOut &inStream) const
+{
+	inStream.Write(GetRTTI()->GetHash());
+	inStream.Write(mIsLooping);
+}
+
+void PathConstraintPath::RestoreBinaryState(StreamIn &inStream)
+{
+	// Type hash read by sRestoreFromBinaryState
+	inStream.Read(mIsLooping);
+}
+
+PathConstraintPath::PathResult PathConstraintPath::sRestoreFromBinaryState(StreamIn &inStream)
+{
+	return StreamUtils::RestoreObject<PathConstraintPath>(inStream, &PathConstraintPath::RestoreBinaryState);
+}
+
+JPH_NAMESPACE_END

package/native/third_party/JoltPhysics/Jolt/Physics/Constraints/PathConstraintPath.h

@@ -0,0 +1,76 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/Reference.h>
+#include <Jolt/Core/Result.h>
+#include <Jolt/ObjectStream/SerializableObject.h>
+
+JPH_NAMESPACE_BEGIN
+
+class StreamIn;
+class StreamOut;
+#ifdef JPH_DEBUG_RENDERER
+class DebugRenderer;
+#endif // JPH_DEBUG_RENDERER
+
+/// The path for a path constraint. It allows attaching two bodies to each other while giving the second body the freedom to move along a path relative to the first.
+class JPH_EXPORT PathConstraintPath : public SerializableObject, public RefTarget<PathConstraintPath>
+{
+	JPH_DECLARE_SERIALIZABLE_ABSTRACT(JPH_EXPORT, PathConstraintPath)
+
+public:
+	using PathResult = Result<Ref<PathConstraintPath>>;
+
+	/// Virtual destructor to ensure that derived types get their destructors called
+	virtual				~PathConstraintPath() override = default;
+
+	/// Gets the max fraction along the path. I.e. sort of the length of the path.
+	virtual float		GetPathMaxFraction() const = 0;
+
+	/// Get the globally closest point on the curve (Could be slow!)
+	/// @param inPosition Position to find closest point for
+	/// @param inFractionHint Last known fraction along the path (can be used to speed up the search)
+	/// @return Fraction of closest point along the path
+	virtual float		GetClosestPoint(Vec3Arg inPosition, float inFractionHint) const = 0;
+
+	/// Given the fraction along the path, get the point, tangent and normal.
+	/// @param inFraction Fraction along the path [0, GetPathMaxFraction()].
+	/// @param outPathPosition Returns the closest position to inSearchPosition on the path.
+	/// @param outPathTangent Returns the tangent to the path at outPathPosition (the vector that follows the direction of the path)
+	/// @param outPathNormal Return the normal to the path at outPathPosition (a vector that's perpendicular to outPathTangent)
+	/// @param outPathBinormal Returns the binormal to the path at outPathPosition (a vector so that normal cross tangent = binormal)
+	virtual void		GetPointOnPath(float inFraction, Vec3 &outPathPosition, Vec3 &outPathTangent, Vec3 &outPathNormal, Vec3 &outPathBinormal) const = 0;
+
+	/// If the path is looping or not. If a path is looping, the first and last point are automatically connected to each other. They should not be the same points.
+	void				SetIsLooping(bool inIsLooping)						{ mIsLooping = inIsLooping; }
+	bool				IsLooping() const									{ return mIsLooping; }
+
+#ifdef JPH_DEBUG_RENDERER
+	/// Draw the path relative to inBaseTransform. Used for debug purposes.
+	void				DrawPath(DebugRenderer *inRenderer, RMat44Arg inBaseTransform) const;
+#endif // JPH_DEBUG_RENDERER
+
+	/// Saves the contents of the path in binary form to inStream.
+	virtual void		SaveBinaryState(StreamOut &inStream) const;
+
+	/// Creates a Shape of the correct type and restores its contents from the binary stream inStream.
+	static PathResult	sRestoreFromBinaryState(StreamIn &inStream);
+
+protected:
+	/// Don't allow (copy) constructing this base class, but allow derived classes to (copy) construct themselves
+						PathConstraintPath() = default;
+						PathConstraintPath(const PathConstraintPath &) = default;
+	PathConstraintPath &operator = (const PathConstraintPath &) = default;
+
+	/// This function should not be called directly, it is used by sRestoreFromBinaryState.
+	virtual void		RestoreBinaryState(StreamIn &inStream);
+
+private:
+	/// If the path is looping or not. If a path is looping, the first and last point are automatically connected to each other. They should not be the same points.
+	bool				mIsLooping = false;
+};
+
+JPH_NAMESPACE_END

package/native/third_party/JoltPhysics/Jolt/Physics/Constraints/PathConstraintPathHermite.cpp

@@ -0,0 +1,308 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Constraints/PathConstraintPathHermite.h>
+#include <Jolt/Core/Profiler.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(PathConstraintPathHermite::Point)
+{
+	JPH_ADD_ATTRIBUTE(PathConstraintPathHermite::Point, mPosition)
+	JPH_ADD_ATTRIBUTE(PathConstraintPathHermite::Point, mTangent)
+	JPH_ADD_ATTRIBUTE(PathConstraintPathHermite::Point, mNormal)
+}
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(PathConstraintPathHermite)
+{
+	JPH_ADD_BASE_CLASS(PathConstraintPathHermite, PathConstraintPath)
+
+	JPH_ADD_ATTRIBUTE(PathConstraintPathHermite, mPoints)
+}
+
+// Calculate position and tangent for a Cubic Hermite Spline segment
+static inline void sCalculatePositionAndTangent(Vec3Arg inP1, Vec3Arg inM1, Vec3Arg inP2, Vec3Arg inM2, float inT, Vec3 &outPosition, Vec3 &outTangent)
+{
+	// Calculate factors for Cubic Hermite Spline
+	// See: https://en.wikipedia.org/wiki/Cubic_Hermite_spline
+	float t2 = inT * inT;
+	float t3 = inT * t2;
+	float h00 = 2.0f * t3 - 3.0f * t2 + 1.0f;
+	float h10 = t3 - 2.0f * t2 + inT;
+	float h01 = -2.0f * t3 + 3.0f * t2;
+	float h11 = t3 - t2;
+
+	// Calculate d/dt for factors to calculate the tangent
+	float ddt_h00 = 6.0f * (t2 - inT);
+	float ddt_h10 = 3.0f * t2 - 4.0f * inT + 1.0f;
+	float ddt_h01 = -ddt_h00;
+	float ddt_h11 = 3.0f * t2 - 2.0f * inT;
+
+	outPosition = h00 * inP1 + h10 * inM1 + h01 * inP2 + h11 * inM2;
+	outTangent = ddt_h00 * inP1 + ddt_h10 * inM1 + ddt_h01 * inP2 + ddt_h11 * inM2;
+}
+
+// Calculate the closest point to the origin for a Cubic Hermite Spline segment
+// This is used to get an estimate for the interval in which the closest point can be found,
+// the interval [0, 1] is too big for Newton Raphson to work on because it is solving a 5th degree polynomial which may
+// have multiple local minima that are not the root. This happens especially when the path is straight (tangents aligned with inP2 - inP1).
+// Based on the bisection method: https://en.wikipedia.org/wiki/Bisection_method
+static inline void sCalculateClosestPointThroughBisection(Vec3Arg inP1, Vec3Arg inM1, Vec3Arg inP2, Vec3Arg inM2, float &outTMin, float &outTMax)
+{
+	outTMin = 0.0f;
+	outTMax = 1.0f;
+
+	// To get the closest point of the curve to the origin we need to solve:
+	// d/dt P(t) . P(t) = 0 for t, where P(t) is the point on the curve segment
+	// Using d/dt (a(t) . b(t)) = d/dt a(t) . b(t) + a(t) . d/dt b(t)
+	// See: https://proofwiki.org/wiki/Derivative_of_Dot_Product_of_Vector-Valued_Functions
+	// d/dt P(t) . P(t) = 2 P(t) d/dt P(t) = 2 P(t) . Tangent(t)
+
+	// Calculate the derivative at t = 0, we know P(0) = inP1 and Tangent(0) = inM1
+	float ddt_min = inP1.Dot(inM1); // Leaving out factor 2, we're only interested in the root
+	if (abs(ddt_min) < 1.0e-6f)
+	{
+		// Derivative is near zero, we found our root
+		outTMax = 0.0f;
+		return;
+	}
+	bool ddt_min_negative = ddt_min < 0.0f;
+
+	// Calculate derivative at t = 1, we know P(1) = inP2 and Tangent(1) = inM2
+	float ddt_max = inP2.Dot(inM2);
+	if (abs(ddt_max) < 1.0e-6f)
+	{
+		// Derivative is near zero, we found our root
+		outTMin = 1.0f;
+		return;
+	}
+	bool ddt_max_negative = ddt_max < 0.0f;
+
+	// If the signs of the derivative are not different, this algorithm can't find the root
+	if (ddt_min_negative == ddt_max_negative)
+		return;
+
+	// With 4 iterations we'll get a result accurate to 1 / 2^4 = 0.0625
+	for (int iteration = 0; iteration < 4; ++iteration)
+	{
+		float t_mid = 0.5f * (outTMin + outTMax);
+		Vec3 position, tangent;
+		sCalculatePositionAndTangent(inP1, inM1, inP2, inM2, t_mid, position, tangent);
+		float ddt_mid = position.Dot(tangent);
+		if (abs(ddt_mid) < 1.0e-6f)
+		{
+			// Derivative is near zero, we found our root
+			outTMin = outTMax = t_mid;
+			return;
+		}
+		bool ddt_mid_negative = ddt_mid < 0.0f;
+
+		// Update the search interval so that the signs of the derivative at both ends of the interval are still different
+		if (ddt_mid_negative == ddt_min_negative)
+			outTMin = t_mid;
+		else
+			outTMax = t_mid;
+	}
+}
+
+// Calculate the closest point to the origin for a Cubic Hermite Spline segment
+// Only considers the range t e [inTMin, inTMax] and will stop as soon as the closest point falls outside of that range
+static inline float sCalculateClosestPointThroughNewtonRaphson(Vec3Arg inP1, Vec3Arg inM1, Vec3Arg inP2, Vec3Arg inM2, float inTMin, float inTMax, float &outDistanceSq)
+{
+	// This is the closest position on the curve to the origin that we found
+	Vec3 position;
+
+	// Calculate the size of the interval
+	float interval = inTMax - inTMin;
+
+	// Start in the middle of the interval
+	float t = 0.5f * (inTMin + inTMax);
+
+	// Do max 10 iterations to prevent taking too much CPU time
+	for (int iteration = 0; iteration < 10; ++iteration)
+	{
+		// Calculate derivative at t, see comment at sCalculateClosestPointThroughBisection for derivation of the equations
+		Vec3 tangent;
+		sCalculatePositionAndTangent(inP1, inM1, inP2, inM2, t, position, tangent);
+		float ddt = position.Dot(tangent); // Leaving out factor 2, we're only interested in the root
+
+		// Calculate derivative of ddt: d^2/dt P(t) . P(t) = d/dt (2 P(t) . Tangent(t))
+		// = 2 (d/dt P(t)) . Tangent(t) + P(t) . d/dt Tangent(t)) = 2 (Tangent(t) . Tangent(t) + P(t) . d/dt Tangent(t))
+		float d2dt_h00 = 12.0f * t - 6.0f;
+		float d2dt_h10 = 6.0f * t - 4.0f;
+		float d2dt_h01 = -d2dt_h00;
+		float d2dt_h11 = 6.0f * t - 2.0f;
+		Vec3 ddt_tangent = d2dt_h00 * inP1 + d2dt_h10 * inM1 + d2dt_h01 * inP2 + d2dt_h11 * inM2;
+		float d2dt = tangent.Dot(tangent) + position.Dot(ddt_tangent); // Leaving out factor 2, because we left it out above too
+
+		// If d2dt is zero, the curve is flat and there are multiple t's for which we are closest to the origin, stop now
+		if (d2dt == 0.0f)
+			break;
+
+		// Do a Newton Raphson step
+		// See: https://en.wikipedia.org/wiki/Newton%27s_method
+		// Clamp against [-interval, interval] to avoid overshooting too much, we're not interested outside the interval
+		float delta = Clamp(-ddt / d2dt, -interval, interval);
+
+		// If we're stepping away further from t e [inTMin, inTMax] stop now
+		if ((t > inTMax && delta > 0.0f) || (t < inTMin && delta < 0.0f))
+			break;
+
+		// If we've converged, stop now
+		t += delta;
+		if (abs(delta) < 1.0e-4f)
+			break;
+	}
+
+	// Calculate the distance squared for the origin to the curve
+	outDistanceSq = position.LengthSq();
+	return t;
+}
+
+void PathConstraintPathHermite::GetIndexAndT(float inFraction, int &outIndex, float &outT) const
+{
+	int num_points = int(mPoints.size());
+
+	// Start by truncating the fraction to get the index and storing the remainder in t
+	int index = int(trunc(inFraction));
+	float t = inFraction - float(index);
+
+	if (IsLooping())
+	{
+		JPH_ASSERT(!mPoints.front().mPosition.IsClose(mPoints.back().mPosition), "A looping path should have a different first and last point!");
+
+		// Make sure index is positive by adding a multiple of num_points
+		if (index < 0)
+			index += (-index / num_points + 1) * num_points;
+
+		// Index needs to be modulo num_points
+		index = index % num_points;
+	}
+	else
+	{
+		// Clamp against range of points
+		if (index < 0)
+		{
+			index = 0;
+			t = 0.0f;
+		}
+		else if (index >= num_points - 1)
+		{
+			index = num_points - 2;
+			t = 1.0f;
+		}
+	}
+
+	outIndex = index;
+	outT = t;
+}
+
+float PathConstraintPathHermite::GetClosestPoint(Vec3Arg inPosition, float inFractionHint) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	int num_points = int(mPoints.size());
+
+	// Start with last point on the path, in the non-looping case we won't be visiting this point
+	float best_dist_sq = (mPoints[num_points - 1].mPosition - inPosition).LengthSq();
+	float best_t = float(num_points - 1);
+
+	// Loop over all points
+	for (int i = 0, max_i = IsLooping()? num_points : num_points - 1; i < max_i; ++i)
+	{
+		const Point &p1 = mPoints[i];
+		const Point &p2 = mPoints[(i + 1) % num_points];
+
+		// Make the curve relative to inPosition
+		Vec3 p1_pos = p1.mPosition - inPosition;
+		Vec3 p2_pos = p2.mPosition - inPosition;
+
+		// Get distance to p1
+		float dist_sq = p1_pos.LengthSq();
+		if (dist_sq < best_dist_sq)
+		{
+			best_t = float(i);
+			best_dist_sq = dist_sq;
+		}
+
+		// First find an interval for the closest point so that we can start doing Newton Raphson steps
+		float t_min, t_max;
+		sCalculateClosestPointThroughBisection(p1_pos, p1.mTangent, p2_pos, p2.mTangent, t_min, t_max);
+
+		if (t_min == t_max)
+		{
+			// If the function above returned no interval then it found the root already and we can just calculate the distance
+			Vec3 position, tangent;
+			sCalculatePositionAndTangent(p1_pos, p1.mTangent, p2_pos, p2.mTangent, t_min, position, tangent);
+			dist_sq = position.LengthSq();
+			if (dist_sq < best_dist_sq)
+			{
+				best_t = float(i) + t_min;
+				best_dist_sq = dist_sq;
+			}
+		}
+		else
+		{
+			// Get closest distance along curve segment
+			float t = sCalculateClosestPointThroughNewtonRaphson(p1_pos, p1.mTangent, p2_pos, p2.mTangent, t_min, t_max, dist_sq);
+			if (t >= 0.0f && t <= 1.0f && dist_sq < best_dist_sq)
+			{
+				best_t = float(i) + t;
+				best_dist_sq = dist_sq;
+			}
+		}
+	}
+
+	return best_t;
+}
+
+void PathConstraintPathHermite::GetPointOnPath(float inFraction, Vec3 &outPathPosition, Vec3 &outPathTangent, Vec3 &outPathNormal, Vec3 &outPathBinormal) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Determine which hermite spline segment we need
+	int index;
+	float t;
+	GetIndexAndT(inFraction, index, t);
+
+	// Get the points on the segment
+	const Point &p1 = mPoints[index];
+	const Point &p2 = mPoints[(index + 1) % int(mPoints.size())];
+
+	// Calculate the position and tangent on the path
+	Vec3 tangent;
+	sCalculatePositionAndTangent(p1.mPosition, p1.mTangent, p2.mPosition, p2.mTangent, t, outPathPosition, tangent);
+	outPathTangent = tangent.Normalized();
+
+	// Just linearly interpolate the normal
+	Vec3 normal = (1.0f - t) * p1.mNormal + t * p2.mNormal;
+
+	// Calculate binormal
+	outPathBinormal = normal.Cross(outPathTangent).Normalized();
+
+	// Recalculate normal so it is perpendicular to both (linear interpolation will cause it not to be)
+	outPathNormal = outPathTangent.Cross(outPathBinormal);
+	JPH_ASSERT(outPathNormal.IsNormalized());
+}
+
+void PathConstraintPathHermite::SaveBinaryState(StreamOut &inStream) const
+{
+	PathConstraintPath::SaveBinaryState(inStream);
+
+	inStream.Write(mPoints);
+}
+
+void PathConstraintPathHermite::RestoreBinaryState(StreamIn &inStream)
+{
+	PathConstraintPath::RestoreBinaryState(inStream);
+
+	inStream.Read(mPoints);
+}
+
+JPH_NAMESPACE_END