Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

Ignore:
Timestamp:
Apr 28, 2011, 7:15:14 AM (13 years ago)
Author:
rgrieder
Message:

Merged kicklib2 branch back to trunk (includes former branches ois_update, mac_osx and kicklib).

Notes for updating

Linux:
You don't need an extra package for CEGUILua and Tolua, it's already shipped with CEGUI.
However you do need to make sure that the OgreRenderer is installed too with CEGUI 0.7 (may be a separate package).
Also, Orxonox now recognises if you install the CgProgramManager (a separate package available on newer Ubuntu on Debian systems).

Windows:
Download the new dependency packages versioned 6.0 and use these. If you have problems with that or if you don't like the in game console problem mentioned below, you can download the new 4.3 version of the packages (only available for Visual Studio 2005/2008).

Key new features:

  • *Support for Mac OS X*
  • Visual Studio 2010 support
  • Bullet library update to 2.77
  • OIS library update to 1.3
  • Support for CEGUI 0.7 —> Support for Arch Linux and even SuSE
  • Improved install target
  • Compiles now with GCC 4.6
  • Ogre Cg Shader plugin activated for Linux if available
  • And of course lots of bug fixes

There are also some regressions:

  • No support for CEGUI 0.5, Ogre 1.4 and boost 1.35 - 1.39 any more
  • In game console is not working in main menu for CEGUI 0.7
  • Tolua (just the C lib, not the application) and CEGUILua libraries are no longer in our repository. —> You will need to get these as well when compiling Orxonox
  • And of course lots of new bugs we don't yet know about
File:
1 edited

Legend:

Unmodified
Added
Removed

  • code/trunk/src/external/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp

    r5781 r8351  
    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.