JoltPhysics/Jolt/Physics/Constraints/ConeConstraint.cpp

// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT

#include <Jolt.h>

#include <Physics/Constraints/ConeConstraint.h>
#include <Physics/Body/Body.h>
#include <ObjectStream/TypeDeclarations.h>
#include <Core/StreamIn.h>
#include <Core/StreamOut.h>
#ifdef JPH_DEBUG_RENDERER
	#include <Renderer/DebugRenderer.h>
#endif // JPH_DEBUG_RENDERER

namespace JPH {

JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(ConeConstraintSettings)
{
	JPH_ADD_BASE_CLASS(ConeConstraintSettings, TwoBodyConstraintSettings)

	JPH_ADD_ATTRIBUTE(ConeConstraintSettings, mPoint1)
	JPH_ADD_ATTRIBUTE(ConeConstraintSettings, mTwistAxis1)
	JPH_ADD_ATTRIBUTE(ConeConstraintSettings, mPoint2)
	JPH_ADD_ATTRIBUTE(ConeConstraintSettings, mTwistAxis2)
	JPH_ADD_ATTRIBUTE(ConeConstraintSettings, mHalfConeAngle)
}

void ConeConstraintSettings::SaveBinaryState(StreamOut &inStream) const
{ 
	ConstraintSettings::SaveBinaryState(inStream);

	inStream.Write(mPoint1);
	inStream.Write(mTwistAxis1);
	inStream.Write(mPoint2);
	inStream.Write(mTwistAxis2);
	inStream.Write(mHalfConeAngle);
}

void ConeConstraintSettings::RestoreBinaryState(StreamIn &inStream)
{
	ConstraintSettings::RestoreBinaryState(inStream);

	inStream.Read(mPoint1);
	inStream.Read(mTwistAxis1);
	inStream.Read(mPoint2);
	inStream.Read(mTwistAxis2);
	inStream.Read(mHalfConeAngle);
}

TwoBodyConstraint *ConeConstraintSettings::Create(Body &inBody1, Body &inBody2) const
{
	return new ConeConstraint(inBody1, inBody2, *this);
}

ConeConstraint::ConeConstraint(Body &inBody1, Body &inBody2, const ConeConstraintSettings &inSettings) :
	TwoBodyConstraint(inBody1, inBody2, inSettings)
{
	Mat44 inv_transform1 = inBody1.GetInverseCenterOfMassTransform();
	Mat44 inv_transform2 = inBody2.GetInverseCenterOfMassTransform();

	// Store local positions
	mLocalSpacePosition1 = inv_transform1 * inSettings.mPoint1;
	mLocalSpacePosition2 = inv_transform2 * inSettings.mPoint2;

	// Store axis
	mLocalSpaceTwistAxis1 = inv_transform1.Multiply3x3(inSettings.mTwistAxis1);
	mLocalSpaceTwistAxis2 = inv_transform2.Multiply3x3(inSettings.mTwistAxis2);

	// Store limits
	SetHalfConeAngle(inSettings.mHalfConeAngle);

	// Initialize rotation axis to perpendicular of twist axis in case the angle between the twist axis is 0 in the first frame
	mWorldSpaceRotationAxis = inSettings.mTwistAxis1.GetNormalizedPerpendicular();
}

void ConeConstraint::CalculateRotationConstraintProperties(float inDeltaTime, Mat44Arg inRotation1, Mat44Arg inRotation2)
{
	// Rotation is along the cross product of both twist axis
	Vec3 twist1 = inRotation1.Multiply3x3(mLocalSpaceTwistAxis1);
	Vec3 twist2 = inRotation2.Multiply3x3(mLocalSpaceTwistAxis2);

	// Calculate dot product between twist axis, if it's smaller than the cone angle we need to correct
	mCosTheta = twist1.Dot(twist2);
	if (mCosTheta < mCosHalfConeAngle)
	{
		// Rotation axis is defined by the two twist axis
		Vec3 rot_axis = twist2.Cross(twist1);

		// If we can't find a rotation axis because the twist is too small, we'll use last frame's rotation axis
		float len = rot_axis.Length();
		if (len > 0.0f)
			mWorldSpaceRotationAxis = rot_axis / len;

		mAngleConstraintPart.CalculateConstraintProperties(inDeltaTime, *mBody1, *mBody2, mWorldSpaceRotationAxis);		
	}
	else
		mAngleConstraintPart.Deactivate();
}

void ConeConstraint::SetupVelocityConstraint(float inDeltaTime)
{
	Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation());
	Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation());
	mPointConstraintPart.CalculateConstraintProperties(*mBody1, rotation1, mLocalSpacePosition1, *mBody2, rotation2, mLocalSpacePosition2);
	CalculateRotationConstraintProperties(inDeltaTime, rotation1, rotation2);
}

void ConeConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio)
{
	// Warm starting: Apply previous frame impulse
	mPointConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
	mAngleConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
}

bool ConeConstraint::SolveVelocityConstraint(float inDeltaTime)
{
	bool pos = mPointConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2);

	bool rot = false;
	if (mAngleConstraintPart.IsActive())
		rot = mAngleConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceRotationAxis, 0, FLT_MAX);

	return pos || rot;
}

bool ConeConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte)
{
	mPointConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), mLocalSpacePosition1, *mBody2, Mat44::sRotation(mBody2->GetRotation()), mLocalSpacePosition2);
	bool pos = mPointConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inBaumgarte);

	bool rot = false;
	CalculateRotationConstraintProperties(inDeltaTime, Mat44::sRotation(mBody1->GetRotation()), Mat44::sRotation(mBody2->GetRotation()));
	if (mAngleConstraintPart.IsActive())
		rot = mAngleConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mCosTheta - mCosHalfConeAngle, inBaumgarte);

	return pos || rot;
}

#ifdef JPH_DEBUG_RENDERER
void ConeConstraint::DrawConstraint(DebugRenderer *inRenderer) const
{
	Mat44 transform1 = mBody1->GetCenterOfMassTransform();
	Mat44 transform2 = mBody2->GetCenterOfMassTransform();

	Vec3 p1 = transform1 * mLocalSpacePosition1;
	Vec3 p2 = transform2 * mLocalSpacePosition2;

	// Draw constraint
	inRenderer->DrawMarker(p1, Color::sRed, 0.1f);
	inRenderer->DrawMarker(p2, Color::sGreen, 0.1f);

	// Draw twist axis
	inRenderer->DrawLine(p1, p1 + mDrawConstraintSize * transform1.Multiply3x3(mLocalSpaceTwistAxis1), Color::sRed);
	inRenderer->DrawLine(p2, p2 + mDrawConstraintSize * transform2.Multiply3x3(mLocalSpaceTwistAxis2), Color::sGreen);
}

void ConeConstraint::DrawConstraintLimits(DebugRenderer *inRenderer) const
{
	// Get constraint properties in world space
	Mat44 transform1 = mBody1->GetCenterOfMassTransform();
	Vec3 position1 = transform1 * mLocalSpacePosition1;
	Vec3 twist_axis1 = transform1.Multiply3x3(mLocalSpaceTwistAxis1);
	Vec3 normal_axis1 = transform1.Multiply3x3(mLocalSpaceTwistAxis1.GetNormalizedPerpendicular());

	inRenderer->DrawOpenCone(position1, twist_axis1, normal_axis1, acos(mCosHalfConeAngle), mDrawConstraintSize * mCosHalfConeAngle, Color::sPurple, DebugRenderer::ECastShadow::Off);
}
#endif // JPH_DEBUG_RENDERER

void ConeConstraint::SaveState(StateRecorder &inStream) const
{
	TwoBodyConstraint::SaveState(inStream);

	mPointConstraintPart.SaveState(inStream);
	mAngleConstraintPart.SaveState(inStream);
	inStream.Write(mWorldSpaceRotationAxis); // When twist is too small, the rotation is used from last frame so we need to store it
}

void ConeConstraint::RestoreState(StateRecorder &inStream)
{
	TwoBodyConstraint::RestoreState(inStream);

	mPointConstraintPart.RestoreState(inStream);
	mAngleConstraintPart.RestoreState(inStream);
	inStream.Read(mWorldSpaceRotationAxis);
}

Mat44 ConeConstraint::GetConstraintToBody1Matrix() const 
{ 
	Vec3 perp = mLocalSpaceTwistAxis1.GetNormalizedPerpendicular(); 
	Vec3 perp2 = mLocalSpaceTwistAxis1.Cross(perp); 
	return Mat44(Vec4(mLocalSpaceTwistAxis1, 0), Vec4(perp, 0), Vec4(perp2, 0), Vec4(mLocalSpacePosition1, 1)); 
}

Mat44 ConeConstraint::GetConstraintToBody2Matrix() const 
{ 
	// Note: Incorrect in rotation around the twist axis (the perpendicular does not match that of body 1),
	// this should not matter as we're not limiting rotation around the twist axis.
	Vec3 perp = mLocalSpaceTwistAxis2.GetNormalizedPerpendicular(); 
	Vec3 perp2 = mLocalSpaceTwistAxis2.Cross(perp); 
	return Mat44(Vec4(mLocalSpaceTwistAxis2, 0), Vec4(perp, 0), Vec4(perp2, 0), Vec4(mLocalSpacePosition2, 1)); 
}

} // JPH