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 void setLimit(int limitIndex,btScalar limitValue)
175 {
176 switch (limitIndex)
177 {
178 case 3:
179 {
180 m_twistSpan = limitValue;
181 break;
182 }
183 case 4:
184 {
185 m_swingSpan2 = limitValue;
186 break;
187 }
188 case 5:
189 {
190 m_swingSpan1 = limitValue;
191 break;
192 }
193 default:
194 {
195 }
196 };
197 }
198
199 // setLimit(), a few notes:
200 // _softness:
201 // 0->1, recommend ~0.8->1.
202 // describes % of limits where movement is free.
203 // beyond this softness %, the limit is gradually enforced until the "hard" (1.0) limit is reached.
204 // _biasFactor:
205 // 0->1?, recommend 0.3 +/-0.3 or so.
206 // strength with which constraint resists zeroth order (angular, not angular velocity) limit violation.
207 // __relaxationFactor:
208 // 0->1, recommend to stay near 1.
209 // the lower the value, the less the constraint will fight velocities which violate the angular limits.
210 void setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan, btScalar _softness = 1.f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
211 {
212 m_swingSpan1 = _swingSpan1;
213 m_swingSpan2 = _swingSpan2;
214 m_twistSpan = _twistSpan;
215
216 m_limitSoftness = _softness;
217 m_biasFactor = _biasFactor;
218 m_relaxationFactor = _relaxationFactor;
219 }
220
221 const btTransform& getAFrame() { return m_rbAFrame; };
222 const btTransform& getBFrame() { return m_rbBFrame; };
223
224 inline int getSolveTwistLimit()
225 {
226 return m_solveTwistLimit;
227 }
228
229 inline int getSolveSwingLimit()
230 {
231 return m_solveTwistLimit;
232 }
233
234 inline btScalar getTwistLimitSign()
235 {
236 return m_twistLimitSign;
237 }
238
239 void calcAngleInfo();
240 void calcAngleInfo2(const btTransform& transA, const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB);
241
242 inline btScalar getSwingSpan1()
243 {
244 return m_swingSpan1;
245 }
246 inline btScalar getSwingSpan2()
247 {
248 return m_swingSpan2;
249 }
250 inline btScalar getTwistSpan()
251 {
252 return m_twistSpan;
253 }
254 inline btScalar getTwistAngle()
255 {
256 return m_twistAngle;
257 }
258 bool isPastSwingLimit() { return m_solveSwingLimit; }
259
260 void setDamping(btScalar damping) { m_damping = damping; }
261
262 void enableMotor(bool b) { m_bMotorEnabled = b; }
263 void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = false; }
264 void setMaxMotorImpulseNormalized(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = true; }
265
266 btScalar getFixThresh() { return m_fixThresh; }
267 void setFixThresh(btScalar fixThresh) { m_fixThresh = fixThresh; }
268
269 // setMotorTarget:
270 // q: the desired rotation of bodyA wrt bodyB.
271 // note: if q violates the joint limits, the internal target is clamped to avoid conflicting impulses (very bad for stability)
272 // note: don't forget to enableMotor()
273 void setMotorTarget(const btQuaternion &q);
274
275 // same as above, but q is the desired rotation of frameA wrt frameB in constraint space
276 void setMotorTargetInConstraintSpace(const btQuaternion &q);
277
278 btVector3 GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const;
279
280 ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
281 ///If no axis is provided, it uses the default axis for this constraint.
282 virtual void setParam(int num, btScalar value, int axis = -1);
283
284 virtual void setFrames(const btTransform& frameA, const btTransform& frameB);
285
286 const btTransform& getFrameOffsetA() const
287 {
288 return m_rbAFrame;
289 }
290
291 const btTransform& getFrameOffsetB() const
292 {
293 return m_rbBFrame;
294 }
295
296
297 ///return the local value of parameter
298 virtual btScalar getParam(int num, int axis = -1) const;
299
300 virtual int calculateSerializeBufferSize() const;
301
302 ///fills the dataBuffer and returns the struct name (and 0 on failure)
303 virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
304
305 };
306
307
308
309 struct btConeTwistConstraintDoubleData
310 {
311 btTypedConstraintDoubleData m_typeConstraintData;
312 btTransformDoubleData m_rbAFrame;
313 btTransformDoubleData m_rbBFrame;
314
315 //limits
316 double m_swingSpan1;
317 double m_swingSpan2;
318 double m_twistSpan;
319 double m_limitSoftness;
320 double m_biasFactor;
321 double m_relaxationFactor;
322
323 double m_damping;
324
325
326
327 };
328
329 #ifdef BT_BACKWARDS_COMPATIBLE_SERIALIZATION
330 ///this structure is not used, except for loading pre-2.82 .bullet files
331 struct btConeTwistConstraintData
332 {
333 btTypedConstraintData m_typeConstraintData;
334 btTransformFloatData m_rbAFrame;
335 btTransformFloatData m_rbBFrame;
336
337 //limits
338 float m_swingSpan1;
339 float m_swingSpan2;
340 float m_twistSpan;
341 float m_limitSoftness;
342 float m_biasFactor;
343 float m_relaxationFactor;
344
345 float m_damping;
346
347 char m_pad[4];
348
349 };
350 #endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION
351 //
352
calculateSerializeBufferSize()353 SIMD_FORCE_INLINE int btConeTwistConstraint::calculateSerializeBufferSize() const
354 {
355 return sizeof(btConeTwistConstraintData2);
356
357 }
358
359
360 ///fills the dataBuffer and returns the struct name (and 0 on failure)
serialize(void * dataBuffer,btSerializer * serializer)361 SIMD_FORCE_INLINE const char* btConeTwistConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
362 {
363 btConeTwistConstraintData2* cone = (btConeTwistConstraintData2*) dataBuffer;
364 btTypedConstraint::serialize(&cone->m_typeConstraintData,serializer);
365
366 m_rbAFrame.serialize(cone->m_rbAFrame);
367 m_rbBFrame.serialize(cone->m_rbBFrame);
368
369 cone->m_swingSpan1 = m_swingSpan1;
370 cone->m_swingSpan2 = m_swingSpan2;
371 cone->m_twistSpan = m_twistSpan;
372 cone->m_limitSoftness = m_limitSoftness;
373 cone->m_biasFactor = m_biasFactor;
374 cone->m_relaxationFactor = m_relaxationFactor;
375 cone->m_damping = m_damping;
376
377 return btConeTwistConstraintDataName;
378 }
379
380
381 #endif //BT_CONETWISTCONSTRAINT_H
382