502 lines
18 KiB
C++
502 lines
18 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/SliderConstraint.h>
|
|
#include <Jolt/Physics/Body/Body.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
|
|
|
|
using namespace literals;
|
|
|
|
JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(SliderConstraintSettings)
|
|
{
|
|
JPH_ADD_BASE_CLASS(SliderConstraintSettings, TwoBodyConstraintSettings)
|
|
|
|
JPH_ADD_ENUM_ATTRIBUTE(SliderConstraintSettings, mSpace)
|
|
JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mAutoDetectPoint)
|
|
JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mPoint1)
|
|
JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mSliderAxis1)
|
|
JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mNormalAxis1)
|
|
JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mPoint2)
|
|
JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mSliderAxis2)
|
|
JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mNormalAxis2)
|
|
JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mLimitsMin)
|
|
JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mLimitsMax)
|
|
JPH_ADD_ENUM_ATTRIBUTE_WITH_ALIAS(SliderConstraintSettings, mLimitsSpringSettings.mMode, "mSpringMode")
|
|
JPH_ADD_ATTRIBUTE_WITH_ALIAS(SliderConstraintSettings, mLimitsSpringSettings.mFrequency, "mFrequency") // Renaming attributes to stay compatible with old versions of the library
|
|
JPH_ADD_ATTRIBUTE_WITH_ALIAS(SliderConstraintSettings, mLimitsSpringSettings.mDamping, "mDamping")
|
|
JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mMaxFrictionForce)
|
|
JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mMotorSettings)
|
|
}
|
|
|
|
void SliderConstraintSettings::SetSliderAxis(Vec3Arg inSliderAxis)
|
|
{
|
|
JPH_ASSERT(mSpace == EConstraintSpace::WorldSpace);
|
|
|
|
mSliderAxis1 = mSliderAxis2 = inSliderAxis;
|
|
mNormalAxis1 = mNormalAxis2 = inSliderAxis.GetNormalizedPerpendicular();
|
|
}
|
|
|
|
void SliderConstraintSettings::SaveBinaryState(StreamOut &inStream) const
|
|
{
|
|
ConstraintSettings::SaveBinaryState(inStream);
|
|
|
|
inStream.Write(mSpace);
|
|
inStream.Write(mAutoDetectPoint);
|
|
inStream.Write(mPoint1);
|
|
inStream.Write(mSliderAxis1);
|
|
inStream.Write(mNormalAxis1);
|
|
inStream.Write(mPoint2);
|
|
inStream.Write(mSliderAxis2);
|
|
inStream.Write(mNormalAxis2);
|
|
inStream.Write(mLimitsMin);
|
|
inStream.Write(mLimitsMax);
|
|
inStream.Write(mMaxFrictionForce);
|
|
mLimitsSpringSettings.SaveBinaryState(inStream);
|
|
mMotorSettings.SaveBinaryState(inStream);
|
|
}
|
|
|
|
void SliderConstraintSettings::RestoreBinaryState(StreamIn &inStream)
|
|
{
|
|
ConstraintSettings::RestoreBinaryState(inStream);
|
|
|
|
inStream.Read(mSpace);
|
|
inStream.Read(mAutoDetectPoint);
|
|
inStream.Read(mPoint1);
|
|
inStream.Read(mSliderAxis1);
|
|
inStream.Read(mNormalAxis1);
|
|
inStream.Read(mPoint2);
|
|
inStream.Read(mSliderAxis2);
|
|
inStream.Read(mNormalAxis2);
|
|
inStream.Read(mLimitsMin);
|
|
inStream.Read(mLimitsMax);
|
|
inStream.Read(mMaxFrictionForce);
|
|
mLimitsSpringSettings.RestoreBinaryState(inStream);
|
|
mMotorSettings.RestoreBinaryState(inStream);
|
|
}
|
|
|
|
TwoBodyConstraint *SliderConstraintSettings::Create(Body &inBody1, Body &inBody2) const
|
|
{
|
|
return new SliderConstraint(inBody1, inBody2, *this);
|
|
}
|
|
|
|
SliderConstraint::SliderConstraint(Body &inBody1, Body &inBody2, const SliderConstraintSettings &inSettings) :
|
|
TwoBodyConstraint(inBody1, inBody2, inSettings),
|
|
mMaxFrictionForce(inSettings.mMaxFrictionForce),
|
|
mMotorSettings(inSettings.mMotorSettings)
|
|
{
|
|
// Store inverse of initial rotation from body 1 to body 2 in body 1 space
|
|
mInvInitialOrientation = RotationEulerConstraintPart::sGetInvInitialOrientationXY(inSettings.mSliderAxis1, inSettings.mNormalAxis1, inSettings.mSliderAxis2, inSettings.mNormalAxis2);
|
|
|
|
if (inSettings.mSpace == EConstraintSpace::WorldSpace)
|
|
{
|
|
RMat44 inv_transform1 = inBody1.GetInverseCenterOfMassTransform();
|
|
RMat44 inv_transform2 = inBody2.GetInverseCenterOfMassTransform();
|
|
|
|
if (inSettings.mAutoDetectPoint)
|
|
{
|
|
// Determine anchor point: If any of the bodies can never be dynamic use the other body as anchor point
|
|
RVec3 anchor;
|
|
if (!inBody1.CanBeKinematicOrDynamic())
|
|
anchor = inBody2.GetCenterOfMassPosition();
|
|
else if (!inBody2.CanBeKinematicOrDynamic())
|
|
anchor = inBody1.GetCenterOfMassPosition();
|
|
else
|
|
{
|
|
// Otherwise use weighted anchor point towards the lightest body
|
|
Real inv_m1 = Real(inBody1.GetMotionPropertiesUnchecked()->GetInverseMassUnchecked());
|
|
Real inv_m2 = Real(inBody2.GetMotionPropertiesUnchecked()->GetInverseMassUnchecked());
|
|
Real total_inv_mass = inv_m1 + inv_m2;
|
|
if (total_inv_mass != 0.0_r)
|
|
anchor = (inv_m1 * inBody1.GetCenterOfMassPosition() + inv_m2 * inBody2.GetCenterOfMassPosition()) / total_inv_mass;
|
|
else
|
|
anchor = inBody1.GetCenterOfMassPosition();
|
|
}
|
|
|
|
// Store local positions
|
|
mLocalSpacePosition1 = Vec3(inv_transform1 * anchor);
|
|
mLocalSpacePosition2 = Vec3(inv_transform2 * anchor);
|
|
}
|
|
else
|
|
{
|
|
// Store local positions
|
|
mLocalSpacePosition1 = Vec3(inv_transform1 * inSettings.mPoint1);
|
|
mLocalSpacePosition2 = Vec3(inv_transform2 * inSettings.mPoint2);
|
|
}
|
|
|
|
// If all properties were specified in world space, take them to local space now
|
|
mLocalSpaceSliderAxis1 = inv_transform1.Multiply3x3(inSettings.mSliderAxis1).Normalized();
|
|
mLocalSpaceNormal1 = inv_transform1.Multiply3x3(inSettings.mNormalAxis1).Normalized();
|
|
|
|
// Constraints were specified in world space, so we should have replaced c1 with q10^-1 c1 and c2 with q20^-1 c2
|
|
// => r0^-1 = (q20^-1 c2) (q10^-1 c1)^1 = q20^-1 (c2 c1^-1) q10
|
|
mInvInitialOrientation = inBody2.GetRotation().Conjugated() * mInvInitialOrientation * inBody1.GetRotation();
|
|
}
|
|
else
|
|
{
|
|
// Store local positions
|
|
mLocalSpacePosition1 = Vec3(inSettings.mPoint1);
|
|
mLocalSpacePosition2 = Vec3(inSettings.mPoint2);
|
|
|
|
// Store local space axis
|
|
mLocalSpaceSliderAxis1 = inSettings.mSliderAxis1;
|
|
mLocalSpaceNormal1 = inSettings.mNormalAxis1;
|
|
}
|
|
|
|
// Calculate 2nd local space normal
|
|
mLocalSpaceNormal2 = mLocalSpaceSliderAxis1.Cross(mLocalSpaceNormal1);
|
|
|
|
// Store limits
|
|
JPH_ASSERT(inSettings.mLimitsMin != inSettings.mLimitsMax || inSettings.mLimitsSpringSettings.mFrequency > 0.0f, "Better use a fixed constraint");
|
|
SetLimits(inSettings.mLimitsMin, inSettings.mLimitsMax);
|
|
|
|
// Store spring settings
|
|
SetLimitsSpringSettings(inSettings.mLimitsSpringSettings);
|
|
}
|
|
|
|
void SliderConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM)
|
|
{
|
|
if (mBody1->GetID() == inBodyID)
|
|
mLocalSpacePosition1 -= inDeltaCOM;
|
|
else if (mBody2->GetID() == inBodyID)
|
|
mLocalSpacePosition2 -= inDeltaCOM;
|
|
}
|
|
|
|
float SliderConstraint::GetCurrentPosition() const
|
|
{
|
|
// See: CalculateR1R2U and CalculateSlidingAxisAndPosition
|
|
Vec3 r1 = mBody1->GetRotation() * mLocalSpacePosition1;
|
|
Vec3 r2 = mBody2->GetRotation() * mLocalSpacePosition2;
|
|
Vec3 u = Vec3(mBody2->GetCenterOfMassPosition() - mBody1->GetCenterOfMassPosition()) + r2 - r1;
|
|
return u.Dot(mBody1->GetRotation() * mLocalSpaceSliderAxis1);
|
|
}
|
|
|
|
void SliderConstraint::SetLimits(float inLimitsMin, float inLimitsMax)
|
|
{
|
|
JPH_ASSERT(inLimitsMin <= 0.0f);
|
|
JPH_ASSERT(inLimitsMax >= 0.0f);
|
|
mLimitsMin = inLimitsMin;
|
|
mLimitsMax = inLimitsMax;
|
|
mHasLimits = mLimitsMin != -FLT_MAX || mLimitsMax != FLT_MAX;
|
|
}
|
|
|
|
void SliderConstraint::CalculateR1R2U(Mat44Arg inRotation1, Mat44Arg inRotation2)
|
|
{
|
|
// Calculate points relative to body
|
|
mR1 = inRotation1 * mLocalSpacePosition1;
|
|
mR2 = inRotation2 * mLocalSpacePosition2;
|
|
|
|
// Calculate X2 + R2 - X1 - R1
|
|
mU = Vec3(mBody2->GetCenterOfMassPosition() - mBody1->GetCenterOfMassPosition()) + mR2 - mR1;
|
|
}
|
|
|
|
void SliderConstraint::CalculatePositionConstraintProperties(Mat44Arg inRotation1, Mat44Arg inRotation2)
|
|
{
|
|
// Calculate world space normals
|
|
mN1 = inRotation1 * mLocalSpaceNormal1;
|
|
mN2 = inRotation1 * mLocalSpaceNormal2;
|
|
|
|
mPositionConstraintPart.CalculateConstraintProperties(*mBody1, inRotation1, mR1 + mU, *mBody2, inRotation2, mR2, mN1, mN2);
|
|
}
|
|
|
|
void SliderConstraint::CalculateSlidingAxisAndPosition(Mat44Arg inRotation1)
|
|
{
|
|
if (mHasLimits || mMotorState != EMotorState::Off || mMaxFrictionForce > 0.0f)
|
|
{
|
|
// Calculate world space slider axis
|
|
mWorldSpaceSliderAxis = inRotation1 * mLocalSpaceSliderAxis1;
|
|
|
|
// Calculate slide distance along axis
|
|
mD = mU.Dot(mWorldSpaceSliderAxis);
|
|
}
|
|
}
|
|
|
|
void SliderConstraint::CalculatePositionLimitsConstraintProperties(float inDeltaTime)
|
|
{
|
|
// Check if distance is within limits
|
|
bool below_min = mD <= mLimitsMin;
|
|
if (mHasLimits && (below_min || mD >= mLimitsMax))
|
|
mPositionLimitsConstraintPart.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, mR1 + mU, *mBody2, mR2, mWorldSpaceSliderAxis, 0.0f, mD - (below_min? mLimitsMin : mLimitsMax), mLimitsSpringSettings);
|
|
else
|
|
mPositionLimitsConstraintPart.Deactivate();
|
|
}
|
|
|
|
void SliderConstraint::CalculateMotorConstraintProperties(float inDeltaTime)
|
|
{
|
|
switch (mMotorState)
|
|
{
|
|
case EMotorState::Off:
|
|
if (mMaxFrictionForce > 0.0f)
|
|
mMotorConstraintPart.CalculateConstraintProperties(*mBody1, mR1 + mU, *mBody2, mR2, mWorldSpaceSliderAxis);
|
|
else
|
|
mMotorConstraintPart.Deactivate();
|
|
break;
|
|
|
|
case EMotorState::Velocity:
|
|
mMotorConstraintPart.CalculateConstraintProperties(*mBody1, mR1 + mU, *mBody2, mR2, mWorldSpaceSliderAxis, -mTargetVelocity);
|
|
break;
|
|
|
|
case EMotorState::Position:
|
|
if (mMotorSettings.mSpringSettings.HasStiffness())
|
|
mMotorConstraintPart.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, mR1 + mU, *mBody2, mR2, mWorldSpaceSliderAxis, 0.0f, mD - mTargetPosition, mMotorSettings.mSpringSettings);
|
|
else
|
|
mMotorConstraintPart.Deactivate();
|
|
break;
|
|
}
|
|
}
|
|
|
|
void SliderConstraint::SetupVelocityConstraint(float inDeltaTime)
|
|
{
|
|
// Calculate constraint properties that are constant while bodies don't move
|
|
Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation());
|
|
Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation());
|
|
CalculateR1R2U(rotation1, rotation2);
|
|
CalculatePositionConstraintProperties(rotation1, rotation2);
|
|
mRotationConstraintPart.CalculateConstraintProperties(*mBody1, rotation1, *mBody2, rotation2);
|
|
CalculateSlidingAxisAndPosition(rotation1);
|
|
CalculatePositionLimitsConstraintProperties(inDeltaTime);
|
|
CalculateMotorConstraintProperties(inDeltaTime);
|
|
}
|
|
|
|
void SliderConstraint::ResetWarmStart()
|
|
{
|
|
mMotorConstraintPart.Deactivate();
|
|
mPositionConstraintPart.Deactivate();
|
|
mRotationConstraintPart.Deactivate();
|
|
mPositionLimitsConstraintPart.Deactivate();
|
|
}
|
|
|
|
void SliderConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio)
|
|
{
|
|
// Warm starting: Apply previous frame impulse
|
|
mMotorConstraintPart.WarmStart(*mBody1, *mBody2, mWorldSpaceSliderAxis, inWarmStartImpulseRatio);
|
|
mPositionConstraintPart.WarmStart(*mBody1, *mBody2, mN1, mN2, inWarmStartImpulseRatio);
|
|
mRotationConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
|
|
mPositionLimitsConstraintPart.WarmStart(*mBody1, *mBody2, mWorldSpaceSliderAxis, inWarmStartImpulseRatio);
|
|
}
|
|
|
|
bool SliderConstraint::SolveVelocityConstraint(float inDeltaTime)
|
|
{
|
|
// Solve motor
|
|
bool motor = false;
|
|
if (mMotorConstraintPart.IsActive())
|
|
{
|
|
switch (mMotorState)
|
|
{
|
|
case EMotorState::Off:
|
|
{
|
|
float max_lambda = mMaxFrictionForce * inDeltaTime;
|
|
motor = mMotorConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceSliderAxis, -max_lambda, max_lambda);
|
|
break;
|
|
}
|
|
|
|
case EMotorState::Velocity:
|
|
case EMotorState::Position:
|
|
motor = mMotorConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceSliderAxis, inDeltaTime * mMotorSettings.mMinForceLimit, inDeltaTime * mMotorSettings.mMaxForceLimit);
|
|
break;
|
|
}
|
|
}
|
|
|
|
// Solve position constraint along 2 axis
|
|
bool pos = mPositionConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mN1, mN2);
|
|
|
|
// Solve rotation constraint
|
|
bool rot = mRotationConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2);
|
|
|
|
// Solve limits along slider axis
|
|
bool limit = false;
|
|
if (mPositionLimitsConstraintPart.IsActive())
|
|
{
|
|
float min_lambda, max_lambda;
|
|
if (mLimitsMin == mLimitsMax)
|
|
{
|
|
min_lambda = -FLT_MAX;
|
|
max_lambda = FLT_MAX;
|
|
}
|
|
else if (mD <= mLimitsMin)
|
|
{
|
|
min_lambda = 0.0f;
|
|
max_lambda = FLT_MAX;
|
|
}
|
|
else
|
|
{
|
|
min_lambda = -FLT_MAX;
|
|
max_lambda = 0.0f;
|
|
}
|
|
limit = mPositionLimitsConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceSliderAxis, min_lambda, max_lambda);
|
|
}
|
|
|
|
return motor || pos || rot || limit;
|
|
}
|
|
|
|
bool SliderConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte)
|
|
{
|
|
// Motor operates on velocities only, don't call SolvePositionConstraint
|
|
|
|
// Solve position constraint along 2 axis
|
|
Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation());
|
|
Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation());
|
|
CalculateR1R2U(rotation1, rotation2);
|
|
CalculatePositionConstraintProperties(rotation1, rotation2);
|
|
bool pos = mPositionConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mU, mN1, mN2, inBaumgarte);
|
|
|
|
// Solve rotation constraint
|
|
mRotationConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), *mBody2, Mat44::sRotation(mBody2->GetRotation()));
|
|
bool rot = mRotationConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mInvInitialOrientation, inBaumgarte);
|
|
|
|
// Solve limits along slider axis
|
|
bool limit = false;
|
|
if (mHasLimits && mLimitsSpringSettings.mFrequency <= 0.0f)
|
|
{
|
|
rotation1 = Mat44::sRotation(mBody1->GetRotation());
|
|
rotation2 = Mat44::sRotation(mBody2->GetRotation());
|
|
CalculateR1R2U(rotation1, rotation2);
|
|
CalculateSlidingAxisAndPosition(rotation1);
|
|
CalculatePositionLimitsConstraintProperties(inDeltaTime);
|
|
if (mPositionLimitsConstraintPart.IsActive())
|
|
{
|
|
if (mD <= mLimitsMin)
|
|
limit = mPositionLimitsConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mWorldSpaceSliderAxis, mD - mLimitsMin, inBaumgarte);
|
|
else
|
|
{
|
|
JPH_ASSERT(mD >= mLimitsMax);
|
|
limit = mPositionLimitsConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mWorldSpaceSliderAxis, mD - mLimitsMax, inBaumgarte);
|
|
}
|
|
}
|
|
}
|
|
|
|
return pos || rot || limit;
|
|
}
|
|
|
|
#ifdef JPH_DEBUG_RENDERER
|
|
void SliderConstraint::DrawConstraint(DebugRenderer *inRenderer) const
|
|
{
|
|
RMat44 transform1 = mBody1->GetCenterOfMassTransform();
|
|
RMat44 transform2 = mBody2->GetCenterOfMassTransform();
|
|
|
|
// Transform the local positions into world space
|
|
Vec3 slider_axis = transform1.Multiply3x3(mLocalSpaceSliderAxis1);
|
|
RVec3 position1 = transform1 * mLocalSpacePosition1;
|
|
RVec3 position2 = transform2 * mLocalSpacePosition2;
|
|
|
|
// Draw constraint
|
|
inRenderer->DrawMarker(position1, Color::sRed, 0.1f);
|
|
inRenderer->DrawMarker(position2, Color::sGreen, 0.1f);
|
|
inRenderer->DrawLine(position1, position2, Color::sGreen);
|
|
|
|
// Draw motor
|
|
switch (mMotorState)
|
|
{
|
|
case EMotorState::Position:
|
|
inRenderer->DrawMarker(position1 + mTargetPosition * slider_axis, Color::sYellow, 1.0f);
|
|
break;
|
|
|
|
case EMotorState::Velocity:
|
|
{
|
|
Vec3 cur_vel = (mBody2->GetLinearVelocity() - mBody1->GetLinearVelocity()).Dot(slider_axis) * slider_axis;
|
|
inRenderer->DrawLine(position2, position2 + cur_vel, Color::sBlue);
|
|
inRenderer->DrawArrow(position2 + cur_vel, position2 + mTargetVelocity * slider_axis, Color::sRed, 0.1f);
|
|
break;
|
|
}
|
|
|
|
case EMotorState::Off:
|
|
break;
|
|
}
|
|
}
|
|
|
|
void SliderConstraint::DrawConstraintLimits(DebugRenderer *inRenderer) const
|
|
{
|
|
if (mHasLimits)
|
|
{
|
|
RMat44 transform1 = mBody1->GetCenterOfMassTransform();
|
|
RMat44 transform2 = mBody2->GetCenterOfMassTransform();
|
|
|
|
// Transform the local positions into world space
|
|
Vec3 slider_axis = transform1.Multiply3x3(mLocalSpaceSliderAxis1);
|
|
RVec3 position1 = transform1 * mLocalSpacePosition1;
|
|
RVec3 position2 = transform2 * mLocalSpacePosition2;
|
|
|
|
// Calculate the limits in world space
|
|
RVec3 limits_min = position1 + mLimitsMin * slider_axis;
|
|
RVec3 limits_max = position1 + mLimitsMax * slider_axis;
|
|
|
|
inRenderer->DrawLine(limits_min, position1, Color::sWhite);
|
|
inRenderer->DrawLine(position2, limits_max, Color::sWhite);
|
|
|
|
inRenderer->DrawMarker(limits_min, Color::sWhite, 0.1f);
|
|
inRenderer->DrawMarker(limits_max, Color::sWhite, 0.1f);
|
|
}
|
|
}
|
|
#endif // JPH_DEBUG_RENDERER
|
|
|
|
void SliderConstraint::SaveState(StateRecorder &inStream) const
|
|
{
|
|
TwoBodyConstraint::SaveState(inStream);
|
|
|
|
mMotorConstraintPart.SaveState(inStream);
|
|
mPositionConstraintPart.SaveState(inStream);
|
|
mRotationConstraintPart.SaveState(inStream);
|
|
mPositionLimitsConstraintPart.SaveState(inStream);
|
|
|
|
inStream.Write(mMotorState);
|
|
inStream.Write(mTargetVelocity);
|
|
inStream.Write(mTargetPosition);
|
|
}
|
|
|
|
void SliderConstraint::RestoreState(StateRecorder &inStream)
|
|
{
|
|
TwoBodyConstraint::RestoreState(inStream);
|
|
|
|
mMotorConstraintPart.RestoreState(inStream);
|
|
mPositionConstraintPart.RestoreState(inStream);
|
|
mRotationConstraintPart.RestoreState(inStream);
|
|
mPositionLimitsConstraintPart.RestoreState(inStream);
|
|
|
|
inStream.Read(mMotorState);
|
|
inStream.Read(mTargetVelocity);
|
|
inStream.Read(mTargetPosition);
|
|
}
|
|
|
|
Ref<ConstraintSettings> SliderConstraint::GetConstraintSettings() const
|
|
{
|
|
SliderConstraintSettings *settings = new SliderConstraintSettings;
|
|
ToConstraintSettings(*settings);
|
|
settings->mSpace = EConstraintSpace::LocalToBodyCOM;
|
|
settings->mPoint1 = RVec3(mLocalSpacePosition1);
|
|
settings->mSliderAxis1 = mLocalSpaceSliderAxis1;
|
|
settings->mNormalAxis1 = mLocalSpaceNormal1;
|
|
settings->mPoint2 = RVec3(mLocalSpacePosition2);
|
|
Mat44 inv_initial_rotation = Mat44::sRotation(mInvInitialOrientation);
|
|
settings->mSliderAxis2 = inv_initial_rotation.Multiply3x3(mLocalSpaceSliderAxis1);
|
|
settings->mNormalAxis2 = inv_initial_rotation.Multiply3x3(mLocalSpaceNormal1);
|
|
settings->mLimitsMin = mLimitsMin;
|
|
settings->mLimitsMax = mLimitsMax;
|
|
settings->mLimitsSpringSettings = mLimitsSpringSettings;
|
|
settings->mMaxFrictionForce = mMaxFrictionForce;
|
|
settings->mMotorSettings = mMotorSettings;
|
|
return settings;
|
|
}
|
|
|
|
Mat44 SliderConstraint::GetConstraintToBody1Matrix() const
|
|
{
|
|
return Mat44(Vec4(mLocalSpaceSliderAxis1, 0), Vec4(mLocalSpaceNormal1, 0), Vec4(mLocalSpaceNormal2, 0), Vec4(mLocalSpacePosition1, 1));
|
|
}
|
|
|
|
Mat44 SliderConstraint::GetConstraintToBody2Matrix() const
|
|
{
|
|
Mat44 mat = Mat44::sRotation(mInvInitialOrientation).Multiply3x3(Mat44(Vec4(mLocalSpaceSliderAxis1, 0), Vec4(mLocalSpaceNormal1, 0), Vec4(mLocalSpaceNormal2, 0), Vec4(0, 0, 0, 1)));
|
|
mat.SetTranslation(mLocalSpacePosition2);
|
|
return mat;
|
|
}
|
|
|
|
JPH_NAMESPACE_END
|