1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
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
16 /* Hinge Constraint by Dirk Gregorius. Limits added by Marcus Hennix at Starbreeze Studios */
17
18 #ifndef BT_HINGECONSTRAINT_H
19 #define BT_HINGECONSTRAINT_H
20
21 #define _BT_USE_CENTER_LIMIT_ 1
22
23
24 #include "LinearMath/btVector3.h"
25 #include "btJacobianEntry.h"
26 #include "btTypedConstraint.h"
27
28 class btRigidBody;
29
30 #ifdef BT_USE_DOUBLE_PRECISION
31 #define btHingeConstraintData btHingeConstraintDoubleData2 //rename to 2 for backwards compatibility, so we can still load the 'btHingeConstraintDoubleData' version
32 #define btHingeConstraintDataName "btHingeConstraintDoubleData2"
33 #else
34 #define btHingeConstraintData btHingeConstraintFloatData
35 #define btHingeConstraintDataName "btHingeConstraintFloatData"
36 #endif //BT_USE_DOUBLE_PRECISION
37
38
39
40 enum btHingeFlags
41 {
42 BT_HINGE_FLAGS_CFM_STOP = 1,
43 BT_HINGE_FLAGS_ERP_STOP = 2,
44 BT_HINGE_FLAGS_CFM_NORM = 4,
45 BT_HINGE_FLAGS_ERP_NORM = 8
46 };
47
48
49 /// hinge constraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
50 /// axis defines the orientation of the hinge axis
ATTRIBUTE_ALIGNED16(class)51 ATTRIBUTE_ALIGNED16(class) btHingeConstraint : public btTypedConstraint
52 {
53 #ifdef IN_PARALLELL_SOLVER
54 public:
55 #endif
56 btJacobianEntry m_jac[3]; //3 orthogonal linear constraints
57 btJacobianEntry m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor
58
59 btTransform m_rbAFrame; // constraint axii. Assumes z is hinge axis.
60 btTransform m_rbBFrame;
61
62 btScalar m_motorTargetVelocity;
63 btScalar m_maxMotorImpulse;
64
65
66 #ifdef _BT_USE_CENTER_LIMIT_
67 btAngularLimit m_limit;
68 #else
69 btScalar m_lowerLimit;
70 btScalar m_upperLimit;
71 btScalar m_limitSign;
72 btScalar m_correction;
73
74 btScalar m_limitSoftness;
75 btScalar m_biasFactor;
76 btScalar m_relaxationFactor;
77
78 bool m_solveLimit;
79 #endif
80
81 btScalar m_kHinge;
82
83
84 btScalar m_accLimitImpulse;
85 btScalar m_hingeAngle;
86 btScalar m_referenceSign;
87
88 bool m_angularOnly;
89 bool m_enableAngularMotor;
90 bool m_useSolveConstraintObsolete;
91 bool m_useOffsetForConstraintFrame;
92 bool m_useReferenceFrameA;
93
94 btScalar m_accMotorImpulse;
95
96 int m_flags;
97 btScalar m_normalCFM;
98 btScalar m_normalERP;
99 btScalar m_stopCFM;
100 btScalar m_stopERP;
101
102
103 public:
104
105 BT_DECLARE_ALIGNED_ALLOCATOR();
106
107 btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA = false);
108
109 btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA = false);
110
111 btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false);
112
113 btHingeConstraint(btRigidBody& rbA,const btTransform& rbAFrame, bool useReferenceFrameA = false);
114
115
116 virtual void buildJacobian();
117
118 virtual void getInfo1 (btConstraintInfo1* info);
119
120 void getInfo1NonVirtual(btConstraintInfo1* info);
121
122 virtual void getInfo2 (btConstraintInfo2* info);
123
124 void getInfo2NonVirtual(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB);
125
126 void getInfo2Internal(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB);
127 void getInfo2InternalUsingFrameOffset(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB);
128
129
130 void updateRHS(btScalar timeStep);
131
132 const btRigidBody& getRigidBodyA() const
133 {
134 return m_rbA;
135 }
136 const btRigidBody& getRigidBodyB() const
137 {
138 return m_rbB;
139 }
140
141 btRigidBody& getRigidBodyA()
142 {
143 return m_rbA;
144 }
145
146 btRigidBody& getRigidBodyB()
147 {
148 return m_rbB;
149 }
150
151 btTransform& getFrameOffsetA()
152 {
153 return m_rbAFrame;
154 }
155
156 btTransform& getFrameOffsetB()
157 {
158 return m_rbBFrame;
159 }
160
161 void setFrames(const btTransform& frameA, const btTransform& frameB);
162
163 void setAngularOnly(bool angularOnly)
164 {
165 m_angularOnly = angularOnly;
166 }
167
168 void enableAngularMotor(bool enableMotor,btScalar targetVelocity,btScalar maxMotorImpulse)
169 {
170 m_enableAngularMotor = enableMotor;
171 m_motorTargetVelocity = targetVelocity;
172 m_maxMotorImpulse = maxMotorImpulse;
173 }
174
175 // extra motor API, including ability to set a target rotation (as opposed to angular velocity)
176 // note: setMotorTarget sets angular velocity under the hood, so you must call it every tick to
177 // maintain a given angular target.
178 void enableMotor(bool enableMotor) { m_enableAngularMotor = enableMotor; }
179 void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; }
180 void setMotorTarget(const btQuaternion& qAinB, btScalar dt); // qAinB is rotation of body A wrt body B.
181 void setMotorTarget(btScalar targetAngle, btScalar dt);
182
183
184 void setLimit(btScalar low,btScalar high,btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
185 {
186 #ifdef _BT_USE_CENTER_LIMIT_
187 m_limit.set(low, high, _softness, _biasFactor, _relaxationFactor);
188 #else
189 m_lowerLimit = btNormalizeAngle(low);
190 m_upperLimit = btNormalizeAngle(high);
191 m_limitSoftness = _softness;
192 m_biasFactor = _biasFactor;
193 m_relaxationFactor = _relaxationFactor;
194 #endif
195 }
196
197 void setAxis(btVector3& axisInA)
198 {
199 btVector3 rbAxisA1, rbAxisA2;
200 btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2);
201 btVector3 pivotInA = m_rbAFrame.getOrigin();
202 // m_rbAFrame.getOrigin() = pivotInA;
203 m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
204 rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
205 rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
206
207 btVector3 axisInB = m_rbA.getCenterOfMassTransform().getBasis() * axisInA;
208
209 btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
210 btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1);
211 btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
212
213 m_rbBFrame.getOrigin() = m_rbB.getCenterOfMassTransform().inverse()(m_rbA.getCenterOfMassTransform()(pivotInA));
214
215 m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
216 rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
217 rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
218 m_rbBFrame.getBasis() = m_rbB.getCenterOfMassTransform().getBasis().inverse() * m_rbBFrame.getBasis();
219
220 }
221
222 bool hasLimit() const {
223 #ifdef _BT_USE_CENTER_LIMIT_
224 return m_limit.getHalfRange() > 0;
225 #else
226 return m_lowerLimit <= m_upperLimit;
227 #endif
228 }
229
230 btScalar getLowerLimit() const
231 {
232 #ifdef _BT_USE_CENTER_LIMIT_
233 return m_limit.getLow();
234 #else
235 return m_lowerLimit;
236 #endif
237 }
238
239 btScalar getUpperLimit() const
240 {
241 #ifdef _BT_USE_CENTER_LIMIT_
242 return m_limit.getHigh();
243 #else
244 return m_upperLimit;
245 #endif
246 }
247
248
249 ///The getHingeAngle gives the hinge angle in range [-PI,PI]
250 btScalar getHingeAngle();
251
252 btScalar getHingeAngle(const btTransform& transA,const btTransform& transB);
253
254 void testLimit(const btTransform& transA,const btTransform& transB);
255
256
257 const btTransform& getAFrame() const { return m_rbAFrame; };
258 const btTransform& getBFrame() const { return m_rbBFrame; };
259
260 btTransform& getAFrame() { return m_rbAFrame; };
261 btTransform& getBFrame() { return m_rbBFrame; };
262
263 inline int getSolveLimit()
264 {
265 #ifdef _BT_USE_CENTER_LIMIT_
266 return m_limit.isLimit();
267 #else
268 return m_solveLimit;
269 #endif
270 }
271
272 inline btScalar getLimitSign()
273 {
274 #ifdef _BT_USE_CENTER_LIMIT_
275 return m_limit.getSign();
276 #else
277 return m_limitSign;
278 #endif
279 }
280
281 inline bool getAngularOnly()
282 {
283 return m_angularOnly;
284 }
285 inline bool getEnableAngularMotor()
286 {
287 return m_enableAngularMotor;
288 }
289 inline btScalar getMotorTargetVelosity()
290 {
291 return m_motorTargetVelocity;
292 }
293 inline btScalar getMaxMotorImpulse()
294 {
295 return m_maxMotorImpulse;
296 }
297 // access for UseFrameOffset
298 bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
299 void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
300
301
302 ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
303 ///If no axis is provided, it uses the default axis for this constraint.
304 virtual void setParam(int num, btScalar value, int axis = -1);
305 ///return the local value of parameter
306 virtual btScalar getParam(int num, int axis = -1) const;
307
308 virtual int calculateSerializeBufferSize() const;
309
310 ///fills the dataBuffer and returns the struct name (and 0 on failure)
311 virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
312
313
314 };
315
316
317 //only for backward compatibility
318 #ifdef BT_BACKWARDS_COMPATIBLE_SERIALIZATION
319 ///this structure is not used, except for loading pre-2.82 .bullet files
320 struct btHingeConstraintDoubleData
321 {
322 btTypedConstraintData m_typeConstraintData;
323 btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
324 btTransformDoubleData m_rbBFrame;
325 int m_useReferenceFrameA;
326 int m_angularOnly;
327 int m_enableAngularMotor;
328 float m_motorTargetVelocity;
329 float m_maxMotorImpulse;
330
331 float m_lowerLimit;
332 float m_upperLimit;
333 float m_limitSoftness;
334 float m_biasFactor;
335 float m_relaxationFactor;
336
337 };
338 #endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION
339
340 ///The getAccumulatedHingeAngle returns the accumulated hinge angle, taking rotation across the -PI/PI boundary into account
ATTRIBUTE_ALIGNED16(class)341 ATTRIBUTE_ALIGNED16(class) btHingeAccumulatedAngleConstraint : public btHingeConstraint
342 {
343 protected:
344 btScalar m_accumulatedAngle;
345 public:
346
347 BT_DECLARE_ALIGNED_ALLOCATOR();
348
349 btHingeAccumulatedAngleConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA = false)
350 :btHingeConstraint(rbA,rbB,pivotInA,pivotInB, axisInA,axisInB, useReferenceFrameA )
351 {
352 m_accumulatedAngle=getHingeAngle();
353 }
354
355 btHingeAccumulatedAngleConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA = false)
356 :btHingeConstraint(rbA,pivotInA,axisInA, useReferenceFrameA)
357 {
358 m_accumulatedAngle=getHingeAngle();
359 }
360
361 btHingeAccumulatedAngleConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false)
362 :btHingeConstraint(rbA,rbB, rbAFrame, rbBFrame, useReferenceFrameA )
363 {
364 m_accumulatedAngle=getHingeAngle();
365 }
366
367 btHingeAccumulatedAngleConstraint(btRigidBody& rbA,const btTransform& rbAFrame, bool useReferenceFrameA = false)
368 :btHingeConstraint(rbA,rbAFrame, useReferenceFrameA )
369 {
370 m_accumulatedAngle=getHingeAngle();
371 }
372 btScalar getAccumulatedHingeAngle();
373 void setAccumulatedHingeAngle(btScalar accAngle);
374 virtual void getInfo1 (btConstraintInfo1* info);
375
376 };
377
378 struct btHingeConstraintFloatData
379 {
380 btTypedConstraintData m_typeConstraintData;
381 btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
382 btTransformFloatData m_rbBFrame;
383 int m_useReferenceFrameA;
384 int m_angularOnly;
385
386 int m_enableAngularMotor;
387 float m_motorTargetVelocity;
388 float m_maxMotorImpulse;
389
390 float m_lowerLimit;
391 float m_upperLimit;
392 float m_limitSoftness;
393 float m_biasFactor;
394 float m_relaxationFactor;
395
396 };
397
398
399
400 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
401 struct btHingeConstraintDoubleData2
402 {
403 btTypedConstraintDoubleData m_typeConstraintData;
404 btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
405 btTransformDoubleData m_rbBFrame;
406 int m_useReferenceFrameA;
407 int m_angularOnly;
408 int m_enableAngularMotor;
409 double m_motorTargetVelocity;
410 double m_maxMotorImpulse;
411
412 double m_lowerLimit;
413 double m_upperLimit;
414 double m_limitSoftness;
415 double m_biasFactor;
416 double m_relaxationFactor;
417 char m_padding1[4];
418
419 };
420
421
422
423
calculateSerializeBufferSize()424 SIMD_FORCE_INLINE int btHingeConstraint::calculateSerializeBufferSize() const
425 {
426 return sizeof(btHingeConstraintData);
427 }
428
429 ///fills the dataBuffer and returns the struct name (and 0 on failure)
serialize(void * dataBuffer,btSerializer * serializer)430 SIMD_FORCE_INLINE const char* btHingeConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
431 {
432 btHingeConstraintData* hingeData = (btHingeConstraintData*)dataBuffer;
433 btTypedConstraint::serialize(&hingeData->m_typeConstraintData,serializer);
434
435 m_rbAFrame.serialize(hingeData->m_rbAFrame);
436 m_rbBFrame.serialize(hingeData->m_rbBFrame);
437
438 hingeData->m_angularOnly = m_angularOnly;
439 hingeData->m_enableAngularMotor = m_enableAngularMotor;
440 hingeData->m_maxMotorImpulse = float(m_maxMotorImpulse);
441 hingeData->m_motorTargetVelocity = float(m_motorTargetVelocity);
442 hingeData->m_useReferenceFrameA = m_useReferenceFrameA;
443 #ifdef _BT_USE_CENTER_LIMIT_
444 hingeData->m_lowerLimit = float(m_limit.getLow());
445 hingeData->m_upperLimit = float(m_limit.getHigh());
446 hingeData->m_limitSoftness = float(m_limit.getSoftness());
447 hingeData->m_biasFactor = float(m_limit.getBiasFactor());
448 hingeData->m_relaxationFactor = float(m_limit.getRelaxationFactor());
449 #else
450 hingeData->m_lowerLimit = float(m_lowerLimit);
451 hingeData->m_upperLimit = float(m_upperLimit);
452 hingeData->m_limitSoftness = float(m_limitSoftness);
453 hingeData->m_biasFactor = float(m_biasFactor);
454 hingeData->m_relaxationFactor = float(m_relaxationFactor);
455 #endif
456
457 return btHingeConstraintDataName;
458 }
459
460 #endif //BT_HINGECONSTRAINT_H
461