Upgrade Bullet to version 2.83.
[blender.git] / extern / bullet2 / 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 #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
47 //! Rotation Limit structure for generic joints
48 class btRotationalLimitMotor
49 {
50 public:
51     //! limit_parameters
52     //!@{
53     btScalar m_loLimit;//!< joint limit
54     btScalar m_hiLimit;//!< joint limit
55     btScalar m_targetVelocity;//!< target motor velocity
56     btScalar m_maxMotorForce;//!< max force on motor
57     btScalar m_maxLimitForce;//!< max force on limit
58     btScalar m_damping;//!< Damping.
59     btScalar m_limitSoftness;//! Relaxation factor
60     btScalar m_normalCFM;//!< Constraint force mixing factor
61     btScalar m_stopERP;//!< Error tolerance factor when joint is at limit
62     btScalar m_stopCFM;//!< Constraint force mixing factor when joint is at limit
63     btScalar m_bounce;//!< restitution factor
64     bool m_enableMotor;
65
66     //!@}
67
68     //! temp_variables
69     //!@{
70     btScalar m_currentLimitError;//!  How much is violated this limit
71     btScalar m_currentPosition;     //!  current value of angle 
72     int m_currentLimit;//!< 0=free, 1=at lo limit, 2=at hi limit
73     btScalar m_accumulatedImpulse;
74     //!@}
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
113         //! Is limited
114     bool isLimited() const
115     {
116         if(m_loLimit > m_hiLimit) return false;
117         return true;
118     }
119
120         //! Need apply correction
121     bool needApplyTorques() const
122     {
123         if(m_currentLimit == 0 && m_enableMotor == false) return false;
124         return true;
125     }
126
127         //! calculates  error
128         /*!
129         calculates m_currentLimit and m_currentLimitError.
130         */
131         int testLimitValue(btScalar test_value);
132
133         //! apply the correction impulses for two bodies
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;//!< the constraint lower limits
144     btVector3 m_upperLimit;//!< the constraint upper limits
145     btVector3 m_accumulatedImpulse;
146     //! Linear_Limit_parameters
147     //!@{
148     btScalar    m_limitSoftness;//!< Softness for linear limit
149     btScalar    m_damping;//!< Damping for linear limit
150     btScalar    m_restitution;//! Bounce parameter for linear limit
151         btVector3       m_normalCFM;//!< Constraint force mixing factor
152     btVector3   m_stopERP;//!< Error tolerance factor when joint is at limit
153         btVector3       m_stopCFM;//!< Constraint force mixing factor when joint is at limit
154     //!@}
155         bool            m_enableMotor[3];
156     btVector3   m_targetVelocity;//!< target motor velocity
157     btVector3   m_maxMotorForce;//!< max force on motor
158     btVector3   m_currentLimitError;//!  How much is violated this limit
159     btVector3   m_currentLinearDiff;//!  Current relative offset of constraint frames
160     int                 m_currentLimit[3];//!< 0=free, 1=at lower limit, 2=at upper limit
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
203     //! Test limit
204         /*!
205     - free means upper < lower,
206     - locked means upper == lower
207     - limited means upper > lower
208     - limitIndex: first 3 are linear, next 3 are angular
209     */
210     inline bool isLimited(int limitIndex) const
211     {
212        return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
213     }
214     inline bool needApplyForce(int limitIndex) const
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
243 /// btGeneric6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
244 /*!
245 btGeneric6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked'.
246 currently this limit supports rotational motors<br>
247 <ul>
248 <li> For Linear limits, use btGeneric6DofConstraint.setLinearUpperLimit, btGeneric6DofConstraint.setLinearLowerLimit. You can set the parameters with the btTranslationalLimitMotor structure accsesible through the btGeneric6DofConstraint.getTranslationalLimitMotor method.
249 At this moment translational motors are not supported. May be in the future. </li>
250
251 <li> For Angular limits, use the btRotationalLimitMotor structure for configuring the limit.
252 This is accessible through btGeneric6DofConstraint.getLimitMotor method,
253 This brings support for limit parameters and motors. </li>
254
255 <li> Angulars limits have these possible ranges:
256 <table border=1 >
257 <tr>
258         <td><b>AXIS</b></td>
259         <td><b>MIN ANGLE</b></td>
260         <td><b>MAX ANGLE</b></td>
261 </tr><tr>
262         <td>X</td>
263         <td>-PI</td>
264         <td>PI</td>
265 </tr><tr>
266         <td>Y</td>
267         <td>-PI/2</td>
268         <td>PI/2</td>
269 </tr><tr>
270         <td>Z</td>
271         <td>-PI</td>
272         <td>PI</td>
273 </tr>
274 </table>
275 </li>
276 </ul>
277
278 */
279 ATTRIBUTE_ALIGNED16(class) btGeneric6DofConstraint : public btTypedConstraint
280 {
281 protected:
282
283         //! relative_frames
284     //!@{
285         btTransform     m_frameInA;//!< the constraint space w.r.t body A
286     btTransform m_frameInB;//!< the constraint space w.r.t body B
287     //!@}
288
289     //! Jacobians
290     //!@{
291     btJacobianEntry     m_jacLinear[3];//!< 3 orthogonal linear constraints
292     btJacobianEntry     m_jacAng[3];//!< 3 orthogonal angular constraints
293     //!@}
294
295         //! Linear_Limit_parameters
296     //!@{
297     btTranslationalLimitMotor m_linearLimits;
298     //!@}
299
300
301     //! hinge_parameters
302     //!@{
303     btRotationalLimitMotor m_angularLimits[3];
304         //!@}
305
306
307 protected:
308     //! temporal variables
309     //!@{
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
327     //!@}
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
350         //! calcs the euler angles between the two bodies.
351     void calculateAngleInfo();
352
353
354
355 public:
356
357         BT_DECLARE_ALIGNED_ALLOCATOR();
358         
359         ///for backwards compatibility during the transition to 'getInfo/getInfo2'
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     
365         //! Calcs global transform of the offsets
366         /*!
367         Calcs the global transform for the joint offset for body A an B, and also calcs the agle differences between the bodies.
368         \sa btGeneric6DofConstraint.getCalculatedTransformA , btGeneric6DofConstraint.getCalculatedTransformB, btGeneric6DofConstraint.calculateAngleInfo
369         */
370     void calculateTransforms(const btTransform& transA,const btTransform& transB);
371
372         void calculateTransforms();
373
374         //! Gets the global transform of the offset for body A
375     /*!
376     \sa btGeneric6DofConstraint.getFrameOffsetA, btGeneric6DofConstraint.getFrameOffsetB, btGeneric6DofConstraint.calculateAngleInfo.
377     */
378     const btTransform & getCalculatedTransformA() const
379     {
380         return m_calculatedTransformA;
381     }
382
383     //! Gets the global transform of the offset for body B
384     /*!
385     \sa btGeneric6DofConstraint.getFrameOffsetA, btGeneric6DofConstraint.getFrameOffsetB, btGeneric6DofConstraint.calculateAngleInfo.
386     */
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
414         //! performs Jacobian calculation, and also calculates angle differences and axis
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
428         //! Get the rotation axis in global coordinates
429         /*!
430         \pre btGeneric6DofConstraint.buildJacobian must be called previously.
431         */
432     btVector3 getAxis(int axis_index) const;
433
434     //! Get the relative Euler angle
435     /*!
436         \pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
437         */
438     btScalar getAngle(int axis_index) const;
439
440         //! Get the relative position of the constraint pivot
441     /*!
442         \pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
443         */
444         btScalar getRelativePivotPosition(int axis_index) const;
445
446         void setFrames(const btTransform & frameA, const btTransform & frameB);
447
448         //! Test angular limit.
449         /*!
450         Calculates angular correction and returns true if limit needs to be corrected.
451         \pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
452         */
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) const
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) const
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) const
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) const
494         {
495                 for(int i = 0; i < 3; i++)
496                         angularUpper[i] = m_angularLimits[i].m_hiLimit;
497         }
498
499         //! Retrieves the angular limit informacion
500     btRotationalLimitMotor * getRotationalLimitMotor(int index)
501     {
502         return &m_angularLimits[index];
503     }
504
505     //! Retrieves the  limit informacion
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
528         //! Test limit
529         /*!
530     - free means upper < lower,
531     - locked means upper == lower
532     - limited means upper > lower
533     - limitIndex: first 3 are linear, next 3 are angular
534     */
535     bool        isLimited(int limitIndex) const
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() const { return m_useOffsetForConstraintFrame; }
553         void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
554         
555         bool getUseLinearReferenceFrameA() const { return m_useLinearReferenceFrameA; }
556         void setUseLinearReferenceFrameA(bool linearReferenceFrameA) { m_useLinearReferenceFrameA = linearReferenceFrameA; }
557
558         ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
559         ///If no axis is provided, it uses the default axis for this constraint.
560         virtual void setParam(int num, btScalar value, int axis = -1);
561         ///return the local value of parameter
562         virtual btScalar getParam(int num, int axis = -1) const;
563
564         void setAxis( const btVector3& axis1, const btVector3& axis2);
565
566         virtual int getFlags() const
567         {
568                 return m_flags;
569         }
570
571         virtual int     calculateSerializeBufferSize() const;
572
573         ///fills the dataBuffer and returns the struct name (and 0 on failure)
574         virtual const char*     serialize(void* dataBuffer, btSerializer* serializer) const;
575
576         
577 };
578
579
580 struct btGeneric6DofConstraintData
581 {
582         btTypedConstraintData   m_typeConstraintData;
583         btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
584         btTransformFloatData m_rbBFrame;
585         
586         btVector3FloatData      m_linearUpperLimit;
587         btVector3FloatData      m_linearLowerLimit;
588
589         btVector3FloatData      m_angularUpperLimit;
590         btVector3FloatData      m_angularLowerLimit;
591         
592         int     m_useLinearReferenceFrameA;
593         int m_useOffsetForConstraintFrame;
594 };
595
596 struct btGeneric6DofConstraintDoubleData2
597 {
598         btTypedConstraintDoubleData     m_typeConstraintData;
599         btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
600         btTransformDoubleData m_rbBFrame;
601         
602         btVector3DoubleData     m_linearUpperLimit;
603         btVector3DoubleData     m_linearLowerLimit;
604
605         btVector3DoubleData     m_angularUpperLimit;
606         btVector3DoubleData     m_angularLowerLimit;
607         
608         int     m_useLinearReferenceFrameA;
609         int m_useOffsetForConstraintFrame;
610 };
611
612 SIMD_FORCE_INLINE       int     btGeneric6DofConstraint::calculateSerializeBufferSize() const
613 {
614         return sizeof(btGeneric6DofConstraintData2);
615 }
616
617         ///fills the dataBuffer and returns the struct name (and 0 on failure)
618 SIMD_FORCE_INLINE       const char*     btGeneric6DofConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
619 {
620
621         btGeneric6DofConstraintData2* dof = (btGeneric6DofConstraintData2*)dataBuffer;
622         btTypedConstraint::serialize(&dof->m_typeConstraintData,serializer);
623
624         m_frameInA.serialize(dof->m_rbAFrame);
625         m_frameInB.serialize(dof->m_rbBFrame);
626
627                 
628         int i;
629         for (i=0;i<3;i++)
630         {
631                 dof->m_angularLowerLimit.m_floats[i] =  m_angularLimits[i].m_loLimit;
632                 dof->m_angularUpperLimit.m_floats[i] =  m_angularLimits[i].m_hiLimit;
633                 dof->m_linearLowerLimit.m_floats[i] = m_linearLimits.m_lowerLimit[i];
634                 dof->m_linearUpperLimit.m_floats[i] = m_linearLimits.m_upperLimit[i];
635         }
636         
637         dof->m_useLinearReferenceFrameA = m_useLinearReferenceFrameA? 1 : 0;
638         dof->m_useOffsetForConstraintFrame = m_useOffsetForConstraintFrame ? 1 : 0;
639
640         return btGeneric6DofConstraintDataName;
641 }
642
643
644
645
646
647 #endif //BT_GENERIC_6DOF_CONSTRAINT_H