Jolt Physics
A multi core friendly Game Physics Engine
Loading...
Searching...
No Matches
AngleConstraintPart.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
11
13
37{
39 JPH_INLINE bool ApplyVelocityStep(Body &ioBody1, Body &ioBody2, float inLambda) const
40 {
41 // Apply impulse if delta is not zero
42 if (inLambda != 0.0f)
43 {
44 // Calculate velocity change due to constraint
45 //
46 // Impulse:
47 // P = J^T lambda
48 //
49 // Euler velocity integration:
50 // v' = v + M^-1 P
51 if (ioBody1.IsDynamic())
52 ioBody1.GetMotionProperties()->SubAngularVelocityStep(inLambda * mInvI1_Axis);
53 if (ioBody2.IsDynamic())
54 ioBody2.GetMotionProperties()->AddAngularVelocityStep(inLambda * mInvI2_Axis);
55 return true;
56 }
57
58 return false;
59 }
60
62 JPH_INLINE float CalculateInverseEffectiveMass(const Body &inBody1, const Body &inBody2, Vec3Arg inWorldSpaceAxis)
63 {
64 JPH_ASSERT(inWorldSpaceAxis.IsNormalized(1.0e-4f));
65
66 // Calculate properties used below
67 mInvI1_Axis = inBody1.IsDynamic()? inBody1.GetMotionProperties()->MultiplyWorldSpaceInverseInertiaByVector(inBody1.GetRotation(), inWorldSpaceAxis) : Vec3::sZero();
68 mInvI2_Axis = inBody2.IsDynamic()? inBody2.GetMotionProperties()->MultiplyWorldSpaceInverseInertiaByVector(inBody2.GetRotation(), inWorldSpaceAxis) : Vec3::sZero();
69
70 // Calculate inverse effective mass: K = J M^-1 J^T
71 return inWorldSpaceAxis.Dot(mInvI1_Axis + mInvI2_Axis);
72 }
73
74public:
81 inline void CalculateConstraintProperties(const Body &inBody1, const Body &inBody2, Vec3Arg inWorldSpaceAxis, float inBias = 0.0f)
82 {
83 float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inBody2, inWorldSpaceAxis);
84
85 if (inv_effective_mass == 0.0f)
86 Deactivate();
87 else
88 {
89 mEffectiveMass = 1.0f / inv_effective_mass;
90 mSpringPart.CalculateSpringPropertiesWithBias(inBias);
91 }
92 }
93
104 inline void CalculateConstraintPropertiesWithFrequencyAndDamping(float inDeltaTime, const Body &inBody1, const Body &inBody2, Vec3Arg inWorldSpaceAxis, float inBias, float inC, float inFrequency, float inDamping)
105 {
106 float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inBody2, inWorldSpaceAxis);
107
108 if (inv_effective_mass == 0.0f)
109 Deactivate();
110 else
111 mSpringPart.CalculateSpringPropertiesWithFrequencyAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inFrequency, inDamping, mEffectiveMass);
112 }
113
124 inline void CalculateConstraintPropertiesWithStiffnessAndDamping(float inDeltaTime, const Body &inBody1, const Body &inBody2, Vec3Arg inWorldSpaceAxis, float inBias, float inC, float inStiffness, float inDamping)
125 {
126 float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inBody2, inWorldSpaceAxis);
127
128 if (inv_effective_mass == 0.0f)
129 Deactivate();
130 else
131 mSpringPart.CalculateSpringPropertiesWithStiffnessAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inStiffness, inDamping, mEffectiveMass);
132 }
133
135 inline void CalculateConstraintPropertiesWithSettings(float inDeltaTime, const Body &inBody1, const Body &inBody2, Vec3Arg inWorldSpaceAxis, float inBias, float inC, const SpringSettings &inSpringSettings)
136 {
137 float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inBody2, inWorldSpaceAxis);
138
139 if (inv_effective_mass == 0.0f)
140 Deactivate();
141 else if (inSpringSettings.mMode == ESpringMode::FrequencyAndDamping)
142 mSpringPart.CalculateSpringPropertiesWithFrequencyAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inSpringSettings.mFrequency, inSpringSettings.mDamping, mEffectiveMass);
143 else
144 mSpringPart.CalculateSpringPropertiesWithStiffnessAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inSpringSettings.mStiffness, inSpringSettings.mDamping, mEffectiveMass);
145 }
146
148 inline void Deactivate()
149 {
150 mEffectiveMass = 0.0f;
151 mTotalLambda = 0.0f;
152 }
153
155 inline bool IsActive() const
156 {
157 return mEffectiveMass != 0.0f;
158 }
159
164 inline void WarmStart(Body &ioBody1, Body &ioBody2, float inWarmStartImpulseRatio)
165 {
166 mTotalLambda *= inWarmStartImpulseRatio;
167 ApplyVelocityStep(ioBody1, ioBody2, mTotalLambda);
168 }
169
176 inline bool SolveVelocityConstraint(Body &ioBody1, Body &ioBody2, Vec3Arg inWorldSpaceAxis, float inMinLambda, float inMaxLambda)
177 {
178 // Lagrange multiplier is:
179 //
180 // lambda = -K^-1 (J v + b)
181 float lambda = mEffectiveMass * (inWorldSpaceAxis.Dot(ioBody1.GetAngularVelocity() - ioBody2.GetAngularVelocity()) - mSpringPart.GetBias(mTotalLambda));
182 float new_lambda = Clamp(mTotalLambda + lambda, inMinLambda, inMaxLambda); // Clamp impulse
183 lambda = new_lambda - mTotalLambda; // Lambda potentially got clamped, calculate the new impulse to apply
184 mTotalLambda = new_lambda; // Store accumulated impulse
185
186 return ApplyVelocityStep(ioBody1, ioBody2, lambda);
187 }
188
190 float GetTotalLambda() const
191 {
192 return mTotalLambda;
193 }
194
200 inline bool SolvePositionConstraint(Body &ioBody1, Body &ioBody2, float inC, float inBaumgarte) const
201 {
202 // Only apply position constraint when the constraint is hard, otherwise the velocity bias will fix the constraint
203 if (inC != 0.0f && !mSpringPart.IsActive())
204 {
205 // Calculate lagrange multiplier (lambda) for Baumgarte stabilization:
206 //
207 // lambda = -K^-1 * beta / dt * C
208 //
209 // We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out
210 float lambda = -mEffectiveMass * inBaumgarte * inC;
211
212 // Directly integrate velocity change for one time step
213 //
214 // Euler velocity integration:
215 // dv = M^-1 P
216 //
217 // Impulse:
218 // P = J^T lambda
219 //
220 // Euler position integration:
221 // x' = x + dv * dt
222 //
223 // Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and
224 // Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte
225 // stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity
226 // integrate + a position integrate and then discard the velocity change.
227 if (ioBody1.IsDynamic())
228 ioBody1.SubRotationStep(lambda * mInvI1_Axis);
229 if (ioBody2.IsDynamic())
230 ioBody2.AddRotationStep(lambda * mInvI2_Axis);
231 return true;
232 }
233
234 return false;
235 }
236
238 void SaveState(StateRecorder &inStream) const
239 {
240 inStream.Write(mTotalLambda);
241 }
242
245 {
246 inStream.Read(mTotalLambda);
247 }
248
249private:
250 Vec3 mInvI1_Axis;
251 Vec3 mInvI2_Axis;
252 float mEffectiveMass = 0.0f;
253 SpringPart mSpringPart;
254 float mTotalLambda = 0.0f;
255};
256
#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
Definition: AngleConstraintPart.h:37
bool SolvePositionConstraint(Body &ioBody1, Body &ioBody2, float inC, float inBaumgarte) const
Definition: AngleConstraintPart.h:200
void CalculateConstraintPropertiesWithStiffnessAndDamping(float inDeltaTime, const Body &inBody1, const Body &inBody2, Vec3Arg inWorldSpaceAxis, float inBias, float inC, float inStiffness, float inDamping)
Definition: AngleConstraintPart.h:124
void Deactivate()
Deactivate this constraint.
Definition: AngleConstraintPart.h:148
bool IsActive() const
Check if constraint is active.
Definition: AngleConstraintPart.h:155
void CalculateConstraintProperties(const Body &inBody1, const Body &inBody2, Vec3Arg inWorldSpaceAxis, float inBias=0.0f)
Definition: AngleConstraintPart.h:81
void RestoreState(StateRecorder &inStream)
Restore state of this constraint part.
Definition: AngleConstraintPart.h:244
void WarmStart(Body &ioBody1, Body &ioBody2, float inWarmStartImpulseRatio)
Definition: AngleConstraintPart.h:164
bool SolveVelocityConstraint(Body &ioBody1, Body &ioBody2, Vec3Arg inWorldSpaceAxis, float inMinLambda, float inMaxLambda)
Definition: AngleConstraintPart.h:176
void SaveState(StateRecorder &inStream) const
Save state of this constraint part.
Definition: AngleConstraintPart.h:238
void CalculateConstraintPropertiesWithSettings(float inDeltaTime, const Body &inBody1, const Body &inBody2, Vec3Arg inWorldSpaceAxis, float inBias, float inC, const SpringSettings &inSpringSettings)
Selects one of the above functions based on the spring settings.
Definition: AngleConstraintPart.h:135
void CalculateConstraintPropertiesWithFrequencyAndDamping(float inDeltaTime, const Body &inBody1, const Body &inBody2, Vec3Arg inWorldSpaceAxis, float inBias, float inC, float inFrequency, float inDamping)
Definition: AngleConstraintPart.h:104
float GetTotalLambda() const
Return lagrange multiplier.
Definition: AngleConstraintPart.h:190
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
Quat GetRotation() const
World space rotation of the body.
Definition: Body.h:237
void SubRotationStep(Vec3Arg inAngularVelocityTimesDeltaTime)
Definition: Body.inl:100
Vec3 GetAngularVelocity() const
Get world space angular velocity of the center of mass (unit: rad/s)
Definition: Body.h:159
void SubAngularVelocityStep(Vec3Arg inAngularVelocityChange)
Definition: MotionProperties.h:194
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
Class used in other constraint parts to calculate the required bias factor in the lagrange multiplier...
Definition: SpringPart.h:14
void CalculateSpringPropertiesWithFrequencyAndDamping(float inDeltaTime, float inInvEffectiveMass, float inBias, float inC, float inFrequency, float inDamping, float &outEffectiveMass)
Definition: SpringPart.h:86
void CalculateSpringPropertiesWithStiffnessAndDamping(float inDeltaTime, float inInvEffectiveMass, float inBias, float inC, float inStiffness, float inDamping, float &outEffectiveMass)
Definition: SpringPart.h:116
float GetBias(float inTotalLambda) const
Get total bias b, including supplied bias and bias for spring: lambda = J v + b.
Definition: SpringPart.h:137
void CalculateSpringPropertiesWithBias(float inBias)
Definition: SpringPart.h:71
bool IsActive() const
Returns if this spring is active.
Definition: SpringPart.h:131
Settings for a linear or angular spring.
Definition: SpringSettings.h:23
float mStiffness
Definition: SpringSettings.h:60
float mDamping
Definition: SpringSettings.h:67
ESpringMode mMode
Definition: SpringSettings.h:44
float mFrequency
Definition: SpringSettings.h:51
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
static JPH_INLINE Vec3 sZero()
Vector with all zeros.
Definition: Vec3.inl:107