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 class btRigidBody;
44
45 enum btConeTwistFlags
46 {
47 BT_CONETWIST_FLAGS_LIN_CFM = 1,
48 BT_CONETWIST_FLAGS_LIN_ERP = 2,
49 BT_CONETWIST_FLAGS_ANG_CFM = 4
50 };
51
52 ///btConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc)
53 class btConeTwistConstraint : public btTypedConstraint
54 {
55 #ifdef IN_PARALLELL_SOLVER
56 public:
57 #endif
58 btJacobianEntry m_jac[3]; //3 orthogonal linear constraints
59
60 btTransform m_rbAFrame;
61 btTransform m_rbBFrame;
62
63 btScalar m_limitSoftness;
64 btScalar m_biasFactor;
65 btScalar m_relaxationFactor;
66
67 btScalar m_damping;
68
69 btScalar m_swingSpan1;
70 btScalar m_swingSpan2;
71 btScalar m_twistSpan;
72
73 btScalar m_fixThresh;
74
75 btVector3 m_swingAxis;
76 btVector3 m_twistAxis;
77
78 btScalar m_kSwing;
79 btScalar m_kTwist;
80
81 btScalar m_twistLimitSign;
82 btScalar m_swingCorrection;
83 btScalar m_twistCorrection;
84
85 btScalar m_twistAngle;
86
87 btScalar m_accSwingLimitImpulse;
88 btScalar m_accTwistLimitImpulse;
89
90 bool m_angularOnly;
91 bool m_solveTwistLimit;
92 bool m_solveSwingLimit;
93
94 bool m_useSolveConstraintObsolete;
95
96 // not yet used...
97 btScalar m_swingLimitRatio;
98 btScalar m_twistLimitRatio;
99 btVector3 m_twistAxisA;
100
101 // motor
102 bool m_bMotorEnabled;
103 bool m_bNormalizedMotorStrength;
104 btQuaternion m_qTarget;
105 btScalar m_maxMotorImpulse;
106 btVector3 m_accMotorImpulse;
107
108 // parameters
109 int m_flags;
110 btScalar m_linCFM;
111 btScalar m_linERP;
112 btScalar m_angCFM;
113
114 protected:
115
116 void init();
117
118 void computeConeLimitInfo(const btQuaternion& qCone, // in
119 btScalar& swingAngle, btVector3& vSwingAxis, btScalar& swingLimit); // all outs
120
121 void computeTwistLimitInfo(const btQuaternion& qTwist, // in
122 btScalar& twistAngle, btVector3& vTwistAxis); // all outs
123
124 void adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const;
125
126
127 public:
128
129 btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB,const btTransform& rbAFrame, const btTransform& rbBFrame);
130
131 btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame);
132
133 virtual void buildJacobian();
134
135 virtual void getInfo1 (btConstraintInfo1* info);
136
137 void getInfo1NonVirtual(btConstraintInfo1* info);
138
139 virtual void getInfo2 (btConstraintInfo2* info);
140
141 void getInfo2NonVirtual(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB);
142
143 virtual void solveConstraintObsolete(btRigidBody& bodyA,btRigidBody& bodyB,btScalar timeStep);
144
145 void updateRHS(btScalar timeStep);
146
147
getRigidBodyA()148 const btRigidBody& getRigidBodyA() const
149 {
150 return m_rbA;
151 }
getRigidBodyB()152 const btRigidBody& getRigidBodyB() const
153 {
154 return m_rbB;
155 }
156
setAngularOnly(bool angularOnly)157 void setAngularOnly(bool angularOnly)
158 {
159 m_angularOnly = angularOnly;
160 }
161
setLimit(int limitIndex,btScalar limitValue)162 void setLimit(int limitIndex,btScalar limitValue)
163 {
164 switch (limitIndex)
165 {
166 case 3:
167 {
168 m_twistSpan = limitValue;
169 break;
170 }
171 case 4:
172 {
173 m_swingSpan2 = limitValue;
174 break;
175 }
176 case 5:
177 {
178 m_swingSpan1 = limitValue;
179 break;
180 }
181 default:
182 {
183 }
184 };
185 }
186
187 // setLimit(), a few notes:
188 // _softness:
189 // 0->1, recommend ~0.8->1.
190 // describes % of limits where movement is free.
191 // beyond this softness %, the limit is gradually enforced until the "hard" (1.0) limit is reached.
192 // _biasFactor:
193 // 0->1?, recommend 0.3 +/-0.3 or so.
194 // strength with which constraint resists zeroth order (angular, not angular velocity) limit violation.
195 // __relaxationFactor:
196 // 0->1, recommend to stay near 1.
197 // the lower the value, the less the constraint will fight velocities which violate the angular limits.
198 void setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan, btScalar _softness = 1.f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
199 {
200 m_swingSpan1 = _swingSpan1;
201 m_swingSpan2 = _swingSpan2;
202 m_twistSpan = _twistSpan;
203
204 m_limitSoftness = _softness;
205 m_biasFactor = _biasFactor;
206 m_relaxationFactor = _relaxationFactor;
207 }
208
getAFrame()209 const btTransform& getAFrame() { return m_rbAFrame; };
getBFrame()210 const btTransform& getBFrame() { return m_rbBFrame; };
211
getSolveTwistLimit()212 inline int getSolveTwistLimit()
213 {
214 return m_solveTwistLimit;
215 }
216
getSolveSwingLimit()217 inline int getSolveSwingLimit()
218 {
219 return m_solveTwistLimit;
220 }
221
getTwistLimitSign()222 inline btScalar getTwistLimitSign()
223 {
224 return m_twistLimitSign;
225 }
226
227 void calcAngleInfo();
228 void calcAngleInfo2(const btTransform& transA, const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB);
229
getSwingSpan1()230 inline btScalar getSwingSpan1()
231 {
232 return m_swingSpan1;
233 }
getSwingSpan2()234 inline btScalar getSwingSpan2()
235 {
236 return m_swingSpan2;
237 }
getTwistSpan()238 inline btScalar getTwistSpan()
239 {
240 return m_twistSpan;
241 }
getTwistAngle()242 inline btScalar getTwistAngle()
243 {
244 return m_twistAngle;
245 }
isPastSwingLimit()246 bool isPastSwingLimit() { return m_solveSwingLimit; }
247
setDamping(btScalar damping)248 void setDamping(btScalar damping) { m_damping = damping; }
249
enableMotor(bool b)250 void enableMotor(bool b) { m_bMotorEnabled = b; }
setMaxMotorImpulse(btScalar maxMotorImpulse)251 void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = false; }
setMaxMotorImpulseNormalized(btScalar maxMotorImpulse)252 void setMaxMotorImpulseNormalized(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = true; }
253
getFixThresh()254 btScalar getFixThresh() { return m_fixThresh; }
setFixThresh(btScalar fixThresh)255 void setFixThresh(btScalar fixThresh) { m_fixThresh = fixThresh; }
256
257 // setMotorTarget:
258 // q: the desired rotation of bodyA wrt bodyB.
259 // note: if q violates the joint limits, the internal target is clamped to avoid conflicting impulses (very bad for stability)
260 // note: don't forget to enableMotor()
261 void setMotorTarget(const btQuaternion &q);
262
263 // same as above, but q is the desired rotation of frameA wrt frameB in constraint space
264 void setMotorTargetInConstraintSpace(const btQuaternion &q);
265
266 btVector3 GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const;
267
268 ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
269 ///If no axis is provided, it uses the default axis for this constraint.
270 virtual void setParam(int num, btScalar value, int axis = -1);
271
272 virtual void setFrames(const btTransform& frameA, const btTransform& frameB);
273
getFrameOffsetA()274 const btTransform& getFrameOffsetA() const
275 {
276 return m_rbAFrame;
277 }
278
getFrameOffsetB()279 const btTransform& getFrameOffsetB() const
280 {
281 return m_rbBFrame;
282 }
283
284
285 ///return the local value of parameter
286 virtual btScalar getParam(int num, int axis = -1) const;
287
288 virtual int calculateSerializeBufferSize() const;
289
290 ///fills the dataBuffer and returns the struct name (and 0 on failure)
291 virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
292
293 };
294
295 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
296 struct btConeTwistConstraintData
297 {
298 btTypedConstraintData m_typeConstraintData;
299 btTransformFloatData m_rbAFrame;
300 btTransformFloatData m_rbBFrame;
301
302 //limits
303 float m_swingSpan1;
304 float m_swingSpan2;
305 float m_twistSpan;
306 float m_limitSoftness;
307 float m_biasFactor;
308 float m_relaxationFactor;
309
310 float m_damping;
311
312 char m_pad[4];
313
314 };
315
316
317
calculateSerializeBufferSize()318 SIMD_FORCE_INLINE int btConeTwistConstraint::calculateSerializeBufferSize() const
319 {
320 return sizeof(btConeTwistConstraintData);
321
322 }
323
324
325 ///fills the dataBuffer and returns the struct name (and 0 on failure)
serialize(void * dataBuffer,btSerializer * serializer)326 SIMD_FORCE_INLINE const char* btConeTwistConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
327 {
328 btConeTwistConstraintData* cone = (btConeTwistConstraintData*) dataBuffer;
329 btTypedConstraint::serialize(&cone->m_typeConstraintData,serializer);
330
331 m_rbAFrame.serializeFloat(cone->m_rbAFrame);
332 m_rbBFrame.serializeFloat(cone->m_rbBFrame);
333
334 cone->m_swingSpan1 = float(m_swingSpan1);
335 cone->m_swingSpan2 = float(m_swingSpan2);
336 cone->m_twistSpan = float(m_twistSpan);
337 cone->m_limitSoftness = float(m_limitSoftness);
338 cone->m_biasFactor = float(m_biasFactor);
339 cone->m_relaxationFactor = float(m_relaxationFactor);
340 cone->m_damping = float(m_damping);
341
342 return "btConeTwistConstraintData";
343 }
344
345
346 #endif //BT_CONETWISTCONSTRAINT_H
347