Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/trunk/src/external/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h @ 12155

Last change on this file since 12155 was 8393, checked in by rgrieder, 14 years ago

Updated Bullet from v2.77 to v2.78.
(I'm not going to make a branch for that since the update from 2.74 to 2.77 hasn't been tested that much either).

You will HAVE to do a complete RECOMPILE! I tested with MSVC and MinGW and they both threw linker errors at me.

  • Property svn:eol-style set to native
File size: 18.3 KB
Line 
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. 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/*
202007-09-09
21btGeneric6DofConstraint Refactored by Francisco Le?n
22email: projectileman@yahoo.com
23http://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
34class btRigidBody;
35
36
37
38
39//! Rotation Limit structure for generic joints
40class btRotationalLimitMotor
41{
42public:
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
132class btTranslationalLimitMotor
133{
134public:
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
226enum 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/*!
237btGeneric6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked'.
238currently 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.
241At 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.
244This is accessible through btGeneric6DofConstraint.getLimitMotor method,
245This 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*/
271class btGeneric6DofConstraint : public btTypedConstraint
272{
273protected:
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
299protected:
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
347public:
348
349        ///for backwards compatibility during the transition to 'getInfo/getInfo2'
350        bool            m_useSolveConstraintObsolete;
351
352    btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
353    btGeneric6DofConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB);
354   
355        //! Calcs global transform of the offsets
356        /*!
357        Calcs the global transform for the joint offset for body A an B, and also calcs the agle differences between the bodies.
358        \sa btGeneric6DofConstraint.getCalculatedTransformA , btGeneric6DofConstraint.getCalculatedTransformB, btGeneric6DofConstraint.calculateAngleInfo
359        */
360    void calculateTransforms(const btTransform& transA,const btTransform& transB);
361
362        void calculateTransforms();
363
364        //! Gets the global transform of the offset for body A
365    /*!
366    \sa btGeneric6DofConstraint.getFrameOffsetA, btGeneric6DofConstraint.getFrameOffsetB, btGeneric6DofConstraint.calculateAngleInfo.
367    */
368    const btTransform & getCalculatedTransformA() const
369    {
370        return m_calculatedTransformA;
371    }
372
373    //! Gets the global transform of the offset for body B
374    /*!
375    \sa btGeneric6DofConstraint.getFrameOffsetA, btGeneric6DofConstraint.getFrameOffsetB, btGeneric6DofConstraint.calculateAngleInfo.
376    */
377    const btTransform & getCalculatedTransformB() const
378    {
379        return m_calculatedTransformB;
380    }
381
382    const btTransform & getFrameOffsetA() const
383    {
384        return m_frameInA;
385    }
386
387    const btTransform & getFrameOffsetB() const
388    {
389        return m_frameInB;
390    }
391
392
393    btTransform & getFrameOffsetA()
394    {
395        return m_frameInA;
396    }
397
398    btTransform & getFrameOffsetB()
399    {
400        return m_frameInB;
401    }
402
403
404        //! performs Jacobian calculation, and also calculates angle differences and axis
405    virtual void        buildJacobian();
406
407        virtual void getInfo1 (btConstraintInfo1* info);
408
409        void getInfo1NonVirtual (btConstraintInfo1* info);
410
411        virtual void getInfo2 (btConstraintInfo2* info);
412
413        void getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
414
415
416    void        updateRHS(btScalar      timeStep);
417
418        //! Get the rotation axis in global coordinates
419        /*!
420        \pre btGeneric6DofConstraint.buildJacobian must be called previously.
421        */
422    btVector3 getAxis(int axis_index) const;
423
424    //! Get the relative Euler angle
425    /*!
426        \pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
427        */
428    btScalar getAngle(int axis_index) const;
429
430        //! Get the relative position of the constraint pivot
431    /*!
432        \pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
433        */
434        btScalar getRelativePivotPosition(int axis_index) const;
435
436        void setFrames(const btTransform & frameA, const btTransform & frameB);
437
438        //! Test angular limit.
439        /*!
440        Calculates angular correction and returns true if limit needs to be corrected.
441        \pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
442        */
443    bool testAngularLimitMotor(int axis_index);
444
445    void        setLinearLowerLimit(const btVector3& linearLower)
446    {
447        m_linearLimits.m_lowerLimit = linearLower;
448    }
449
450        void    getLinearLowerLimit(btVector3& linearLower)
451        {
452                linearLower = m_linearLimits.m_lowerLimit;
453        }
454
455        void    setLinearUpperLimit(const btVector3& linearUpper)
456        {
457                m_linearLimits.m_upperLimit = linearUpper;
458        }
459
460        void    getLinearUpperLimit(btVector3& linearUpper)
461        {
462                linearUpper = m_linearLimits.m_upperLimit;
463        }
464
465    void        setAngularLowerLimit(const btVector3& angularLower)
466    {
467                for(int i = 0; i < 3; i++) 
468                        m_angularLimits[i].m_loLimit = btNormalizeAngle(angularLower[i]);
469    }
470
471        void    getAngularLowerLimit(btVector3& angularLower)
472        {
473                for(int i = 0; i < 3; i++) 
474                        angularLower[i] = m_angularLimits[i].m_loLimit;
475        }
476
477    void        setAngularUpperLimit(const btVector3& angularUpper)
478    {
479                for(int i = 0; i < 3; i++)
480                        m_angularLimits[i].m_hiLimit = btNormalizeAngle(angularUpper[i]);
481    }
482
483        void    getAngularUpperLimit(btVector3& angularUpper)
484        {
485                for(int i = 0; i < 3; i++)
486                        angularUpper[i] = m_angularLimits[i].m_hiLimit;
487        }
488
489        //! Retrieves the angular limit informacion
490    btRotationalLimitMotor * getRotationalLimitMotor(int index)
491    {
492        return &m_angularLimits[index];
493    }
494
495    //! Retrieves the  limit informacion
496    btTranslationalLimitMotor * getTranslationalLimitMotor()
497    {
498        return &m_linearLimits;
499    }
500
501    //first 3 are linear, next 3 are angular
502    void setLimit(int axis, btScalar lo, btScalar hi)
503    {
504        if(axis<3)
505        {
506                m_linearLimits.m_lowerLimit[axis] = lo;
507                m_linearLimits.m_upperLimit[axis] = hi;
508        }
509        else
510        {
511                        lo = btNormalizeAngle(lo);
512                        hi = btNormalizeAngle(hi);
513                m_angularLimits[axis-3].m_loLimit = lo;
514                m_angularLimits[axis-3].m_hiLimit = hi;
515        }
516    }
517
518        //! Test limit
519        /*!
520    - free means upper < lower,
521    - locked means upper == lower
522    - limited means upper > lower
523    - limitIndex: first 3 are linear, next 3 are angular
524    */
525    bool        isLimited(int limitIndex)
526    {
527        if(limitIndex<3)
528        {
529                        return m_linearLimits.isLimited(limitIndex);
530
531        }
532        return m_angularLimits[limitIndex-3].isLimited();
533    }
534
535        virtual void calcAnchorPos(void); // overridable
536
537        int get_limit_motor_info2(      btRotationalLimitMotor * limot,
538                                                                const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
539                                                                btConstraintInfo2 *info, int row, btVector3& ax1, int rotational, int rotAllowed = false);
540
541        // access for UseFrameOffset
542        bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
543        void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
544
545        ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
546        ///If no axis is provided, it uses the default axis for this constraint.
547        virtual void setParam(int num, btScalar value, int axis = -1);
548        ///return the local value of parameter
549        virtual btScalar getParam(int num, int axis = -1) const;
550
551        void setAxis( const btVector3& axis1, const btVector3& axis2);
552
553
554        virtual int     calculateSerializeBufferSize() const;
555
556        ///fills the dataBuffer and returns the struct name (and 0 on failure)
557        virtual const char*     serialize(void* dataBuffer, btSerializer* serializer) const;
558
559       
560};
561
562///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
563struct btGeneric6DofConstraintData
564{
565        btTypedConstraintData   m_typeConstraintData;
566        btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
567        btTransformFloatData m_rbBFrame;
568       
569        btVector3FloatData      m_linearUpperLimit;
570        btVector3FloatData      m_linearLowerLimit;
571
572        btVector3FloatData      m_angularUpperLimit;
573        btVector3FloatData      m_angularLowerLimit;
574       
575        int     m_useLinearReferenceFrameA;
576        int m_useOffsetForConstraintFrame;
577};
578
579SIMD_FORCE_INLINE       int     btGeneric6DofConstraint::calculateSerializeBufferSize() const
580{
581        return sizeof(btGeneric6DofConstraintData);
582}
583
584        ///fills the dataBuffer and returns the struct name (and 0 on failure)
585SIMD_FORCE_INLINE       const char*     btGeneric6DofConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
586{
587
588        btGeneric6DofConstraintData* dof = (btGeneric6DofConstraintData*)dataBuffer;
589        btTypedConstraint::serialize(&dof->m_typeConstraintData,serializer);
590
591        m_frameInA.serializeFloat(dof->m_rbAFrame);
592        m_frameInB.serializeFloat(dof->m_rbBFrame);
593
594               
595        int i;
596        for (i=0;i<3;i++)
597        {
598                dof->m_angularLowerLimit.m_floats[i] =  float(m_angularLimits[i].m_loLimit);
599                dof->m_angularUpperLimit.m_floats[i] =  float(m_angularLimits[i].m_hiLimit);
600                dof->m_linearLowerLimit.m_floats[i] = float(m_linearLimits.m_lowerLimit[i]);
601                dof->m_linearUpperLimit.m_floats[i] = float(m_linearLimits.m_upperLimit[i]);
602        }
603       
604        dof->m_useLinearReferenceFrameA = m_useLinearReferenceFrameA? 1 : 0;
605        dof->m_useOffsetForConstraintFrame = m_useOffsetForConstraintFrame ? 1 : 0;
606
607        return "btGeneric6DofConstraintData";
608}
609
610
611
612
613
614#endif //BT_GENERIC_6DOF_CONSTRAINT_H
Note: See TracBrowser for help on using the repository browser.