901 lines
33 KiB
C++
901 lines
33 KiB
C++
// 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/SixDOFConstraint.h>
|
|
#include <Jolt/Physics/Body/Body.h>
|
|
#include <Jolt/Geometry/Ellipse.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(SixDOFConstraintSettings)
|
|
{
|
|
JPH_ADD_BASE_CLASS(SixDOFConstraintSettings, TwoBodyConstraintSettings)
|
|
|
|
JPH_ADD_ENUM_ATTRIBUTE(SixDOFConstraintSettings, mSpace)
|
|
JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mPosition1)
|
|
JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mAxisX1)
|
|
JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mAxisY1)
|
|
JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mPosition2)
|
|
JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mAxisX2)
|
|
JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mAxisY2)
|
|
JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mMaxFriction)
|
|
JPH_ADD_ENUM_ATTRIBUTE(SixDOFConstraintSettings, mSwingType)
|
|
JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mLimitMin)
|
|
JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mLimitMax)
|
|
JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mLimitsSpringSettings)
|
|
JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mMotorSettings)
|
|
}
|
|
|
|
void SixDOFConstraintSettings::SaveBinaryState(StreamOut &inStream) const
|
|
{
|
|
ConstraintSettings::SaveBinaryState(inStream);
|
|
|
|
inStream.Write(mSpace);
|
|
inStream.Write(mPosition1);
|
|
inStream.Write(mAxisX1);
|
|
inStream.Write(mAxisY1);
|
|
inStream.Write(mPosition2);
|
|
inStream.Write(mAxisX2);
|
|
inStream.Write(mAxisY2);
|
|
inStream.Write(mMaxFriction);
|
|
inStream.Write(mSwingType);
|
|
inStream.Write(mLimitMin);
|
|
inStream.Write(mLimitMax);
|
|
for (const SpringSettings &s : mLimitsSpringSettings)
|
|
s.SaveBinaryState(inStream);
|
|
for (const MotorSettings &m : mMotorSettings)
|
|
m.SaveBinaryState(inStream);
|
|
}
|
|
|
|
void SixDOFConstraintSettings::RestoreBinaryState(StreamIn &inStream)
|
|
{
|
|
ConstraintSettings::RestoreBinaryState(inStream);
|
|
|
|
inStream.Read(mSpace);
|
|
inStream.Read(mPosition1);
|
|
inStream.Read(mAxisX1);
|
|
inStream.Read(mAxisY1);
|
|
inStream.Read(mPosition2);
|
|
inStream.Read(mAxisX2);
|
|
inStream.Read(mAxisY2);
|
|
inStream.Read(mMaxFriction);
|
|
inStream.Read(mSwingType);
|
|
inStream.Read(mLimitMin);
|
|
inStream.Read(mLimitMax);
|
|
for (SpringSettings &s : mLimitsSpringSettings)
|
|
s.RestoreBinaryState(inStream);
|
|
for (MotorSettings &m : mMotorSettings)
|
|
m.RestoreBinaryState(inStream);
|
|
}
|
|
|
|
TwoBodyConstraint *SixDOFConstraintSettings::Create(Body &inBody1, Body &inBody2) const
|
|
{
|
|
return new SixDOFConstraint(inBody1, inBody2, *this);
|
|
}
|
|
|
|
void SixDOFConstraint::UpdateTranslationLimits()
|
|
{
|
|
// Set to zero if the limits are inversed
|
|
for (int i = EAxis::TranslationX; i <= EAxis::TranslationZ; ++i)
|
|
if (mLimitMin[i] > mLimitMax[i])
|
|
mLimitMin[i] = mLimitMax[i] = 0.0f;
|
|
}
|
|
|
|
void SixDOFConstraint::UpdateRotationLimits()
|
|
{
|
|
if (mSwingTwistConstraintPart.GetSwingType() == ESwingType::Cone)
|
|
{
|
|
// Cone swing upper limit needs to be positive
|
|
mLimitMax[EAxis::RotationY] = max(0.0f, mLimitMax[EAxis::RotationY]);
|
|
mLimitMax[EAxis::RotationZ] = max(0.0f, mLimitMax[EAxis::RotationZ]);
|
|
|
|
// Cone swing limits only support symmetric ranges
|
|
mLimitMin[EAxis::RotationY] = -mLimitMax[EAxis::RotationY];
|
|
mLimitMin[EAxis::RotationZ] = -mLimitMax[EAxis::RotationZ];
|
|
}
|
|
|
|
for (int i = EAxis::RotationX; i <= EAxis::RotationZ; ++i)
|
|
{
|
|
// Clamp to [-PI, PI] range
|
|
mLimitMin[i] = Clamp(mLimitMin[i], -JPH_PI, JPH_PI);
|
|
mLimitMax[i] = Clamp(mLimitMax[i], -JPH_PI, JPH_PI);
|
|
|
|
// Set to zero if the limits are inversed
|
|
if (mLimitMin[i] > mLimitMax[i])
|
|
mLimitMin[i] = mLimitMax[i] = 0.0f;
|
|
}
|
|
|
|
// Pass limits on to constraint part
|
|
mSwingTwistConstraintPart.SetLimits(mLimitMin[EAxis::RotationX], mLimitMax[EAxis::RotationX], mLimitMin[EAxis::RotationY], mLimitMax[EAxis::RotationY], mLimitMin[EAxis::RotationZ], mLimitMax[EAxis::RotationZ]);
|
|
}
|
|
|
|
void SixDOFConstraint::UpdateFixedFreeAxis()
|
|
{
|
|
uint8 old_free_axis = mFreeAxis;
|
|
uint8 old_fixed_axis = mFixedAxis;
|
|
|
|
// Cache which axis are fixed and which ones are free
|
|
mFreeAxis = 0;
|
|
mFixedAxis = 0;
|
|
for (int a = 0; a < EAxis::Num; ++a)
|
|
{
|
|
float limit = a >= EAxis::RotationX? JPH_PI : FLT_MAX;
|
|
|
|
if (mLimitMin[a] >= mLimitMax[a])
|
|
mFixedAxis |= 1 << a;
|
|
else if (mLimitMin[a] <= -limit && mLimitMax[a] >= limit)
|
|
mFreeAxis |= 1 << a;
|
|
}
|
|
|
|
// On change we deactivate all constraints to reset warm starting
|
|
if (old_free_axis != mFreeAxis || old_fixed_axis != mFixedAxis)
|
|
{
|
|
for (AxisConstraintPart &c : mTranslationConstraintPart)
|
|
c.Deactivate();
|
|
mPointConstraintPart.Deactivate();
|
|
mSwingTwistConstraintPart.Deactivate();
|
|
mRotationConstraintPart.Deactivate();
|
|
for (AxisConstraintPart &c : mMotorTranslationConstraintPart)
|
|
c.Deactivate();
|
|
for (AngleConstraintPart &c : mMotorRotationConstraintPart)
|
|
c.Deactivate();
|
|
}
|
|
}
|
|
|
|
SixDOFConstraint::SixDOFConstraint(Body &inBody1, Body &inBody2, const SixDOFConstraintSettings &inSettings) :
|
|
TwoBodyConstraint(inBody1, inBody2, inSettings)
|
|
{
|
|
// Override swing type
|
|
mSwingTwistConstraintPart.SetSwingType(inSettings.mSwingType);
|
|
|
|
// Calculate rotation needed to go from constraint space to body1 local space
|
|
Vec3 axis_z1 = inSettings.mAxisX1.Cross(inSettings.mAxisY1);
|
|
Mat44 c_to_b1(Vec4(inSettings.mAxisX1, 0), Vec4(inSettings.mAxisY1, 0), Vec4(axis_z1, 0), Vec4(0, 0, 0, 1));
|
|
mConstraintToBody1 = c_to_b1.GetQuaternion();
|
|
|
|
// Calculate rotation needed to go from constraint space to body2 local space
|
|
Vec3 axis_z2 = inSettings.mAxisX2.Cross(inSettings.mAxisY2);
|
|
Mat44 c_to_b2(Vec4(inSettings.mAxisX2, 0), Vec4(inSettings.mAxisY2, 0), Vec4(axis_z2, 0), Vec4(0, 0, 0, 1));
|
|
mConstraintToBody2 = c_to_b2.GetQuaternion();
|
|
|
|
if (inSettings.mSpace == EConstraintSpace::WorldSpace)
|
|
{
|
|
// If all properties were specified in world space, take them to local space now
|
|
mLocalSpacePosition1 = Vec3(inBody1.GetInverseCenterOfMassTransform() * inSettings.mPosition1);
|
|
mConstraintToBody1 = inBody1.GetRotation().Conjugated() * mConstraintToBody1;
|
|
|
|
mLocalSpacePosition2 = Vec3(inBody2.GetInverseCenterOfMassTransform() * inSettings.mPosition2);
|
|
mConstraintToBody2 = inBody2.GetRotation().Conjugated() * mConstraintToBody2;
|
|
}
|
|
else
|
|
{
|
|
mLocalSpacePosition1 = Vec3(inSettings.mPosition1);
|
|
mLocalSpacePosition2 = Vec3(inSettings.mPosition2);
|
|
}
|
|
|
|
// Copy translation and rotation limits
|
|
memcpy(mLimitMin, inSettings.mLimitMin, sizeof(mLimitMin));
|
|
memcpy(mLimitMax, inSettings.mLimitMax, sizeof(mLimitMax));
|
|
memcpy(mLimitsSpringSettings, inSettings.mLimitsSpringSettings, sizeof(mLimitsSpringSettings));
|
|
UpdateTranslationLimits();
|
|
UpdateRotationLimits();
|
|
UpdateFixedFreeAxis();
|
|
CacheHasSpringLimits();
|
|
|
|
// Store friction settings
|
|
memcpy(mMaxFriction, inSettings.mMaxFriction, sizeof(mMaxFriction));
|
|
|
|
// Store motor settings
|
|
for (int i = 0; i < EAxis::Num; ++i)
|
|
mMotorSettings[i] = inSettings.mMotorSettings[i];
|
|
|
|
// Cache if motors are active (motors are off initially, but we may have friction)
|
|
CacheTranslationMotorActive();
|
|
CacheRotationMotorActive();
|
|
}
|
|
|
|
void SixDOFConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM)
|
|
{
|
|
if (mBody1->GetID() == inBodyID)
|
|
mLocalSpacePosition1 -= inDeltaCOM;
|
|
else if (mBody2->GetID() == inBodyID)
|
|
mLocalSpacePosition2 -= inDeltaCOM;
|
|
}
|
|
|
|
void SixDOFConstraint::SetTranslationLimits(Vec3Arg inLimitMin, Vec3Arg inLimitMax)
|
|
{
|
|
mLimitMin[EAxis::TranslationX] = inLimitMin.GetX();
|
|
mLimitMin[EAxis::TranslationY] = inLimitMin.GetY();
|
|
mLimitMin[EAxis::TranslationZ] = inLimitMin.GetZ();
|
|
mLimitMax[EAxis::TranslationX] = inLimitMax.GetX();
|
|
mLimitMax[EAxis::TranslationY] = inLimitMax.GetY();
|
|
mLimitMax[EAxis::TranslationZ] = inLimitMax.GetZ();
|
|
|
|
UpdateTranslationLimits();
|
|
UpdateFixedFreeAxis();
|
|
}
|
|
|
|
void SixDOFConstraint::SetRotationLimits(Vec3Arg inLimitMin, Vec3Arg inLimitMax)
|
|
{
|
|
mLimitMin[EAxis::RotationX] = inLimitMin.GetX();
|
|
mLimitMin[EAxis::RotationY] = inLimitMin.GetY();
|
|
mLimitMin[EAxis::RotationZ] = inLimitMin.GetZ();
|
|
mLimitMax[EAxis::RotationX] = inLimitMax.GetX();
|
|
mLimitMax[EAxis::RotationY] = inLimitMax.GetY();
|
|
mLimitMax[EAxis::RotationZ] = inLimitMax.GetZ();
|
|
|
|
UpdateRotationLimits();
|
|
UpdateFixedFreeAxis();
|
|
}
|
|
|
|
void SixDOFConstraint::SetMaxFriction(EAxis inAxis, float inFriction)
|
|
{
|
|
mMaxFriction[inAxis] = inFriction;
|
|
|
|
if (inAxis >= EAxis::TranslationX && inAxis <= EAxis::TranslationZ)
|
|
CacheTranslationMotorActive();
|
|
else
|
|
CacheRotationMotorActive();
|
|
}
|
|
|
|
void SixDOFConstraint::GetPositionConstraintProperties(Vec3 &outR1PlusU, Vec3 &outR2, Vec3 &outU) const
|
|
{
|
|
RVec3 p1 = mBody1->GetCenterOfMassTransform() * mLocalSpacePosition1;
|
|
RVec3 p2 = mBody2->GetCenterOfMassTransform() * mLocalSpacePosition2;
|
|
outR1PlusU = Vec3(p2 - mBody1->GetCenterOfMassPosition()); // r1 + u = (p1 - x1) + (p2 - p1) = p2 - x1
|
|
outR2 = Vec3(p2 - mBody2->GetCenterOfMassPosition());
|
|
outU = Vec3(p2 - p1);
|
|
}
|
|
|
|
Quat SixDOFConstraint::GetRotationInConstraintSpace() const
|
|
{
|
|
// Let b1, b2 be the center of mass transform of body1 and body2 (For body1 this is mBody1->GetCenterOfMassTransform())
|
|
// Let c1, c2 be the transform that takes a vector from constraint space to local space of body1 and body2 (For body1 this is Mat44::sRotationTranslation(mConstraintToBody1, mLocalSpacePosition1))
|
|
// Let q be the rotation of the constraint in constraint space
|
|
// b2 takes a vector from the local space of body2 to world space
|
|
// To express this in terms of b1: b2 = b1 * c1 * q * c2^-1
|
|
// c2^-1 goes from local body 2 space to constraint space
|
|
// q rotates the constraint
|
|
// c1 goes from constraint space to body 1 local space
|
|
// b1 goes from body 1 local space to world space
|
|
// So when the body rotations are given, q = (b1 * c1)^-1 * b2 c2
|
|
// Or: q = (q1 * c1)^-1 * (q2 * c2) if we're only interested in rotations
|
|
return (mBody1->GetRotation() * mConstraintToBody1).Conjugated() * mBody2->GetRotation() * mConstraintToBody2;
|
|
}
|
|
|
|
void SixDOFConstraint::CacheTranslationMotorActive()
|
|
{
|
|
mTranslationMotorActive = mMotorState[EAxis::TranslationX] != EMotorState::Off
|
|
|| mMotorState[EAxis::TranslationY] != EMotorState::Off
|
|
|| mMotorState[EAxis::TranslationZ] != EMotorState::Off
|
|
|| HasFriction(EAxis::TranslationX)
|
|
|| HasFriction(EAxis::TranslationY)
|
|
|| HasFriction(EAxis::TranslationZ);
|
|
}
|
|
|
|
void SixDOFConstraint::CacheRotationMotorActive()
|
|
{
|
|
mRotationMotorActive = mMotorState[EAxis::RotationX] != EMotorState::Off
|
|
|| mMotorState[EAxis::RotationY] != EMotorState::Off
|
|
|| mMotorState[EAxis::RotationZ] != EMotorState::Off
|
|
|| HasFriction(EAxis::RotationX)
|
|
|| HasFriction(EAxis::RotationY)
|
|
|| HasFriction(EAxis::RotationZ);
|
|
}
|
|
|
|
void SixDOFConstraint::CacheRotationPositionMotorActive()
|
|
{
|
|
mRotationPositionMotorActive = 0;
|
|
for (int i = 0; i < 3; ++i)
|
|
if (mMotorState[EAxis::RotationX + i] == EMotorState::Position)
|
|
mRotationPositionMotorActive |= 1 << i;
|
|
}
|
|
|
|
void SixDOFConstraint::CacheHasSpringLimits()
|
|
{
|
|
mHasSpringLimits = mLimitsSpringSettings[EAxis::TranslationX].mFrequency > 0.0f
|
|
|| mLimitsSpringSettings[EAxis::TranslationY].mFrequency > 0.0f
|
|
|| mLimitsSpringSettings[EAxis::TranslationZ].mFrequency > 0.0f;
|
|
}
|
|
|
|
void SixDOFConstraint::SetMotorState(EAxis inAxis, EMotorState inState)
|
|
{
|
|
JPH_ASSERT(inState == EMotorState::Off || mMotorSettings[inAxis].IsValid());
|
|
|
|
if (mMotorState[inAxis] != inState)
|
|
{
|
|
mMotorState[inAxis] = inState;
|
|
|
|
// Ensure that warm starting next frame doesn't apply any impulses (motor parts are repurposed for different modes)
|
|
if (inAxis >= EAxis::TranslationX && inAxis <= EAxis::TranslationZ)
|
|
{
|
|
mMotorTranslationConstraintPart[inAxis - EAxis::TranslationX].Deactivate();
|
|
|
|
CacheTranslationMotorActive();
|
|
}
|
|
else
|
|
{
|
|
JPH_ASSERT(inAxis >= EAxis::RotationX && inAxis <= EAxis::RotationZ);
|
|
|
|
mMotorRotationConstraintPart[inAxis - EAxis::RotationX].Deactivate();
|
|
|
|
CacheRotationMotorActive();
|
|
CacheRotationPositionMotorActive();
|
|
}
|
|
}
|
|
}
|
|
|
|
void SixDOFConstraint::SetTargetOrientationCS(QuatArg inOrientation)
|
|
{
|
|
Quat q_swing, q_twist;
|
|
inOrientation.GetSwingTwist(q_swing, q_twist);
|
|
|
|
uint clamped_axis;
|
|
mSwingTwistConstraintPart.ClampSwingTwist(q_swing, q_twist, clamped_axis);
|
|
|
|
if (clamped_axis != 0)
|
|
mTargetOrientation = q_swing * q_twist;
|
|
else
|
|
mTargetOrientation = inOrientation;
|
|
}
|
|
|
|
void SixDOFConstraint::SetupVelocityConstraint(float inDeltaTime)
|
|
{
|
|
// Get body rotations
|
|
Quat rotation1 = mBody1->GetRotation();
|
|
Quat rotation2 = mBody2->GetRotation();
|
|
|
|
// Quaternion that rotates from body1's constraint space to world space
|
|
Quat constraint_body1_to_world = rotation1 * mConstraintToBody1;
|
|
|
|
// Store world space axis of constraint space
|
|
Mat44 translation_axis_mat = Mat44::sRotation(constraint_body1_to_world);
|
|
for (int i = 0; i < 3; ++i)
|
|
mTranslationAxis[i] = translation_axis_mat.GetColumn3(i);
|
|
|
|
if (IsTranslationFullyConstrained())
|
|
{
|
|
// All translation locked: Setup point constraint
|
|
mPointConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(rotation1), mLocalSpacePosition1, *mBody2, Mat44::sRotation(rotation2), mLocalSpacePosition2);
|
|
}
|
|
else if (IsTranslationConstrained() || mTranslationMotorActive)
|
|
{
|
|
// Update world space positions (the bodies may have moved)
|
|
Vec3 r1_plus_u, r2, u;
|
|
GetPositionConstraintProperties(r1_plus_u, r2, u);
|
|
|
|
// Setup axis constraint parts
|
|
for (int i = 0; i < 3; ++i)
|
|
{
|
|
EAxis axis = EAxis(EAxis::TranslationX + i);
|
|
|
|
Vec3 translation_axis = mTranslationAxis[i];
|
|
|
|
// Calculate displacement along this axis
|
|
float d = translation_axis.Dot(u);
|
|
mDisplacement[i] = d; // Store for SolveVelocityConstraint
|
|
|
|
// Setup limit constraint
|
|
bool constraint_active = false;
|
|
float constraint_value = 0.0f;
|
|
if (IsFixedAxis(axis))
|
|
{
|
|
// When constraint is fixed it is always active
|
|
constraint_value = d - mLimitMin[i];
|
|
constraint_active = true;
|
|
}
|
|
else if (!IsFreeAxis(axis))
|
|
{
|
|
// When constraint is limited, it is only active when outside of the allowed range
|
|
if (d <= mLimitMin[i])
|
|
{
|
|
constraint_value = d - mLimitMin[i];
|
|
constraint_active = true;
|
|
}
|
|
else if (d >= mLimitMax[i])
|
|
{
|
|
constraint_value = d - mLimitMax[i];
|
|
constraint_active = true;
|
|
}
|
|
}
|
|
|
|
if (constraint_active)
|
|
mTranslationConstraintPart[i].CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, r1_plus_u, *mBody2, r2, translation_axis, 0.0f, constraint_value, mLimitsSpringSettings[i]);
|
|
else
|
|
mTranslationConstraintPart[i].Deactivate();
|
|
|
|
// Setup motor constraint
|
|
switch (mMotorState[i])
|
|
{
|
|
case EMotorState::Off:
|
|
if (HasFriction(axis))
|
|
mMotorTranslationConstraintPart[i].CalculateConstraintProperties(*mBody1, r1_plus_u, *mBody2, r2, translation_axis);
|
|
else
|
|
mMotorTranslationConstraintPart[i].Deactivate();
|
|
break;
|
|
|
|
case EMotorState::Velocity:
|
|
mMotorTranslationConstraintPart[i].CalculateConstraintProperties(*mBody1, r1_plus_u, *mBody2, r2, translation_axis, -mTargetVelocity[i]);
|
|
break;
|
|
|
|
case EMotorState::Position:
|
|
{
|
|
const SpringSettings &spring_settings = mMotorSettings[i].mSpringSettings;
|
|
if (spring_settings.HasStiffness())
|
|
mMotorTranslationConstraintPart[i].CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, r1_plus_u, *mBody2, r2, translation_axis, 0.0f, translation_axis.Dot(u) - mTargetPosition[i], spring_settings);
|
|
else
|
|
mMotorTranslationConstraintPart[i].Deactivate();
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// Setup rotation constraints
|
|
if (IsRotationFullyConstrained())
|
|
{
|
|
// All rotation locked: Setup rotation constraint
|
|
mRotationConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), *mBody2, Mat44::sRotation(mBody2->GetRotation()));
|
|
}
|
|
else if (IsRotationConstrained() || mRotationMotorActive)
|
|
{
|
|
// GetRotationInConstraintSpace without redoing the calculation of constraint_body1_to_world
|
|
Quat constraint_body2_to_world = mBody2->GetRotation() * mConstraintToBody2;
|
|
Quat q = constraint_body1_to_world.Conjugated() * constraint_body2_to_world;
|
|
|
|
// Use swing twist constraint part
|
|
if (IsRotationConstrained())
|
|
mSwingTwistConstraintPart.CalculateConstraintProperties(*mBody1, *mBody2, q, constraint_body1_to_world);
|
|
else
|
|
mSwingTwistConstraintPart.Deactivate();
|
|
|
|
if (mRotationMotorActive)
|
|
{
|
|
// Calculate rotation motor axis
|
|
Mat44 ws_axis = Mat44::sRotation(constraint_body2_to_world);
|
|
for (int i = 0; i < 3; ++i)
|
|
mRotationAxis[i] = ws_axis.GetColumn3(i);
|
|
|
|
// Get target orientation along the shortest path from q
|
|
Quat target_orientation = q.Dot(mTargetOrientation) > 0.0f? mTargetOrientation : -mTargetOrientation;
|
|
|
|
// The definition of the constraint rotation q:
|
|
// R2 * ConstraintToBody2 = R1 * ConstraintToBody1 * q (1)
|
|
//
|
|
// R2' is the rotation of body 2 when reaching the target_orientation:
|
|
// R2' * ConstraintToBody2 = R1 * ConstraintToBody1 * target_orientation (2)
|
|
//
|
|
// The difference in body 2 space:
|
|
// R2' = R2 * diff_body2 (3)
|
|
//
|
|
// We want to specify the difference in the constraint space of body 2:
|
|
// diff_body2 = ConstraintToBody2 * diff * ConstraintToBody2^* (4)
|
|
//
|
|
// Extracting R2' from 2: R2' = R1 * ConstraintToBody1 * target_orientation * ConstraintToBody2^* (5)
|
|
// Combining 3 & 4: R2' = R2 * ConstraintToBody2 * diff * ConstraintToBody2^* (6)
|
|
// Combining 1 & 6: R2' = R1 * ConstraintToBody1 * q * diff * ConstraintToBody2^* (7)
|
|
// Combining 5 & 7: R1 * ConstraintToBody1 * target_orientation * ConstraintToBody2^* = R1 * ConstraintToBody1 * q * diff * ConstraintToBody2^*
|
|
// <=> target_orientation = q * diff
|
|
// <=> diff = q^* * target_orientation
|
|
Quat diff = q.Conjugated() * target_orientation;
|
|
|
|
// Project diff so that only rotation around axis that have a position motor are remaining
|
|
Quat projected_diff;
|
|
switch (mRotationPositionMotorActive)
|
|
{
|
|
case 0b001:
|
|
// Keep only rotation around X
|
|
projected_diff = diff.GetTwist(Vec3::sAxisX());
|
|
break;
|
|
|
|
case 0b010:
|
|
// Keep only rotation around Y
|
|
projected_diff = diff.GetTwist(Vec3::sAxisY());
|
|
break;
|
|
|
|
case 0b100:
|
|
// Keep only rotation around Z
|
|
projected_diff = diff.GetTwist(Vec3::sAxisZ());
|
|
break;
|
|
|
|
case 0b011:
|
|
// Remove rotation around Z
|
|
// q = swing_xy * twist_z <=> swing_xy = q * twist_z^*
|
|
projected_diff = diff * diff.GetTwist(Vec3::sAxisZ()).Conjugated();
|
|
break;
|
|
|
|
case 0b101:
|
|
// Remove rotation around Y
|
|
// q = swing_xz * twist_y <=> swing_xz = q * twist_y^*
|
|
projected_diff = diff * diff.GetTwist(Vec3::sAxisY()).Conjugated();
|
|
break;
|
|
|
|
case 0b110:
|
|
// Remove rotation around X
|
|
// q = swing_yz * twist_x <=> swing_yz = q * twist_x^*
|
|
projected_diff = diff * diff.GetTwist(Vec3::sAxisX()).Conjugated();
|
|
break;
|
|
|
|
case 0b111:
|
|
default: // All motors off is handled here but the results are unused
|
|
// Keep entire rotation
|
|
projected_diff = diff;
|
|
break;
|
|
}
|
|
|
|
// Approximate error angles
|
|
// The imaginary part of a quaternion is rotation_axis * sin(angle / 2)
|
|
// If angle is small, sin(x) = x so angle[i] ~ 2.0f * rotation_axis[i]
|
|
// We'll be making small time steps, so if the angle is not small at least the sign will be correct and we'll move in the right direction
|
|
Vec3 rotation_error = -2.0f * projected_diff.GetXYZ();
|
|
|
|
// Setup motors
|
|
for (int i = 0; i < 3; ++i)
|
|
{
|
|
EAxis axis = EAxis(EAxis::RotationX + i);
|
|
|
|
Vec3 rotation_axis = mRotationAxis[i];
|
|
|
|
switch (mMotorState[axis])
|
|
{
|
|
case EMotorState::Off:
|
|
if (HasFriction(axis))
|
|
mMotorRotationConstraintPart[i].CalculateConstraintProperties(*mBody1, *mBody2, rotation_axis);
|
|
else
|
|
mMotorRotationConstraintPart[i].Deactivate();
|
|
break;
|
|
|
|
case EMotorState::Velocity:
|
|
mMotorRotationConstraintPart[i].CalculateConstraintProperties(*mBody1, *mBody2, rotation_axis, -mTargetAngularVelocity[i]);
|
|
break;
|
|
|
|
case EMotorState::Position:
|
|
{
|
|
const SpringSettings &spring_settings = mMotorSettings[axis].mSpringSettings;
|
|
if (spring_settings.HasStiffness())
|
|
mMotorRotationConstraintPart[i].CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, *mBody2, rotation_axis, 0.0f, rotation_error[i], spring_settings);
|
|
else
|
|
mMotorRotationConstraintPart[i].Deactivate();
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void SixDOFConstraint::ResetWarmStart()
|
|
{
|
|
for (AxisConstraintPart &c : mMotorTranslationConstraintPart)
|
|
c.Deactivate();
|
|
for (AngleConstraintPart &c : mMotorRotationConstraintPart)
|
|
c.Deactivate();
|
|
mRotationConstraintPart.Deactivate();
|
|
mSwingTwistConstraintPart.Deactivate();
|
|
mPointConstraintPart.Deactivate();
|
|
for (AxisConstraintPart &c : mTranslationConstraintPart)
|
|
c.Deactivate();
|
|
}
|
|
|
|
void SixDOFConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio)
|
|
{
|
|
// Warm start translation motors
|
|
if (mTranslationMotorActive)
|
|
for (int i = 0; i < 3; ++i)
|
|
if (mMotorTranslationConstraintPart[i].IsActive())
|
|
mMotorTranslationConstraintPart[i].WarmStart(*mBody1, *mBody2, mTranslationAxis[i], inWarmStartImpulseRatio);
|
|
|
|
// Warm start rotation motors
|
|
if (mRotationMotorActive)
|
|
for (AngleConstraintPart &c : mMotorRotationConstraintPart)
|
|
if (c.IsActive())
|
|
c.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
|
|
|
|
// Warm start rotation constraints
|
|
if (IsRotationFullyConstrained())
|
|
mRotationConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
|
|
else if (IsRotationConstrained())
|
|
mSwingTwistConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
|
|
|
|
// Warm start translation constraints
|
|
if (IsTranslationFullyConstrained())
|
|
mPointConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
|
|
else if (IsTranslationConstrained())
|
|
for (int i = 0; i < 3; ++i)
|
|
if (mTranslationConstraintPart[i].IsActive())
|
|
mTranslationConstraintPart[i].WarmStart(*mBody1, *mBody2, mTranslationAxis[i], inWarmStartImpulseRatio);
|
|
}
|
|
|
|
bool SixDOFConstraint::SolveVelocityConstraint(float inDeltaTime)
|
|
{
|
|
bool impulse = false;
|
|
|
|
// Solve translation motor
|
|
if (mTranslationMotorActive)
|
|
for (int i = 0; i < 3; ++i)
|
|
if (mMotorTranslationConstraintPart[i].IsActive())
|
|
switch (mMotorState[i])
|
|
{
|
|
case EMotorState::Off:
|
|
{
|
|
// Apply friction only
|
|
float max_lambda = mMaxFriction[i] * inDeltaTime;
|
|
impulse |= mMotorTranslationConstraintPart[i].SolveVelocityConstraint(*mBody1, *mBody2, mTranslationAxis[i], -max_lambda, max_lambda);
|
|
break;
|
|
}
|
|
|
|
case EMotorState::Velocity:
|
|
case EMotorState::Position:
|
|
// Drive motor
|
|
impulse |= mMotorTranslationConstraintPart[i].SolveVelocityConstraint(*mBody1, *mBody2, mTranslationAxis[i], inDeltaTime * mMotorSettings[i].mMinForceLimit, inDeltaTime * mMotorSettings[i].mMaxForceLimit);
|
|
break;
|
|
}
|
|
|
|
// Solve rotation motor
|
|
if (mRotationMotorActive)
|
|
for (int i = 0; i < 3; ++i)
|
|
{
|
|
EAxis axis = EAxis(EAxis::RotationX + i);
|
|
if (mMotorRotationConstraintPart[i].IsActive())
|
|
switch (mMotorState[axis])
|
|
{
|
|
case EMotorState::Off:
|
|
{
|
|
// Apply friction only
|
|
float max_lambda = mMaxFriction[axis] * inDeltaTime;
|
|
impulse |= mMotorRotationConstraintPart[i].SolveVelocityConstraint(*mBody1, *mBody2, mRotationAxis[i], -max_lambda, max_lambda);
|
|
break;
|
|
}
|
|
|
|
case EMotorState::Velocity:
|
|
case EMotorState::Position:
|
|
// Drive motor
|
|
impulse |= mMotorRotationConstraintPart[i].SolveVelocityConstraint(*mBody1, *mBody2, mRotationAxis[i], inDeltaTime * mMotorSettings[axis].mMinTorqueLimit, inDeltaTime * mMotorSettings[axis].mMaxTorqueLimit);
|
|
break;
|
|
}
|
|
}
|
|
|
|
// Solve rotation constraint
|
|
if (IsRotationFullyConstrained())
|
|
impulse |= mRotationConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2);
|
|
else if (IsRotationConstrained())
|
|
impulse |= mSwingTwistConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2);
|
|
|
|
// Solve position constraint
|
|
if (IsTranslationFullyConstrained())
|
|
impulse |= mPointConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2);
|
|
else if (IsTranslationConstrained())
|
|
for (int i = 0; i < 3; ++i)
|
|
if (mTranslationConstraintPart[i].IsActive())
|
|
{
|
|
// If the axis is not fixed it must be limited (or else the constraint would not be active)
|
|
// Calculate the min and max constraint force based on on which side we're limited
|
|
float limit_min = -FLT_MAX, limit_max = FLT_MAX;
|
|
if (!IsFixedAxis(EAxis(EAxis::TranslationX + i)))
|
|
{
|
|
JPH_ASSERT(!IsFreeAxis(EAxis(EAxis::TranslationX + i)));
|
|
if (mDisplacement[i] <= mLimitMin[i])
|
|
limit_min = 0;
|
|
else if (mDisplacement[i] >= mLimitMax[i])
|
|
limit_max = 0;
|
|
}
|
|
|
|
impulse |= mTranslationConstraintPart[i].SolveVelocityConstraint(*mBody1, *mBody2, mTranslationAxis[i], limit_min, limit_max);
|
|
}
|
|
|
|
return impulse;
|
|
}
|
|
|
|
bool SixDOFConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte)
|
|
{
|
|
bool impulse = false;
|
|
|
|
if (IsRotationFullyConstrained())
|
|
{
|
|
// Rotation locked: Solve rotation constraint
|
|
|
|
// Inverse of initial rotation from body 1 to body 2 in body 1 space
|
|
// Definition of initial orientation r0: q2 = q1 r0
|
|
// Initial rotation (see: GetRotationInConstraintSpace): q2 = q1 c1 c2^-1
|
|
// So: r0^-1 = (c1 c2^-1)^-1 = c2 * c1^-1
|
|
Quat constraint_to_body1 = mConstraintToBody1 * Quat::sEulerAngles(GetRotationLimitsMin());
|
|
Quat inv_initial_orientation = mConstraintToBody2 * constraint_to_body1.Conjugated();
|
|
|
|
// Solve rotation violations
|
|
mRotationConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), *mBody2, Mat44::sRotation(mBody2->GetRotation()));
|
|
impulse |= mRotationConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inv_initial_orientation, inBaumgarte);
|
|
}
|
|
else if (IsRotationConstrained())
|
|
{
|
|
// Rotation partially constraint
|
|
|
|
// Solve rotation violations
|
|
Quat q = GetRotationInConstraintSpace();
|
|
impulse |= mSwingTwistConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, q, mConstraintToBody1, mConstraintToBody2, inBaumgarte);
|
|
}
|
|
|
|
// Solve position violations
|
|
if (IsTranslationFullyConstrained())
|
|
{
|
|
// Translation locked: Solve point constraint
|
|
Vec3 local_space_position1 = mLocalSpacePosition1 + mConstraintToBody1 * GetTranslationLimitsMin();
|
|
mPointConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), local_space_position1, *mBody2, Mat44::sRotation(mBody2->GetRotation()), mLocalSpacePosition2);
|
|
impulse |= mPointConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inBaumgarte);
|
|
}
|
|
else if (IsTranslationConstrained())
|
|
{
|
|
// Translation partially locked: Solve per axis
|
|
for (int i = 0; i < 3; ++i)
|
|
if (mLimitsSpringSettings[i].mFrequency <= 0.0f) // If not soft limit
|
|
{
|
|
// Update world space positions (the bodies may have moved)
|
|
Vec3 r1_plus_u, r2, u;
|
|
GetPositionConstraintProperties(r1_plus_u, r2, u);
|
|
|
|
// Quaternion that rotates from body1's constraint space to world space
|
|
Quat constraint_body1_to_world = mBody1->GetRotation() * mConstraintToBody1;
|
|
|
|
// Calculate axis
|
|
Vec3 translation_axis;
|
|
switch (i)
|
|
{
|
|
case 0: translation_axis = constraint_body1_to_world.RotateAxisX(); break;
|
|
case 1: translation_axis = constraint_body1_to_world.RotateAxisY(); break;
|
|
default: JPH_ASSERT(i == 2); translation_axis = constraint_body1_to_world.RotateAxisZ(); break;
|
|
}
|
|
|
|
// Determine position error
|
|
float error = 0.0f;
|
|
EAxis axis(EAxis(EAxis::TranslationX + i));
|
|
if (IsFixedAxis(axis))
|
|
error = u.Dot(translation_axis) - mLimitMin[axis];
|
|
else if (!IsFreeAxis(axis))
|
|
{
|
|
float displacement = u.Dot(translation_axis);
|
|
if (displacement <= mLimitMin[axis])
|
|
error = displacement - mLimitMin[axis];
|
|
else if (displacement >= mLimitMax[axis])
|
|
error = displacement - mLimitMax[axis];
|
|
}
|
|
|
|
if (error != 0.0f)
|
|
{
|
|
// Setup axis constraint part and solve it
|
|
mTranslationConstraintPart[i].CalculateConstraintProperties(*mBody1, r1_plus_u, *mBody2, r2, translation_axis);
|
|
impulse |= mTranslationConstraintPart[i].SolvePositionConstraint(*mBody1, *mBody2, translation_axis, error, inBaumgarte);
|
|
}
|
|
}
|
|
}
|
|
|
|
return impulse;
|
|
}
|
|
|
|
#ifdef JPH_DEBUG_RENDERER
|
|
void SixDOFConstraint::DrawConstraint(DebugRenderer *inRenderer) const
|
|
{
|
|
// Get constraint properties in world space
|
|
RVec3 position1 = mBody1->GetCenterOfMassTransform() * mLocalSpacePosition1;
|
|
Quat rotation1 = mBody1->GetRotation() * mConstraintToBody1;
|
|
Quat rotation2 = mBody2->GetRotation() * mConstraintToBody2;
|
|
|
|
// Draw constraint orientation
|
|
inRenderer->DrawCoordinateSystem(RMat44::sRotationTranslation(rotation1, position1), mDrawConstraintSize);
|
|
|
|
if ((IsRotationConstrained() || mRotationPositionMotorActive != 0) && !IsRotationFullyConstrained())
|
|
{
|
|
// Draw current swing and twist
|
|
Quat q = GetRotationInConstraintSpace();
|
|
Quat q_swing, q_twist;
|
|
q.GetSwingTwist(q_swing, q_twist);
|
|
inRenderer->DrawLine(position1, position1 + mDrawConstraintSize * (rotation1 * q_twist).RotateAxisY(), Color::sWhite);
|
|
inRenderer->DrawLine(position1, position1 + mDrawConstraintSize * (rotation1 * q_swing).RotateAxisX(), Color::sWhite);
|
|
}
|
|
|
|
// Draw target rotation
|
|
Quat m_swing, m_twist;
|
|
mTargetOrientation.GetSwingTwist(m_swing, m_twist);
|
|
if (mMotorState[EAxis::RotationX] == EMotorState::Position)
|
|
inRenderer->DrawLine(position1, position1 + mDrawConstraintSize * (rotation1 * m_twist).RotateAxisY(), Color::sYellow);
|
|
if (mMotorState[EAxis::RotationY] == EMotorState::Position || mMotorState[EAxis::RotationZ] == EMotorState::Position)
|
|
inRenderer->DrawLine(position1, position1 + mDrawConstraintSize * (rotation1 * m_swing).RotateAxisX(), Color::sYellow);
|
|
|
|
// Draw target angular velocity
|
|
Vec3 target_angular_velocity = Vec3::sZero();
|
|
for (int i = 0; i < 3; ++i)
|
|
if (mMotorState[EAxis::RotationX + i] == EMotorState::Velocity)
|
|
target_angular_velocity.SetComponent(i, mTargetAngularVelocity[i]);
|
|
if (target_angular_velocity != Vec3::sZero())
|
|
inRenderer->DrawArrow(position1, position1 + rotation2 * target_angular_velocity, Color::sRed, 0.1f);
|
|
}
|
|
|
|
void SixDOFConstraint::DrawConstraintLimits(DebugRenderer *inRenderer) const
|
|
{
|
|
// Get matrix that transforms from constraint space to world space
|
|
RMat44 constraint_body1_to_world = RMat44::sRotationTranslation(mBody1->GetRotation() * mConstraintToBody1, mBody1->GetCenterOfMassTransform() * mLocalSpacePosition1);
|
|
|
|
// Draw limits
|
|
if (mSwingTwistConstraintPart.GetSwingType() == ESwingType::Pyramid)
|
|
inRenderer->DrawSwingPyramidLimits(constraint_body1_to_world, mLimitMin[EAxis::RotationY], mLimitMax[EAxis::RotationY], mLimitMin[EAxis::RotationZ], mLimitMax[EAxis::RotationZ], mDrawConstraintSize, Color::sGreen, DebugRenderer::ECastShadow::Off);
|
|
else
|
|
inRenderer->DrawSwingConeLimits(constraint_body1_to_world, mLimitMax[EAxis::RotationY], mLimitMax[EAxis::RotationZ], mDrawConstraintSize, Color::sGreen, DebugRenderer::ECastShadow::Off);
|
|
inRenderer->DrawPie(constraint_body1_to_world.GetTranslation(), mDrawConstraintSize, constraint_body1_to_world.GetAxisX(), constraint_body1_to_world.GetAxisY(), mLimitMin[EAxis::RotationX], mLimitMax[EAxis::RotationX], Color::sPurple, DebugRenderer::ECastShadow::Off);
|
|
}
|
|
#endif // JPH_DEBUG_RENDERER
|
|
|
|
void SixDOFConstraint::SaveState(StateRecorder &inStream) const
|
|
{
|
|
TwoBodyConstraint::SaveState(inStream);
|
|
|
|
for (const AxisConstraintPart &c : mTranslationConstraintPart)
|
|
c.SaveState(inStream);
|
|
mPointConstraintPart.SaveState(inStream);
|
|
mSwingTwistConstraintPart.SaveState(inStream);
|
|
mRotationConstraintPart.SaveState(inStream);
|
|
for (const AxisConstraintPart &c : mMotorTranslationConstraintPart)
|
|
c.SaveState(inStream);
|
|
for (const AngleConstraintPart &c : mMotorRotationConstraintPart)
|
|
c.SaveState(inStream);
|
|
|
|
inStream.Write(mMotorState);
|
|
inStream.Write(mTargetVelocity);
|
|
inStream.Write(mTargetAngularVelocity);
|
|
inStream.Write(mTargetPosition);
|
|
inStream.Write(mTargetOrientation);
|
|
}
|
|
|
|
void SixDOFConstraint::RestoreState(StateRecorder &inStream)
|
|
{
|
|
TwoBodyConstraint::RestoreState(inStream);
|
|
|
|
for (AxisConstraintPart &c : mTranslationConstraintPart)
|
|
c.RestoreState(inStream);
|
|
mPointConstraintPart.RestoreState(inStream);
|
|
mSwingTwistConstraintPart.RestoreState(inStream);
|
|
mRotationConstraintPart.RestoreState(inStream);
|
|
for (AxisConstraintPart &c : mMotorTranslationConstraintPart)
|
|
c.RestoreState(inStream);
|
|
for (AngleConstraintPart &c : mMotorRotationConstraintPart)
|
|
c.RestoreState(inStream);
|
|
|
|
inStream.Read(mMotorState);
|
|
inStream.Read(mTargetVelocity);
|
|
inStream.Read(mTargetAngularVelocity);
|
|
inStream.Read(mTargetPosition);
|
|
inStream.Read(mTargetOrientation);
|
|
|
|
CacheTranslationMotorActive();
|
|
CacheRotationMotorActive();
|
|
CacheRotationPositionMotorActive();
|
|
}
|
|
|
|
Ref<ConstraintSettings> SixDOFConstraint::GetConstraintSettings() const
|
|
{
|
|
SixDOFConstraintSettings *settings = new SixDOFConstraintSettings;
|
|
ToConstraintSettings(*settings);
|
|
settings->mSpace = EConstraintSpace::LocalToBodyCOM;
|
|
settings->mPosition1 = RVec3(mLocalSpacePosition1);
|
|
settings->mAxisX1 = mConstraintToBody1.RotateAxisX();
|
|
settings->mAxisY1 = mConstraintToBody1.RotateAxisY();
|
|
settings->mPosition2 = RVec3(mLocalSpacePosition2);
|
|
settings->mAxisX2 = mConstraintToBody2.RotateAxisX();
|
|
settings->mAxisY2 = mConstraintToBody2.RotateAxisY();
|
|
settings->mSwingType = mSwingTwistConstraintPart.GetSwingType();
|
|
memcpy(settings->mLimitMin, mLimitMin, sizeof(mLimitMin));
|
|
memcpy(settings->mLimitMax, mLimitMax, sizeof(mLimitMax));
|
|
memcpy(settings->mMaxFriction, mMaxFriction, sizeof(mMaxFriction));
|
|
for (int i = 0; i < EAxis::Num; ++i)
|
|
settings->mMotorSettings[i] = mMotorSettings[i];
|
|
return settings;
|
|
}
|
|
|
|
JPH_NAMESPACE_END
|