// 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/PathConstraint.h>
#include <Jolt/Physics/Body/Body.h>
#include <Jolt/Core/StringTools.h>
#include <Jolt/ObjectStream/TypeDeclarations.h>
#include <Jolt/Core/StreamIn.h>
#include <Jolt/Core/StreamOut.h>
#ifdef JPH_DEBUG_RENDERER
#include <Jolt/Renderer/DebugRenderer.h>
#endif // JPH_DEBUG_RENDERER
JPH_NAMESPACE_BEGIN
JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(PathConstraintSettings)
{
JPH_ADD_BASE_CLASS(PathConstraintSettings, TwoBodyConstraintSettings)
JPH_ADD_ATTRIBUTE(PathConstraintSettings, mPath)
JPH_ADD_ATTRIBUTE(PathConstraintSettings, mPathPosition)
JPH_ADD_ATTRIBUTE(PathConstraintSettings, mPathRotation)
JPH_ADD_ATTRIBUTE(PathConstraintSettings, mPathFraction)
JPH_ADD_ATTRIBUTE(PathConstraintSettings, mMaxFrictionForce)
JPH_ADD_ATTRIBUTE(PathConstraintSettings, mPositionMotorSettings)
JPH_ADD_ENUM_ATTRIBUTE(PathConstraintSettings, mRotationConstraintType)
}
void PathConstraintSettings::SaveBinaryState(StreamOut &inStream) const
{
ConstraintSettings::SaveBinaryState(inStream);
mPath->SaveBinaryState(inStream);
inStream.Write(mPathPosition);
inStream.Write(mPathRotation);
inStream.Write(mPathFraction);
inStream.Write(mMaxFrictionForce);
inStream.Write(mRotationConstraintType);
mPositionMotorSettings.SaveBinaryState(inStream);
}
void PathConstraintSettings::RestoreBinaryState(StreamIn &inStream)
{
ConstraintSettings::RestoreBinaryState(inStream);
PathConstraintPath::PathResult result = PathConstraintPath::sRestoreFromBinaryState(inStream);
if (!result.HasError())
mPath = result.Get();
inStream.Read(mPathPosition);
inStream.Read(mPathRotation);
inStream.Read(mPathFraction);
inStream.Read(mMaxFrictionForce);
inStream.Read(mRotationConstraintType);
mPositionMotorSettings.RestoreBinaryState(inStream);
}
TwoBodyConstraint *PathConstraintSettings::Create(Body &inBody1, Body &inBody2) const
{
return new PathConstraint(inBody1, inBody2, *this);
}
PathConstraint::PathConstraint(Body &inBody1, Body &inBody2, const PathConstraintSettings &inSettings) :
TwoBodyConstraint(inBody1, inBody2, inSettings),
mRotationConstraintType(inSettings.mRotationConstraintType),
mMaxFrictionForce(inSettings.mMaxFrictionForce),
mPositionMotorSettings(inSettings.mPositionMotorSettings)
{
// Calculate transform that takes us from the path start to center of mass space of body 1
mPathToBody1 = Mat44::sRotationTranslation(inSettings.mPathRotation, inSettings.mPathPosition - inBody1.GetShape()->GetCenterOfMass());
SetPath(inSettings.mPath, inSettings.mPathFraction);
}
void PathConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM)
{
if (mBody1->GetID() == inBodyID)
mPathToBody1.SetTranslation(mPathToBody1.GetTranslation() - inDeltaCOM);
else if (mBody2->GetID() == inBodyID)
mPathToBody2.SetTranslation(mPathToBody2.GetTranslation() - inDeltaCOM);
}
void PathConstraint::SetPath(const PathConstraintPath *inPath, float inPathFraction)
{
mPath = inPath;
mPathFraction = inPathFraction;
if (mPath != nullptr)
{
// Get the point on the path for this fraction
Vec3 path_point, path_tangent, path_normal, path_binormal;
mPath->GetPointOnPath(mPathFraction, path_point, path_tangent, path_normal, path_binormal);
// Construct the matrix that takes us from the closest point on the path to body 2 center of mass space
Mat44 closest_point_to_path(Vec4(path_tangent, 0), Vec4(path_binormal, 0), Vec4(path_normal, 0), Vec4(path_point, 1));
Mat44 cp_to_body1 = mPathToBody1 * closest_point_to_path;
mPathToBody2 = (mBody2->GetInverseCenterOfMassTransform() * mBody1->GetCenterOfMassTransform()).ToMat44() * cp_to_body1;
// Calculate initial orientation
if (mRotationConstraintType == EPathRotationConstraintType::FullyConstrained)
mInvInitialOrientation = RotationEulerConstraintPart::sGetInvInitialOrientation(*mBody1, *mBody2);
}
}
void PathConstraint::CalculateConstraintProperties(float inDeltaTime)
{
// Get transforms of body 1 and 2
RMat44 transform1 = mBody1->GetCenterOfMassTransform();
RMat44 transform2 = mBody2->GetCenterOfMassTransform();
// Get the transform of the path transform as seen from body 1 in world space
RMat44 path_to_world_1 = transform1 * mPathToBody1;
// Get the transform of from the point on path that body 2 is attached to in world space
RMat44 path_to_world_2 = transform2 * mPathToBody2;
// Calculate new closest point on path
RVec3 position2 = path_to_world_2.GetTranslation();
Vec3 position2_local_to_path = Vec3(path_to_world_1.InversedRotationTranslation() * position2);
mPathFraction = mPath->GetClosestPoint(position2_local_to_path, mPathFraction);
// Get the point on the path for this fraction
Vec3 path_point, path_tangent, path_normal, path_binormal;
mPath->GetPointOnPath(mPathFraction, path_point, path_tangent, path_normal, path_binormal);
// Calculate R1 and R2
RVec3 path_point_ws = path_to_world_1 * path_point;
mR1 = Vec3(path_point_ws - mBody1->GetCenterOfMassPosition());
mR2 = Vec3(position2 - mBody2->GetCenterOfMassPosition());
// Calculate U = X2 + R2 - X1 - R1
mU = Vec3(position2 - path_point_ws);
// Calculate world space normals
mPathNormal = path_to_world_1.Multiply3x3(path_normal);
mPathBinormal = path_to_world_1.Multiply3x3(path_binormal);
// Calculate slide axis
mPathTangent = path_to_world_1.Multiply3x3(path_tangent);
// Prepare constraint part for position constraint to slide along the path
mPositionConstraintPart.CalculateConstraintProperties(*mBody1, transform1.GetRotation(), mR1 + mU, *mBody2, transform2.GetRotation(), mR2, mPathNormal, mPathBinormal);
// Check if closest point is on the boundary of the path and if so apply limit
if (!mPath->IsLooping() && (mPathFraction <= 0.0f || mPathFraction >= mPath->GetPathMaxFraction()))
mPositionLimitsConstraintPart.CalculateConstraintProperties(*mBody1, mR1 + mU, *mBody2, mR2, mPathTangent);
else
mPositionLimitsConstraintPart.Deactivate();
// Prepare rotation constraint part
switch (mRotationConstraintType)
{
case EPathRotationConstraintType::Free:
// No rotational limits
break;
case EPathRotationConstraintType::ConstrainAroundTangent:
mHingeConstraintPart.CalculateConstraintProperties(*mBody1, transform1.GetRotation(), mPathTangent, *mBody2, transform2.GetRotation(), path_to_world_2.GetAxisX());
break;
case EPathRotationConstraintType::ConstrainAroundNormal:
mHingeConstraintPart.CalculateConstraintProperties(*mBody1, transform1.GetRotation(), mPathNormal, *mBody2, transform2.GetRotation(), path_to_world_2.GetAxisZ());
break;
case EPathRotationConstraintType::ConstrainAroundBinormal:
mHingeConstraintPart.CalculateConstraintProperties(*mBody1, transform1.GetRotation(), mPathBinormal, *mBody2, transform2.GetRotation(), path_to_world_2.GetAxisY());
break;
case EPathRotationConstraintType::ConstrainToPath:
// We need to calculate the inverse of the rotation from body 1 to body 2 for the current path position (see: RotationEulerConstraintPart::sGetInvInitialOrientation)
// RotationBody2 = RotationBody1 * InitialOrientation <=> InitialOrientation^-1 = RotationBody2^-1 * RotationBody1
// We can express RotationBody2 in terms of RotationBody1: RotationBody2 = RotationBody1 * PathToBody1 * RotationClosestPointOnPath * PathToBody2^-1
// Combining these two: InitialOrientation^-1 = PathToBody2 * (PathToBody1 * RotationClosestPointOnPath)^-1
mInvInitialOrientation = mPathToBody2.Multiply3x3RightTransposed(mPathToBody1.Multiply3x3(Mat44(Vec4(path_tangent, 0), Vec4(path_binormal, 0), Vec4(path_normal, 0), Vec4::sZero()))).GetQuaternion();
[[fallthrough]];
case EPathRotationConstraintType::FullyConstrained:
mRotationConstraintPart.CalculateConstraintProperties(*mBody1, transform1.GetRotation(), *mBody2, transform2.GetRotation());
break;
}
// Motor properties
switch (mPositionMotorState)
{
case EMotorState::Off:
if (mMaxFrictionForce > 0.0f)
mPositionMotorConstraintPart.CalculateConstraintProperties(*mBody1, mR1 + mU, *mBody2, mR2, mPathTangent);
else
mPositionMotorConstraintPart.Deactivate();
break;
case EMotorState::Velocity:
mPositionMotorConstraintPart.CalculateConstraintProperties(*mBody1, mR1 + mU, *mBody2, mR2, mPathTangent, -mTargetVelocity);
break;
case EMotorState::Position:
if (mPositionMotorSettings.mSpringSettings.HasStiffness())
{
// Calculate constraint value to drive to
float c;
if (mPath->IsLooping())
{
float max_fraction = mPath->GetPathMaxFraction();
c = fmod(mPathFraction - mTargetPathFraction, max_fraction);
float half_max_fraction = 0.5f * max_fraction;
if (c > half_max_fraction)
c -= max_fraction;
else if (c < -half_max_fraction)
c += max_fraction;
}
else
c = mPathFraction - mTargetPathFraction;
mPositionMotorConstraintPart.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, mR1 + mU, *mBody2, mR2, mPathTangent, 0.0f, c, mPositionMotorSettings.mSpringSettings);
}
else
mPositionMotorConstraintPart.Deactivate();
break;
}
}
void PathConstraint::SetupVelocityConstraint(float inDeltaTime)
{
CalculateConstraintProperties(inDeltaTime);
}
void PathConstraint::ResetWarmStart()
{
mPositionMotorConstraintPart.Deactivate();
mPositionConstraintPart.Deactivate();
mPositionLimitsConstraintPart.Deactivate();
mHingeConstraintPart.Deactivate();
mRotationConstraintPart.Deactivate();
}
void PathConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio)
{
// Warm starting: Apply previous frame impulse
mPositionMotorConstraintPart.WarmStart(*mBody1, *mBody2, mPathTangent, inWarmStartImpulseRatio);
mPositionConstraintPart.WarmStart(*mBody1, *mBody2, mPathNormal, mPathBinormal, inWarmStartImpulseRatio);
mPositionLimitsConstraintPart.WarmStart(*mBody1, *mBody2, mPathTangent, inWarmStartImpulseRatio);
switch (mRotationConstraintType)
{
case EPathRotationConstraintType::Free:
// No rotational limits
break;
case EPathRotationConstraintType::ConstrainAroundTangent:
case EPathRotationConstraintType::ConstrainAroundNormal:
case EPathRotationConstraintType::ConstrainAroundBinormal:
mHingeConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
break;
case EPathRotationConstraintType::ConstrainToPath:
case EPathRotationConstraintType::FullyConstrained:
mRotationConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
break;
}
}
bool PathConstraint::SolveVelocityConstraint(float inDeltaTime)
{
// Solve motor
bool motor = false;
if (mPositionMotorConstraintPart.IsActive())
{
switch (mPositionMotorState)
{
case EMotorState::Off:
{
float max_lambda = mMaxFrictionForce * inDeltaTime;
motor = mPositionMotorConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mPathTangent, -max_lambda, max_lambda);
break;
}
case EMotorState::Velocity:
case EMotorState::Position:
motor = mPositionMotorConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mPathTangent, inDeltaTime * mPositionMotorSettings.mMinForceLimit, inDeltaTime * mPositionMotorSettings.mMaxForceLimit);
break;
}
}
// Solve position constraint along 2 axis
bool pos = mPositionConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mPathNormal, mPathBinormal);
// Solve limits along path axis
bool limit = false;
if (mPositionLimitsConstraintPart.IsActive())
{
if (mPathFraction <= 0.0f)
limit = mPositionLimitsConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mPathTangent, 0, FLT_MAX);
else
{
JPH_ASSERT(mPathFraction >= mPath->GetPathMaxFraction());
limit = mPositionLimitsConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mPathTangent, -FLT_MAX, 0);
}
}
// Solve rotational constraint
// Note, this is not entirely correct, we should apply a velocity constraint so that the body will actually follow the path
// by looking at the derivative of the tangent, normal or binormal but we don't. This means the position constraint solver
// will need to correct the orientation error that builds up, which in turn means that the simulation is not physically correct.
bool rot = false;
switch (mRotationConstraintType)
{
case EPathRotationConstraintType::Free:
// No rotational limits
break;
case EPathRotationConstraintType::ConstrainAroundTangent:
case EPathRotationConstraintType::ConstrainAroundNormal:
case EPathRotationConstraintType::ConstrainAroundBinormal:
rot = mHingeConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2);
break;
case EPathRotationConstraintType::ConstrainToPath:
case EPathRotationConstraintType::FullyConstrained:
rot = mRotationConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2);
break;
}
return motor || pos || limit || rot;
}
bool PathConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte)
{
// Update constraint properties (bodies may have moved)
CalculateConstraintProperties(inDeltaTime);
// Solve position constraint along 2 axis
bool pos = mPositionConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mU, mPathNormal, mPathBinormal, inBaumgarte);
// Solve limits along path axis
bool limit = false;
if (mPositionLimitsConstraintPart.IsActive())
{
if (mPathFraction <= 0.0f)
limit = mPositionLimitsConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mPathTangent, mU.Dot(mPathTangent), inBaumgarte);
else
{
JPH_ASSERT(mPathFraction >= mPath->GetPathMaxFraction());
limit = mPositionLimitsConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mPathTangent, mU.Dot(mPathTangent), inBaumgarte);
}
}
// Solve rotational constraint
bool rot = false;
switch (mRotationConstraintType)
{
case EPathRotationConstraintType::Free:
// No rotational limits
break;
case EPathRotationConstraintType::ConstrainAroundTangent:
case EPathRotationConstraintType::ConstrainAroundNormal:
case EPathRotationConstraintType::ConstrainAroundBinormal:
rot = mHingeConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inBaumgarte);
break;
case EPathRotationConstraintType::ConstrainToPath:
case EPathRotationConstraintType::FullyConstrained:
rot = mRotationConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mInvInitialOrientation, inBaumgarte);
break;
}
return pos || limit || rot;
}
#ifdef JPH_DEBUG_RENDERER
void PathConstraint::DrawConstraint(DebugRenderer *inRenderer) const
{
if (mPath != nullptr)
{
// Draw the path in world space
RMat44 path_to_world = mBody1->GetCenterOfMassTransform() * mPathToBody1;
mPath->DrawPath(inRenderer, path_to_world);
// Draw anchor point of both bodies in world space
RVec3 x1 = mBody1->GetCenterOfMassPosition() + mR1;
RVec3 x2 = mBody2->GetCenterOfMassPosition() + mR2;
inRenderer->DrawMarker(x1, Color::sYellow, 0.1f);
inRenderer->DrawMarker(x2, Color::sYellow, 0.1f);
inRenderer->DrawArrow(x1, x1 + mPathTangent, Color::sBlue, 0.1f);
inRenderer->DrawArrow(x1, x1 + mPathNormal, Color::sRed, 0.1f);
inRenderer->DrawArrow(x1, x1 + mPathBinormal, Color::sGreen, 0.1f);
inRenderer->DrawText3D(x1, StringFormat("%.1f", (double)mPathFraction));
// Draw motor
switch (mPositionMotorState)
{
case EMotorState::Position:
{
// Draw target marker
Vec3 position, tangent, normal, binormal;
mPath->GetPointOnPath(mTargetPathFraction, position, tangent, normal, binormal);
inRenderer->DrawMarker(path_to_world * position, Color::sYellow, 1.0f);
break;
}
case EMotorState::Velocity:
{
RVec3 position = mBody2->GetCenterOfMassPosition() + mR2;
inRenderer->DrawArrow(position, position + mPathTangent * mTargetVelocity, Color::sRed, 0.1f);
break;
}
case EMotorState::Off:
break;
}
}
}
#endif // JPH_DEBUG_RENDERER
void PathConstraint::SaveState(StateRecorder &inStream) const
{
TwoBodyConstraint::SaveState(inStream);
mPositionConstraintPart.SaveState(inStream);
mPositionLimitsConstraintPart.SaveState(inStream);
mPositionMotorConstraintPart.SaveState(inStream);
mHingeConstraintPart.SaveState(inStream);
mRotationConstraintPart.SaveState(inStream);
inStream.Write(mMaxFrictionForce);
inStream.Write(mPositionMotorSettings);
inStream.Write(mPositionMotorState);
inStream.Write(mTargetVelocity);
inStream.Write(mTargetPathFraction);
inStream.Write(mPathFraction);
}
void PathConstraint::RestoreState(StateRecorder &inStream)
{
TwoBodyConstraint::RestoreState(inStream);
mPositionConstraintPart.RestoreState(inStream);
mPositionLimitsConstraintPart.RestoreState(inStream);
mPositionMotorConstraintPart.RestoreState(inStream);
mHingeConstraintPart.RestoreState(inStream);
mRotationConstraintPart.RestoreState(inStream);
inStream.Read(mMaxFrictionForce);
inStream.Read(mPositionMotorSettings);
inStream.Read(mPositionMotorState);
inStream.Read(mTargetVelocity);
inStream.Read(mTargetPathFraction);
inStream.Read(mPathFraction);
}
Ref<ConstraintSettings> PathConstraint::GetConstraintSettings() const
{
JPH_ASSERT(false); // Not implemented yet
return nullptr;
}
JPH_NAMESPACE_END