Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

Ignore:
Timestamp:
Apr 21, 2011, 6:58:23 PM (13 years ago)
Author:
rgrieder
Message:

Merged revisions 7978 - 8096 from kicklib to kicklib2.

Location:
code/branches/kicklib2
Files:
2 edited

Legend:

Unmodified
Added
Removed

  • code/branches/kicklib2

  • code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h

    r5781 r8284  
    13133. This notice may not be removed or altered from any source distribution.
    1414*/
     15
     16/// 2009 March: btGeneric6DofConstraint refactored by Roman Ponomarev
     17/// Added support for generic constraint solver through getInfo1/getInfo2 methods
     18
    1519/*
    16202007-09-09
    … …  
    4650    btScalar m_damping;//!< Damping.
    4751    btScalar m_limitSoftness;//! Relaxation factor
    48     btScalar m_ERP;//!< Error tolerance factor when joint is at limit
     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
    4955    btScalar m_bounce;//!< restitution factor
    5056    bool m_enableMotor;
    … …  
    5561    //!@{
    5662    btScalar m_currentLimitError;//!  How much is violated this limit
     63    btScalar m_currentPosition;     //!  current value of angle
    5764    int m_currentLimit;//!< 0=free, 1=at lo limit, 2=at hi limit
    5865    btScalar m_accumulatedImpulse;
    … …  
    6572        m_maxMotorForce = 0.1f;
    6673        m_maxLimitForce = 300.0f;
    67         m_loLimit = -SIMD_INFINITY;
    68         m_hiLimit = SIMD_INFINITY;
    69         m_ERP = 0.5f;
     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;
    7079        m_bounce = 0.0f;
    7180        m_damping = 1.0f;
    … …  
    8392        m_loLimit = limot.m_loLimit;
    8493        m_hiLimit = limot.m_hiLimit;
    85         m_ERP = limot.m_ERP;
     94                m_normalCFM = limot.m_normalCFM;
     95                m_stopERP = limot.m_stopERP;
     96                m_stopCFM =     limot.m_stopCFM;
    8697        m_bounce = limot.m_bounce;
    8798        m_currentLimit = limot.m_currentLimit;
    … …  
    113124
    114125        //! apply the correction impulses for two bodies
    115     btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btSolverBody& bodyA,btRigidBody * body1,btSolverBody& bodyB);
     126    btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btRigidBody * body1);
    116127
    117128};
    … …  
    130141    btScalar    m_damping;//!< Damping for linear limit
    131142    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
    132146    //!@}
    133147        bool            m_enableMotor[3];
    … …  
    135149    btVector3   m_maxMotorForce;//!< max force on motor
    136150    btVector3   m_currentLimitError;//!  How much is violated this limit
     151    btVector3   m_currentLinearDiff;//!  Current relative offset of constraint frames
    137152    int                 m_currentLimit[3];//!< 0=free, 1=at lower limit, 2=at upper limit
    138153
    … …  
    142157        m_upperLimit.setValue(0.f,0.f,0.f);
    143158        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);
    144162
    145163        m_limitSoftness = 0.7f;
    … …  
    163181        m_damping = other.m_damping;
    164182        m_restitution = other.m_restitution;
     183                m_normalCFM = other.m_normalCFM;
     184                m_stopERP = other.m_stopERP;
     185                m_stopCFM = other.m_stopCFM;
     186
    165187                for(int i=0; i < 3; i++)
    166188                {
    … …  
    193215        btScalar timeStep,
    194216        btScalar jacDiagABInv,
    195         btRigidBody& body1,btSolverBody& bodyA,const btVector3 &pointInA,
    196         btRigidBody& body2,btSolverBody& bodyB,const btVector3 &pointInB,
     217        btRigidBody& body1,const btVector3 &pointInA,
     218        btRigidBody& body2,const btVector3 &pointInB,
    197219        int limit_index,
    198220        const btVector3 & axis_normal_on_a,
    … …  
    201223
    202224};
     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
    203234
    204235/// btGeneric6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
    … …  
    216247<li> Angulars limits have these possible ranges:
    217248<table border=1 >
    218 <tr
    219 
     249<tr>
    220250        <td><b>AXIS</b></td>
    221251        <td><b>MIN ANGLE</b></td>
    222252        <td><b>MAX ANGLE</b></td>
     253</tr><tr>
    223254        <td>X</td>
    224                 <td>-PI</td>
    225                 <td>PI</td>
     255        <td>-PI</td>
     256        <td>PI</td>
     257</tr><tr>
    226258        <td>Y</td>
    227                 <td>-PI/2</td>
    228                 <td>PI/2</td>
     259        <td>-PI/2</td>
     260        <td>PI/2</td>
     261</tr><tr>
    229262        <td>Z</td>
    230                 <td>-PI/2</td>
    231                 <td>PI/2</td>
     263        <td>-PI</td>
     264        <td>PI</td>
    232265</tr>
    233266</table>
    … …  
    273306    btVector3 m_calculatedAxis[3];
    274307    btVector3 m_calculatedLinearDiff;
     308        btScalar        m_factA;
     309        btScalar        m_factB;
     310        bool            m_hasStaticBody;
    275311   
    276312        btVector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes
    277313
    278314    bool        m_useLinearReferenceFrameA;
     315        bool    m_useOffsetForConstraintFrame;
    279316   
     317        int             m_flags;
     318
    280319    //!@}
    281320
    … …  
    288327
    289328
    290         int setAngularLimits(btConstraintInfo2 *info, int row_offset);
    291 
    292         int setLinearLimits(btConstraintInfo2 *info);
     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);
    293332
    294333    void buildLinearJacobian(
    … …  
    312351
    313352    btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
    314 
    315     btGeneric6DofConstraint();
    316 
     353    btGeneric6DofConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB);
     354   
    317355        //! Calcs global transform of the offsets
    318356        /*!
    … …  
    320358        \sa btGeneric6DofConstraint.getCalculatedTransformA , btGeneric6DofConstraint.getCalculatedTransformB, btGeneric6DofConstraint.calculateAngleInfo
    321359        */
    322     void calculateTransforms();
     360    void calculateTransforms(const btTransform& transA,const btTransform& transB);
     361
     362        void calculateTransforms();
    323363
    324364        //! Gets the global transform of the offset for body A
    … …  
    367407        virtual void getInfo1 (btConstraintInfo1* info);
    368408
     409        void getInfo1NonVirtual (btConstraintInfo1* info);
     410
    369411        virtual void getInfo2 (btConstraintInfo2* info);
    370412
    371     virtual     void    solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar        timeStep);
     413        void getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
     414
    372415
    373416    void        updateRHS(btScalar      timeStep);
    … …  
    381424    //! Get the relative Euler angle
    382425    /*!
    383         \pre btGeneric6DofConstraint.buildJacobian must be called previously.
     426        \pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
    384427        */
    385428    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
    386436
    387437        //! Test angular limit.
    388438        /*!
    389439        Calculates angular correction and returns true if limit needs to be corrected.
    390         \pre btGeneric6DofConstraint.buildJacobian must be called previously.
     440        \pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
    391441        */
    392442    bool testAngularLimitMotor(int axis_index);
    … …  
    404454    void        setAngularLowerLimit(const btVector3& angularLower)
    405455    {
    406         m_angularLimits[0].m_loLimit = angularLower.getX();
    407         m_angularLimits[1].m_loLimit = angularLower.getY();
    408         m_angularLimits[2].m_loLimit = angularLower.getZ();
     456                for(int i = 0; i < 3; i++)
     457                        m_angularLimits[i].m_loLimit = btNormalizeAngle(angularLower[i]);
    409458    }
    410459
    411460    void        setAngularUpperLimit(const btVector3& angularUpper)
    412461    {
    413         m_angularLimits[0].m_hiLimit = angularUpper.getX();
    414         m_angularLimits[1].m_hiLimit = angularUpper.getY();
    415         m_angularLimits[2].m_hiLimit = angularUpper.getZ();
     462                for(int i = 0; i < 3; i++)
     463                        m_angularLimits[i].m_hiLimit = btNormalizeAngle(angularUpper[i]);
    416464    }
    417465
    … …  
    438486        else
    439487        {
     488                        lo = btNormalizeAngle(lo);
     489                        hi = btNormalizeAngle(hi);
    440490                m_angularLimits[axis-3].m_loLimit = lo;
    441491                m_angularLimits[axis-3].m_hiLimit = hi;
    … …  
    460510    }
    461511
    462     const btRigidBody& getRigidBodyA() const
    463     {
    464         return m_rbA;
    465     }
    466     const btRigidBody& getRigidBodyB() const
    467     {
    468         return m_rbB;
    469     }
    470 
    471512        virtual void calcAnchorPos(void); // overridable
    472513
    473514        int get_limit_motor_info2(      btRotationalLimitMotor * limot,
    474                                                                 btRigidBody * body0, btRigidBody * body1,
    475                                                                 btConstraintInfo2 *info, int row, btVector3& ax1, int rotational);
    476 
    477 
     515                                                                const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
     516                                                                btConstraintInfo2 *info, int row, btVector3& ax1, int rotational, int rotAllowed = false);
     517
     518        // access for UseFrameOffset
     519        bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
     520        void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
     521
     522        ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
     523        ///If no axis is provided, it uses the default axis for this constraint.
     524        virtual void setParam(int num, btScalar value, int axis = -1);
     525        ///return the local value of parameter
     526        virtual btScalar getParam(int num, int axis = -1) const;
     527
     528        virtual int     calculateSerializeBufferSize() const;
     529
     530        ///fills the dataBuffer and returns the struct name (and 0 on failure)
     531        virtual const char*     serialize(void* dataBuffer, btSerializer* serializer) const;
     532
     533       
    478534};
    479535
     536///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
     537struct btGeneric6DofConstraintData
     538{
     539        btTypedConstraintData   m_typeConstraintData;
     540        btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
     541        btTransformFloatData m_rbBFrame;
     542       
     543        btVector3FloatData      m_linearUpperLimit;
     544        btVector3FloatData      m_linearLowerLimit;
     545
     546        btVector3FloatData      m_angularUpperLimit;
     547        btVector3FloatData      m_angularLowerLimit;
     548       
     549        int     m_useLinearReferenceFrameA;
     550        int m_useOffsetForConstraintFrame;
     551};
     552
     553SIMD_FORCE_INLINE       int     btGeneric6DofConstraint::calculateSerializeBufferSize() const
     554{
     555        return sizeof(btGeneric6DofConstraintData);
     556}
     557
     558        ///fills the dataBuffer and returns the struct name (and 0 on failure)
     559SIMD_FORCE_INLINE       const char*     btGeneric6DofConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
     560{
     561
     562        btGeneric6DofConstraintData* dof = (btGeneric6DofConstraintData*)dataBuffer;
     563        btTypedConstraint::serialize(&dof->m_typeConstraintData,serializer);
     564
     565        m_frameInA.serializeFloat(dof->m_rbAFrame);
     566        m_frameInB.serializeFloat(dof->m_rbBFrame);
     567
     568               
     569        int i;
     570        for (i=0;i<3;i++)
     571        {
     572                dof->m_angularLowerLimit.m_floats[i] =  float(m_angularLimits[i].m_loLimit);
     573                dof->m_angularUpperLimit.m_floats[i] =  float(m_angularLimits[i].m_hiLimit);
     574                dof->m_linearLowerLimit.m_floats[i] = float(m_linearLimits.m_lowerLimit[i]);
     575                dof->m_linearUpperLimit.m_floats[i] = float(m_linearLimits.m_upperLimit[i]);
     576        }
     577       
     578        dof->m_useLinearReferenceFrameA = m_useLinearReferenceFrameA? 1 : 0;
     579        dof->m_useOffsetForConstraintFrame = m_useOffsetForConstraintFrame ? 1 : 0;
     580
     581        return "btGeneric6DofConstraintData";
     582}
     583
     584
     585
     586
     587
    480588#endif //GENERIC_6DOF_CONSTRAINT_H
Note: See TracChangeset for help on using the changeset viewer.