Jolt Physics
A multi core friendly Game Physics Engine
Loading...
Searching...
No Matches
RackAndPinionConstraintPart.h
Go to the documentation of this file.
1// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
2// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
3// SPDX-License-Identifier: MIT
4
5#pragma once
6
9
11
40{
42 JPH_INLINE bool ApplyVelocityStep(Body &ioBody1, Body &ioBody2, float inLambda) const
43 {
44 // Apply impulse if delta is not zero
45 if (inLambda != 0.0f)
46 {
47 // Calculate velocity change due to constraint
48 //
49 // Impulse:
50 // P = J^T lambda
51 //
52 // Euler velocity integration:
53 // v' = v + M^-1 P
54 ioBody1.GetMotionProperties()->AddAngularVelocityStep(inLambda * mInvI1_A);
55 ioBody2.GetMotionProperties()->SubLinearVelocityStep(inLambda * mRatio_InvM2_B);
56 return true;
57 }
58
59 return false;
60 }
61
62public:
69 inline void CalculateConstraintProperties(const Body &inBody1, Vec3Arg inWorldSpaceHingeAxis, const Body &inBody2, Vec3Arg inWorldSpaceSliderAxis, float inRatio)
70 {
71 JPH_ASSERT(inWorldSpaceHingeAxis.IsNormalized(1.0e-4f));
72 JPH_ASSERT(inWorldSpaceSliderAxis.IsNormalized(1.0e-4f));
73
74 // Calculate: I1^-1 a
75 mInvI1_A = inBody1.GetMotionProperties()->MultiplyWorldSpaceInverseInertiaByVector(inBody1.GetRotation(), inWorldSpaceHingeAxis);
76
77 // Calculate: r/m2 b
78 float inv_m2 = inBody2.GetMotionProperties()->GetInverseMass();
79 mRatio_InvM2_B = inRatio * inv_m2 * inWorldSpaceSliderAxis;
80
81 // K^-1 = 1 / (J M^-1 J^T) = 1 / (a^T I1^-1 a + 1/m2 * r^2 * b . b)
82 float inv_effective_mass = (inWorldSpaceHingeAxis.Dot(mInvI1_A) + inv_m2 * Square(inRatio));
83 if (inv_effective_mass == 0.0f)
84 Deactivate();
85 else
86 mEffectiveMass = 1.0f / inv_effective_mass;
87 }
88
90 inline void Deactivate()
91 {
92 mEffectiveMass = 0.0f;
93 mTotalLambda = 0.0f;
94 }
95
97 inline bool IsActive() const
98 {
99 return mEffectiveMass != 0.0f;
100 }
101
106 inline void WarmStart(Body &ioBody1, Body &ioBody2, float inWarmStartImpulseRatio)
107 {
108 mTotalLambda *= inWarmStartImpulseRatio;
109 ApplyVelocityStep(ioBody1, ioBody2, mTotalLambda);
110 }
111
118 inline bool SolveVelocityConstraint(Body &ioBody1, Vec3Arg inWorldSpaceHingeAxis, Body &ioBody2, Vec3Arg inWorldSpaceSliderAxis, float inRatio)
119 {
120 // Lagrange multiplier is:
121 //
122 // lambda = -K^-1 (J v + b)
123 float lambda = mEffectiveMass * (inRatio * inWorldSpaceSliderAxis.Dot(ioBody2.GetLinearVelocity()) - inWorldSpaceHingeAxis.Dot(ioBody1.GetAngularVelocity()));
124 mTotalLambda += lambda; // Store accumulated impulse
125
126 return ApplyVelocityStep(ioBody1, ioBody2, lambda);
127 }
128
130 float GetTotalLambda() const
131 {
132 return mTotalLambda;
133 }
134
140 inline bool SolvePositionConstraint(Body &ioBody1, Body &ioBody2, float inC, float inBaumgarte) const
141 {
142 // Only apply position constraint when the constraint is hard, otherwise the velocity bias will fix the constraint
143 if (inC != 0.0f)
144 {
145 // Calculate lagrange multiplier (lambda) for Baumgarte stabilization:
146 //
147 // lambda = -K^-1 * beta / dt * C
148 //
149 // We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out
150 float lambda = -mEffectiveMass * inBaumgarte * inC;
151
152 // Directly integrate velocity change for one time step
153 //
154 // Euler velocity integration:
155 // dv = M^-1 P
156 //
157 // Impulse:
158 // P = J^T lambda
159 //
160 // Euler position integration:
161 // x' = x + dv * dt
162 //
163 // Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and
164 // Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte
165 // stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity
166 // integrate + a position integrate and then discard the velocity change.
167 if (ioBody1.IsDynamic())
168 ioBody1.AddRotationStep(lambda * mInvI1_A);
169 if (ioBody2.IsDynamic())
170 ioBody2.SubPositionStep(lambda * mRatio_InvM2_B);
171 return true;
172 }
173
174 return false;
175 }
176
178 void SaveState(StateRecorder &inStream) const
179 {
180 inStream.Write(mTotalLambda);
181 }
182
185 {
186 inStream.Read(mTotalLambda);
187 }
188
189private:
190 Vec3 mInvI1_A;
191 Vec3 mRatio_InvM2_B;
192 float mEffectiveMass = 0.0f;
193 float mTotalLambda = 0.0f;
194};
195
#define JPH_NAMESPACE_END
Definition: Core.h:367
#define JPH_NAMESPACE_BEGIN
Definition: Core.h:361
#define JPH_ASSERT(...)
Definition: IssueReporting.h:33
constexpr T Square(T inV)
Square a value.
Definition: Math.h:52
Definition: Body.h:35
const MotionProperties * GetMotionProperties() const
Access to the motion properties.
Definition: Body.h:255
bool IsDynamic() const
Check if this body is dynamic, which means that it moves and forces can act on it.
Definition: Body.h:67
void AddRotationStep(Vec3Arg inAngularVelocityTimesDeltaTime)
Update rotation using an Euler step (using during position integrate & constraint solving)
Definition: Body.inl:81
void SubPositionStep(Vec3Arg inLinearVelocityTimesDeltaTime)
Definition: Body.h:290
Vec3 GetLinearVelocity() const
Get world space linear velocity of the center of mass (unit: m/s)
Definition: Body.h:150
Quat GetRotation() const
World space rotation of the body.
Definition: Body.h:237
Vec3 GetAngularVelocity() const
Get world space angular velocity of the center of mass (unit: rad/s)
Definition: Body.h:159
void SubLinearVelocityStep(Vec3Arg inLinearVelocityChange)
Definition: MotionProperties.h:192
float GetInverseMass() const
Get inverse mass (1 / mass). Should only be called on a dynamic object (static or kinematic bodies ha...
Definition: MotionProperties.h:95
JPH_INLINE Vec3 MultiplyWorldSpaceInverseInertiaByVector(QuatArg inBodyRotation, Vec3Arg inV) const
Multiply a vector with the inverse world space inertia tensor ( ). Zero if object is static or kinema...
Definition: MotionProperties.inl:76
void AddAngularVelocityStep(Vec3Arg inAngularVelocityChange)
Definition: MotionProperties.h:193
Definition: RackAndPinionConstraintPart.h:40
void CalculateConstraintProperties(const Body &inBody1, Vec3Arg inWorldSpaceHingeAxis, const Body &inBody2, Vec3Arg inWorldSpaceSliderAxis, float inRatio)
Definition: RackAndPinionConstraintPart.h:69
bool SolveVelocityConstraint(Body &ioBody1, Vec3Arg inWorldSpaceHingeAxis, Body &ioBody2, Vec3Arg inWorldSpaceSliderAxis, float inRatio)
Definition: RackAndPinionConstraintPart.h:118
float GetTotalLambda() const
Return lagrange multiplier.
Definition: RackAndPinionConstraintPart.h:130
void WarmStart(Body &ioBody1, Body &ioBody2, float inWarmStartImpulseRatio)
Definition: RackAndPinionConstraintPart.h:106
void SaveState(StateRecorder &inStream) const
Save state of this constraint part.
Definition: RackAndPinionConstraintPart.h:178
bool SolvePositionConstraint(Body &ioBody1, Body &ioBody2, float inC, float inBaumgarte) const
Definition: RackAndPinionConstraintPart.h:140
void RestoreState(StateRecorder &inStream)
Restore state of this constraint part.
Definition: RackAndPinionConstraintPart.h:184
void Deactivate()
Deactivate this constraint.
Definition: RackAndPinionConstraintPart.h:90
bool IsActive() const
Check if constraint is active.
Definition: RackAndPinionConstraintPart.h:97
Definition: StateRecorder.h:48
void Read(T &outT)
Read a primitive (e.g. float, int, etc.) from the binary stream.
Definition: StreamIn.h:29
void Write(const T &inT)
Write a primitive (e.g. float, int, etc.) to the binary stream.
Definition: StreamOut.h:26
Definition: Vec3.h:16
JPH_INLINE float Dot(Vec3Arg inV2) const
Dot product.
Definition: Vec3.inl:637
JPH_INLINE bool IsNormalized(float inTolerance=1.0e-6f) const
Test if vector is normalized.
Definition: Vec3.inl:737