1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 btConeTwistConstraint is Copyright (c) 2007 Starbreeze Studios
4
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
10
11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14
15 Written by: Marcus Hennix
16 */
17
18 /*
19 Overview:
20
21 btConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc).
22 It is a fixed translation, 3 degree-of-freedom (DOF) rotational "joint".
23 It divides the 3 rotational DOFs into swing (movement within a cone) and twist.
24 Swing is divided into swing1 and swing2 which can have different limits, giving an elliptical shape.
25 (Note: the cone's base isn't flat, so this ellipse is "embedded" on the surface of a sphere.)
26
27 In the contraint's frame of reference:
28 twist is along the x-axis,
29 and swing 1 and 2 are along the z and y axes respectively.
30 */
31
32 #ifndef BT_CONETWISTCONSTRAINT_H
33 #define BT_CONETWISTCONSTRAINT_H
34
35 #include "LinearMath/btVector3.h"
36 #include "btJacobianEntry.h"
37 #include "btTypedConstraint.h"
38
39 #ifdef BT_USE_DOUBLE_PRECISION
40 #define btConeTwistConstraintData2 btConeTwistConstraintDoubleData
41 #define btConeTwistConstraintDataName "btConeTwistConstraintDoubleData"
42 #else
43 #define btConeTwistConstraintData2 btConeTwistConstraintData
44 #define btConeTwistConstraintDataName "btConeTwistConstraintData"
45 #endif //BT_USE_DOUBLE_PRECISION
46
47 class btRigidBody;
48
49 enum btConeTwistFlags
50 {
51 BT_CONETWIST_FLAGS_LIN_CFM = 1,
52 BT_CONETWIST_FLAGS_LIN_ERP = 2,
53 BT_CONETWIST_FLAGS_ANG_CFM = 4
54 };
55
56 ///btConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc)
ATTRIBUTE_ALIGNED16(class)57 ATTRIBUTE_ALIGNED16(class)
58 btConeTwistConstraint : public btTypedConstraint
59 {
60 #ifdef IN_PARALLELL_SOLVER
61 public:
62 #endif
63 btJacobianEntry m_jac[3]; //3 orthogonal linear constraints
64
65 btTransform m_rbAFrame;
66 btTransform m_rbBFrame;
67
68 btScalar m_limitSoftness;
69 btScalar m_biasFactor;
70 btScalar m_relaxationFactor;
71
72 btScalar m_damping;
73
74 btScalar m_swingSpan1;
75 btScalar m_swingSpan2;
76 btScalar m_twistSpan;
77
78 btScalar m_fixThresh;
79
80 btVector3 m_swingAxis;
81 btVector3 m_twistAxis;
82
83 btScalar m_kSwing;
84 btScalar m_kTwist;
85
86 btScalar m_twistLimitSign;
87 btScalar m_swingCorrection;
88 btScalar m_twistCorrection;
89
90 btScalar m_twistAngle;
91
92 btScalar m_accSwingLimitImpulse;
93 btScalar m_accTwistLimitImpulse;
94
95 bool m_angularOnly;
96 bool m_solveTwistLimit;
97 bool m_solveSwingLimit;
98
99 bool m_useSolveConstraintObsolete;
100
101 // not yet used...
102 btScalar m_swingLimitRatio;
103 btScalar m_twistLimitRatio;
104 btVector3 m_twistAxisA;
105
106 // motor
107 bool m_bMotorEnabled;
108 bool m_bNormalizedMotorStrength;
109 btQuaternion m_qTarget;
110 btScalar m_maxMotorImpulse;
111 btVector3 m_accMotorImpulse;
112
113 // parameters
114 int m_flags;
115 btScalar m_linCFM;
116 btScalar m_linERP;
117 btScalar m_angCFM;
118
119 protected:
120 void init();
121
122 void computeConeLimitInfo(const btQuaternion& qCone, // in
123 btScalar& swingAngle, btVector3& vSwingAxis, btScalar& swingLimit); // all outs
124
125 void computeTwistLimitInfo(const btQuaternion& qTwist, // in
126 btScalar& twistAngle, btVector3& vTwistAxis); // all outs
127
128 void adjustSwingAxisToUseEllipseNormal(btVector3 & vSwingAxis) const;
129
130 public:
131 BT_DECLARE_ALIGNED_ALLOCATOR();
132
133 btConeTwistConstraint(btRigidBody & rbA, btRigidBody & rbB, const btTransform& rbAFrame, const btTransform& rbBFrame);
134
135 btConeTwistConstraint(btRigidBody & rbA, const btTransform& rbAFrame);
136
137 virtual void buildJacobian();
138
139 virtual void getInfo1(btConstraintInfo1 * info);
140
141 void getInfo1NonVirtual(btConstraintInfo1 * info);
142
143 virtual void getInfo2(btConstraintInfo2 * info);
144
145 void getInfo2NonVirtual(btConstraintInfo2 * info, const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA, const btMatrix3x3& invInertiaWorldB);
146
147 virtual void solveConstraintObsolete(btSolverBody & bodyA, btSolverBody & bodyB, btScalar timeStep);
148
149 void updateRHS(btScalar timeStep);
150
151 const btRigidBody& getRigidBodyA() const
152 {
153 return m_rbA;
154 }
155 const btRigidBody& getRigidBodyB() const
156 {
157 return m_rbB;
158 }
159
160 void setAngularOnly(bool angularOnly)
161 {
162 m_angularOnly = angularOnly;
163 }
164
165 bool getAngularOnly() const
166 {
167 return m_angularOnly;
168 }
169
170 void setLimit(int limitIndex, btScalar limitValue)
171 {
172 switch (limitIndex)
173 {
174 case 3:
175 {
176 m_twistSpan = limitValue;
177 break;
178 }
179 case 4:
180 {
181 m_swingSpan2 = limitValue;
182 break;
183 }
184 case 5:
185 {
186 m_swingSpan1 = limitValue;
187 break;
188 }
189 default:
190 {
191 }
192 };
193 }
194
195 btScalar getLimit(int limitIndex) const
196 {
197 switch (limitIndex)
198 {
199 case 3:
200 {
201 return m_twistSpan;
202 break;
203 }
204 case 4:
205 {
206 return m_swingSpan2;
207 break;
208 }
209 case 5:
210 {
211 return m_swingSpan1;
212 break;
213 }
214 default:
215 {
216 btAssert(0 && "Invalid limitIndex specified for btConeTwistConstraint");
217 return 0.0;
218 }
219 };
220 }
221
222 // setLimit(), a few notes:
223 // _softness:
224 // 0->1, recommend ~0.8->1.
225 // describes % of limits where movement is free.
226 // beyond this softness %, the limit is gradually enforced until the "hard" (1.0) limit is reached.
227 // _biasFactor:
228 // 0->1?, recommend 0.3 +/-0.3 or so.
229 // strength with which constraint resists zeroth order (angular, not angular velocity) limit violation.
230 // __relaxationFactor:
231 // 0->1, recommend to stay near 1.
232 // the lower the value, the less the constraint will fight velocities which violate the angular limits.
233 void setLimit(btScalar _swingSpan1, btScalar _swingSpan2, btScalar _twistSpan, btScalar _softness = 1.f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
234 {
235 m_swingSpan1 = _swingSpan1;
236 m_swingSpan2 = _swingSpan2;
237 m_twistSpan = _twistSpan;
238
239 m_limitSoftness = _softness;
240 m_biasFactor = _biasFactor;
241 m_relaxationFactor = _relaxationFactor;
242 }
243
244 const btTransform& getAFrame() const { return m_rbAFrame; };
245 const btTransform& getBFrame() const { return m_rbBFrame; };
246
247 inline int getSolveTwistLimit()
248 {
249 return m_solveTwistLimit;
250 }
251
252 inline int getSolveSwingLimit()
253 {
254 return m_solveSwingLimit;
255 }
256
257 inline btScalar getTwistLimitSign()
258 {
259 return m_twistLimitSign;
260 }
261
262 void calcAngleInfo();
263 void calcAngleInfo2(const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA, const btMatrix3x3& invInertiaWorldB);
264
265 inline btScalar getSwingSpan1() const
266 {
267 return m_swingSpan1;
268 }
269 inline btScalar getSwingSpan2() const
270 {
271 return m_swingSpan2;
272 }
273 inline btScalar getTwistSpan() const
274 {
275 return m_twistSpan;
276 }
277 inline btScalar getLimitSoftness() const
278 {
279 return m_limitSoftness;
280 }
281 inline btScalar getBiasFactor() const
282 {
283 return m_biasFactor;
284 }
285 inline btScalar getRelaxationFactor() const
286 {
287 return m_relaxationFactor;
288 }
289 inline btScalar getTwistAngle() const
290 {
291 return m_twistAngle;
292 }
293 bool isPastSwingLimit() { return m_solveSwingLimit; }
294
295 btScalar getDamping() const { return m_damping; }
296 void setDamping(btScalar damping) { m_damping = damping; }
297
298 void enableMotor(bool b) { m_bMotorEnabled = b; }
299 bool isMotorEnabled() const { return m_bMotorEnabled; }
300 btScalar getMaxMotorImpulse() const { return m_maxMotorImpulse; }
301 bool isMaxMotorImpulseNormalized() const { return m_bNormalizedMotorStrength; }
302 void setMaxMotorImpulse(btScalar maxMotorImpulse)
303 {
304 m_maxMotorImpulse = maxMotorImpulse;
305 m_bNormalizedMotorStrength = false;
306 }
307 void setMaxMotorImpulseNormalized(btScalar maxMotorImpulse)
308 {
309 m_maxMotorImpulse = maxMotorImpulse;
310 m_bNormalizedMotorStrength = true;
311 }
312
313 btScalar getFixThresh() { return m_fixThresh; }
314 void setFixThresh(btScalar fixThresh) { m_fixThresh = fixThresh; }
315
316 // setMotorTarget:
317 // q: the desired rotation of bodyA wrt bodyB.
318 // note: if q violates the joint limits, the internal target is clamped to avoid conflicting impulses (very bad for stability)
319 // note: don't forget to enableMotor()
320 void setMotorTarget(const btQuaternion& q);
321 const btQuaternion& getMotorTarget() const { return m_qTarget; }
322
323 // same as above, but q is the desired rotation of frameA wrt frameB in constraint space
324 void setMotorTargetInConstraintSpace(const btQuaternion& q);
325
326 btVector3 GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const;
327
328 ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
329 ///If no axis is provided, it uses the default axis for this constraint.
330 virtual void setParam(int num, btScalar value, int axis = -1);
331
332 virtual void setFrames(const btTransform& frameA, const btTransform& frameB);
333
334 const btTransform& getFrameOffsetA() const
335 {
336 return m_rbAFrame;
337 }
338
339 const btTransform& getFrameOffsetB() const
340 {
341 return m_rbBFrame;
342 }
343
344 ///return the local value of parameter
345 virtual btScalar getParam(int num, int axis = -1) const;
346
347 int getFlags() const
348 {
349 return m_flags;
350 }
351
352 virtual int calculateSerializeBufferSize() const;
353
354 ///fills the dataBuffer and returns the struct name (and 0 on failure)
355 virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
356 };
357
358 struct btConeTwistConstraintDoubleData
359 {
360 btTypedConstraintDoubleData m_typeConstraintData;
361 btTransformDoubleData m_rbAFrame;
362 btTransformDoubleData m_rbBFrame;
363
364 //limits
365 double m_swingSpan1;
366 double m_swingSpan2;
367 double m_twistSpan;
368 double m_limitSoftness;
369 double m_biasFactor;
370 double m_relaxationFactor;
371
372 double m_damping;
373 };
374
375 #ifdef BT_BACKWARDS_COMPATIBLE_SERIALIZATION
376 ///this structure is not used, except for loading pre-2.82 .bullet files
377 struct btConeTwistConstraintData
378 {
379 btTypedConstraintData m_typeConstraintData;
380 btTransformFloatData m_rbAFrame;
381 btTransformFloatData m_rbBFrame;
382
383 //limits
384 float m_swingSpan1;
385 float m_swingSpan2;
386 float m_twistSpan;
387 float m_limitSoftness;
388 float m_biasFactor;
389 float m_relaxationFactor;
390
391 float m_damping;
392
393 char m_pad[4];
394 };
395 #endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION
396 //
397
calculateSerializeBufferSize()398 SIMD_FORCE_INLINE int btConeTwistConstraint::calculateSerializeBufferSize() const
399 {
400 return sizeof(btConeTwistConstraintData2);
401 }
402
403 ///fills the dataBuffer and returns the struct name (and 0 on failure)
serialize(void * dataBuffer,btSerializer * serializer)404 SIMD_FORCE_INLINE const char* btConeTwistConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
405 {
406 btConeTwistConstraintData2* cone = (btConeTwistConstraintData2*)dataBuffer;
407 btTypedConstraint::serialize(&cone->m_typeConstraintData, serializer);
408
409 m_rbAFrame.serialize(cone->m_rbAFrame);
410 m_rbBFrame.serialize(cone->m_rbBFrame);
411
412 cone->m_swingSpan1 = m_swingSpan1;
413 cone->m_swingSpan2 = m_swingSpan2;
414 cone->m_twistSpan = m_twistSpan;
415 cone->m_limitSoftness = m_limitSoftness;
416 cone->m_biasFactor = m_biasFactor;
417 cone->m_relaxationFactor = m_relaxationFactor;
418 cone->m_damping = m_damping;
419
420 return btConeTwistConstraintDataName;
421 }
422
423 #endif //BT_CONETWISTCONSTRAINT_H
424