JoltPhysics/Jolt/Physics/Constraints/ConeConstraint.h

// 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/ConstraintPart/PointConstraintPart.h>
#include <Jolt/Physics/Constraints/ConstraintPart/AngleConstraintPart.h>

JPH_NAMESPACE_BEGIN

/// Cone constraint settings, used to create a cone constraint
class JPH_EXPORT ConeConstraintSettings final : public TwoBodyConstraintSettings
{
public:
	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, ConeConstraintSettings)

	// See: ConstraintSettings::SaveBinaryState
	virtual void				SaveBinaryState(StreamOut &inStream) const override;

	/// Create an an instance of this constraint
	virtual TwoBodyConstraint *	Create(Body &inBody1, Body &inBody2) const override;

	/// This determines in which space the constraint is setup, all properties below should be in the specified space
	EConstraintSpace			mSpace = EConstraintSpace::WorldSpace;

	/// Body 1 constraint reference frame (space determined by mSpace)
	RVec3						mPoint1 = RVec3::sZero();
	Vec3						mTwistAxis1 = Vec3::sAxisX();

	/// Body 2 constraint reference frame (space determined by mSpace)
	RVec3						mPoint2 = RVec3::sZero();
	Vec3						mTwistAxis2 = Vec3::sAxisX();

	/// Half of maximum angle between twist axis of body 1 and 2
	float						mHalfConeAngle = 0.0f;

protected:
	// See: ConstraintSettings::RestoreBinaryState
	virtual void				RestoreBinaryState(StreamIn &inStream) override;
};

/// A cone constraint constraints 2 bodies to a single point and limits the swing between the twist axis within a cone:
///
/// t1 . t2 <= cos(theta)
///
/// Where:
///
/// t1 = twist axis of body 1.
/// t2 = twist axis of body 2.
/// theta = half cone angle (angle from the principal axis of the cone to the edge).
///
/// Calculating the Jacobian:
///
/// Constraint equation:
///
/// C = t1 . t2 - cos(theta)
///
/// Derivative:
///
/// d/dt C = d/dt (t1 . t2) = (d/dt t1) . t2 + t1 . (d/dt t2) = (w1 x t1) . t2 + t1 . (w2 x t2) = (t1 x t2) . w1 + (t2 x t1) . w2
///
/// d/dt C = J v = [0, -t2 x t1, 0, t2 x t1] [v1, w1, v2, w2]
///
/// Where J is the Jacobian.
///
/// Note that this is the exact same equation as used in AngleConstraintPart if we use t2 x t1 as the world space axis
class JPH_EXPORT ConeConstraint final : public TwoBodyConstraint
{
public:
	JPH_OVERRIDE_NEW_DELETE

	/// Construct cone constraint
								ConeConstraint(Body &inBody1, Body &inBody2, const ConeConstraintSettings &inSettings);

	// Generic interface of a constraint
	virtual EConstraintSubType	GetSubType() const override					{ return EConstraintSubType::Cone; }
	virtual void				NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override;
	virtual void				SetupVelocityConstraint(float inDeltaTime) 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;
	virtual void				DrawConstraintLimits(DebugRenderer *inRenderer) const override;
#endif // JPH_DEBUG_RENDERER
	virtual void				SaveState(StateRecorder &inStream) const override;
	virtual void				RestoreState(StateRecorder &inStream) override;
	virtual Ref<ConstraintSettings> GetConstraintSettings() const override;

	// See: TwoBodyConstraint
	virtual Mat44				GetConstraintToBody1Matrix() const override;
	virtual Mat44				GetConstraintToBody2Matrix() const override;

	/// Update maximum angle between body 1 and 2 (see ConeConstraintSettings)
	void						SetHalfConeAngle(float inHalfConeAngle)		{ JPH_ASSERT(inHalfConeAngle >= 0.0f && inHalfConeAngle <= JPH_PI); mCosHalfConeAngle = Cos(inHalfConeAngle); }
	float						GetCosHalfConeAngle() const					{ return mCosHalfConeAngle; }

	///@name Get Lagrange multiplier from last physics update (the linear/angular impulse applied to satisfy the constraint)
	inline Vec3 				GetTotalLambdaPosition() const				{ return mPointConstraintPart.GetTotalLambda(); }
	inline float				GetTotalLambdaRotation() const				{ return mAngleConstraintPart.GetTotalLambda(); }

private:
	// Internal helper function to calculate the values below
	void						CalculateRotationConstraintProperties(Mat44Arg inRotation1, Mat44Arg inRotation2);

	// CONFIGURATION PROPERTIES FOLLOW

	// Local space constraint positions
	Vec3						mLocalSpacePosition1;
	Vec3						mLocalSpacePosition2;

	// Local space constraint axis
	Vec3						mLocalSpaceTwistAxis1;
	Vec3						mLocalSpaceTwistAxis2;

	// Angular limits
	float						mCosHalfConeAngle;

	// RUN TIME PROPERTIES FOLLOW

	// Axis and angle of rotation between the two bodies
	Vec3						mWorldSpaceRotationAxis;
	float						mCosTheta;

	// The constraint parts
	PointConstraintPart			mPointConstraintPart;
	AngleConstraintPart			mAngleConstraintPart;
};

JPH_NAMESPACE_END