Jolt Physics
A multi core friendly Game Physics Engine
Loading...
Searching...
No Matches
IndependentAxisConstraintPart.h
Go to the documentation of this file.
1// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
2// SPDX-FileCopyrightText: 2022 Jorrit Rouwe
3// SPDX-License-Identifier: MIT
4
5#pragma once
6
9
11
48{
50 JPH_INLINE bool ApplyVelocityStep(Body &ioBody1, Body &ioBody2, Vec3Arg inN1, Vec3Arg inN2, float inRatio, float inLambda) const
51 {
52 // Apply impulse if delta is not zero
53 if (inLambda != 0.0f)
54 {
55 // Calculate velocity change due to constraint
56 //
57 // Impulse:
58 // P = J^T lambda
59 //
60 // Euler velocity integration:
61 // v' = v + M^-1 P
62 if (ioBody1.IsDynamic())
63 {
65 mp1->AddLinearVelocityStep((mp1->GetInverseMass() * inLambda) * inN1);
66 mp1->AddAngularVelocityStep(mInvI1_R1xN1 * inLambda);
67 }
68 if (ioBody2.IsDynamic())
69 {
71 mp2->AddLinearVelocityStep((inRatio * mp2->GetInverseMass() * inLambda) * inN2);
72 mp2->AddAngularVelocityStep(mInvI2_RatioR2xN2 * inLambda);
73 }
74 return true;
75 }
76
77 return false;
78 }
79
80public:
89 inline void CalculateConstraintProperties(const Body &inBody1, const Body &inBody2, Vec3Arg inR1, Vec3Arg inN1, Vec3Arg inR2, Vec3Arg inN2, float inRatio)
90 {
91 JPH_ASSERT(inN1.IsNormalized(1.0e-4f) && inN2.IsNormalized(1.0e-4f));
92
93 float inv_effective_mass = 0.0f;
94
95 if (!inBody1.IsStatic())
96 {
97 const MotionProperties *mp1 = inBody1.GetMotionProperties();
98
99 mR1xN1 = inR1.Cross(inN1);
100 mInvI1_R1xN1 = mp1->MultiplyWorldSpaceInverseInertiaByVector(inBody1.GetRotation(), mR1xN1);
101
102 inv_effective_mass += mp1->GetInverseMass() + mInvI1_R1xN1.Dot(mR1xN1);
103 }
104
105 if (!inBody2.IsStatic())
106 {
107 const MotionProperties *mp2 = inBody2.GetMotionProperties();
108
109 mRatioR2xN2 = inRatio * inR2.Cross(inN2);
110 mInvI2_RatioR2xN2 = mp2->MultiplyWorldSpaceInverseInertiaByVector(inBody2.GetRotation(), mRatioR2xN2);
111
112 inv_effective_mass += Square(inRatio) * mp2->GetInverseMass() + mInvI2_RatioR2xN2.Dot(mRatioR2xN2);
113 }
114
115 // Calculate inverse effective mass: K = J M^-1 J^T
116 if (inv_effective_mass == 0.0f)
117 Deactivate();
118 else
119 mEffectiveMass = 1.0f / inv_effective_mass;
120 }
121
123 inline void Deactivate()
124 {
125 mEffectiveMass = 0.0f;
126 mTotalLambda = 0.0f;
127 }
128
130 inline bool IsActive() const
131 {
132 return mEffectiveMass != 0.0f;
133 }
134
142 inline void WarmStart(Body &ioBody1, Body &ioBody2, Vec3Arg inN1, Vec3Arg inN2, float inRatio, float inWarmStartImpulseRatio)
143 {
144 mTotalLambda *= inWarmStartImpulseRatio;
145 ApplyVelocityStep(ioBody1, ioBody2, inN1, inN2, inRatio, mTotalLambda);
146 }
147
156 inline bool SolveVelocityConstraint(Body &ioBody1, Body &ioBody2, Vec3Arg inN1, Vec3Arg inN2, float inRatio, float inMinLambda, float inMaxLambda)
157 {
158 // Lagrange multiplier is:
159 //
160 // lambda = -K^-1 (J v + b)
161 float lambda = -mEffectiveMass * (inN1.Dot(ioBody1.GetLinearVelocity()) + mR1xN1.Dot(ioBody1.GetAngularVelocity()) + inRatio * inN2.Dot(ioBody2.GetLinearVelocity()) + mRatioR2xN2.Dot(ioBody2.GetAngularVelocity()));
162 float new_lambda = Clamp(mTotalLambda + lambda, inMinLambda, inMaxLambda); // Clamp impulse
163 lambda = new_lambda - mTotalLambda; // Lambda potentially got clamped, calculate the new impulse to apply
164 mTotalLambda = new_lambda; // Store accumulated impulse
165
166 return ApplyVelocityStep(ioBody1, ioBody2, inN1, inN2, inRatio, lambda);
167 }
168
170 float GetTotalLambda() const
171 {
172 return mTotalLambda;
173 }
174
183 inline bool SolvePositionConstraint(Body &ioBody1, Body &ioBody2, Vec3Arg inN1, Vec3Arg inN2, float inRatio, float inC, float inBaumgarte) const
184 {
185 if (inC != 0.0f)
186 {
187 // Calculate lagrange multiplier (lambda) for Baumgarte stabilization:
188 //
189 // lambda = -K^-1 * beta / dt * C
190 //
191 // We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out
192 float lambda = -mEffectiveMass * inBaumgarte * inC;
193
194 // Directly integrate velocity change for one time step
195 //
196 // Euler velocity integration:
197 // dv = M^-1 P
198 //
199 // Impulse:
200 // P = J^T lambda
201 //
202 // Euler position integration:
203 // x' = x + dv * dt
204 //
205 // Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and
206 // Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte
207 // stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity
208 // integrate + a position integrate and then discard the velocity change.
209 if (ioBody1.IsDynamic())
210 {
211 ioBody1.AddPositionStep((lambda * ioBody1.GetMotionPropertiesUnchecked()->GetInverseMass()) * inN1);
212 ioBody1.AddRotationStep(lambda * mInvI1_R1xN1);
213 }
214 if (ioBody2.IsDynamic())
215 {
216 ioBody2.AddPositionStep((lambda * inRatio * ioBody2.GetMotionPropertiesUnchecked()->GetInverseMass()) * inN2);
217 ioBody2.AddRotationStep(lambda * mInvI2_RatioR2xN2);
218 }
219 return true;
220 }
221
222 return false;
223 }
224
226 void SaveState(StateRecorder &inStream) const
227 {
228 inStream.Write(mTotalLambda);
229 }
230
233 {
234 inStream.Read(mTotalLambda);
235 }
236
237private:
238 Vec3 mR1xN1;
239 Vec3 mInvI1_R1xN1;
240 Vec3 mRatioR2xN2;
241 Vec3 mInvI2_RatioR2xN2;
242 float mEffectiveMass = 0.0f;
243 float mTotalLambda = 0.0f;
244};
245
#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 Clamp(T inV, T inMin, T inMax)
Clamp a value between two values.
Definition: Math.h:45
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
Vec3 GetLinearVelocity() const
Get world space linear velocity of the center of mass (unit: m/s)
Definition: Body.h:150
bool IsStatic() const
Check if this body is static (not movable)
Definition: Body.h:61
Quat GetRotation() const
World space rotation of the body.
Definition: Body.h:237
const MotionProperties * GetMotionPropertiesUnchecked() const
Access to the motion properties (version that does not check if the object is kinematic or dynamic)
Definition: Body.h:259
Vec3 GetAngularVelocity() const
Get world space angular velocity of the center of mass (unit: rad/s)
Definition: Body.h:159
void AddPositionStep(Vec3Arg inLinearVelocityTimesDeltaTime)
Update position using an Euler step (used during position integrate & constraint solving)
Definition: Body.h:289
Definition: IndependentAxisConstraintPart.h:48
void CalculateConstraintProperties(const Body &inBody1, const Body &inBody2, Vec3Arg inR1, Vec3Arg inN1, Vec3Arg inR2, Vec3Arg inN2, float inRatio)
Definition: IndependentAxisConstraintPart.h:89
float GetTotalLambda() const
Return lagrange multiplier.
Definition: IndependentAxisConstraintPart.h:170
void RestoreState(StateRecorder &inStream)
Restore state of this constraint part.
Definition: IndependentAxisConstraintPart.h:232
bool IsActive() const
Check if constraint is active.
Definition: IndependentAxisConstraintPart.h:130
void WarmStart(Body &ioBody1, Body &ioBody2, Vec3Arg inN1, Vec3Arg inN2, float inRatio, float inWarmStartImpulseRatio)
Definition: IndependentAxisConstraintPart.h:142
bool SolveVelocityConstraint(Body &ioBody1, Body &ioBody2, Vec3Arg inN1, Vec3Arg inN2, float inRatio, float inMinLambda, float inMaxLambda)
Definition: IndependentAxisConstraintPart.h:156
void Deactivate()
Deactivate this constraint.
Definition: IndependentAxisConstraintPart.h:123
bool SolvePositionConstraint(Body &ioBody1, Body &ioBody2, Vec3Arg inN1, Vec3Arg inN2, float inRatio, float inC, float inBaumgarte) const
Definition: IndependentAxisConstraintPart.h:183
void SaveState(StateRecorder &inStream) const
Save state of this constraint part.
Definition: IndependentAxisConstraintPart.h:226
The Body class only keeps track of state for static bodies, the MotionProperties class keeps the addi...
Definition: MotionProperties.h:29
void AddLinearVelocityStep(Vec3Arg inLinearVelocityChange)
Definition: MotionProperties.h:191
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: 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 Vec3 Cross(Vec3Arg inV2) const
Cross product.
Definition: Vec3.inl:582
JPH_INLINE bool IsNormalized(float inTolerance=1.0e-6f) const
Test if vector is normalized.
Definition: Vec3.inl:737