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