Jolt Physics
A multi core friendly Game Physics Engine
Loading...
Searching...
No Matches
DualAxisConstraintPart.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#include <Jolt/Math/Vector.h>
10#include <Jolt/Math/Matrix.h>
11
13
48{
49public:
50 using Vec2 = Vector<2>;
52
53private:
55 JPH_INLINE bool ApplyVelocityStep(Body &ioBody1, Body &ioBody2, Vec3Arg inN1, Vec3Arg inN2, const Vec2 &inLambda) const
56 {
57 // Apply impulse if delta is not zero
58 if (!inLambda.IsZero())
59 {
60 // Calculate velocity change due to constraint
61 //
62 // Impulse:
63 // P = J^T lambda
64 //
65 // Euler velocity integration:
66 // v' = v + M^-1 P
67 Vec3 impulse = inN1 * inLambda[0] + inN2 * inLambda[1];
68 if (ioBody1.IsDynamic())
69 {
71 mp1->SubLinearVelocityStep(mp1->GetInverseMass() * impulse);
72 mp1->SubAngularVelocityStep(mInvI1_R1PlusUxN1 * inLambda[0] + mInvI1_R1PlusUxN2 * inLambda[1]);
73 }
74 if (ioBody2.IsDynamic())
75 {
77 mp2->AddLinearVelocityStep(mp2->GetInverseMass() * impulse);
78 mp2->AddAngularVelocityStep(mInvI2_R2xN1 * inLambda[0] + mInvI2_R2xN2 * inLambda[1]);
79 }
80 return true;
81 }
82
83 return false;
84 }
85
87 inline void CalculateLagrangeMultiplier(const Body &inBody1, const Body &inBody2, Vec3Arg inN1, Vec3Arg inN2, Vec2 &outLambda) const
88 {
89 // Calculate lagrange multiplier:
90 //
91 // lambda = -K^-1 (J v + b)
92 Vec3 delta_lin = inBody1.GetLinearVelocity() - inBody2.GetLinearVelocity();
93 Vec2 jv;
94 jv[0] = inN1.Dot(delta_lin) + mR1PlusUxN1.Dot(inBody1.GetAngularVelocity()) - mR2xN1.Dot(inBody2.GetAngularVelocity());
95 jv[1] = inN2.Dot(delta_lin) + mR1PlusUxN2.Dot(inBody1.GetAngularVelocity()) - mR2xN2.Dot(inBody2.GetAngularVelocity());
96 outLambda = mEffectiveMass * jv;
97 }
98
99public:
102 inline void CalculateConstraintProperties(const Body &inBody1, Mat44Arg inRotation1, Vec3Arg inR1PlusU, const Body &inBody2, Mat44Arg inRotation2, Vec3Arg inR2, Vec3Arg inN1, Vec3Arg inN2)
103 {
104 JPH_ASSERT(inN1.IsNormalized(1.0e-5f));
105 JPH_ASSERT(inN2.IsNormalized(1.0e-5f));
106
107 // Calculate properties used during constraint solving
108 mR1PlusUxN1 = inR1PlusU.Cross(inN1);
109 mR1PlusUxN2 = inR1PlusU.Cross(inN2);
110 mR2xN1 = inR2.Cross(inN1);
111 mR2xN2 = inR2.Cross(inN2);
112
113 // Calculate effective mass: K^-1 = (J M^-1 J^T)^-1, eq 59
114 Mat22 inv_effective_mass;
115 if (inBody1.IsDynamic())
116 {
117 const MotionProperties *mp1 = inBody1.GetMotionProperties();
118 Mat44 inv_i1 = mp1->GetInverseInertiaForRotation(inRotation1);
119 mInvI1_R1PlusUxN1 = inv_i1.Multiply3x3(mR1PlusUxN1);
120 mInvI1_R1PlusUxN2 = inv_i1.Multiply3x3(mR1PlusUxN2);
121
122 inv_effective_mass(0, 0) = mp1->GetInverseMass() + mR1PlusUxN1.Dot(mInvI1_R1PlusUxN1);
123 inv_effective_mass(0, 1) = mR1PlusUxN1.Dot(mInvI1_R1PlusUxN2);
124 inv_effective_mass(1, 0) = mR1PlusUxN2.Dot(mInvI1_R1PlusUxN1);
125 inv_effective_mass(1, 1) = mp1->GetInverseMass() + mR1PlusUxN2.Dot(mInvI1_R1PlusUxN2);
126 }
127 else
128 {
129 JPH_IF_DEBUG(mInvI1_R1PlusUxN1 = Vec3::sNaN();)
130 JPH_IF_DEBUG(mInvI1_R1PlusUxN2 = Vec3::sNaN();)
131
132 inv_effective_mass = Mat22::sZero();
133 }
134
135 if (inBody2.IsDynamic())
136 {
137 const MotionProperties *mp2 = inBody2.GetMotionProperties();
138 Mat44 inv_i2 = mp2->GetInverseInertiaForRotation(inRotation2);
139 mInvI2_R2xN1 = inv_i2.Multiply3x3(mR2xN1);
140 mInvI2_R2xN2 = inv_i2.Multiply3x3(mR2xN2);
141
142 inv_effective_mass(0, 0) += mp2->GetInverseMass() + mR2xN1.Dot(mInvI2_R2xN1);
143 inv_effective_mass(0, 1) += mR2xN1.Dot(mInvI2_R2xN2);
144 inv_effective_mass(1, 0) += mR2xN2.Dot(mInvI2_R2xN1);
145 inv_effective_mass(1, 1) += mp2->GetInverseMass() + mR2xN2.Dot(mInvI2_R2xN2);
146 }
147 else
148 {
149 JPH_IF_DEBUG(mInvI2_R2xN1 = Vec3::sNaN();)
150 JPH_IF_DEBUG(mInvI2_R2xN2 = Vec3::sNaN();)
151 }
152
153 if (!mEffectiveMass.SetInversed(inv_effective_mass))
154 Deactivate();
155 }
156
158 inline void Deactivate()
159 {
160 mEffectiveMass.SetZero();
161 mTotalLambda.SetZero();
162 }
163
165 inline bool IsActive() const
166 {
167 return !mEffectiveMass.IsZero();
168 }
169
172 inline void WarmStart(Body &ioBody1, Body &ioBody2, Vec3Arg inN1, Vec3Arg inN2, float inWarmStartImpulseRatio)
173 {
174 mTotalLambda *= inWarmStartImpulseRatio;
175 ApplyVelocityStep(ioBody1, ioBody2, inN1, inN2, mTotalLambda);
176 }
177
180 inline bool SolveVelocityConstraint(Body &ioBody1, Body &ioBody2, Vec3Arg inN1, Vec3Arg inN2)
181 {
182 Vec2 lambda;
183 CalculateLagrangeMultiplier(ioBody1, ioBody2, inN1, inN2, lambda);
184
185 // Store accumulated lambda
186 mTotalLambda += lambda;
187
188 return ApplyVelocityStep(ioBody1, ioBody2, inN1, inN2, lambda);
189 }
190
193 inline bool SolvePositionConstraint(Body &ioBody1, Body &ioBody2, Vec3Arg inU, Vec3Arg inN1, Vec3Arg inN2, float inBaumgarte) const
194 {
195 Vec2 c;
196 c[0] = inU.Dot(inN1);
197 c[1] = inU.Dot(inN2);
198 if (!c.IsZero())
199 {
200 // Calculate lagrange multiplier (lambda) for Baumgarte stabilization:
201 //
202 // lambda = -K^-1 * beta / dt * C
203 //
204 // We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out
205 Vec2 lambda = -inBaumgarte * (mEffectiveMass * c);
206
207 // Directly integrate velocity change for one time step
208 //
209 // Euler velocity integration:
210 // dv = M^-1 P
211 //
212 // Impulse:
213 // P = J^T lambda
214 //
215 // Euler position integration:
216 // x' = x + dv * dt
217 //
218 // Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and
219 // Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte
220 // stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity
221 // integrate + a position integrate and then discard the velocity change.
222 Vec3 impulse = inN1 * lambda[0] + inN2 * lambda[1];
223 if (ioBody1.IsDynamic())
224 {
225 ioBody1.SubPositionStep(ioBody1.GetMotionProperties()->GetInverseMass() * impulse);
226 ioBody1.SubRotationStep(mInvI1_R1PlusUxN1 * lambda[0] + mInvI1_R1PlusUxN2 * lambda[1]);
227 }
228 if (ioBody2.IsDynamic())
229 {
230 ioBody2.AddPositionStep(ioBody2.GetMotionProperties()->GetInverseMass() * impulse);
231 ioBody2.AddRotationStep(mInvI2_R2xN1 * lambda[0] + mInvI2_R2xN2 * lambda[1]);
232 }
233 return true;
234 }
235
236 return false;
237 }
238
240 inline void SetTotalLambda(const Vec2 &inLambda)
241 {
242 mTotalLambda = inLambda;
243 }
244
246 inline const Vec2 & GetTotalLambda() const
247 {
248 return mTotalLambda;
249 }
250
252 void SaveState(StateRecorder &inStream) const
253 {
254 inStream.Write(mTotalLambda);
255 }
256
259 {
260 inStream.Read(mTotalLambda);
261 }
262
263private:
264 Vec3 mR1PlusUxN1;
265 Vec3 mR1PlusUxN2;
266 Vec3 mR2xN1;
267 Vec3 mR2xN2;
268 Vec3 mInvI1_R1PlusUxN1;
269 Vec3 mInvI1_R1PlusUxN2;
270 Vec3 mInvI2_R2xN1;
271 Vec3 mInvI2_R2xN2;
272 Mat22 mEffectiveMass;
273 Vec2 mTotalLambda { Vec2::sZero() };
274};
275
#define JPH_IF_DEBUG(...)
Definition: Core.h:486
#define JPH_NAMESPACE_END
Definition: Core.h:367
#define JPH_NAMESPACE_BEGIN
Definition: Core.h:361
#define JPH_ASSERT(...)
Definition: IssueReporting.h:33
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
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 AddPositionStep(Vec3Arg inLinearVelocityTimesDeltaTime)
Update position using an Euler step (used during position integrate & constraint solving)
Definition: Body.h:289
Definition: DualAxisConstraintPart.h:48
bool SolveVelocityConstraint(Body &ioBody1, Body &ioBody2, Vec3Arg inN1, Vec3Arg inN2)
Definition: DualAxisConstraintPart.h:180
void RestoreState(StateRecorder &inStream)
Restore state of this constraint part.
Definition: DualAxisConstraintPart.h:258
Matrix< 2, 2 > Mat22
Definition: DualAxisConstraintPart.h:51
void WarmStart(Body &ioBody1, Body &ioBody2, Vec3Arg inN1, Vec3Arg inN2, float inWarmStartImpulseRatio)
Definition: DualAxisConstraintPart.h:172
bool SolvePositionConstraint(Body &ioBody1, Body &ioBody2, Vec3Arg inU, Vec3Arg inN1, Vec3Arg inN2, float inBaumgarte) const
Definition: DualAxisConstraintPart.h:193
Vector< 2 > Vec2
Definition: DualAxisConstraintPart.h:50
void SaveState(StateRecorder &inStream) const
Save state of this constraint part.
Definition: DualAxisConstraintPart.h:252
const Vec2 & GetTotalLambda() const
Return lagrange multiplier.
Definition: DualAxisConstraintPart.h:246
void SetTotalLambda(const Vec2 &inLambda)
Override total lagrange multiplier, can be used to set the initial value for warm starting.
Definition: DualAxisConstraintPart.h:240
bool IsActive() const
Check if constraint is active.
Definition: DualAxisConstraintPart.h:165
void Deactivate()
Deactivate this constraint.
Definition: DualAxisConstraintPart.h:158
void CalculateConstraintProperties(const Body &inBody1, Mat44Arg inRotation1, Vec3Arg inR1PlusU, const Body &inBody2, Mat44Arg inRotation2, Vec3Arg inR2, Vec3Arg inN1, Vec3Arg inN2)
Definition: DualAxisConstraintPart.h:102
Holds a 4x4 matrix of floats, but supports also operations on the 3x3 upper left part of the matrix.
Definition: Mat44.h:13
JPH_INLINE Vec3 Multiply3x3(Vec3Arg inV) const
Multiply vector by only 3x3 part of the matrix.
Definition: Mat44.inl:316
static Matrix sZero()
Definition: Matrix.h:32
bool SetInversed(const Matrix &inM)
Inverse matrix.
Definition: Matrix.h:200
void SetZero()
Zero matrix.
Definition: Matrix.h:26
bool IsZero() const
Check if this matrix consists of all zeros.
Definition: Matrix.h:35
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
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
void SubAngularVelocityStep(Vec3Arg inAngularVelocityChange)
Definition: MotionProperties.h:194
Mat44 GetInverseInertiaForRotation(Mat44Arg inRotation) const
Get inverse inertia matrix ( ) for a given object rotation (translation will be ignored)....
Definition: MotionProperties.inl:59
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
static JPH_INLINE Vec3 sNaN()
Vector with all NaN's.
Definition: Vec3.inl:129
void SetZero()
Vector with all zeros.
Definition: Vector.h:22
bool IsZero() const
Test if vector consists of all zeros.
Definition: Vector.h:69
static Vector sZero()
Definition: Vector.h:28