src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h

   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 /// 2009 March: btGeneric6DofConstraint refactored by Roman Ponomarev
  17 /// Added support for generic constraint solver through getInfo1/getInfo2 methods
  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
  39 //! Rotation Limit structure for generic joints
  40 class btRotationalLimitMotor
  41 {
  42 public:
  43     //! limit_parameters
  44     //!@{
  45     btScalar m_loLimit;//!< joint limit
  46     btScalar m_hiLimit;//!< joint limit
  47     btScalar m_targetVelocity;//!< target motor velocity
  48     btScalar m_maxMotorForce;//!< max force on motor
  49     btScalar m_maxLimitForce;//!< max force on limit
  50     btScalar m_damping;//!< Damping.
  51     btScalar m_limitSoftness;//! Relaxation factor
  52     btScalar m_normalCFM;//!< Constraint force mixing factor
  53     btScalar m_stopERP;//!< Error tolerance factor when joint is at limit
  54     btScalar m_stopCFM;//!< Constraint force mixing factor when joint is at limit
  55     btScalar m_bounce;//!< restitution factor
  56     bool m_enableMotor;
  57
  58     //!@}
  59
  60     //! temp_variables
  61     //!@{
  62     btScalar m_currentLimitError;//!  How much is violated this limit
  63     btScalar m_currentPosition;     //!  current value of angle
  64     int m_currentLimit;//!< 0=free, 1=at lo limit, 2=at hi limit
  65     btScalar m_accumulatedImpulse;
  66     //!@}
  67
  68     btRotationalLimitMotor()
  69     {
  70         m_accumulatedImpulse = 0.f;
  71         m_targetVelocity = 0;
  72         m_maxMotorForce = 0.1f;
  73         m_maxLimitForce = 300.0f;
  74         m_loLimit = 1.0f;
  75         m_hiLimit = -1.0f;
  76                 m_normalCFM = 0.f;
  77                 m_stopERP = 0.2f;
  78                 m_stopCFM = 0.f;
  79         m_bounce = 0.0f;
  80         m_damping = 1.0f;
  81         m_limitSoftness = 0.5f;
  82         m_currentLimit = 0;
  83         m_currentLimitError = 0;
  84         m_enableMotor = false;
  85     }
  86
  87     btRotationalLimitMotor(const btRotationalLimitMotor & limot)
  88     {
  89         m_targetVelocity = limot.m_targetVelocity;
  90         m_maxMotorForce = limot.m_maxMotorForce;
  91         m_limitSoftness = limot.m_limitSoftness;
  92         m_loLimit = limot.m_loLimit;
  93         m_hiLimit = limot.m_hiLimit;
  94                 m_normalCFM = limot.m_normalCFM;
  95                 m_stopERP = limot.m_stopERP;
  96                 m_stopCFM =     limot.m_stopCFM;
  97         m_bounce = limot.m_bounce;
  98         m_currentLimit = limot.m_currentLimit;
  99         m_currentLimitError = limot.m_currentLimitError;
 100         m_enableMotor = limot.m_enableMotor;
 101     }
 102
 103
 104
 105         //! Is limited
 106     bool isLimited()
 107     {
 108         if(m_loLimit > m_hiLimit) return false;
 109         return true;
 110     }
 111
 112         //! Need apply correction
 113     bool needApplyTorques()
 114     {
 115         if(m_currentLimit == 0 && m_enableMotor == false) return false;
 116         return true;
 117     }
 118
 119         //! calculates  error
 120         /*!
 121         calculates m_currentLimit and m_currentLimitError.
 122         */
 123         int testLimitValue(btScalar test_value);
 124
 125         //! apply the correction impulses for two bodies
 126     btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btRigidBody * body1);
 127
 128 };
 129
 130
 131
 132 class btTranslationalLimitMotor
 133 {
 134 public:
 135         btVector3 m_lowerLimit;//!< the constraint lower limits
 136     btVector3 m_upperLimit;//!< the constraint upper limits
 137     btVector3 m_accumulatedImpulse;
 138     //! Linear_Limit_parameters
 139     //!@{
 140     btScalar    m_limitSoftness;//!< Softness for linear limit
 141     btScalar    m_damping;//!< Damping for linear limit
 142     btScalar    m_restitution;//! Bounce parameter for linear limit
 143         btVector3       m_normalCFM;//!< Constraint force mixing factor
 144     btVector3   m_stopERP;//!< Error tolerance factor when joint is at limit
 145         btVector3       m_stopCFM;//!< Constraint force mixing factor when joint is at limit
 146     //!@}
 147         bool            m_enableMotor[3];
 148     btVector3   m_targetVelocity;//!< target motor velocity
 149     btVector3   m_maxMotorForce;//!< max force on motor
 150     btVector3   m_currentLimitError;//!  How much is violated this limit
 151     btVector3   m_currentLinearDiff;//!  Current relative offset of constraint frames
 152     int                 m_currentLimit[3];//!< 0=free, 1=at lower limit, 2=at upper limit
 153
 154     btTranslationalLimitMotor()
 155     {
 156         m_lowerLimit.setValue(0.f,0.f,0.f);
 157         m_upperLimit.setValue(0.f,0.f,0.f);
 158         m_accumulatedImpulse.setValue(0.f,0.f,0.f);
 159                 m_normalCFM.setValue(0.f, 0.f, 0.f);
 160                 m_stopERP.setValue(0.2f, 0.2f, 0.2f);
 161                 m_stopCFM.setValue(0.f, 0.f, 0.f);
 162
 163         m_limitSoftness = 0.7f;
 164         m_damping = btScalar(1.0f);
 165         m_restitution = btScalar(0.5f);
 166                 for(int i=0; i < 3; i++)
 167                 {
 168                         m_enableMotor[i] = false;
 169                         m_targetVelocity[i] = btScalar(0.f);
 170                         m_maxMotorForce[i] = btScalar(0.f);
 171                 }
 172     }
 173
 174     btTranslationalLimitMotor(const btTranslationalLimitMotor & other )
 175     {
 176         m_lowerLimit = other.m_lowerLimit;
 177         m_upperLimit = other.m_upperLimit;
 178         m_accumulatedImpulse = other.m_accumulatedImpulse;
 179
 180         m_limitSoftness = other.m_limitSoftness ;
 181         m_damping = other.m_damping;
 182         m_restitution = other.m_restitution;
 183                 m_normalCFM = other.m_normalCFM;
 184                 m_stopERP = other.m_stopERP;
 185                 m_stopCFM = other.m_stopCFM;
 186
 187                 for(int i=0; i < 3; i++)
 188                 {
 189                         m_enableMotor[i] = other.m_enableMotor[i];
 190                         m_targetVelocity[i] = other.m_targetVelocity[i];
 191                         m_maxMotorForce[i] = other.m_maxMotorForce[i];
 192                 }
 193     }
 194
 195     //! Test limit
 196         /*!
 197     - free means upper < lower,
 198     - locked means upper == lower
 199     - limited means upper > lower
 200     - limitIndex: first 3 are linear, next 3 are angular
 201     */
 202     inline bool isLimited(int limitIndex)
 203     {
 204        return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
 205     }
 206     inline bool needApplyForce(int limitIndex)
 207     {
 208         if(m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false;
 209         return true;
 210     }
 211         int testLimitValue(int limitIndex, btScalar test_value);
 212
 213
 214     btScalar solveLinearAxis(
 215         btScalar timeStep,
 216         btScalar jacDiagABInv,
 217         btRigidBody& body1,const btVector3 &pointInA,
 218         btRigidBody& body2,const btVector3 &pointInB,
 219         int limit_index,
 220         const btVector3 & axis_normal_on_a,
 221                 const btVector3 & anchorPos);
 222
 223
 224 };
 225
 226 enum bt6DofFlags
 227 {
 228         BT_6DOF_FLAGS_CFM_NORM = 1,
 229         BT_6DOF_FLAGS_CFM_STOP = 2,
 230         BT_6DOF_FLAGS_ERP_STOP = 4
 231 };
 232 #define BT_6DOF_FLAGS_AXIS_SHIFT 3 // bits per axis
 233
 234
 235 /// btGeneric6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
 236 /*!
 237 btGeneric6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked'.
 238 currently this limit supports rotational motors<br>
 239 <ul>
 240 <li> For Linear limits, use btGeneric6DofConstraint.setLinearUpperLimit, btGeneric6DofConstraint.setLinearLowerLimit. You can set the parameters with the btTranslationalLimitMotor structure accsesible through the btGeneric6DofConstraint.getTranslationalLimitMotor method.
 241 At this moment translational motors are not supported. May be in the future. </li>
 242
 243 <li> For Angular limits, use the btRotationalLimitMotor structure for configuring the limit.
 244 This is accessible through btGeneric6DofConstraint.getLimitMotor method,
 245 This brings support for limit parameters and motors. </li>
 246
 247 <li> Angulars limits have these possible ranges:
 248 <table border=1 >
 249 <tr>
 250         <td><b>AXIS</b></td>
 251         <td><b>MIN ANGLE</b></td>
 252         <td><b>MAX ANGLE</b></td>
 253 </tr><tr>
 254         <td>X</td>
 255         <td>-PI</td>
 256         <td>PI</td>
 257 </tr><tr>
 258         <td>Y</td>
 259         <td>-PI/2</td>
 260         <td>PI/2</td>
 261 </tr><tr>
 262         <td>Z</td>
 263         <td>-PI</td>
 264         <td>PI</td>
 265 </tr>
 266 </table>
 267 </li>
 268 </ul>
 269
 270 */
 271 ATTRIBUTE_ALIGNED16(class) btGeneric6DofConstraint : public btTypedConstraint
 272 {
 273 protected:
 274
 275         //! relative_frames
 276     //!@{
 277         btTransform     m_frameInA;//!< the constraint space w.r.t body A
 278     btTransform m_frameInB;//!< the constraint space w.r.t body B
 279     //!@}
 280
 281     //! Jacobians
 282     //!@{
 283     btJacobianEntry     m_jacLinear[3];//!< 3 orthogonal linear constraints
 284     btJacobianEntry     m_jacAng[3];//!< 3 orthogonal angular constraints
 285     //!@}
 286
 287         //! Linear_Limit_parameters
 288     //!@{
 289     btTranslationalLimitMotor m_linearLimits;
 290     //!@}
 291
 292
 293     //! hinge_parameters
 294     //!@{
 295     btRotationalLimitMotor m_angularLimits[3];
 296         //!@}
 297
 298
 299 protected:
 300     //! temporal variables
 301     //!@{
 302     btScalar m_timeStep;
 303     btTransform m_calculatedTransformA;
 304     btTransform m_calculatedTransformB;
 305     btVector3 m_calculatedAxisAngleDiff;
 306     btVector3 m_calculatedAxis[3];
 307     btVector3 m_calculatedLinearDiff;
 308         btScalar        m_factA;
 309         btScalar        m_factB;
 310         bool            m_hasStaticBody;
 311
 312         btVector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes
 313
 314     bool        m_useLinearReferenceFrameA;
 315         bool    m_useOffsetForConstraintFrame;
 316
 317         int             m_flags;
 318
 319     //!@}
 320
 321     btGeneric6DofConstraint&    operator=(btGeneric6DofConstraint&      other)
 322     {
 323         btAssert(0);
 324         (void) other;
 325         return *this;
 326     }
 327
 328
 329         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);
 330
 331         int setLinearLimits(btConstraintInfo2 *info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
 332
 333     void buildLinearJacobian(
 334         btJacobianEntry & jacLinear,const btVector3 & normalWorld,
 335         const btVector3 & pivotAInW,const btVector3 & pivotBInW);
 336
 337     void buildAngularJacobian(btJacobianEntry & jacAngular,const btVector3 & jointAxisW);
 338
 339         // tests linear limits
 340         void calculateLinearInfo();
 341
 342         //! calcs the euler angles between the two bodies.
 343     void calculateAngleInfo();
 344
 345
 346
 347 public:
 348
 349         BT_DECLARE_ALIGNED_ALLOCATOR();
 350
 351         ///for backwards compatibility during the transition to 'getInfo/getInfo2'
 352         bool            m_useSolveConstraintObsolete;
 353
 354     btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
 355     btGeneric6DofConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB);
 356
 357         //! Calcs global transform of the offsets
 358         /*!
 359         Calcs the global transform for the joint offset for body A an B, and also calcs the agle differences between the bodies.
 360         \sa btGeneric6DofConstraint.getCalculatedTransformA , btGeneric6DofConstraint.getCalculatedTransformB, btGeneric6DofConstraint.calculateAngleInfo
 361         */
 362     void calculateTransforms(const btTransform& transA,const btTransform& transB);
 363
 364         void calculateTransforms();
 365
 366         //! Gets the global transform of the offset for body A
 367     /*!
 368     \sa btGeneric6DofConstraint.getFrameOffsetA, btGeneric6DofConstraint.getFrameOffsetB, btGeneric6DofConstraint.calculateAngleInfo.
 369     */
 370     const btTransform & getCalculatedTransformA() const
 371     {
 372         return m_calculatedTransformA;
 373     }
 374
 375     //! Gets the global transform of the offset for body B
 376     /*!
 377     \sa btGeneric6DofConstraint.getFrameOffsetA, btGeneric6DofConstraint.getFrameOffsetB, btGeneric6DofConstraint.calculateAngleInfo.
 378     */
 379     const btTransform & getCalculatedTransformB() const
 380     {
 381         return m_calculatedTransformB;
 382     }
 383
 384     const btTransform & getFrameOffsetA() const
 385     {
 386         return m_frameInA;
 387     }
 388
 389     const btTransform & getFrameOffsetB() const
 390     {
 391         return m_frameInB;
 392     }
 393
 394
 395     btTransform & getFrameOffsetA()
 396     {
 397         return m_frameInA;
 398     }
 399
 400     btTransform & getFrameOffsetB()
 401     {
 402         return m_frameInB;
 403     }
 404
 405
 406         //! performs Jacobian calculation, and also calculates angle differences and axis
 407     virtual void        buildJacobian();
 408
 409         virtual void getInfo1 (btConstraintInfo1* info);
 410
 411         void getInfo1NonVirtual (btConstraintInfo1* info);
 412
 413         virtual void getInfo2 (btConstraintInfo2* info);
 414
 415         void getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
 416
 417
 418     void        updateRHS(btScalar      timeStep);
 419
 420         //! Get the rotation axis in global coordinates
 421         /*!
 422         \pre btGeneric6DofConstraint.buildJacobian must be called previously.
 423         */
 424     btVector3 getAxis(int axis_index) const;
 425
 426     //! Get the relative Euler angle
 427     /*!
 428         \pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
 429         */
 430     btScalar getAngle(int axis_index) const;
 431
 432         //! Get the relative position of the constraint pivot
 433     /*!
 434         \pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
 435         */
 436         btScalar getRelativePivotPosition(int axis_index) const;
 437
 438         void setFrames(const btTransform & frameA, const btTransform & frameB);
 439
 440         //! Test angular limit.
 441         /*!
 442         Calculates angular correction and returns true if limit needs to be corrected.
 443         \pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
 444         */
 445     bool testAngularLimitMotor(int axis_index);
 446
 447     void        setLinearLowerLimit(const btVector3& linearLower)
 448     {
 449         m_linearLimits.m_lowerLimit = linearLower;
 450     }
 451
 452         void    getLinearLowerLimit(btVector3& linearLower)
 453         {
 454                 linearLower = m_linearLimits.m_lowerLimit;
 455         }
 456
 457         void    setLinearUpperLimit(const btVector3& linearUpper)
 458         {
 459                 m_linearLimits.m_upperLimit = linearUpper;
 460         }
 461
 462         void    getLinearUpperLimit(btVector3& linearUpper)
 463         {
 464                 linearUpper = m_linearLimits.m_upperLimit;
 465         }
 466
 467     void        setAngularLowerLimit(const btVector3& angularLower)
 468     {
 469                 for(int i = 0; i < 3; i++)
 470                         m_angularLimits[i].m_loLimit = btNormalizeAngle(angularLower[i]);
 471     }
 472
 473         void    getAngularLowerLimit(btVector3& angularLower)
 474         {
 475                 for(int i = 0; i < 3; i++)
 476                         angularLower[i] = m_angularLimits[i].m_loLimit;
 477         }
 478
 479     void        setAngularUpperLimit(const btVector3& angularUpper)
 480     {
 481                 for(int i = 0; i < 3; i++)
 482                         m_angularLimits[i].m_hiLimit = btNormalizeAngle(angularUpper[i]);
 483     }
 484
 485         void    getAngularUpperLimit(btVector3& angularUpper)
 486         {
 487                 for(int i = 0; i < 3; i++)
 488                         angularUpper[i] = m_angularLimits[i].m_hiLimit;
 489         }
 490
 491         //! Retrieves the angular limit informacion
 492     btRotationalLimitMotor * getRotationalLimitMotor(int index)
 493     {
 494         return &m_angularLimits[index];
 495     }
 496
 497     //! Retrieves the  limit informacion
 498     btTranslationalLimitMotor * getTranslationalLimitMotor()
 499     {
 500         return &m_linearLimits;
 501     }
 502
 503     //first 3 are linear, next 3 are angular
 504     void setLimit(int axis, btScalar lo, btScalar hi)
 505     {
 506         if(axis<3)
 507         {
 508                 m_linearLimits.m_lowerLimit[axis] = lo;
 509                 m_linearLimits.m_upperLimit[axis] = hi;
 510         }
 511         else
 512         {
 513                         lo = btNormalizeAngle(lo);
 514                         hi = btNormalizeAngle(hi);
 515                 m_angularLimits[axis-3].m_loLimit = lo;
 516                 m_angularLimits[axis-3].m_hiLimit = hi;
 517         }
 518     }
 519
 520         //! Test limit
 521         /*!
 522     - free means upper < lower,
 523     - locked means upper == lower
 524     - limited means upper > lower
 525     - limitIndex: first 3 are linear, next 3 are angular
 526     */
 527     bool        isLimited(int limitIndex)
 528     {
 529         if(limitIndex<3)
 530         {
 531                         return m_linearLimits.isLimited(limitIndex);
 532
 533         }
 534         return m_angularLimits[limitIndex-3].isLimited();
 535     }
 536
 537         virtual void calcAnchorPos(void); // overridable
 538
 539         int get_limit_motor_info2(      btRotationalLimitMotor * limot,
 540                                                                 const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
 541                                                                 btConstraintInfo2 *info, int row, btVector3& ax1, int rotational, int rotAllowed = false);
 542
 543         // access for UseFrameOffset
 544         bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
 545         void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
 546
 547         ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
 548         ///If no axis is provided, it uses the default axis for this constraint.
 549         virtual void setParam(int num, btScalar value, int axis = -1);
 550         ///return the local value of parameter
 551         virtual btScalar getParam(int num, int axis = -1) const;
 552
 553         void setAxis( const btVector3& axis1, const btVector3& axis2);
 554
 555
 556         virtual int     calculateSerializeBufferSize() const;
 557
 558         ///fills the dataBuffer and returns the struct name (and 0 on failure)
 559         virtual const char*     serialize(void* dataBuffer, btSerializer* serializer) const;
 560
 561
 562 };
 563
 564 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
 565 struct btGeneric6DofConstraintData
 566 {
 567         btTypedConstraintData   m_typeConstraintData;
 568         btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
 569         btTransformFloatData m_rbBFrame;
 570
 571         btVector3FloatData      m_linearUpperLimit;
 572         btVector3FloatData      m_linearLowerLimit;
 573
 574         btVector3FloatData      m_angularUpperLimit;
 575         btVector3FloatData      m_angularLowerLimit;
 576
 577         int     m_useLinearReferenceFrameA;
 578         int m_useOffsetForConstraintFrame;
 579 };
 580
 581 SIMD_FORCE_INLINE       int     btGeneric6DofConstraint::calculateSerializeBufferSize() const
 582 {
 583         return sizeof(btGeneric6DofConstraintData);
 584 }
 585
 586         ///fills the dataBuffer and returns the struct name (and 0 on failure)
 587 SIMD_FORCE_INLINE       const char*     btGeneric6DofConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
 588 {
 589
 590         btGeneric6DofConstraintData* dof = (btGeneric6DofConstraintData*)dataBuffer;
 591         btTypedConstraint::serialize(&dof->m_typeConstraintData,serializer);
 592
 593         m_frameInA.serializeFloat(dof->m_rbAFrame);
 594         m_frameInB.serializeFloat(dof->m_rbBFrame);
 595
 596
 597         int i;
 598         for (i=0;i<3;i++)
 599         {
 600                 dof->m_angularLowerLimit.m_floats[i] =  float(m_angularLimits[i].m_loLimit);
 601                 dof->m_angularUpperLimit.m_floats[i] =  float(m_angularLimits[i].m_hiLimit);
 602                 dof->m_linearLowerLimit.m_floats[i] = float(m_linearLimits.m_lowerLimit[i]);
 603                 dof->m_linearUpperLimit.m_floats[i] = float(m_linearLimits.m_upperLimit[i]);
 604         }
 605
 606         dof->m_useLinearReferenceFrameA = m_useLinearReferenceFrameA? 1 : 0;
 607         dof->m_useOffsetForConstraintFrame = m_useOffsetForConstraintFrame ? 1 : 0;
 608
 609         return "btGeneric6DofConstraintData";
 610 }
 611
 612
 613
 614
 615
 616 #endif //BT_GENERIC_6DOF_CONSTRAINT_H