Jolt Physics
A multi core friendly Game Physics Engine
Loading...
Searching...
No Matches
PointConstraintPart.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
41{
42 JPH_INLINE bool ApplyVelocityStep(Body &ioBody1, Body &ioBody2, Vec3Arg inLambda) const
43 {
44 // Apply impulse if delta is not zero
45 if (inLambda != Vec3::sZero())
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 if (ioBody1.IsDynamic())
55 {
57 mp1->SubLinearVelocityStep(mp1->GetInverseMass() * inLambda);
58 mp1->SubAngularVelocityStep(mInvI1_R1X * inLambda);
59 }
60 if (ioBody2.IsDynamic())
61 {
63 mp2->AddLinearVelocityStep(mp2->GetInverseMass() * inLambda);
64 mp2->AddAngularVelocityStep(mInvI2_R2X * inLambda);
65 }
66 return true;
67 }
68
69 return false;
70 }
71
72public:
80 inline void CalculateConstraintProperties(const Body &inBody1, Mat44Arg inRotation1, Vec3Arg inR1, const Body &inBody2, Mat44Arg inRotation2, Vec3Arg inR2)
81 {
82 // Positions where the point constraint acts on (middle point between center of masses) in world space
83 mR1 = inRotation1.Multiply3x3(inR1);
84 mR2 = inRotation2.Multiply3x3(inR2);
85
86 // Calculate effective mass: K^-1 = (J M^-1 J^T)^-1
87 // Using: I^-1 = R * Ibody^-1 * R^T
88 float summed_inv_mass;
89 Mat44 inv_effective_mass;
90 if (inBody1.IsDynamic())
91 {
92 const MotionProperties *mp1 = inBody1.GetMotionProperties();
93 Mat44 inv_i1 = mp1->GetInverseInertiaForRotation(inRotation1);
94 summed_inv_mass = mp1->GetInverseMass();
95
96 Mat44 r1x = Mat44::sCrossProduct(mR1);
97 mInvI1_R1X = inv_i1.Multiply3x3(r1x);
98 inv_effective_mass = r1x.Multiply3x3(inv_i1).Multiply3x3RightTransposed(r1x);
99 }
100 else
101 {
102 JPH_IF_DEBUG(mInvI1_R1X = Mat44::sNaN();)
103
104 summed_inv_mass = 0.0f;
105 inv_effective_mass = Mat44::sZero();
106 }
107
108 if (inBody2.IsDynamic())
109 {
110 const MotionProperties *mp2 = inBody2.GetMotionProperties();
111 Mat44 inv_i2 = mp2->GetInverseInertiaForRotation(inRotation2);
112 summed_inv_mass += mp2->GetInverseMass();
113
114 Mat44 r2x = Mat44::sCrossProduct(mR2);
115 mInvI2_R2X = inv_i2.Multiply3x3(r2x);
116 inv_effective_mass += r2x.Multiply3x3(inv_i2).Multiply3x3RightTransposed(r2x);
117 }
118 else
119 {
120 JPH_IF_DEBUG(mInvI2_R2X = Mat44::sNaN();)
121 }
122
123 inv_effective_mass += Mat44::sScale(summed_inv_mass);
124 if (!mEffectiveMass.SetInversed3x3(inv_effective_mass))
125 Deactivate();
126 }
127
129 inline void Deactivate()
130 {
131 mEffectiveMass = Mat44::sZero();
132 mTotalLambda = Vec3::sZero();
133 }
134
136 inline bool IsActive() const
137 {
138 return mEffectiveMass(3, 3) != 0.0f;
139 }
140
145 inline void WarmStart(Body &ioBody1, Body &ioBody2, float inWarmStartImpulseRatio)
146 {
147 mTotalLambda *= inWarmStartImpulseRatio;
148 ApplyVelocityStep(ioBody1, ioBody2, mTotalLambda);
149 }
150
154 inline bool SolveVelocityConstraint(Body &ioBody1, Body &ioBody2)
155 {
156 // Calculate lagrange multiplier:
157 //
158 // lambda = -K^-1 (J v + b)
159 Vec3 lambda = mEffectiveMass * (ioBody1.GetLinearVelocity() - mR1.Cross(ioBody1.GetAngularVelocity()) - ioBody2.GetLinearVelocity() + mR2.Cross(ioBody2.GetAngularVelocity()));
160 mTotalLambda += lambda; // Store accumulated lambda
161 return ApplyVelocityStep(ioBody1, ioBody2, lambda);
162 }
163
168 inline bool SolvePositionConstraint(Body &ioBody1, Body &ioBody2, float inBaumgarte) const
169 {
170 Vec3 separation = (Vec3(ioBody2.GetCenterOfMassPosition() - ioBody1.GetCenterOfMassPosition()) + mR2 - mR1);
171 if (separation != Vec3::sZero())
172 {
173 // Calculate lagrange multiplier (lambda) for Baumgarte stabilization:
174 //
175 // lambda = -K^-1 * beta / dt * C
176 //
177 // We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out
178 Vec3 lambda = mEffectiveMass * -inBaumgarte * separation;
179
180 // Directly integrate velocity change for one time step
181 //
182 // Euler velocity integration:
183 // dv = M^-1 P
184 //
185 // Impulse:
186 // P = J^T lambda
187 //
188 // Euler position integration:
189 // x' = x + dv * dt
190 //
191 // Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and
192 // Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte
193 // stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity
194 // integrate + a position integrate and then discard the velocity change.
195 if (ioBody1.IsDynamic())
196 {
197 ioBody1.SubPositionStep(ioBody1.GetMotionProperties()->GetInverseMass() * lambda);
198 ioBody1.SubRotationStep(mInvI1_R1X * lambda);
199 }
200 if (ioBody2.IsDynamic())
201 {
202 ioBody2.AddPositionStep(ioBody2.GetMotionProperties()->GetInverseMass() * lambda);
203 ioBody2.AddRotationStep(mInvI2_R2X * lambda);
204 }
205
206 return true;
207 }
208
209 return false;
210 }
211
214 {
215 return mTotalLambda;
216 }
217
219 void SaveState(StateRecorder &inStream) const
220 {
221 inStream.Write(mTotalLambda);
222 }
223
226 {
227 inStream.Read(mTotalLambda);
228 }
229
230private:
231 Vec3 mR1;
232 Vec3 mR2;
233 Mat44 mInvI1_R1X;
234 Mat44 mInvI2_R2X;
235 Mat44 mEffectiveMass;
236 Vec3 mTotalLambda { Vec3::sZero() };
237};
238
#define JPH_IF_DEBUG(...)
Definition: Core.h:486
#define JPH_NAMESPACE_END
Definition: Core.h:367
#define JPH_NAMESPACE_BEGIN
Definition: Core.h:361
Definition: Body.h:35
const MotionProperties * GetMotionProperties() const
Access to the motion properties.
Definition: Body.h:255
RVec3 GetCenterOfMassPosition() const
Gets the world space position of this body's center of mass.
Definition: Body.h:243
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
Holds a 4x4 matrix of floats, but supports also operations on the 3x3 upper left part of the matrix.
Definition: Mat44.h:13
static JPH_INLINE Mat44 sZero()
Zero matrix.
Definition: Mat44.inl:30
static JPH_INLINE Mat44 sCrossProduct(Vec3Arg inV)
Get matrix that represents a cross product .
Definition: Mat44.inl:179
static JPH_INLINE Mat44 sNaN()
Matrix filled with NaN's.
Definition: Mat44.inl:40
JPH_INLINE bool SetInversed3x3(Mat44Arg inM)
*this = inM.Inversed3x3(), returns false if the matrix is singular in which case *this is unchanged
Definition: Mat44.inl:767
static JPH_INLINE Mat44 sScale(float inScale)
Get matrix that scales uniformly.
Definition: Mat44.inl:163
JPH_INLINE Vec3 Multiply3x3(Vec3Arg inV) const
Multiply vector by only 3x3 part of the matrix.
Definition: Mat44.inl:316
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: PointConstraintPart.h:41
void RestoreState(StateRecorder &inStream)
Restore state of this constraint part.
Definition: PointConstraintPart.h:225
bool SolvePositionConstraint(Body &ioBody1, Body &ioBody2, float inBaumgarte) const
Definition: PointConstraintPart.h:168
bool SolveVelocityConstraint(Body &ioBody1, Body &ioBody2)
Definition: PointConstraintPart.h:154
Vec3 GetTotalLambda() const
Return lagrange multiplier.
Definition: PointConstraintPart.h:213
void WarmStart(Body &ioBody1, Body &ioBody2, float inWarmStartImpulseRatio)
Definition: PointConstraintPart.h:145
void Deactivate()
Deactivate this constraint.
Definition: PointConstraintPart.h:129
bool IsActive() const
Check if constraint is active.
Definition: PointConstraintPart.h:136
void CalculateConstraintProperties(const Body &inBody1, Mat44Arg inRotation1, Vec3Arg inR1, const Body &inBody2, Mat44Arg inRotation2, Vec3Arg inR2)
Definition: PointConstraintPart.h:80
void SaveState(StateRecorder &inStream) const
Save state of this constraint part.
Definition: PointConstraintPart.h:219
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 Vec3 Cross(Vec3Arg inV2) const
Cross product.
Definition: Vec3.inl:582
static JPH_INLINE Vec3 sZero()
Vector with all zeros.
Definition: Vec3.inl:107