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/btSliderConstraint.cpp

    r5781 r8284  
    1919*/
    2020
    21 //-----------------------------------------------------------------------------
     21
    2222
    2323#include "btSliderConstraint.h"
    … …  
    2626#include <new>
    2727
    28 //-----------------------------------------------------------------------------
     28#define USE_OFFSET_FOR_CONSTANT_FRAME true
    2929
    3030void btSliderConstraint::initParams()
    … …  
    3737        m_restitutionDirLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
    3838        m_dampingDirLin = btScalar(0.);
     39        m_cfmDirLin = SLIDER_CONSTRAINT_DEF_CFM;
    3940        m_softnessDirAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
    4041        m_restitutionDirAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
    4142        m_dampingDirAng = btScalar(0.);
     43        m_cfmDirAng = SLIDER_CONSTRAINT_DEF_CFM;
    4244        m_softnessOrthoLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
    4345        m_restitutionOrthoLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
    4446        m_dampingOrthoLin = SLIDER_CONSTRAINT_DEF_DAMPING;
     47        m_cfmOrthoLin = SLIDER_CONSTRAINT_DEF_CFM;
    4548        m_softnessOrthoAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
    4649        m_restitutionOrthoAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
    4750        m_dampingOrthoAng = SLIDER_CONSTRAINT_DEF_DAMPING;
     51        m_cfmOrthoAng = SLIDER_CONSTRAINT_DEF_CFM;
    4852        m_softnessLimLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
    4953        m_restitutionLimLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
    5054        m_dampingLimLin = SLIDER_CONSTRAINT_DEF_DAMPING;
     55        m_cfmLimLin = SLIDER_CONSTRAINT_DEF_CFM;
    5156        m_softnessLimAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
    5257        m_restitutionLimAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
    5358        m_dampingLimAng = SLIDER_CONSTRAINT_DEF_DAMPING;
     59        m_cfmLimAng = SLIDER_CONSTRAINT_DEF_CFM;
    5460
    5561        m_poweredLinMotor = false;
    … …  
    6369        m_accumulatedAngMotorImpulse = btScalar(0.0);
    6470
    65 } // btSliderConstraint::initParams()
    66 
    67 //-----------------------------------------------------------------------------
    68 
    69 btSliderConstraint::btSliderConstraint()
    70         :btTypedConstraint(SLIDER_CONSTRAINT_TYPE),
    71                 m_useLinearReferenceFrameA(true),
    72                 m_useSolveConstraintObsolete(false)
    73 //              m_useSolveConstraintObsolete(true)
     71        m_flags = 0;
     72        m_flags = 0;
     73
     74        m_useOffsetForConstraintFrame = USE_OFFSET_FOR_CONSTANT_FRAME;
     75
     76        calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
     77}
     78
     79
     80
     81
     82
     83btSliderConstraint::btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
     84        : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, rbA, rbB),
     85                m_useSolveConstraintObsolete(false),
     86                m_frameInA(frameInA),
     87        m_frameInB(frameInB),
     88                m_useLinearReferenceFrameA(useLinearReferenceFrameA)
    7489{
    7590        initParams();
    76 } // btSliderConstraint::btSliderConstraint()
    77 
    78 //-----------------------------------------------------------------------------
    79 
    80 btSliderConstraint::btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
    81         : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, rbA, rbB)
    82         , m_frameInA(frameInA)
    83         , m_frameInB(frameInB),
    84                 m_useLinearReferenceFrameA(useLinearReferenceFrameA),
    85                 m_useSolveConstraintObsolete(false)
    86 //              m_useSolveConstraintObsolete(true)
    87 {
     91}
     92
     93
     94
     95btSliderConstraint::btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameA)
     96        : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, getFixedBody(), rbB),
     97                m_useSolveConstraintObsolete(false),
     98                m_frameInB(frameInB),
     99                m_useLinearReferenceFrameA(useLinearReferenceFrameA)
     100{
     101        ///not providing rigidbody A means implicitly using worldspace for body A
     102        m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB;
     103//      m_frameInA.getOrigin() = m_rbA.getCenterOfMassTransform()(m_frameInA.getOrigin());
     104
    88105        initParams();
    89 } // btSliderConstraint::btSliderConstraint()
    90 
    91 //-----------------------------------------------------------------------------
    92 
    93 void btSliderConstraint::buildJacobian()
    94 {
    95         if (!m_useSolveConstraintObsolete)
    96         {
    97                 return;
    98         }
    99         if(m_useLinearReferenceFrameA)
    100         {
    101                 buildJacobianInt(m_rbA, m_rbB, m_frameInA, m_frameInB);
     106}
     107
     108
     109
     110
     111
     112
     113void btSliderConstraint::getInfo1(btConstraintInfo1* info)
     114{
     115        if (m_useSolveConstraintObsolete)
     116        {
     117                info->m_numConstraintRows = 0;
     118                info->nub = 0;
    102119        }
    103120        else
    104121        {
    105                 buildJacobianInt(m_rbB, m_rbA, m_frameInB, m_frameInA);
    106         }
    107 } // btSliderConstraint::buildJacobian()
    108 
    109 //-----------------------------------------------------------------------------
    110 
    111 void btSliderConstraint::buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB)
    112 {
    113         //calculate transforms
    114     m_calculatedTransformA = rbA.getCenterOfMassTransform() * frameInA;
    115     m_calculatedTransformB = rbB.getCenterOfMassTransform() * frameInB;
     122                info->m_numConstraintRows = 4; // Fixed 2 linear + 2 angular
     123                info->nub = 2;
     124                //prepare constraint
     125                calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
     126                testAngLimits();
     127                testLinLimits();
     128                if(getSolveLinLimit() || getPoweredLinMotor())
     129                {
     130                        info->m_numConstraintRows++; // limit 3rd linear as well
     131                        info->nub--;
     132                }
     133                if(getSolveAngLimit() || getPoweredAngMotor())
     134                {
     135                        info->m_numConstraintRows++; // limit 3rd angular as well
     136                        info->nub--;
     137                }
     138        }
     139}
     140
     141void btSliderConstraint::getInfo1NonVirtual(btConstraintInfo1* info)
     142{
     143
     144        info->m_numConstraintRows = 6; // Fixed 2 linear + 2 angular + 1 limit (even if not used)
     145        info->nub = 0;
     146}
     147
     148void btSliderConstraint::getInfo2(btConstraintInfo2* info)
     149{
     150        getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(), m_rbA.getLinearVelocity(),m_rbB.getLinearVelocity(), m_rbA.getInvMass(),m_rbB.getInvMass());
     151}
     152
     153
     154
     155
     156
     157
     158
     159void btSliderConstraint::calculateTransforms(const btTransform& transA,const btTransform& transB)
     160{
     161        if(m_useLinearReferenceFrameA || (!m_useSolveConstraintObsolete))
     162        {
     163                m_calculatedTransformA = transA * m_frameInA;
     164                m_calculatedTransformB = transB * m_frameInB;
     165        }
     166        else
     167        {
     168                m_calculatedTransformA = transB * m_frameInB;
     169                m_calculatedTransformB = transA * m_frameInA;
     170        }
    116171        m_realPivotAInW = m_calculatedTransformA.getOrigin();
    117172        m_realPivotBInW = m_calculatedTransformB.getOrigin();
    118173        m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X
    119         m_delta = m_realPivotBInW - m_realPivotAInW;
     174        if(m_useLinearReferenceFrameA || m_useSolveConstraintObsolete)
     175        {
     176                m_delta = m_realPivotBInW - m_realPivotAInW;
     177        }
     178        else
     179        {
     180                m_delta = m_realPivotAInW - m_realPivotBInW;
     181        }
    120182        m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis;
    121         m_relPosA = m_projPivotInW - rbA.getCenterOfMassPosition();
    122         m_relPosB = m_realPivotBInW - rbB.getCenterOfMassPosition();
    123183    btVector3 normalWorld;
    124184    int i;
    … …  
    127187    {
    128188                normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
    129                 new (&m_jacLin[i]) btJacobianEntry(
    130                         rbA.getCenterOfMassTransform().getBasis().transpose(),
    131                         rbB.getCenterOfMassTransform().getBasis().transpose(),
    132                         m_relPosA,
    133                         m_relPosB,
    134                         normalWorld,
    135                         rbA.getInvInertiaDiagLocal(),
    136                         rbA.getInvMass(),
    137                         rbB.getInvInertiaDiagLocal(),
    138                         rbB.getInvMass()
    139                         );
    140                 m_jacLinDiagABInv[i] = btScalar(1.) / m_jacLin[i].getDiagonal();
    141189                m_depth[i] = m_delta.dot(normalWorld);
    142190    }
    143         testLinLimits();
    144     // angular part
    145     for(i = 0; i < 3; i++)
    146     {
    147                 normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
    148                 new (&m_jacAng[i])      btJacobianEntry(
    149                         normalWorld,
    150             rbA.getCenterOfMassTransform().getBasis().transpose(),
    151             rbB.getCenterOfMassTransform().getBasis().transpose(),
    152             rbA.getInvInertiaDiagLocal(),
    153             rbB.getInvInertiaDiagLocal()
    154                         );
    155         }
    156         testAngLimits();
    157         btVector3 axisA = m_calculatedTransformA.getBasis().getColumn(0);
    158         m_kAngle = btScalar(1.0 )/ (rbA.computeAngularImpulseDenominator(axisA) + rbB.computeAngularImpulseDenominator(axisA));
    159         // clear accumulator for motors
    160         m_accumulatedLinMotorImpulse = btScalar(0.0);
    161         m_accumulatedAngMotorImpulse = btScalar(0.0);
    162 } // btSliderConstraint::buildJacobianInt()
    163 
    164 //-----------------------------------------------------------------------------
    165 
    166 void btSliderConstraint::getInfo1(btConstraintInfo1* info)
    167 {
    168         if (m_useSolveConstraintObsolete)
    169         {
    170                 info->m_numConstraintRows = 0;
    171                 info->nub = 0;
     191}
     192 
     193
     194
     195void btSliderConstraint::testLinLimits(void)
     196{
     197        m_solveLinLim = false;
     198        m_linPos = m_depth[0];
     199        if(m_lowerLinLimit <= m_upperLinLimit)
     200        {
     201                if(m_depth[0] > m_upperLinLimit)
     202                {
     203                        m_depth[0] -= m_upperLinLimit;
     204                        m_solveLinLim = true;
     205                }
     206                else if(m_depth[0] < m_lowerLinLimit)
     207                {
     208                        m_depth[0] -= m_lowerLinLimit;
     209                        m_solveLinLim = true;
     210                }
     211                else
     212                {
     213                        m_depth[0] = btScalar(0.);
     214                }
    172215        }
    173216        else
    174217        {
    175                 info->m_numConstraintRows = 4; // Fixed 2 linear + 2 angular
    176                 info->nub = 2;
    177                 //prepare constraint
    178                 calculateTransforms();
    179                 testLinLimits();
    180                 if(getSolveLinLimit() || getPoweredLinMotor())
    181                 {
    182                         info->m_numConstraintRows++; // limit 3rd linear as well
    183                         info->nub--;
    184                 }
    185                 testAngLimits();
    186                 if(getSolveAngLimit() || getPoweredAngMotor())
    187                 {
    188                         info->m_numConstraintRows++; // limit 3rd angular as well
    189                         info->nub--;
    190                 }
    191         }
    192 } // btSliderConstraint::getInfo1()
    193 
    194 //-----------------------------------------------------------------------------
    195 
    196 void btSliderConstraint::getInfo2(btConstraintInfo2* info)
    197 {
     218                m_depth[0] = btScalar(0.);
     219        }
     220}
     221
     222
     223
     224void btSliderConstraint::testAngLimits(void)
     225{
     226        m_angDepth = btScalar(0.);
     227        m_solveAngLim = false;
     228        if(m_lowerAngLimit <= m_upperAngLimit)
     229        {
     230                const btVector3 axisA0 = m_calculatedTransformA.getBasis().getColumn(1);
     231                const btVector3 axisA1 = m_calculatedTransformA.getBasis().getColumn(2);
     232                const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1);
     233//              btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0)); 
     234                btScalar rot = btAtan2(axisB0.dot(axisA1), axisB0.dot(axisA0)); 
     235                rot = btAdjustAngleToLimits(rot, m_lowerAngLimit, m_upperAngLimit);
     236                m_angPos = rot;
     237                if(rot < m_lowerAngLimit)
     238                {
     239                        m_angDepth = rot - m_lowerAngLimit;
     240                        m_solveAngLim = true;
     241                }
     242                else if(rot > m_upperAngLimit)
     243                {
     244                        m_angDepth = rot - m_upperAngLimit;
     245                        m_solveAngLim = true;
     246                }
     247        }
     248}
     249
     250btVector3 btSliderConstraint::getAncorInA(void)
     251{
     252        btVector3 ancorInA;
     253        ancorInA = m_realPivotAInW + (m_lowerLinLimit + m_upperLinLimit) * btScalar(0.5) * m_sliderAxis;
     254        ancorInA = m_rbA.getCenterOfMassTransform().inverse() * ancorInA;
     255        return ancorInA;
     256}
     257
     258
     259
     260btVector3 btSliderConstraint::getAncorInB(void)
     261{
     262        btVector3 ancorInB;
     263        ancorInB = m_frameInB.getOrigin();
     264        return ancorInB;
     265}
     266
     267
     268void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB, const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass  )
     269{
     270        const btTransform& trA = getCalculatedTransformA();
     271        const btTransform& trB = getCalculatedTransformB();
     272       
    198273        btAssert(!m_useSolveConstraintObsolete);
    199274        int i, s = info->rowskip;
    200         const btTransform& trA = getCalculatedTransformA();
    201         const btTransform& trB = getCalculatedTransformB();
     275       
    202276        btScalar signFact = m_useLinearReferenceFrameA ? btScalar(1.0f) : btScalar(-1.0f);
    203         // make rotations around Y and Z equal
     277       
     278        // difference between frames in WCS
     279        btVector3 ofs = trB.getOrigin() - trA.getOrigin();
     280        // now get weight factors depending on masses
     281        btScalar miA = rbAinvMass;
     282        btScalar miB = rbBinvMass;
     283        bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
     284        btScalar miS = miA + miB;
     285        btScalar factA, factB;
     286        if(miS > btScalar(0.f))
     287        {
     288                factA = miB / miS;
     289        }
     290        else
     291        {
     292                factA = btScalar(0.5f);
     293        }
     294        factB = btScalar(1.0f) - factA;
     295        btVector3 ax1, p, q;
     296        btVector3 ax1A = trA.getBasis().getColumn(0);
     297        btVector3 ax1B = trB.getBasis().getColumn(0);
     298        if(m_useOffsetForConstraintFrame)
     299        {
     300                // get the desired direction of slider axis
     301                // as weighted sum of X-orthos of frameA and frameB in WCS
     302                ax1 = ax1A * factA + ax1B * factB;
     303                ax1.normalize();
     304                // construct two orthos to slider axis
     305                btPlaneSpace1 (ax1, p, q);
     306        }
     307        else
     308        { // old way - use frameA
     309                ax1 = trA.getBasis().getColumn(0);
     310                // get 2 orthos to slider axis (Y, Z)
     311                p = trA.getBasis().getColumn(1);
     312                q = trA.getBasis().getColumn(2);
     313        }
     314        // make rotations around these orthos equal
    204315        // the slider axis should be the only unconstrained
    205316        // rotational axis, the angular velocity of the two bodies perpendicular to
    … …  
    209320        // where p and q are unit vectors normal to the slider axis, and w1 and w2
    210321        // are the angular velocity vectors of the two bodies.
    211         // get slider axis (X)
    212         btVector3 ax1 = trA.getBasis().getColumn(0);
    213         // get 2 orthos to slider axis (Y, Z)
    214         btVector3 p = trA.getBasis().getColumn(1);
    215         btVector3 q = trA.getBasis().getColumn(2);
    216         // set the two slider rows
    217322        info->m_J1angularAxis[0] = p[0];
    218323        info->m_J1angularAxis[1] = p[1];
    … …  
    230335        // compute the right hand side of the constraint equation. set relative
    231336        // body velocities along p and q to bring the slider back into alignment.
    232         // if ax1,ax2 are the unit length slider axes as computed from body1 and
    233         // body2, we need to rotate both bodies along the axis u = (ax1 x ax2).
     337        // if ax1A,ax1B are the unit length slider axes as computed from bodyA and
     338        // bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2).
    234339        // if "theta" is the angle between ax1 and ax2, we need an angular velocity
    235340        // along u to cover angle erp*theta in one step :
    … …  
    243348        // ax1 x ax2 is in the plane space of ax1, so we project the angular
    244349        // velocity to p and q to find the right hand side.
    245         btScalar k = info->fps * info->erp * getSoftnessOrthoAng();
    246     btVector3 ax2 = trB.getBasis().getColumn(0);
    247         btVector3 u = ax1.cross(ax2);
     350//      btScalar k = info->fps * info->erp * getSoftnessOrthoAng();
     351        btScalar currERP = (m_flags & BT_SLIDER_FLAGS_ERP_ORTANG) ? m_softnessOrthoAng : m_softnessOrthoAng * info->erp;
     352        btScalar k = info->fps * currERP;
     353
     354        btVector3 u = ax1A.cross(ax1B);
    248355        info->m_constraintError[0] = k * u.dot(p);
    249356        info->m_constraintError[s] = k * u.dot(q);
    250         // pull out pos and R for both bodies. also get the connection
    251         // vector c = pos2-pos1.
    252         // next two rows. we want: vel2 = vel1 + w1 x c ... but this would
    253         // result in three equations, so we project along the planespace vectors
    254         // so that sliding along the slider axis is disregarded. for symmetry we
    255         // also consider rotation around center of mass of two bodies (factA and factB).
    256         btTransform bodyA_trans = m_rbA.getCenterOfMassTransform();
    257         btTransform bodyB_trans = m_rbB.getCenterOfMassTransform();
    258         int s2 = 2 * s, s3 = 3 * s;
    259         btVector3 c;
    260         btScalar miA = m_rbA.getInvMass();
    261         btScalar miB = m_rbB.getInvMass();
    262         btScalar miS = miA + miB;
    263         btScalar factA, factB;
    264         if(miS > btScalar(0.f))
    265         {
    266                 factA = miB / miS;
    267         }
    268         else
    269         {
    270                 factA = btScalar(0.5f);
    271         }
    272         if(factA > 0.99f) factA = 0.99f;
    273         if(factA < 0.01f) factA = 0.01f;
    274         factB = btScalar(1.0f) - factA;
    275         c = bodyB_trans.getOrigin() - bodyA_trans.getOrigin();
    276         btVector3 tmp = c.cross(p);
    277         for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = factA*tmp[i];
    278         for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = factB*tmp[i];
    279         tmp = c.cross(q);
    280         for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = factA*tmp[i];
    281         for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = factB*tmp[i];
    282 
    283         for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i];
    284         for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i];
    285         // compute two elements of right hand side. we want to align the offset
    286         // point (in body 2's frame) with the center of body 1.
    287         btVector3 ofs; // offset point in global coordinates
    288         ofs = trB.getOrigin() - trA.getOrigin();
    289         k = info->fps * info->erp * getSoftnessOrthoLin();
    290         info->m_constraintError[s2] = k * p.dot(ofs);
    291         info->m_constraintError[s3] = k * q.dot(ofs);
    292         int nrow = 3; // last filled row
     357        if(m_flags & BT_SLIDER_FLAGS_CFM_ORTANG)
     358        {
     359                info->cfm[0] = m_cfmOrthoAng;
     360                info->cfm[s] = m_cfmOrthoAng;
     361        }
     362
     363        int nrow = 1; // last filled row
    293364        int srow;
    294         // check linear limits linear
    295         btScalar limit_err = btScalar(0.0);
    296         int limit = 0;
     365        btScalar limit_err;
     366        int limit;
     367        int powered;
     368
     369        // next two rows.
     370        // we want: velA + wA x relA == velB + wB x relB ... but this would
     371        // result in three equations, so we project along two orthos to the slider axis
     372
     373        btTransform bodyA_trans = transA;
     374        btTransform bodyB_trans = transB;
     375        nrow++;
     376        int s2 = nrow * s;
     377        nrow++;
     378        int s3 = nrow * s;
     379        btVector3 tmpA(0,0,0), tmpB(0,0,0), relA(0,0,0), relB(0,0,0), c(0,0,0);
     380        if(m_useOffsetForConstraintFrame)
     381        {
     382                // get vector from bodyB to frameB in WCS
     383                relB = trB.getOrigin() - bodyB_trans.getOrigin();
     384                // get its projection to slider axis
     385                btVector3 projB = ax1 * relB.dot(ax1);
     386                // get vector directed from bodyB to slider axis (and orthogonal to it)
     387                btVector3 orthoB = relB - projB;
     388                // same for bodyA
     389                relA = trA.getOrigin() - bodyA_trans.getOrigin();
     390                btVector3 projA = ax1 * relA.dot(ax1);
     391                btVector3 orthoA = relA - projA;
     392                // get desired offset between frames A and B along slider axis
     393                btScalar sliderOffs = m_linPos - m_depth[0];
     394                // desired vector from projection of center of bodyA to projection of center of bodyB to slider axis
     395                btVector3 totalDist = projA + ax1 * sliderOffs - projB;
     396                // get offset vectors relA and relB
     397                relA = orthoA + totalDist * factA;
     398                relB = orthoB - totalDist * factB;
     399                // now choose average ortho to slider axis
     400                p = orthoB * factA + orthoA * factB;
     401                btScalar len2 = p.length2();
     402                if(len2 > SIMD_EPSILON)
     403                {
     404                        p /= btSqrt(len2);
     405                }
     406                else
     407                {
     408                        p = trA.getBasis().getColumn(1);
     409                }
     410                // make one more ortho
     411                q = ax1.cross(p);
     412                // fill two rows
     413                tmpA = relA.cross(p);
     414                tmpB = relB.cross(p);
     415                for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i];
     416                for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i];
     417                tmpA = relA.cross(q);
     418                tmpB = relB.cross(q);
     419                if(hasStaticBody && getSolveAngLimit())
     420                { // to make constraint between static and dynamic objects more rigid
     421                        // remove wA (or wB) from equation if angular limit is hit
     422                        tmpB *= factB;
     423                        tmpA *= factA;
     424                }
     425                for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = tmpA[i];
     426                for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = -tmpB[i];
     427                for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i];
     428                for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i];
     429        }
     430        else
     431        {       // old way - maybe incorrect if bodies are not on the slider axis
     432                // see discussion "Bug in slider constraint" http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=9&t=4024&start=0
     433                c = bodyB_trans.getOrigin() - bodyA_trans.getOrigin();
     434                btVector3 tmp = c.cross(p);
     435                for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = factA*tmp[i];
     436                for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = factB*tmp[i];
     437                tmp = c.cross(q);
     438                for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = factA*tmp[i];
     439                for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = factB*tmp[i];
     440
     441                for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i];
     442                for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i];
     443        }
     444        // compute two elements of right hand side
     445
     446        //      k = info->fps * info->erp * getSoftnessOrthoLin();
     447        currERP = (m_flags & BT_SLIDER_FLAGS_ERP_ORTLIN) ? m_softnessOrthoLin : m_softnessOrthoLin * info->erp;
     448        k = info->fps * currERP;
     449
     450        btScalar rhs = k * p.dot(ofs);
     451        info->m_constraintError[s2] = rhs;
     452        rhs = k * q.dot(ofs);
     453        info->m_constraintError[s3] = rhs;
     454        if(m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN)
     455        {
     456                info->cfm[s2] = m_cfmOrthoLin;
     457                info->cfm[s3] = m_cfmOrthoLin;
     458        }
     459
     460
     461        // check linear limits
     462        limit_err = btScalar(0.0);
     463        limit = 0;
    297464        if(getSolveLinLimit())
    298465        {
    … …  
    300467                limit = (limit_err > btScalar(0.0)) ? 2 : 1;
    301468        }
    302         int powered = 0;
     469        powered = 0;
    303470        if(getPoweredLinMotor())
    304471        {
    … …  
    320487                // a torque couple will result in limited slider-jointed free
    321488                // bodies from gaining angular momentum.
    322                 // the solution used here is to apply the constraint forces at the center of mass of the two bodies
    323                 btVector3 ltd;  // Linear Torque Decoupling vector (a torque)
    324 //              c = btScalar(0.5) * c;
    325                 ltd = c.cross(ax1);
    326                 info->m_J1angularAxis[srow+0] = factA*ltd[0];
    327                 info->m_J1angularAxis[srow+1] = factA*ltd[1];
    328                 info->m_J1angularAxis[srow+2] = factA*ltd[2];
    329                 info->m_J2angularAxis[srow+0] = factB*ltd[0];
    330                 info->m_J2angularAxis[srow+1] = factB*ltd[1];
    331                 info->m_J2angularAxis[srow+2] = factB*ltd[2];
     489                if(m_useOffsetForConstraintFrame)
     490                {
     491                        // this is needed only when bodyA and bodyB are both dynamic.
     492                        if(!hasStaticBody)
     493                        {
     494                                tmpA = relA.cross(ax1);
     495                                tmpB = relB.cross(ax1);
     496                                info->m_J1angularAxis[srow+0] = tmpA[0];
     497                                info->m_J1angularAxis[srow+1] = tmpA[1];
     498                                info->m_J1angularAxis[srow+2] = tmpA[2];
     499                                info->m_J2angularAxis[srow+0] = -tmpB[0];
     500                                info->m_J2angularAxis[srow+1] = -tmpB[1];
     501                                info->m_J2angularAxis[srow+2] = -tmpB[2];
     502                        }
     503                }
     504                else
     505                { // The old way. May be incorrect if bodies are not on the slider axis
     506                        btVector3 ltd;  // Linear Torque Decoupling vector (a torque)
     507                        ltd = c.cross(ax1);
     508                        info->m_J1angularAxis[srow+0] = factA*ltd[0];
     509                        info->m_J1angularAxis[srow+1] = factA*ltd[1];
     510                        info->m_J1angularAxis[srow+2] = factA*ltd[2];
     511                        info->m_J2angularAxis[srow+0] = factB*ltd[0];
     512                        info->m_J2angularAxis[srow+1] = factB*ltd[1];
     513                        info->m_J2angularAxis[srow+2] = factB*ltd[2];
     514                }
    332515                // right-hand part
    333516                btScalar lostop = getLowerLinLimit();
    … …  
    340523                info->m_lowerLimit[srow] = 0.;
    341524                info->m_upperLimit[srow] = 0.;
     525                currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMLIN) ? m_softnessLimLin : info->erp;
    342526                if(powered)
    343527                {
    344             info->cfm[nrow] = btScalar(0.0);
     528                        if(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN)
     529                        {
     530                                info->cfm[srow] = m_cfmDirLin;
     531                        }
    345532                        btScalar tag_vel = getTargetLinMotorVelocity();
    346                         btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info->fps * info->erp);
    347 //                      info->m_constraintError[srow] += mot_fact * getTargetLinMotorVelocity();
     533                        btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info->fps * currERP);
    348534                        info->m_constraintError[srow] -= signFact * mot_fact * getTargetLinMotorVelocity();
    349535                        info->m_lowerLimit[srow] += -getMaxLinMotorForce() * info->fps;
    … …  
    352538                if(limit)
    353539                {
    354                         k = info->fps * info->erp;
     540                        k = info->fps * currERP;
    355541                        info->m_constraintError[srow] += k * limit_err;
    356                         info->cfm[srow] = btScalar(0.0); // stop_cfm;
     542                        if(m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN)
     543                        {
     544                                info->cfm[srow] = m_cfmLimLin;
     545                        }
    357546                        if(lostop == histop)
    358547                        {       // limited low and high simultaneously
    … …  
    374563                        if(bounce > btScalar(0.0))
    375564                        {
    376                                 btScalar vel = m_rbA.getLinearVelocity().dot(ax1);
    377                                 vel -= m_rbB.getLinearVelocity().dot(ax1);
     565                                btScalar vel = linVelA.dot(ax1);
     566                                vel -= linVelB.dot(ax1);
    378567                                vel *= signFact;
    379568                                // only apply bounce if the velocity is incoming, and if the
    … …  
    437626                        powered = 0;
    438627                }
     628                currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMANG) ? m_softnessLimAng : info->erp;
    439629                if(powered)
    440630                {
    441             info->cfm[srow] = btScalar(0.0);
    442                         btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * info->erp);
     631                        if(m_flags & BT_SLIDER_FLAGS_CFM_DIRANG)
     632                        {
     633                                info->cfm[srow] = m_cfmDirAng;
     634                        }
     635                        btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * currERP);
    443636                        info->m_constraintError[srow] = mot_fact * getTargetAngMotorVelocity();
    444637                        info->m_lowerLimit[srow] = -getMaxAngMotorForce() * info->fps;
    … …  
    447640                if(limit)
    448641                {
    449                         k = info->fps * info->erp;
     642                        k = info->fps * currERP;
    450643                        info->m_constraintError[srow] += k * limit_err;
    451                         info->cfm[srow] = btScalar(0.0); // stop_cfm;
     644                        if(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG)
     645                        {
     646                                info->cfm[srow] = m_cfmLimAng;
     647                        }
    452648                        if(lostop == histop)
    453649                        {
    … …  
    500696                } // if(limit)
    501697        } // if angular limit or powered
    502 } // btSliderConstraint::getInfo2()
    503 
    504 //-----------------------------------------------------------------------------
    505 
    506 void btSliderConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
    507 {
    508         if (m_useSolveConstraintObsolete)
    509         {
    510                 m_timeStep = timeStep;
    511                 if(m_useLinearReferenceFrameA)
    512                 {
    513                         solveConstraintInt(m_rbA,bodyA, m_rbB,bodyB);
     698}
     699
     700
     701///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
     702///If no axis is provided, it uses the default axis for this constraint.
     703void btSliderConstraint::setParam(int num, btScalar value, int axis)
     704{
     705        switch(num)
     706        {
     707        case BT_CONSTRAINT_STOP_ERP :
     708                if(axis < 1)
     709                {
     710                        m_softnessLimLin = value;
     711                        m_flags |= BT_SLIDER_FLAGS_ERP_LIMLIN;
     712                }
     713                else if(axis < 3)
     714                {
     715                        m_softnessOrthoLin = value;
     716                        m_flags |= BT_SLIDER_FLAGS_ERP_ORTLIN;
     717                }
     718                else if(axis == 3)
     719                {
     720                        m_softnessLimAng = value;
     721                        m_flags |= BT_SLIDER_FLAGS_ERP_LIMANG;
     722                }
     723                else if(axis < 6)
     724                {
     725                        m_softnessOrthoAng = value;
     726                        m_flags |= BT_SLIDER_FLAGS_ERP_ORTANG;
    514727                }
    515728                else
    516729                {
    517                         solveConstraintInt(m_rbB,bodyB, m_rbA,bodyA);
    518                 }
    519         }
    520 } // btSliderConstraint::solveConstraint()
    521 
    522 //-----------------------------------------------------------------------------
    523 
    524 void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB)
    525 {
    526     int i;
    527     // linear
    528     btVector3 velA;
    529         bodyA.getVelocityInLocalPointObsolete(m_relPosA,velA);
    530     btVector3 velB;
    531         bodyB.getVelocityInLocalPointObsolete(m_relPosB,velB);
    532     btVector3 vel = velA - velB;
    533         for(i = 0; i < 3; i++)
    534     {
    535                 const btVector3& normal = m_jacLin[i].m_linearJointAxis;
    536                 btScalar rel_vel = normal.dot(vel);
    537                 // calculate positional error
    538                 btScalar depth = m_depth[i];
    539                 // get parameters
    540                 btScalar softness = (i) ? m_softnessOrthoLin : (m_solveLinLim ? m_softnessLimLin : m_softnessDirLin);
    541                 btScalar restitution = (i) ? m_restitutionOrthoLin : (m_solveLinLim ? m_restitutionLimLin : m_restitutionDirLin);
    542                 btScalar damping = (i) ? m_dampingOrthoLin : (m_solveLinLim ? m_dampingLimLin : m_dampingDirLin);
    543                 // calcutate and apply impulse
    544                 btScalar normalImpulse = softness * (restitution * depth / m_timeStep - damping * rel_vel) * m_jacLinDiagABInv[i];
    545                 btVector3 impulse_vector = normal * normalImpulse;
    546                
    547                 //rbA.applyImpulse( impulse_vector, m_relPosA);
    548                 //rbB.applyImpulse(-impulse_vector, m_relPosB);
    549                 {
    550                         btVector3 ftorqueAxis1 = m_relPosA.cross(normal);
    551                         btVector3 ftorqueAxis2 = m_relPosB.cross(normal);
    552                         bodyA.applyImpulse(normal*rbA.getInvMass(), rbA.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse);
    553                         bodyB.applyImpulse(normal*rbB.getInvMass(), rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse);
    554                 }
    555 
    556 
    557 
    558                 if(m_poweredLinMotor && (!i))
    559                 { // apply linear motor
    560                         if(m_accumulatedLinMotorImpulse < m_maxLinMotorForce)
    561                         {
    562                                 btScalar desiredMotorVel = m_targetLinMotorVelocity;
    563                                 btScalar motor_relvel = desiredMotorVel + rel_vel;
    564                                 normalImpulse = -motor_relvel * m_jacLinDiagABInv[i];
    565                                 // clamp accumulated impulse
    566                                 btScalar new_acc = m_accumulatedLinMotorImpulse + btFabs(normalImpulse);
    567                                 if(new_acc  > m_maxLinMotorForce)
    568                                 {
    569                                         new_acc = m_maxLinMotorForce;
    570                                 }
    571                                 btScalar del = new_acc  - m_accumulatedLinMotorImpulse;
    572                                 if(normalImpulse < btScalar(0.0))
    573                                 {
    574                                         normalImpulse = -del;
    575                                 }
    576                                 else
    577                                 {
    578                                         normalImpulse = del;
    579                                 }
    580                                 m_accumulatedLinMotorImpulse = new_acc;
    581                                 // apply clamped impulse
    582                                 impulse_vector = normal * normalImpulse;
    583                                 //rbA.applyImpulse( impulse_vector, m_relPosA);
    584                                 //rbB.applyImpulse(-impulse_vector, m_relPosB);
    585 
    586                                 {
    587                                         btVector3 ftorqueAxis1 = m_relPosA.cross(normal);
    588                                         btVector3 ftorqueAxis2 = m_relPosB.cross(normal);
    589                                         bodyA.applyImpulse(normal*rbA.getInvMass(), rbA.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse);
    590                                         bodyB.applyImpulse(normal*rbB.getInvMass(), rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse);
    591                                 }
    592 
    593 
    594 
    595                         }
    596                 }
    597     }
    598         // angular
    599         // get axes in world space
    600         btVector3 axisA =  m_calculatedTransformA.getBasis().getColumn(0);
    601         btVector3 axisB =  m_calculatedTransformB.getBasis().getColumn(0);
    602 
    603         btVector3 angVelA;
    604         bodyA.getAngularVelocity(angVelA);
    605         btVector3 angVelB;
    606         bodyB.getAngularVelocity(angVelB);
    607 
    608         btVector3 angVelAroundAxisA = axisA * axisA.dot(angVelA);
    609         btVector3 angVelAroundAxisB = axisB * axisB.dot(angVelB);
    610 
    611         btVector3 angAorthog = angVelA - angVelAroundAxisA;
    612         btVector3 angBorthog = angVelB - angVelAroundAxisB;
    613         btVector3 velrelOrthog = angAorthog-angBorthog;
    614         //solve orthogonal angular velocity correction
    615         btScalar len = velrelOrthog.length();
    616         btScalar orthorImpulseMag = 0.f;
    617 
    618         if (len > btScalar(0.00001))
    619         {
    620                 btVector3 normal = velrelOrthog.normalized();
    621                 btScalar denom = rbA.computeAngularImpulseDenominator(normal) + rbB.computeAngularImpulseDenominator(normal);
    622                 //velrelOrthog *= (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng;
    623                 orthorImpulseMag = (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng;
    624         }
    625         //solve angular positional correction
    626         btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/m_timeStep);
    627         btVector3 angularAxis = angularError;
    628         btScalar angularImpulseMag = 0;
    629 
    630         btScalar len2 = angularError.length();
    631         if (len2>btScalar(0.00001))
    632         {
    633                 btVector3 normal2 = angularError.normalized();
    634                 btScalar denom2 = rbA.computeAngularImpulseDenominator(normal2) + rbB.computeAngularImpulseDenominator(normal2);
    635                 angularImpulseMag = (btScalar(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng;
    636                 angularError *= angularImpulseMag;
    637         }
    638         // apply impulse
    639         //rbA.applyTorqueImpulse(-velrelOrthog+angularError);
    640         //rbB.applyTorqueImpulse(velrelOrthog-angularError);
    641 
    642         bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*velrelOrthog,-orthorImpulseMag);
    643         bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*velrelOrthog,orthorImpulseMag);
    644         bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*angularAxis,angularImpulseMag);
    645         bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*angularAxis,-angularImpulseMag);
    646 
    647 
    648         btScalar impulseMag;
    649         //solve angular limits
    650         if(m_solveAngLim)
    651         {
    652                 impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingLimAng + m_angDepth * m_restitutionLimAng / m_timeStep;
    653                 impulseMag *= m_kAngle * m_softnessLimAng;
    654         }
    655         else
    656         {
    657                 impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingDirAng + m_angDepth * m_restitutionDirAng / m_timeStep;
    658                 impulseMag *= m_kAngle * m_softnessDirAng;
    659         }
    660         btVector3 impulse = axisA * impulseMag;
    661         //rbA.applyTorqueImpulse(impulse);
    662         //rbB.applyTorqueImpulse(-impulse);
    663 
    664         bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*axisA,impulseMag);
    665         bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*axisA,-impulseMag);
    666 
    667 
    668 
    669         //apply angular motor
    670         if(m_poweredAngMotor)
    671         {
    672                 if(m_accumulatedAngMotorImpulse < m_maxAngMotorForce)
    673                 {
    674                         btVector3 velrel = angVelAroundAxisA - angVelAroundAxisB;
    675                         btScalar projRelVel = velrel.dot(axisA);
    676 
    677                         btScalar desiredMotorVel = m_targetAngMotorVelocity;
    678                         btScalar motor_relvel = desiredMotorVel - projRelVel;
    679 
    680                         btScalar angImpulse = m_kAngle * motor_relvel;
    681                         // clamp accumulated impulse
    682                         btScalar new_acc = m_accumulatedAngMotorImpulse + btFabs(angImpulse);
    683                         if(new_acc  > m_maxAngMotorForce)
    684                         {
    685                                 new_acc = m_maxAngMotorForce;
    686                         }
    687                         btScalar del = new_acc  - m_accumulatedAngMotorImpulse;
    688                         if(angImpulse < btScalar(0.0))
    689                         {
    690                                 angImpulse = -del;
    691                         }
    692                         else
    693                         {
    694                                 angImpulse = del;
    695                         }
    696                         m_accumulatedAngMotorImpulse = new_acc;
    697                         // apply clamped impulse
    698                         btVector3 motorImp = angImpulse * axisA;
    699                         //rbA.applyTorqueImpulse(motorImp);
    700                         //rbB.applyTorqueImpulse(-motorImp);
    701 
    702                         bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*axisA,angImpulse);
    703                         bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*axisA,-angImpulse);
    704                 }
    705         }
    706 } // btSliderConstraint::solveConstraint()
    707 
    708 //-----------------------------------------------------------------------------
    709 
    710 //-----------------------------------------------------------------------------
    711 
    712 void btSliderConstraint::calculateTransforms(void){
    713         if(m_useLinearReferenceFrameA || (!m_useSolveConstraintObsolete))
    714         {
    715                 m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA;
    716                 m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB;
    717         }
    718         else
    719         {
    720                 m_calculatedTransformA = m_rbB.getCenterOfMassTransform() * m_frameInB;
    721                 m_calculatedTransformB = m_rbA.getCenterOfMassTransform() * m_frameInA;
    722         }
    723         m_realPivotAInW = m_calculatedTransformA.getOrigin();
    724         m_realPivotBInW = m_calculatedTransformB.getOrigin();
    725         m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X
    726         if(m_useLinearReferenceFrameA || m_useSolveConstraintObsolete)
    727         {
    728                 m_delta = m_realPivotBInW - m_realPivotAInW;
    729         }
    730         else
    731         {
    732                 m_delta = m_realPivotAInW - m_realPivotBInW;
    733         }
    734         m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis;
    735     btVector3 normalWorld;
    736     int i;
    737     //linear part
    738     for(i = 0; i < 3; i++)
    739     {
    740                 normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
    741                 m_depth[i] = m_delta.dot(normalWorld);
    742     }
    743 } // btSliderConstraint::calculateTransforms()
    744  
    745 //-----------------------------------------------------------------------------
    746 
    747 void btSliderConstraint::testLinLimits(void)
    748 {
    749         m_solveLinLim = false;
    750         m_linPos = m_depth[0];
    751         if(m_lowerLinLimit <= m_upperLinLimit)
    752         {
    753                 if(m_depth[0] > m_upperLinLimit)
    754                 {
    755                         m_depth[0] -= m_upperLinLimit;
    756                         m_solveLinLim = true;
    757                 }
    758                 else if(m_depth[0] < m_lowerLinLimit)
    759                 {
    760                         m_depth[0] -= m_lowerLinLimit;
    761                         m_solveLinLim = true;
     730                        btAssertConstrParams(0);
     731                }
     732                break;
     733        case BT_CONSTRAINT_CFM :
     734                if(axis < 1)
     735                {
     736                        m_cfmDirLin = value;
     737                        m_flags |= BT_SLIDER_FLAGS_CFM_DIRLIN;
     738                }
     739                else if(axis == 3)
     740                {
     741                        m_cfmDirAng = value;
     742                        m_flags |= BT_SLIDER_FLAGS_CFM_DIRANG;
    762743                }
    763744                else
    764745                {
    765                         m_depth[0] = btScalar(0.);
    766                 }
    767         }
    768         else
    769         {
    770                 m_depth[0] = btScalar(0.);
    771         }
    772 } // btSliderConstraint::testLinLimits()
    773 
    774 //-----------------------------------------------------------------------------
    775 
    776 void btSliderConstraint::testAngLimits(void)
    777 {
    778         m_angDepth = btScalar(0.);
    779         m_solveAngLim = false;
    780         if(m_lowerAngLimit <= m_upperAngLimit)
    781         {
    782                 const btVector3 axisA0 = m_calculatedTransformA.getBasis().getColumn(1);
    783                 const btVector3 axisA1 = m_calculatedTransformA.getBasis().getColumn(2);
    784                 const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1);
    785                 btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0)); 
    786                 m_angPos = rot;
    787                 if(rot < m_lowerAngLimit)
    788                 {
    789                         m_angDepth = rot - m_lowerAngLimit;
    790                         m_solveAngLim = true;
    791                 }
    792                 else if(rot > m_upperAngLimit)
    793                 {
    794                         m_angDepth = rot - m_upperAngLimit;
    795                         m_solveAngLim = true;
    796                 }
    797         }
    798 } // btSliderConstraint::testAngLimits()
    799        
    800 //-----------------------------------------------------------------------------
    801 
    802 btVector3 btSliderConstraint::getAncorInA(void)
    803 {
    804         btVector3 ancorInA;
    805         ancorInA = m_realPivotAInW + (m_lowerLinLimit + m_upperLinLimit) * btScalar(0.5) * m_sliderAxis;
    806         ancorInA = m_rbA.getCenterOfMassTransform().inverse() * ancorInA;
    807         return ancorInA;
    808 } // btSliderConstraint::getAncorInA()
    809 
    810 //-----------------------------------------------------------------------------
    811 
    812 btVector3 btSliderConstraint::getAncorInB(void)
    813 {
    814         btVector3 ancorInB;
    815         ancorInB = m_frameInB.getOrigin();
    816         return ancorInB;
    817 } // btSliderConstraint::getAncorInB();
     746                        btAssertConstrParams(0);
     747                }
     748                break;
     749        case BT_CONSTRAINT_STOP_CFM :
     750                if(axis < 1)
     751                {
     752                        m_cfmLimLin = value;
     753                        m_flags |= BT_SLIDER_FLAGS_CFM_LIMLIN;
     754                }
     755                else if(axis < 3)
     756                {
     757                        m_cfmOrthoLin = value;
     758                        m_flags |= BT_SLIDER_FLAGS_CFM_ORTLIN;
     759                }
     760                else if(axis == 3)
     761                {
     762                        m_cfmLimAng = value;
     763                        m_flags |= BT_SLIDER_FLAGS_CFM_LIMANG;
     764                }
     765                else if(axis < 6)
     766                {
     767                        m_cfmOrthoAng = value;
     768                        m_flags |= BT_SLIDER_FLAGS_CFM_ORTANG;
     769                }
     770                else
     771                {
     772                        btAssertConstrParams(0);
     773                }
     774                break;
     775        }
     776}
     777
     778///return the local value of parameter
     779btScalar btSliderConstraint::getParam(int num, int axis) const
     780{
     781        btScalar retVal(SIMD_INFINITY);
     782        switch(num)
     783        {
     784        case BT_CONSTRAINT_STOP_ERP :
     785                if(axis < 1)
     786                {
     787                        btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_LIMLIN);
     788                        retVal = m_softnessLimLin;
     789                }
     790                else if(axis < 3)
     791                {
     792                        btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_ORTLIN);
     793                        retVal = m_softnessOrthoLin;
     794                }
     795                else if(axis == 3)
     796                {
     797                        btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_LIMANG);
     798                        retVal = m_softnessLimAng;
     799                }
     800                else if(axis < 6)
     801                {
     802                        btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_ORTANG);
     803                        retVal = m_softnessOrthoAng;
     804                }
     805                else
     806                {
     807                        btAssertConstrParams(0);
     808                }
     809                break;
     810        case BT_CONSTRAINT_CFM :
     811                if(axis < 1)
     812                {
     813                        btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN);
     814                        retVal = m_cfmDirLin;
     815                }
     816                else if(axis == 3)
     817                {
     818                        btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_DIRANG);
     819                        retVal = m_cfmDirAng;
     820                }
     821                else
     822                {
     823                        btAssertConstrParams(0);
     824                }
     825                break;
     826        case BT_CONSTRAINT_STOP_CFM :
     827                if(axis < 1)
     828                {
     829                        btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN);
     830                        retVal = m_cfmLimLin;
     831                }
     832                else if(axis < 3)
     833                {
     834                        btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN);
     835                        retVal = m_cfmOrthoLin;
     836                }
     837                else if(axis == 3)
     838                {
     839                        btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG);
     840                        retVal = m_cfmLimAng;
     841                }
     842                else if(axis < 6)
     843                {
     844                        btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_ORTANG);
     845                        retVal = m_cfmOrthoAng;
     846                }
     847                else
     848                {
     849                        btAssertConstrParams(0);
     850                }
     851                break;
     852        }
     853        return retVal;
     854}
     855
     856
     857
Note: See TracChangeset for help on using the changeset viewer.