Bullet Collision Detection & Physics Library
btGeneric6DofConstraint.h
Go to the documentation of this file.
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 
18 
19 /*
20 2007-09-09
21 btGeneric6DofConstraint Refactored by Francisco Le?n
22 email: projectileman@yahoo.com
23 http://gimpact.sf.net
24 */
25 
26 
27 #ifndef BT_GENERIC_6DOF_CONSTRAINT_H
28 #define BT_GENERIC_6DOF_CONSTRAINT_H
29 
30 #include "LinearMath/btVector3.h"
31 #include "btJacobianEntry.h"
32 #include "btTypedConstraint.h"
33 
34 class btRigidBody;
35 
36 
37 
38 #ifdef BT_USE_DOUBLE_PRECISION
39 #define btGeneric6DofConstraintData2 btGeneric6DofConstraintDoubleData2
40 #define btGeneric6DofConstraintDataName "btGeneric6DofConstraintDoubleData2"
41 #else
42 #define btGeneric6DofConstraintData2 btGeneric6DofConstraintData
43 #define btGeneric6DofConstraintDataName "btGeneric6DofConstraintData"
44 #endif //BT_USE_DOUBLE_PRECISION
45 
46 
48 class btRotationalLimitMotor
49 {
50 public:
53  btScalar m_loLimit;
54  btScalar m_hiLimit;
55  btScalar m_targetVelocity;
56  btScalar m_maxMotorForce;
57  btScalar m_maxLimitForce;
58  btScalar m_damping;
59  btScalar m_limitSoftness;
60  btScalar m_normalCFM;
61  btScalar m_stopERP;
62  btScalar m_stopCFM;
63  btScalar m_bounce;
64  bool m_enableMotor;
65 
67 
70  btScalar m_currentLimitError;
71  btScalar m_currentPosition;
72  int m_currentLimit;
73  btScalar m_accumulatedImpulse;
75 
76  btRotationalLimitMotor()
77  {
78  m_accumulatedImpulse = 0.f;
79  m_targetVelocity = 0;
80  m_maxMotorForce = 0.1f;
81  m_maxLimitForce = 300.0f;
82  m_loLimit = 1.0f;
83  m_hiLimit = -1.0f;
84  m_normalCFM = 0.f;
85  m_stopERP = 0.2f;
86  m_stopCFM = 0.f;
87  m_bounce = 0.0f;
88  m_damping = 1.0f;
89  m_limitSoftness = 0.5f;
90  m_currentLimit = 0;
91  m_currentLimitError = 0;
92  m_enableMotor = false;
93  }
94 
95  btRotationalLimitMotor(const btRotationalLimitMotor & limot)
96  {
97  m_targetVelocity = limot.m_targetVelocity;
98  m_maxMotorForce = limot.m_maxMotorForce;
99  m_limitSoftness = limot.m_limitSoftness;
100  m_loLimit = limot.m_loLimit;
101  m_hiLimit = limot.m_hiLimit;
102  m_normalCFM = limot.m_normalCFM;
103  m_stopERP = limot.m_stopERP;
104  m_stopCFM = limot.m_stopCFM;
105  m_bounce = limot.m_bounce;
106  m_currentLimit = limot.m_currentLimit;
107  m_currentLimitError = limot.m_currentLimitError;
108  m_enableMotor = limot.m_enableMotor;
109  }
110 
111 
112 
114  bool isLimited()
115  {
116  if(m_loLimit > m_hiLimit) return false;
117  return true;
118  }
119 
121  bool needApplyTorques()
122  {
123  if(m_currentLimit == 0 && m_enableMotor == false) return false;
124  return true;
125  }
126 
128 
131  int testLimitValue(btScalar test_value);
132 
134  btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btRigidBody * body1);
135 
136 };
137 
138 
139 
140 class btTranslationalLimitMotor
141 {
142 public:
143  btVector3 m_lowerLimit;
144  btVector3 m_upperLimit;
145  btVector3 m_accumulatedImpulse;
148  btScalar m_limitSoftness;
149  btScalar m_damping;
150  btScalar m_restitution;
151  btVector3 m_normalCFM;
152  btVector3 m_stopERP;
153  btVector3 m_stopCFM;
154 
155  bool m_enableMotor[3];
156  btVector3 m_targetVelocity;
157  btVector3 m_maxMotorForce;
158  btVector3 m_currentLimitError;
159  btVector3 m_currentLinearDiff;
160  int m_currentLimit[3];
161 
162  btTranslationalLimitMotor()
163  {
164  m_lowerLimit.setValue(0.f,0.f,0.f);
165  m_upperLimit.setValue(0.f,0.f,0.f);
166  m_accumulatedImpulse.setValue(0.f,0.f,0.f);
167  m_normalCFM.setValue(0.f, 0.f, 0.f);
168  m_stopERP.setValue(0.2f, 0.2f, 0.2f);
169  m_stopCFM.setValue(0.f, 0.f, 0.f);
170 
171  m_limitSoftness = 0.7f;
172  m_damping = btScalar(1.0f);
173  m_restitution = btScalar(0.5f);
174  for(int i=0; i < 3; i++)
175  {
176  m_enableMotor[i] = false;
177  m_targetVelocity[i] = btScalar(0.f);
178  m_maxMotorForce[i] = btScalar(0.f);
179  }
180  }
181 
182  btTranslationalLimitMotor(const btTranslationalLimitMotor & other )
183  {
184  m_lowerLimit = other.m_lowerLimit;
185  m_upperLimit = other.m_upperLimit;
186  m_accumulatedImpulse = other.m_accumulatedImpulse;
187 
188  m_limitSoftness = other.m_limitSoftness ;
189  m_damping = other.m_damping;
190  m_restitution = other.m_restitution;
191  m_normalCFM = other.m_normalCFM;
192  m_stopERP = other.m_stopERP;
193  m_stopCFM = other.m_stopCFM;
194 
195  for(int i=0; i < 3; i++)
196  {
197  m_enableMotor[i] = other.m_enableMotor[i];
198  m_targetVelocity[i] = other.m_targetVelocity[i];
199  m_maxMotorForce[i] = other.m_maxMotorForce[i];
200  }
201  }
202 
204 
210  inline bool isLimited(int limitIndex)
211  {
212  return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
213  }
214  inline bool needApplyForce(int limitIndex)
215  {
216  if(m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false;
217  return true;
218  }
219  int testLimitValue(int limitIndex, btScalar test_value);
220 
221 
222  btScalar solveLinearAxis(
223  btScalar timeStep,
224  btScalar jacDiagABInv,
225  btRigidBody& body1,const btVector3 &pointInA,
226  btRigidBody& body2,const btVector3 &pointInB,
227  int limit_index,
228  const btVector3 & axis_normal_on_a,
229  const btVector3 & anchorPos);
230 
231 
232 };
233 
234 enum bt6DofFlags
235 {
236  BT_6DOF_FLAGS_CFM_NORM = 1,
237  BT_6DOF_FLAGS_CFM_STOP = 2,
238  BT_6DOF_FLAGS_ERP_STOP = 4
239 };
240 #define BT_6DOF_FLAGS_AXIS_SHIFT 3 // bits per axis
241 
242 
244 
279 ATTRIBUTE_ALIGNED16(class) btGeneric6DofConstraint : public btTypedConstraint
280 {
281 protected:
282 
285  btTransform m_frameInA;
286  btTransform m_frameInB;
287 
288 
291  btJacobianEntry m_jacLinear[3];
292  btJacobianEntry m_jacAng[3];
293 
294 
297  btTranslationalLimitMotor m_linearLimits;
299 
300 
303  btRotationalLimitMotor m_angularLimits[3];
305 
306 
307 protected:
310  btScalar m_timeStep;
311  btTransform m_calculatedTransformA;
312  btTransform m_calculatedTransformB;
313  btVector3 m_calculatedAxisAngleDiff;
314  btVector3 m_calculatedAxis[3];
315  btVector3 m_calculatedLinearDiff;
316  btScalar m_factA;
317  btScalar m_factB;
318  bool m_hasStaticBody;
319 
320  btVector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes
321 
322  bool m_useLinearReferenceFrameA;
323  bool m_useOffsetForConstraintFrame;
324 
325  int m_flags;
326 
328 
329  btGeneric6DofConstraint& operator=(btGeneric6DofConstraint& other)
330  {
331  btAssert(0);
332  (void) other;
333  return *this;
334  }
335 
336 
337  int setAngularLimits(btConstraintInfo2 *info, int row_offset,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
338 
339  int setLinearLimits(btConstraintInfo2 *info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
340 
341  void buildLinearJacobian(
342  btJacobianEntry & jacLinear,const btVector3 & normalWorld,
343  const btVector3 & pivotAInW,const btVector3 & pivotBInW);
344 
345  void buildAngularJacobian(btJacobianEntry & jacAngular,const btVector3 & jointAxisW);
346 
347  // tests linear limits
348  void calculateLinearInfo();
349 
351  void calculateAngleInfo();
352 
353 
354 
355 public:
356 
357  BT_DECLARE_ALIGNED_ALLOCATOR();
358 
360  bool m_useSolveConstraintObsolete;
361 
362  btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
363  btGeneric6DofConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB);
364 
366 
370  void calculateTransforms(const btTransform& transA,const btTransform& transB);
371 
372  void calculateTransforms();
373 
375 
378  const btTransform & getCalculatedTransformA() const
379  {
380  return m_calculatedTransformA;
381  }
382 
384 
387  const btTransform & getCalculatedTransformB() const
388  {
389  return m_calculatedTransformB;
390  }
391 
392  const btTransform & getFrameOffsetA() const
393  {
394  return m_frameInA;
395  }
396 
397  const btTransform & getFrameOffsetB() const
398  {
399  return m_frameInB;
400  }
401 
402 
403  btTransform & getFrameOffsetA()
404  {
405  return m_frameInA;
406  }
407 
408  btTransform & getFrameOffsetB()
409  {
410  return m_frameInB;
411  }
412 
413 
415  virtual void buildJacobian();
416 
417  virtual void getInfo1 (btConstraintInfo1* info);
418 
419  void getInfo1NonVirtual (btConstraintInfo1* info);
420 
421  virtual void getInfo2 (btConstraintInfo2* info);
422 
423  void getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
424 
425 
426  void updateRHS(btScalar timeStep);
427 
429 
432  btVector3 getAxis(int axis_index) const;
433 
435 
438  btScalar getAngle(int axis_index) const;
439 
441 
444  btScalar getRelativePivotPosition(int axis_index) const;
445 
446  void setFrames(const btTransform & frameA, const btTransform & frameB);
447 
449 
453  bool testAngularLimitMotor(int axis_index);
454 
455  void setLinearLowerLimit(const btVector3& linearLower)
456  {
457  m_linearLimits.m_lowerLimit = linearLower;
458  }
459 
460  void getLinearLowerLimit(btVector3& linearLower)
461  {
462  linearLower = m_linearLimits.m_lowerLimit;
463  }
464 
465  void setLinearUpperLimit(const btVector3& linearUpper)
466  {
467  m_linearLimits.m_upperLimit = linearUpper;
468  }
469 
470  void getLinearUpperLimit(btVector3& linearUpper)
471  {
472  linearUpper = m_linearLimits.m_upperLimit;
473  }
474 
475  void setAngularLowerLimit(const btVector3& angularLower)
476  {
477  for(int i = 0; i < 3; i++)
478  m_angularLimits[i].m_loLimit = btNormalizeAngle(angularLower[i]);
479  }
480 
481  void getAngularLowerLimit(btVector3& angularLower)
482  {
483  for(int i = 0; i < 3; i++)
484  angularLower[i] = m_angularLimits[i].m_loLimit;
485  }
486 
487  void setAngularUpperLimit(const btVector3& angularUpper)
488  {
489  for(int i = 0; i < 3; i++)
490  m_angularLimits[i].m_hiLimit = btNormalizeAngle(angularUpper[i]);
491  }
492 
493  void getAngularUpperLimit(btVector3& angularUpper)
494  {
495  for(int i = 0; i < 3; i++)
496  angularUpper[i] = m_angularLimits[i].m_hiLimit;
497  }
498 
500  btRotationalLimitMotor * getRotationalLimitMotor(int index)
501  {
502  return &m_angularLimits[index];
503  }
504 
506  btTranslationalLimitMotor * getTranslationalLimitMotor()
507  {
508  return &m_linearLimits;
509  }
510 
511  //first 3 are linear, next 3 are angular
512  void setLimit(int axis, btScalar lo, btScalar hi)
513  {
514  if(axis<3)
515  {
516  m_linearLimits.m_lowerLimit[axis] = lo;
517  m_linearLimits.m_upperLimit[axis] = hi;
518  }
519  else
520  {
521  lo = btNormalizeAngle(lo);
522  hi = btNormalizeAngle(hi);
523  m_angularLimits[axis-3].m_loLimit = lo;
524  m_angularLimits[axis-3].m_hiLimit = hi;
525  }
526  }
527 
529 
535  bool isLimited(int limitIndex)
536  {
537  if(limitIndex<3)
538  {
539  return m_linearLimits.isLimited(limitIndex);
540 
541  }
542  return m_angularLimits[limitIndex-3].isLimited();
543  }
544 
545  virtual void calcAnchorPos(void); // overridable
546 
547  int get_limit_motor_info2( btRotationalLimitMotor * limot,
548  const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
549  btConstraintInfo2 *info, int row, btVector3& ax1, int rotational, int rotAllowed = false);
550 
551  // access for UseFrameOffset
552  bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
553  void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
554 
557  virtual void setParam(int num, btScalar value, int axis = -1);
559  virtual btScalar getParam(int num, int axis = -1) const;
560 
561  void setAxis( const btVector3& axis1, const btVector3& axis2);
562 
563 
564  virtual int calculateSerializeBufferSize() const;
565 
567  virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
568 
569 
570 };
571 
572 
573 struct btGeneric6DofConstraintData
574 {
575  btTypedConstraintData m_typeConstraintData;
576  btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
577  btTransformFloatData m_rbBFrame;
578 
579  btVector3FloatData m_linearUpperLimit;
580  btVector3FloatData m_linearLowerLimit;
581 
582  btVector3FloatData m_angularUpperLimit;
583  btVector3FloatData m_angularLowerLimit;
584 
585  int m_useLinearReferenceFrameA;
586  int m_useOffsetForConstraintFrame;
587 };
588 
589 struct btGeneric6DofConstraintDoubleData2
590 {
591  btTypedConstraintDoubleData m_typeConstraintData;
592  btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
593  btTransformDoubleData m_rbBFrame;
594 
595  btVector3DoubleData m_linearUpperLimit;
596  btVector3DoubleData m_linearLowerLimit;
597 
598  btVector3DoubleData m_angularUpperLimit;
599  btVector3DoubleData m_angularLowerLimit;
600 
601  int m_useLinearReferenceFrameA;
602  int m_useOffsetForConstraintFrame;
603 };
604 
605 SIMD_FORCE_INLINE int btGeneric6DofConstraint::calculateSerializeBufferSize() const
606 {
607  return sizeof(btGeneric6DofConstraintData2);
608 }
609 
611 SIMD_FORCE_INLINE const char* btGeneric6DofConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
612 {
613 
614  btGeneric6DofConstraintData2* dof = (btGeneric6DofConstraintData2*)dataBuffer;
615  btTypedConstraint::serialize(&dof->m_typeConstraintData,serializer);
616 
617  m_frameInA.serialize(dof->m_rbAFrame);
618  m_frameInB.serialize(dof->m_rbBFrame);
619 
620 
621  int i;
622  for (i=0;i<3;i++)
623  {
624  dof->m_angularLowerLimit.m_floats[i] = m_angularLimits[i].m_loLimit;
625  dof->m_angularUpperLimit.m_floats[i] = m_angularLimits[i].m_hiLimit;
626  dof->m_linearLowerLimit.m_floats[i] = m_linearLimits.m_lowerLimit[i];
627  dof->m_linearUpperLimit.m_floats[i] = m_linearLimits.m_upperLimit[i];
628  }
629 
630  dof->m_useLinearReferenceFrameA = m_useLinearReferenceFrameA? 1 : 0;
631  dof->m_useOffsetForConstraintFrame = m_useOffsetForConstraintFrame ? 1 : 0;
632 
633  return btGeneric6DofConstraintDataName;
634 }
635 
636 
637 
638 
639 
640 #endif //BT_GENERIC_6DOF_CONSTRAINT_H
Generated on Wed Apr 9 2014 02:38:23 for Bullet Collision Detection & Physics Library by   doxygen 1.8.1.2