Jolt Physics
A multi core friendly Game Physics Engine
Loading...
Searching...
No Matches
SixDOFConstraint.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
14
16
19{
20public:
22
23
24 enum EAxis
25 {
29
33
35 NumTranslation = TranslationZ + 1,
36 };
37
38 // See: ConstraintSettings::SaveBinaryState
39 virtual void SaveBinaryState(StreamOut &inStream) const override;
40
42 virtual TwoBodyConstraint * Create(Body &inBody1, Body &inBody2) const override;
43
45 EConstraintSpace mSpace = EConstraintSpace::WorldSpace;
46
48 RVec3 mPosition1 = RVec3::sZero();
49 Vec3 mAxisX1 = Vec3::sAxisX();
50 Vec3 mAxisY1 = Vec3::sAxisY();
51
53 RVec3 mPosition2 = RVec3::sZero();
54 Vec3 mAxisX2 = Vec3::sAxisX();
55 Vec3 mAxisY2 = Vec3::sAxisY();
56
60 float mMaxFriction[EAxis::Num] = { 0, 0, 0, 0, 0, 0 };
61
63 ESwingType mSwingType = ESwingType::Cone;
64
78 float mLimitMin[EAxis::Num] = { -FLT_MAX, -FLT_MAX, -FLT_MAX, -FLT_MAX, -FLT_MAX, -FLT_MAX };
79 float mLimitMax[EAxis::Num] = { FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX };
80
83 SpringSettings mLimitsSpringSettings[EAxis::NumTranslation];
84
86 void MakeFreeAxis(EAxis inAxis) { mLimitMin[inAxis] = -FLT_MAX; mLimitMax[inAxis] = FLT_MAX; }
87 bool IsFreeAxis(EAxis inAxis) const { return mLimitMin[inAxis] == -FLT_MAX && mLimitMax[inAxis] == FLT_MAX; }
88
90 void MakeFixedAxis(EAxis inAxis) { mLimitMin[inAxis] = FLT_MAX; mLimitMax[inAxis] = -FLT_MAX; }
91 bool IsFixedAxis(EAxis inAxis) const { return mLimitMin[inAxis] >= mLimitMax[inAxis]; }
92
94 void SetLimitedAxis(EAxis inAxis, float inMin, float inMax) { mLimitMin[inAxis] = inMin; mLimitMax[inAxis] = inMax; }
95
97 MotorSettings mMotorSettings[EAxis::Num];
98
99protected:
100 // See: ConstraintSettings::RestoreBinaryState
101 virtual void RestoreBinaryState(StreamIn &inStream) override;
102};
103
106{
107public:
109
112
114 SixDOFConstraint(Body &inBody1, Body &inBody2, const SixDOFConstraintSettings &inSettings);
115
117 virtual EConstraintSubType GetSubType() const override { return EConstraintSubType::SixDOF; }
118 virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override;
119 virtual void SetupVelocityConstraint(float inDeltaTime) override;
120 virtual void ResetWarmStart() override;
121 virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
122 virtual bool SolveVelocityConstraint(float inDeltaTime) override;
123 virtual bool SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override;
124#ifdef JPH_DEBUG_RENDERER
125 virtual void DrawConstraint(DebugRenderer *inRenderer) const override;
126 virtual void DrawConstraintLimits(DebugRenderer *inRenderer) const override;
127#endif // JPH_DEBUG_RENDERER
128 virtual void SaveState(StateRecorder &inStream) const override;
129 virtual void RestoreState(StateRecorder &inStream) override;
130 virtual Ref<ConstraintSettings> GetConstraintSettings() const override;
131
132 // See: TwoBodyConstraint
133 virtual Mat44 GetConstraintToBody1Matrix() const override { return Mat44::sRotationTranslation(mConstraintToBody1, mLocalSpacePosition1); }
134 virtual Mat44 GetConstraintToBody2Matrix() const override { return Mat44::sRotationTranslation(mConstraintToBody2, mLocalSpacePosition2); }
135
137 void SetTranslationLimits(Vec3Arg inLimitMin, Vec3Arg inLimitMax);
138
140 void SetRotationLimits(Vec3Arg inLimitMin, Vec3Arg inLimitMax);
141
143 float GetLimitsMin(EAxis inAxis) const { return mLimitMin[inAxis]; }
144 float GetLimitsMax(EAxis inAxis) const { return mLimitMax[inAxis]; }
145 Vec3 GetTranslationLimitsMin() const { return Vec3::sLoadFloat3Unsafe(*reinterpret_cast<const Float3 *>(&mLimitMin[EAxis::TranslationX])); }
146 Vec3 GetTranslationLimitsMax() const { return Vec3::sLoadFloat3Unsafe(*reinterpret_cast<const Float3 *>(&mLimitMax[EAxis::TranslationX])); }
147 Vec3 GetRotationLimitsMin() const { return Vec3::sLoadFloat3Unsafe(*reinterpret_cast<const Float3 *>(&mLimitMin[EAxis::RotationX])); }
148 Vec3 GetRotationLimitsMax() const { return Vec3::sLoadFloat3Unsafe(*reinterpret_cast<const Float3 *>(&mLimitMax[EAxis::RotationX])); }
149
151 inline bool IsFixedAxis(EAxis inAxis) const { return (mFixedAxis & (1 << inAxis)) != 0; }
152 inline bool IsFreeAxis(EAxis inAxis) const { return (mFreeAxis & (1 << inAxis)) != 0; }
153
155 const SpringSettings & GetLimitsSpringSettings(EAxis inAxis) const { JPH_ASSERT(inAxis < EAxis::NumTranslation); return mLimitsSpringSettings[inAxis]; }
156 void SetLimitsSpringSettings(EAxis inAxis, const SpringSettings& inLimitsSpringSettings) { JPH_ASSERT(inAxis < EAxis::NumTranslation); mLimitsSpringSettings[inAxis] = inLimitsSpringSettings; CacheHasSpringLimits(); }
157
159 void SetMaxFriction(EAxis inAxis, float inFriction);
160 float GetMaxFriction(EAxis inAxis) const { return mMaxFriction[inAxis]; }
161
163 Quat GetRotationInConstraintSpace() const;
164
166 MotorSettings & GetMotorSettings(EAxis inAxis) { return mMotorSettings[inAxis]; }
167 const MotorSettings & GetMotorSettings(EAxis inAxis) const { return mMotorSettings[inAxis]; }
168
172 void SetMotorState(EAxis inAxis, EMotorState inState);
173 EMotorState GetMotorState(EAxis inAxis) const { return mMotorState[inAxis]; }
174
176 Vec3 GetTargetVelocityCS() const { return mTargetVelocity; }
177 void SetTargetVelocityCS(Vec3Arg inVelocity) { mTargetVelocity = inVelocity; }
178
180 void SetTargetAngularVelocityCS(Vec3Arg inAngularVelocity) { mTargetAngularVelocity = inAngularVelocity; }
181 Vec3 GetTargetAngularVelocityCS() const { return mTargetAngularVelocity; }
182
184 Vec3 GetTargetPositionCS() const { return mTargetPosition; }
185 void SetTargetPositionCS(Vec3Arg inPosition) { mTargetPosition = inPosition; }
186
188 void SetTargetOrientationCS(QuatArg inOrientation);
189 Quat GetTargetOrientationCS() const { return mTargetOrientation; }
190
193 void SetTargetOrientationBS(QuatArg inOrientation) { SetTargetOrientationCS(mConstraintToBody1.Conjugated() * inOrientation * mConstraintToBody2); }
194
196 inline Vec3 GetTotalLambdaPosition() const { return IsTranslationFullyConstrained()? mPointConstraintPart.GetTotalLambda() : Vec3(mTranslationConstraintPart[0].GetTotalLambda(), mTranslationConstraintPart[1].GetTotalLambda(), mTranslationConstraintPart[2].GetTotalLambda()); }
197 inline Vec3 GetTotalLambdaRotation() const { return IsRotationFullyConstrained()? mRotationConstraintPart.GetTotalLambda() : Vec3(mSwingTwistConstraintPart.GetTotalTwistLambda(), mSwingTwistConstraintPart.GetTotalSwingYLambda(), mSwingTwistConstraintPart.GetTotalSwingZLambda()); }
198 inline Vec3 GetTotalLambdaMotorTranslation() const { return Vec3(mMotorTranslationConstraintPart[0].GetTotalLambda(), mMotorTranslationConstraintPart[1].GetTotalLambda(), mMotorTranslationConstraintPart[2].GetTotalLambda()); }
199 inline Vec3 GetTotalLambdaMotorRotation() const { return Vec3(mMotorRotationConstraintPart[0].GetTotalLambda(), mMotorRotationConstraintPart[1].GetTotalLambda(), mMotorRotationConstraintPart[2].GetTotalLambda()); }
200
201private:
202 // Calculate properties needed for the position constraint
203 inline void GetPositionConstraintProperties(Vec3 &outR1PlusU, Vec3 &outR2, Vec3 &outU) const;
204
205 // Sanitize the translation limits
206 inline void UpdateTranslationLimits();
207
208 // Propagate the rotation limits to the constraint part
209 inline void UpdateRotationLimits();
210
211 // Update the cached state of which axis are free and which ones are fixed
212 inline void UpdateFixedFreeAxis();
213
214 // Cache the state of mTranslationMotorActive
215 void CacheTranslationMotorActive();
216
217 // Cache the state of mRotationMotorActive
218 void CacheRotationMotorActive();
219
220 // Cache the state of mRotationPositionMotorActive
221 void CacheRotationPositionMotorActive();
222
224 void CacheHasSpringLimits();
225
226 // Constraint settings helper functions
227 inline bool IsTranslationConstrained() const { return (mFreeAxis & 0b111) != 0b111; }
228 inline bool IsTranslationFullyConstrained() const { return (mFixedAxis & 0b111) == 0b111 && !mHasSpringLimits; }
229 inline bool IsRotationConstrained() const { return (mFreeAxis & 0b111000) != 0b111000; }
230 inline bool IsRotationFullyConstrained() const { return (mFixedAxis & 0b111000) == 0b111000; }
231 inline bool HasFriction(EAxis inAxis) const { return !IsFixedAxis(inAxis) && mMaxFriction[inAxis] > 0.0f; }
232
233 // CONFIGURATION PROPERTIES FOLLOW
234
235 // Local space constraint positions
236 Vec3 mLocalSpacePosition1;
237 Vec3 mLocalSpacePosition2;
238
239 // Transforms from constraint space to body space
240 Quat mConstraintToBody1;
241 Quat mConstraintToBody2;
242
243 // Limits
244 uint8 mFreeAxis = 0; // Bitmask of free axis (bit 0 = TranslationX)
245 uint8 mFixedAxis = 0; // Bitmask of fixed axis (bit 0 = TranslationX)
246 bool mTranslationMotorActive = false; // If any of the translational frictions / motors are active
247 bool mRotationMotorActive = false; // If any of the rotational frictions / motors are active
248 uint8 mRotationPositionMotorActive = 0; // Bitmask of axis that have position motor active (bit 0 = RotationX)
249 bool mHasSpringLimits = false; // If any of the limit springs have a non-zero frequency/stiffness
250 float mLimitMin[EAxis::Num];
251 float mLimitMax[EAxis::Num];
252 SpringSettings mLimitsSpringSettings[EAxis::NumTranslation];
253
254 // Motor settings for each axis
255 MotorSettings mMotorSettings[EAxis::Num];
256
257 // Friction settings for each axis
258 float mMaxFriction[EAxis::Num];
259
260 // Motor controls
261 EMotorState mMotorState[EAxis::Num] = { EMotorState::Off, EMotorState::Off, EMotorState::Off, EMotorState::Off, EMotorState::Off, EMotorState::Off };
262 Vec3 mTargetVelocity = Vec3::sZero();
263 Vec3 mTargetAngularVelocity = Vec3::sZero();
264 Vec3 mTargetPosition = Vec3::sZero();
265 Quat mTargetOrientation = Quat::sIdentity();
266
267 // RUN TIME PROPERTIES FOLLOW
268
269 // Constraint space axis in world space
270 Vec3 mTranslationAxis[3];
271 Vec3 mRotationAxis[3];
272
273 // Translation displacement (valid when translation axis has a range limit)
274 float mDisplacement[3];
275
276 // Individual constraint parts for translation, or a combined point constraint part if all axis are fixed
277 AxisConstraintPart mTranslationConstraintPart[3];
278 PointConstraintPart mPointConstraintPart;
279
280 // Individual constraint parts for rotation or a combined constraint part if rotation is fixed
281 SwingTwistConstraintPart mSwingTwistConstraintPart;
282 RotationEulerConstraintPart mRotationConstraintPart;
283
284 // Motor or friction constraints
285 AxisConstraintPart mMotorTranslationConstraintPart[3];
286 AngleConstraintPart mMotorRotationConstraintPart[3];
287};
288
@ TranslationZ
Body can move in world space Z axis.
EConstraintSpace
Certain constraints support setting them up in local or world space. This governs what is used.
Definition: Constraint.h:58
EConstraintSubType
Enum to identify constraint sub type.
Definition: Constraint.h:34
std::uint8_t uint8
Definition: Core.h:440
#define JPH_EXPORT
Definition: Core.h:227
#define JPH_NAMESPACE_END
Definition: Core.h:367
#define JPH_NAMESPACE_BEGIN
Definition: Core.h:361
#define JPH_ASSERT(...)
Definition: IssueReporting.h:33
#define JPH_OVERRIDE_NEW_DELETE
Macro to override the new and delete functions.
Definition: Memory.h:29
EMotorState
Definition: MotorSettings.h:17
#define JPH_DECLARE_SERIALIZABLE_VIRTUAL(linkage, class_name)
Definition: SerializableObject.h:100
ESwingType
How the swing limit behaves.
Definition: SwingTwistConstraintPart.h:15
Definition: AngleConstraintPart.h:37
Definition: AxisConstraintPart.h:43
Definition: Body.h:35
ID of a body. This is a way of reasoning about bodies in a multithreaded simulation while avoiding ra...
Definition: BodyID.h:13
virtual void SaveBinaryState(StreamOut &inStream) const
Saves the contents of the constraint settings in binary form to inStream.
Definition: Constraint.cpp:26
virtual void RestoreBinaryState(StreamIn &inStream)
This function should not be called directly, it is used by sRestoreFromBinaryState.
Definition: Constraint.cpp:36
Definition: DebugRenderer.h:47
Class that holds 3 floats. Used as a storage class. Convert to Vec3 for calculations.
Definition: Float3.h:13
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 sRotationTranslation(QuatArg inR, Vec3Arg inT)
Get matrix that rotates and translates.
Definition: Mat44.inl:149
Definition: MotorSettings.h:26
Definition: PointConstraintPart.h:41
Definition: Quat.h:33
static JPH_INLINE Quat sIdentity()
Definition: Quat.h:103
Definition: Reference.h:101
Definition: RotationEulerConstraintPart.h:36
6 Degree Of Freedom Constraint. Allows control over each of the 6 degrees of freedom.
Definition: SixDOFConstraint.h:106
Vec3 GetTotalLambdaPosition() const
Definition: SixDOFConstraint.h:196
void SetLimitsSpringSettings(EAxis inAxis, const SpringSettings &inLimitsSpringSettings)
Definition: SixDOFConstraint.h:156
const MotorSettings & GetMotorSettings(EAxis inAxis) const
Definition: SixDOFConstraint.h:167
float GetLimitsMin(EAxis inAxis) const
Get constraint Limits.
Definition: SixDOFConstraint.h:143
Vec3 GetTotalLambdaMotorTranslation() const
Definition: SixDOFConstraint.h:198
void SetTargetOrientationBS(QuatArg inOrientation)
Definition: SixDOFConstraint.h:193
MotorSettings & GetMotorSettings(EAxis inAxis)
Motor settings.
Definition: SixDOFConstraint.h:166
void SetTargetAngularVelocityCS(Vec3Arg inAngularVelocity)
Set the target angular velocity in body 2 constraint space (!)
Definition: SixDOFConstraint.h:180
Vec3 GetTargetPositionCS() const
Set the target position in body 1 constraint space.
Definition: SixDOFConstraint.h:184
Vec3 GetTotalLambdaRotation() const
Definition: SixDOFConstraint.h:197
const SpringSettings & GetLimitsSpringSettings(EAxis inAxis) const
Update the limits spring settings.
Definition: SixDOFConstraint.h:155
float GetLimitsMax(EAxis inAxis) const
Definition: SixDOFConstraint.h:144
void SetTargetPositionCS(Vec3Arg inPosition)
Definition: SixDOFConstraint.h:185
Vec3 GetTotalLambdaMotorRotation() const
Definition: SixDOFConstraint.h:199
float GetMaxFriction(EAxis inAxis) const
Definition: SixDOFConstraint.h:160
virtual Mat44 GetConstraintToBody2Matrix() const override
Calculates the transform that transforms from constraint space to body 2 space. The first column of t...
Definition: SixDOFConstraint.h:134
bool IsFixedAxis(EAxis inAxis) const
Check which axis are fixed/free.
Definition: SixDOFConstraint.h:151
virtual Mat44 GetConstraintToBody1Matrix() const override
Calculates the transform that transforms from constraint space to body 1 space. The first column of t...
Definition: SixDOFConstraint.h:133
Vec3 GetRotationLimitsMax() const
Definition: SixDOFConstraint.h:148
Vec3 GetTranslationLimitsMax() const
Definition: SixDOFConstraint.h:146
void SetTargetVelocityCS(Vec3Arg inVelocity)
Definition: SixDOFConstraint.h:177
Vec3 GetTargetAngularVelocityCS() const
Definition: SixDOFConstraint.h:181
bool IsFreeAxis(EAxis inAxis) const
Definition: SixDOFConstraint.h:152
Quat GetTargetOrientationCS() const
Definition: SixDOFConstraint.h:189
Vec3 GetTranslationLimitsMin() const
Definition: SixDOFConstraint.h:145
EMotorState GetMotorState(EAxis inAxis) const
Definition: SixDOFConstraint.h:173
Vec3 GetTargetVelocityCS() const
Set the target velocity in body 1 constraint space.
Definition: SixDOFConstraint.h:176
Vec3 GetRotationLimitsMin() const
Definition: SixDOFConstraint.h:147
virtual EConstraintSubType GetSubType() const override
Generic interface of a constraint.
Definition: SixDOFConstraint.h:117
6 Degree Of Freedom Constraint setup structure. Allows control over each of the 6 degrees of freedom.
Definition: SixDOFConstraint.h:19
bool IsFixedAxis(EAxis inAxis) const
Definition: SixDOFConstraint.h:91
void MakeFreeAxis(EAxis inAxis)
Make axis free (unconstrained)
Definition: SixDOFConstraint.h:86
EAxis
Constraint is split up into translation/rotation around X, Y and Z axis.
Definition: SixDOFConstraint.h:25
@ TranslationY
Definition: SixDOFConstraint.h:27
@ RotationX
Definition: SixDOFConstraint.h:30
@ TranslationZ
Definition: SixDOFConstraint.h:28
@ TranslationX
Definition: SixDOFConstraint.h:26
@ RotationY
Definition: SixDOFConstraint.h:31
@ Num
Definition: SixDOFConstraint.h:34
@ RotationZ
Definition: SixDOFConstraint.h:32
void SetLimitedAxis(EAxis inAxis, float inMin, float inMax)
Set a valid range for the constraint (if inMax < inMin, the axis will become fixed)
Definition: SixDOFConstraint.h:94
void MakeFixedAxis(EAxis inAxis)
Make axis fixed (fixed at value 0)
Definition: SixDOFConstraint.h:90
bool IsFreeAxis(EAxis inAxis) const
Definition: SixDOFConstraint.h:87
Settings for a linear or angular spring.
Definition: SpringSettings.h:23
Definition: StateRecorder.h:48
Simple binary input stream.
Definition: StreamIn.h:13
Simple binary output stream.
Definition: StreamOut.h:13
Definition: SwingTwistConstraintPart.h:33
Base class for all constraints that involve 2 bodies. Body1 is usually considered the parent,...
Definition: TwoBodyConstraint.h:27
Base class for settings for all constraints that involve 2 bodies.
Definition: TwoBodyConstraint.h:16
virtual TwoBodyConstraint * Create(Body &inBody1, Body &inBody2) const =0
Definition: Vec3.h:16
static JPH_INLINE Vec3 sAxisX()
Vectors with the principal axis.
Definition: Vec3.h:52
static JPH_INLINE Vec3 sAxisY()
Definition: Vec3.h:53
static JPH_INLINE Vec3 sZero()
Vector with all zeros.
Definition: Vec3.inl:107
static JPH_INLINE Vec3 sLoadFloat3Unsafe(const Float3 &inV)
Load 3 floats from memory (reads 32 bits extra which it doesn't use)
Definition: Vec3.inl:134