Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

Ignore:
Timestamp:
Apr 8, 2009, 12:58:47 AM (16 years ago)
Author:
dafrick
Message:

Reverted to revision 2906 (because I'm too stupid to merge correctly, 2nd try will follow shortly. ;))

Location:
code/branches/questsystem5
Files:
19 edited

Legend:

Unmodified
Added
Removed

  • code/branches/questsystem5

  • code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp

    r2907 r2908  
    2323#include <new>
    2424
    25 //-----------------------------------------------------------------------------
    26 
    27 #define CONETWIST_USE_OBSOLETE_SOLVER false
    28 #define CONETWIST_DEF_FIX_THRESH btScalar(.05f)
    29 
    30 //-----------------------------------------------------------------------------
    31 
    3225btConeTwistConstraint::btConeTwistConstraint()
    33 :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE),
    34 m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
     26:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE)
    3527{
    3628}
    … …  
    4032                                                                                         const btTransform& rbAFrame,const btTransform& rbBFrame)
    4133                                                                                         :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
    42                                                                                          m_angularOnly(false),
    43                                                                                          m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
    44 {
    45         init();
     34                                                                                         m_angularOnly(false)
     35{
     36        m_swingSpan1 = btScalar(1e30);
     37        m_swingSpan2 = btScalar(1e30);
     38        m_twistSpan  = btScalar(1e30);
     39        m_biasFactor = 0.3f;
     40        m_relaxationFactor = 1.0f;
     41
     42        m_solveTwistLimit = false;
     43        m_solveSwingLimit = false;
     44
    4645}
    4746
    4847btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame)
    4948                                                                                        :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE,rbA),m_rbAFrame(rbAFrame),
    50                                                                                          m_angularOnly(false),
    51                                                                                          m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
     49                                                                                         m_angularOnly(false)
    5250{
    5351        m_rbBFrame = m_rbAFrame;
    54         init();
    55 }
    56 
    57 
    58 void btConeTwistConstraint::init()
    59 {
    60         m_angularOnly = false;
     52       
     53        m_swingSpan1 = btScalar(1e30);
     54        m_swingSpan2 = btScalar(1e30);
     55        m_twistSpan  = btScalar(1e30);
     56        m_biasFactor = 0.3f;
     57        m_relaxationFactor = 1.0f;
     58
    6159        m_solveTwistLimit = false;
    6260        m_solveSwingLimit = false;
    63         m_bMotorEnabled = false;
    64         m_maxMotorImpulse = btScalar(-1);
    65 
    66         setLimit(btScalar(1e30), btScalar(1e30), btScalar(1e30));
    67         m_damping = btScalar(0.01);
    68         m_fixThresh = CONETWIST_DEF_FIX_THRESH;
    69 }
    70 
    71 
    72 //-----------------------------------------------------------------------------
    73 
    74 void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info)
    75 {
    76         if (m_useSolveConstraintObsolete)
    77         {
    78                 info->m_numConstraintRows = 0;
    79                 info->nub = 0;
    80         }
    81         else
    82         {
    83                 info->m_numConstraintRows = 3;
    84                 info->nub = 3;
    85                 calcAngleInfo2();
    86                 if(m_solveSwingLimit)
    87                 {
    88                         info->m_numConstraintRows++;
    89                         info->nub--;
    90                         if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
    91                         {
    92                                 info->m_numConstraintRows++;
    93                                 info->nub--;
    94                         }
    95                 }
    96                 if(m_solveTwistLimit)
    97                 {
    98                         info->m_numConstraintRows++;
    99                         info->nub--;
    100                 }
    101         }
    102 } // btConeTwistConstraint::getInfo1()
    103        
    104 //-----------------------------------------------------------------------------
    105 
    106 void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info)
    107 {
    108         btAssert(!m_useSolveConstraintObsolete);
    109         //retrieve matrices
    110         btTransform body0_trans;
    111         body0_trans = m_rbA.getCenterOfMassTransform();
    112     btTransform body1_trans;
    113         body1_trans = m_rbB.getCenterOfMassTransform();
    114     // set jacobian
    115     info->m_J1linearAxis[0] = 1;
    116     info->m_J1linearAxis[info->rowskip+1] = 1;
    117     info->m_J1linearAxis[2*info->rowskip+2] = 1;
    118         btVector3 a1 = body0_trans.getBasis() * m_rbAFrame.getOrigin();
    119         {
    120                 btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
    121                 btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
    122                 btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip);
    123                 btVector3 a1neg = -a1;
    124                 a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
    125         }
    126         btVector3 a2 = body1_trans.getBasis() * m_rbBFrame.getOrigin();
    127         {
    128                 btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
    129                 btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
    130                 btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip);
    131                 a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
    132         }
    133     // set right hand side
    134     btScalar k = info->fps * info->erp;
    135     int j;
    136         for (j=0; j<3; j++)
    137     {
    138         info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
    139                 info->m_lowerLimit[j*info->rowskip] = -SIMD_INFINITY;
    140                 info->m_upperLimit[j*info->rowskip] = SIMD_INFINITY;
    141     }
    142         int row = 3;
    143     int srow = row * info->rowskip;
    144         btVector3 ax1;
    145         // angular limits
    146         if(m_solveSwingLimit)
    147         {
    148                 btScalar *J1 = info->m_J1angularAxis;
    149                 btScalar *J2 = info->m_J2angularAxis;
    150                 if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
    151                 {
    152                         btTransform trA = m_rbA.getCenterOfMassTransform()*m_rbAFrame;
    153                         btVector3 p = trA.getBasis().getColumn(1);
    154                         btVector3 q = trA.getBasis().getColumn(2);
    155                         int srow1 = srow + info->rowskip;
    156                         J1[srow+0] = p[0];
    157                         J1[srow+1] = p[1];
    158                         J1[srow+2] = p[2];
    159                         J1[srow1+0] = q[0];
    160                         J1[srow1+1] = q[1];
    161                         J1[srow1+2] = q[2];
    162                         J2[srow+0] = -p[0];
    163                         J2[srow+1] = -p[1];
    164                         J2[srow+2] = -p[2];
    165                         J2[srow1+0] = -q[0];
    166                         J2[srow1+1] = -q[1];
    167                         J2[srow1+2] = -q[2];
    168                         btScalar fact = info->fps * m_relaxationFactor;
    169                         info->m_constraintError[srow] =   fact * m_swingAxis.dot(p);
    170                         info->m_constraintError[srow1] =  fact * m_swingAxis.dot(q);
    171                         info->m_lowerLimit[srow] = -SIMD_INFINITY;
    172                         info->m_upperLimit[srow] = SIMD_INFINITY;
    173                         info->m_lowerLimit[srow1] = -SIMD_INFINITY;
    174                         info->m_upperLimit[srow1] = SIMD_INFINITY;
    175                         srow = srow1 + info->rowskip;
     61       
     62}                       
     63
     64void    btConeTwistConstraint::buildJacobian()
     65{
     66        m_appliedImpulse = btScalar(0.);
     67
     68        //set bias, sign, clear accumulator
     69        m_swingCorrection = btScalar(0.);
     70        m_twistLimitSign = btScalar(0.);
     71        m_solveTwistLimit = false;
     72        m_solveSwingLimit = false;
     73        m_accTwistLimitImpulse = btScalar(0.);
     74        m_accSwingLimitImpulse = btScalar(0.);
     75
     76        if (!m_angularOnly)
     77        {
     78                btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
     79                btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
     80                btVector3 relPos = pivotBInW - pivotAInW;
     81
     82                btVector3 normal[3];
     83                if (relPos.length2() > SIMD_EPSILON)
     84                {
     85                        normal[0] = relPos.normalized();
    17686                }
    17787                else
    17888                {
    179                         ax1 = m_swingAxis * m_relaxationFactor * m_relaxationFactor;
    180                         J1[srow+0] = ax1[0];
    181                         J1[srow+1] = ax1[1];
    182                         J1[srow+2] = ax1[2];
    183                         J2[srow+0] = -ax1[0];
    184                         J2[srow+1] = -ax1[1];
    185                         J2[srow+2] = -ax1[2];
    186                         btScalar k = info->fps * m_biasFactor;
    187 
    188                         info->m_constraintError[srow] = k * m_swingCorrection;
    189                         info->cfm[srow] = 0.0f;
    190                         // m_swingCorrection is always positive or 0
    191                         info->m_lowerLimit[srow] = 0;
    192                         info->m_upperLimit[srow] = SIMD_INFINITY;
    193                         srow += info->rowskip;
    194                 }
    195         }
    196         if(m_solveTwistLimit)
    197         {
    198                 ax1 = m_twistAxis * m_relaxationFactor * m_relaxationFactor;
    199                 btScalar *J1 = info->m_J1angularAxis;
    200                 btScalar *J2 = info->m_J2angularAxis;
    201                 J1[srow+0] = ax1[0];
    202                 J1[srow+1] = ax1[1];
    203                 J1[srow+2] = ax1[2];
    204                 J2[srow+0] = -ax1[0];
    205                 J2[srow+1] = -ax1[1];
    206                 J2[srow+2] = -ax1[2];
    207                 btScalar k = info->fps * m_biasFactor;
    208                 info->m_constraintError[srow] = k * m_twistCorrection;
    209                 info->cfm[srow] = 0.0f;
    210                 if(m_twistSpan > 0.0f)
    211                 {
    212 
    213                         if(m_twistCorrection > 0.0f)
    214                         {
    215                                 info->m_lowerLimit[srow] = 0;
    216                                 info->m_upperLimit[srow] = SIMD_INFINITY;
    217                         }
    218                         else
    219                         {
    220                                 info->m_lowerLimit[srow] = -SIMD_INFINITY;
    221                                 info->m_upperLimit[srow] = 0;
    222                         }
    223                 }
    224                 else
    225                 {
    226                         info->m_lowerLimit[srow] = -SIMD_INFINITY;
    227                         info->m_upperLimit[srow] = SIMD_INFINITY;
    228                 }
    229                 srow += info->rowskip;
    230         }
    231 }
    232        
    233 //-----------------------------------------------------------------------------
    234 
    235 void    btConeTwistConstraint::buildJacobian()
    236 {
    237         if (m_useSolveConstraintObsolete)
    238         {
    239                 m_appliedImpulse = btScalar(0.);
    240                 m_accTwistLimitImpulse = btScalar(0.);
    241                 m_accSwingLimitImpulse = btScalar(0.);
    242 
    243                 if (!m_angularOnly)
    244                 {
    245                         btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
    246                         btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
    247                         btVector3 relPos = pivotBInW - pivotAInW;
    248 
    249                         btVector3 normal[3];
    250                         if (relPos.length2() > SIMD_EPSILON)
    251                         {
    252                                 normal[0] = relPos.normalized();
    253                         }
    254                         else
    255                         {
    256                                 normal[0].setValue(btScalar(1.0),0,0);
    257                         }
    258 
    259                         btPlaneSpace1(normal[0], normal[1], normal[2]);
    260 
    261                         for (int i=0;i<3;i++)
    262                         {
    263                                 new (&m_jac[i]) btJacobianEntry(
     89                        normal[0].setValue(btScalar(1.0),0,0);
     90                }
     91
     92                btPlaneSpace1(normal[0], normal[1], normal[2]);
     93
     94                for (int i=0;i<3;i++)
     95                {
     96                        new (&m_jac[i]) btJacobianEntry(
    26497                                m_rbA.getCenterOfMassTransform().getBasis().transpose(),
    26598                                m_rbB.getCenterOfMassTransform().getBasis().transpose(),
    … …  
    271104                                m_rbB.getInvInertiaDiagLocal(),
    272105                                m_rbB.getInvMass());
    273                         }
    274                 }
    275 
    276                 calcAngleInfo2();
    277         }
    278 }
    279 
    280 //-----------------------------------------------------------------------------
    281 
    282 void    btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
    283 {
    284         if (m_useSolveConstraintObsolete)
    285         {
    286                 btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
    287                 btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
    288 
    289                 btScalar tau = btScalar(0.3);
    290 
    291                 //linear part
    292                 if (!m_angularOnly)
    293                 {
    294                         btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
    295                         btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
    296 
    297                         btVector3 vel1;
    298                         bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
    299                         btVector3 vel2;
    300                         bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
    301                         btVector3 vel = vel1 - vel2;
    302 
    303                         for (int i=0;i<3;i++)
    304                         {               
    305                                 const btVector3& normal = m_jac[i].m_linearJointAxis;
    306                                 btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
    307 
    308                                 btScalar rel_vel;
    309                                 rel_vel = normal.dot(vel);
    310                                 //positional error (zeroth order error)
    311                                 btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
    312                                 btScalar impulse = depth*tau/timeStep  * jacDiagABInv -  rel_vel * jacDiagABInv;
    313                                 m_appliedImpulse += impulse;
    314                                
    315                                 btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
    316                                 btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
    317                                 bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse);
    318                                 bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse);
    319                
    320                         }
    321                 }
    322 
    323                 // apply motor
    324                 if (m_bMotorEnabled)
    325                 {
    326                         // compute current and predicted transforms
    327                         btTransform trACur = m_rbA.getCenterOfMassTransform();
    328                         btTransform trBCur = m_rbB.getCenterOfMassTransform();
    329                         btVector3 omegaA; bodyA.getAngularVelocity(omegaA);
    330                         btVector3 omegaB; bodyB.getAngularVelocity(omegaB);
    331                         btTransform trAPred; trAPred.setIdentity();
    332                         btVector3 zerovec(0,0,0);
    333                         btTransformUtil::integrateTransform(
    334                                 trACur, zerovec, omegaA, timeStep, trAPred);
    335                         btTransform trBPred; trBPred.setIdentity();
    336                         btTransformUtil::integrateTransform(
    337                                 trBCur, zerovec, omegaB, timeStep, trBPred);
    338 
    339                         // compute desired transforms in world
    340                         btTransform trPose(m_qTarget);
    341                         btTransform trABDes = m_rbBFrame * trPose * m_rbAFrame.inverse();
    342                         btTransform trADes = trBPred * trABDes;
    343                         btTransform trBDes = trAPred * trABDes.inverse();
    344 
    345                         // compute desired omegas in world
    346                         btVector3 omegaADes, omegaBDes;
    347                        
    348                         btTransformUtil::calculateVelocity(trACur, trADes, timeStep, zerovec, omegaADes);
    349                         btTransformUtil::calculateVelocity(trBCur, trBDes, timeStep, zerovec, omegaBDes);
    350 
    351                         // compute delta omegas
    352                         btVector3 dOmegaA = omegaADes - omegaA;
    353                         btVector3 dOmegaB = omegaBDes - omegaB;
    354 
    355                         // compute weighted avg axis of dOmega (weighting based on inertias)
    356                         btVector3 axisA, axisB;
    357                         btScalar kAxisAInv = 0, kAxisBInv = 0;
    358 
    359                         if (dOmegaA.length2() > SIMD_EPSILON)
    360                         {
    361                                 axisA = dOmegaA.normalized();
    362                                 kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(axisA);
    363                         }
    364 
    365                         if (dOmegaB.length2() > SIMD_EPSILON)
    366                         {
    367                                 axisB = dOmegaB.normalized();
    368                                 kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(axisB);
    369                         }
    370 
    371                         btVector3 avgAxis = kAxisAInv * axisA + kAxisBInv * axisB;
    372 
    373                         static bool bDoTorque = true;
    374                         if (bDoTorque && avgAxis.length2() > SIMD_EPSILON)
    375                         {
    376                                 avgAxis.normalize();
    377                                 kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(avgAxis);
    378                                 kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(avgAxis);
    379                                 btScalar kInvCombined = kAxisAInv + kAxisBInv;
    380 
    381                                 btVector3 impulse = (kAxisAInv * dOmegaA - kAxisBInv * dOmegaB) /
    382                                                                         (kInvCombined * kInvCombined);
    383 
    384                                 if (m_maxMotorImpulse >= 0)
    385                                 {
    386                                         btScalar fMaxImpulse = m_maxMotorImpulse;
    387                                         if (m_bNormalizedMotorStrength)
    388                                                 fMaxImpulse = fMaxImpulse/kAxisAInv;
    389 
    390                                         btVector3 newUnclampedAccImpulse = m_accMotorImpulse + impulse;
    391                                         btScalar  newUnclampedMag = newUnclampedAccImpulse.length();
    392                                         if (newUnclampedMag > fMaxImpulse)
    393                                         {
    394                                                 newUnclampedAccImpulse.normalize();
    395                                                 newUnclampedAccImpulse *= fMaxImpulse;
    396                                                 impulse = newUnclampedAccImpulse - m_accMotorImpulse;
    397                                         }
    398                                         m_accMotorImpulse += impulse;
    399                                 }
    400 
    401                                 btScalar  impulseMag  = impulse.length();
    402                                 btVector3 impulseAxis =  impulse / impulseMag;
    403 
    404                                 bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
    405                                 bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
    406 
    407                         }
    408                 }
    409                 else // no motor: do a little damping
    410                 {
    411                         const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
    412                         const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
    413                         btVector3 relVel = angVelB - angVelA;
    414                         if (relVel.length2() > SIMD_EPSILON)
    415                         {
    416                                 btVector3 relVelAxis = relVel.normalized();
    417                                 btScalar m_kDamping =  btScalar(1.) /
    418                                         (getRigidBodyA().computeAngularImpulseDenominator(relVelAxis) +
    419                                          getRigidBodyB().computeAngularImpulseDenominator(relVelAxis));
    420                                 btVector3 impulse = m_damping * m_kDamping * relVel;
    421 
    422                                 btScalar  impulseMag  = impulse.length();
    423                                 btVector3 impulseAxis = impulse / impulseMag;
    424                                 bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
    425                                 bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
    426                         }
    427                 }
    428 
    429                 // joint limits
    430                 {
    431                         ///solve angular part
    432                         btVector3 angVelA;
    433                         bodyA.getAngularVelocity(angVelA);
    434                         btVector3 angVelB;
    435                         bodyB.getAngularVelocity(angVelB);
    436 
    437                         // solve swing limit
    438                         if (m_solveSwingLimit)
    439                         {
    440                                 btScalar amplitude = m_swingLimitRatio * m_swingCorrection*m_biasFactor/timeStep;
    441                                 btScalar relSwingVel = (angVelB - angVelA).dot(m_swingAxis);
    442                                 if (relSwingVel > 0)
    443                                         amplitude += m_swingLimitRatio * relSwingVel * m_relaxationFactor;
    444                                 btScalar impulseMag = amplitude * m_kSwing;
    445 
    446                                 // Clamp the accumulated impulse
    447                                 btScalar temp = m_accSwingLimitImpulse;
    448                                 m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) );
    449                                 impulseMag = m_accSwingLimitImpulse - temp;
    450 
    451                                 btVector3 impulse = m_swingAxis * impulseMag;
    452 
    453                                 // don't let cone response affect twist
    454                                 // (this can happen since body A's twist doesn't match body B's AND we use an elliptical cone limit)
    455                                 {
    456                                         btVector3 impulseTwistCouple = impulse.dot(m_twistAxisA) * m_twistAxisA;
    457                                         btVector3 impulseNoTwistCouple = impulse - impulseTwistCouple;
    458                                         impulse = impulseNoTwistCouple;
    459                                 }
    460 
    461                                 impulseMag = impulse.length();
    462                                 btVector3 noTwistSwingAxis = impulse / impulseMag;
    463 
    464                                 bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*noTwistSwingAxis, impulseMag);
    465                                 bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*noTwistSwingAxis, -impulseMag);
    466                         }
    467 
    468 
    469                         // solve twist limit
    470                         if (m_solveTwistLimit)
    471                         {
    472                                 btScalar amplitude = m_twistLimitRatio * m_twistCorrection*m_biasFactor/timeStep;
    473                                 btScalar relTwistVel = (angVelB - angVelA).dot( m_twistAxis );
    474                                 if (relTwistVel > 0) // only damp when moving towards limit (m_twistAxis flipping is important)
    475                                         amplitude += m_twistLimitRatio * relTwistVel * m_relaxationFactor;
    476                                 btScalar impulseMag = amplitude * m_kTwist;
    477 
    478                                 // Clamp the accumulated impulse
    479                                 btScalar temp = m_accTwistLimitImpulse;
    480                                 m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) );
    481                                 impulseMag = m_accTwistLimitImpulse - temp;
    482 
    483                                 btVector3 impulse = m_twistAxis * impulseMag;
    484 
    485                                 bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_twistAxis,impulseMag);
    486                                 bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_twistAxis,-impulseMag);
    487                         }               
    488                 }
    489         }
    490 
    491 }
    492 
    493 //-----------------------------------------------------------------------------
    494 
    495 void    btConeTwistConstraint::updateRHS(btScalar       timeStep)
    496 {
    497         (void)timeStep;
    498 
    499 }
    500 
    501 //-----------------------------------------------------------------------------
    502 
    503 void btConeTwistConstraint::calcAngleInfo()
    504 {
    505         m_swingCorrection = btScalar(0.);
    506         m_twistLimitSign = btScalar(0.);
    507         m_solveTwistLimit = false;
    508         m_solveSwingLimit = false;
     106                }
     107        }
    509108
    510109        btVector3 b1Axis1,b1Axis2,b1Axis3;
    … …  
    524123        {
    525124                b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1);
     125//              swing1  = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) );
    526126                swx = b2Axis1.dot(b1Axis1);
    527127                swy = b2Axis1.dot(b1Axis2);
    … …  
    530130                fact = fact / (fact + btScalar(1.0));
    531131                swing1 *= fact;
     132
    532133        }
    533134
    … …  
    535136        {
    536137                b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2);                     
     138//              swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) );
    537139                swx = b2Axis1.dot(b1Axis1);
    538140                swy = b2Axis1.dot(b1Axis3);
    … …  
    551153                m_swingCorrection = EllipseAngle-1.0f;
    552154                m_solveSwingLimit = true;
     155               
    553156                // Calculate necessary axis & factors
    554157                m_swingAxis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3));
    555158                m_swingAxis.normalize();
     159
    556160                btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
    557161                m_swingAxis *= swingAxisSign;
     162
     163                m_kSwing =  btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_swingAxis) +
     164                        getRigidBodyB().computeAngularImpulseDenominator(m_swingAxis));
     165
    558166        }
    559167
    … …  
    565173                btVector3 TwistRef = quatRotate(rotationArc,b2Axis2);
    566174                btScalar twist = btAtan2Fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) );
    567                 m_twistAngle = twist;
    568 
    569 //              btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.);
    570                 btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? btScalar(1.0f) : btScalar(0.);
     175
     176                btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.);
    571177                if (twist <= -m_twistSpan*lockedFreeFactor)
    572178                {
    573179                        m_twistCorrection = -(twist + m_twistSpan);
    574180                        m_solveTwistLimit = true;
     181
    575182                        m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
    576183                        m_twistAxis.normalize();
    577184                        m_twistAxis *= -1.0f;
    578                 }
    579                 else if (twist >  m_twistSpan*lockedFreeFactor)
    580                 {
    581                         m_twistCorrection = (twist - m_twistSpan);
    582                         m_solveTwistLimit = true;
    583                         m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
    584                         m_twistAxis.normalize();
    585                 }
    586         }
    587 } // btConeTwistConstraint::calcAngleInfo()
    588 
    589 
    590 static btVector3 vTwist(1,0,0); // twist axis in constraint's space
    591 
    592 //-----------------------------------------------------------------------------
    593 
    594 void btConeTwistConstraint::calcAngleInfo2()
    595 {
    596         m_swingCorrection = btScalar(0.);
    597         m_twistLimitSign = btScalar(0.);
    598         m_solveTwistLimit = false;
    599         m_solveSwingLimit = false;
    600 
    601         {
    602                 // compute rotation of A wrt B (in constraint space)
    603                 btQuaternion qA = getRigidBodyA().getCenterOfMassTransform().getRotation() * m_rbAFrame.getRotation();
    604                 btQuaternion qB = getRigidBodyB().getCenterOfMassTransform().getRotation() * m_rbBFrame.getRotation();
    605                 btQuaternion qAB = qB.inverse() * qA;
    606 
    607                 // split rotation into cone and twist
    608                 // (all this is done from B's perspective. Maybe I should be averaging axes...)
    609                 btVector3 vConeNoTwist = quatRotate(qAB, vTwist); vConeNoTwist.normalize();
    610                 btQuaternion qABCone  = shortestArcQuat(vTwist, vConeNoTwist); qABCone.normalize();
    611                 btQuaternion qABTwist = qABCone.inverse() * qAB; qABTwist.normalize();
    612 
    613                 if (m_swingSpan1 >= m_fixThresh && m_swingSpan2 >= m_fixThresh)
    614                 {
    615                         btScalar swingAngle, swingLimit = 0; btVector3 swingAxis;
    616                         computeConeLimitInfo(qABCone, swingAngle, swingAxis, swingLimit);
    617 
    618                         if (swingAngle > swingLimit * m_limitSoftness)
     185
     186                        m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
     187                                getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));
     188
     189                }       else
     190                        if (twist >  m_twistSpan*lockedFreeFactor)
    619191                        {
    620                                 m_solveSwingLimit = true;
    621 
    622                                 // compute limit ratio: 0->1, where
    623                                 // 0 == beginning of soft limit
    624                                 // 1 == hard/real limit
    625                                 m_swingLimitRatio = 1.f;
    626                                 if (swingAngle < swingLimit && m_limitSoftness < 1.f - SIMD_EPSILON)
    627                                 {
    628                                         m_swingLimitRatio = (swingAngle - swingLimit * m_limitSoftness)/
    629                                                                                 (swingLimit - swingLimit * m_limitSoftness);
    630                                 }                               
    631 
    632                                 // swing correction tries to get back to soft limit
    633                                 m_swingCorrection = swingAngle - (swingLimit * m_limitSoftness);
    634 
    635                                 // adjustment of swing axis (based on ellipse normal)
    636                                 adjustSwingAxisToUseEllipseNormal(swingAxis);
    637 
    638                                 // Calculate necessary axis & factors           
    639                                 m_swingAxis = quatRotate(qB, -swingAxis);
    640 
    641                                 m_twistAxisA.setValue(0,0,0);
    642 
    643                                 m_kSwing =  btScalar(1.) /
    644                                         (getRigidBodyA().computeAngularImpulseDenominator(m_swingAxis) +
    645                                          getRigidBodyB().computeAngularImpulseDenominator(m_swingAxis));
     192                                m_twistCorrection = (twist - m_twistSpan);
     193                                m_solveTwistLimit = true;
     194
     195                                m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
     196                                m_twistAxis.normalize();
     197
     198                                m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
     199                                        getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));
     200
    646201                        }
    647                 }
    648                 else
    649                 {
    650                         // you haven't set any limits;
    651                         // or you're trying to set at least one of the swing limits too small. (if so, do you really want a conetwist constraint?)
    652                         // anyway, we have either hinge or fixed joint
    653                         btVector3 ivA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0);
    654                         btVector3 jvA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1);
    655                         btVector3 kvA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
    656                         btVector3 ivB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(0);
    657                         btVector3 target;
    658                         btScalar x = ivB.dot(ivA);
    659                         btScalar y = ivB.dot(jvA);
    660                         btScalar z = ivB.dot(kvA);
    661                         if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
    662                         { // fixed. We'll need to add one more row to constraint
    663                                 if((!btFuzzyZero(y)) || (!(btFuzzyZero(z))))
    664                                 {
    665                                         m_solveSwingLimit = true;
    666                                         m_swingAxis = -ivB.cross(ivA);
    667                                 }
    668                         }
    669                         else
    670                         {
    671                                 if(m_swingSpan1 < m_fixThresh)
    672                                 { // hinge around Y axis
    673                                         if(!(btFuzzyZero(y)))
    674                                         {
    675                                                 m_solveSwingLimit = true;
    676                                                 if(m_swingSpan2 >= m_fixThresh)
    677                                                 {
    678                                                         y = btScalar(0.f);
    679                                                         btScalar span2 = btAtan2(z, x);
    680                                                         if(span2 > m_swingSpan2)
    681                                                         {
    682                                                                 x = btCos(m_swingSpan2);
    683                                                                 z = btSin(m_swingSpan2);
    684                                                         }
    685                                                         else if(span2 < -m_swingSpan2)
    686                                                         {
    687                                                                 x =  btCos(m_swingSpan2);
    688                                                                 z = -btSin(m_swingSpan2);
    689                                                         }
    690                                                 }
    691                                         }
    692                                 }
    693                                 else
    694                                 { // hinge around Z axis
    695                                         if(!btFuzzyZero(z))
    696                                         {
    697                                                 m_solveSwingLimit = true;
    698                                                 if(m_swingSpan1 >= m_fixThresh)
    699                                                 {
    700                                                         z = btScalar(0.f);
    701                                                         btScalar span1 = btAtan2(y, x);
    702                                                         if(span1 > m_swingSpan1)
    703                                                         {
    704                                                                 x = btCos(m_swingSpan1);
    705                                                                 y = btSin(m_swingSpan1);
    706                                                         }
    707                                                         else if(span1 < -m_swingSpan1)
    708                                                         {
    709                                                                 x =  btCos(m_swingSpan1);
    710                                                                 y = -btSin(m_swingSpan1);
    711                                                         }
    712                                                 }
    713                                         }
    714                                 }
    715                                 target[0] = x * ivA[0] + y * jvA[0] + z * kvA[0];
    716                                 target[1] = x * ivA[1] + y * jvA[1] + z * kvA[1];
    717                                 target[2] = x * ivA[2] + y * jvA[2] + z * kvA[2];
    718                                 target.normalize();
    719                                 m_swingAxis = -ivB.cross(target);
    720                                 m_swingCorrection = m_swingAxis.length();
    721                                 m_swingAxis.normalize();
    722                         }
    723                 }
    724 
    725                 if (m_twistSpan >= btScalar(0.f))
    726                 {
    727                         btVector3 twistAxis;
    728                         computeTwistLimitInfo(qABTwist, m_twistAngle, twistAxis);
    729 
    730                         if (m_twistAngle > m_twistSpan*m_limitSoftness)
    731                         {
    732                                 m_solveTwistLimit = true;
    733 
    734                                 m_twistLimitRatio = 1.f;
    735                                 if (m_twistAngle < m_twistSpan && m_limitSoftness < 1.f - SIMD_EPSILON)
    736                                 {
    737                                         m_twistLimitRatio = (m_twistAngle - m_twistSpan * m_limitSoftness)/
    738                                                                                 (m_twistSpan  - m_twistSpan * m_limitSoftness);
    739                                 }
    740 
    741                                 // twist correction tries to get back to soft limit
    742                                 m_twistCorrection = m_twistAngle - (m_twistSpan * m_limitSoftness);
    743 
    744                                 m_twistAxis = quatRotate(qB, -twistAxis);
    745 
    746                                 m_kTwist = btScalar(1.) /
    747                                         (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
    748                                          getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));
    749                         }
    750 
    751                         if (m_solveSwingLimit)
    752                                 m_twistAxisA = quatRotate(qA, -twistAxis);
    753                 }
    754                 else
    755                 {
    756                         m_twistAngle = btScalar(0.f);
    757                 }
    758         }
    759 } // btConeTwistConstraint::calcAngleInfo2()
    760 
    761 
    762 
    763 // given a cone rotation in constraint space, (pre: twist must already be removed)
    764 // this method computes its corresponding swing angle and axis.
    765 // more interestingly, it computes the cone/swing limit (angle) for this cone "pose".
    766 void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone,
    767                                                                                                  btScalar& swingAngle, // out
    768                                                                                                  btVector3& vSwingAxis, // out
    769                                                                                                  btScalar& swingLimit) // out
    770 {
    771         swingAngle = qCone.getAngle();
    772         if (swingAngle > SIMD_EPSILON)
    773         {
    774                 vSwingAxis = btVector3(qCone.x(), qCone.y(), qCone.z());
    775                 vSwingAxis.normalize();
    776                 if (fabs(vSwingAxis.x()) > SIMD_EPSILON)
    777                 {
    778                         // non-zero twist?! this should never happen.
    779                         int wtf = 0; wtf = wtf;
    780                 }
    781 
    782                 // Compute limit for given swing. tricky:
    783                 // Given a swing axis, we're looking for the intersection with the bounding cone ellipse.
    784                 // (Since we're dealing with angles, this ellipse is embedded on the surface of a sphere.)
    785 
    786                 // For starters, compute the direction from center to surface of ellipse.
    787                 // This is just the perpendicular (ie. rotate 2D vector by PI/2) of the swing axis.
    788                 // (vSwingAxis is the cone rotation (in z,y); change vars and rotate to (x,y) coords.)
    789                 btScalar xEllipse =  vSwingAxis.y();
    790                 btScalar yEllipse = -vSwingAxis.z();
    791 
    792                 // Now, we use the slope of the vector (using x/yEllipse) and find the length
    793                 // of the line that intersects the ellipse:
    794                 //  x^2   y^2
    795                 //  --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits)
    796                 //  a^2   b^2
    797                 // Do the math and it should be clear.
    798 
    799                 swingLimit = m_swingSpan1; // if xEllipse == 0, we have a pure vSwingAxis.z rotation: just use swingspan1
    800                 if (fabs(xEllipse) > SIMD_EPSILON)
    801                 {
    802                         btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse);
    803                         btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2);
    804                         norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1);
    805                         btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
    806                         swingLimit = sqrt(swingLimit2);
    807                 }
    808 
    809                 // test!
    810                 /*swingLimit = m_swingSpan2;
    811                 if (fabs(vSwingAxis.z()) > SIMD_EPSILON)
    812                 {
    813                 btScalar mag_2 = m_swingSpan1*m_swingSpan1 + m_swingSpan2*m_swingSpan2;
    814                 btScalar sinphi = m_swingSpan2 / sqrt(mag_2);
    815                 btScalar phi = asin(sinphi);
    816                 btScalar theta = atan2(fabs(vSwingAxis.y()),fabs(vSwingAxis.z()));
    817                 btScalar alpha = 3.14159f - theta - phi;
    818                 btScalar sinalpha = sin(alpha);
    819                 swingLimit = m_swingSpan1 * sinphi/sinalpha;
    820                 }*/
    821         }
    822         else if (swingAngle < 0)
    823         {
    824                 // this should never happen!
    825                 int wtf = 0; wtf = wtf;
    826         }
    827 }
    828 
    829 btVector3 btConeTwistConstraint::GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const
    830 {
    831         // compute x/y in ellipse using cone angle (0 -> 2*PI along surface of cone)
    832         btScalar xEllipse = btCos(fAngleInRadians);
    833         btScalar yEllipse = btSin(fAngleInRadians);
    834 
    835         // Use the slope of the vector (using x/yEllipse) and find the length
    836         // of the line that intersects the ellipse:
    837         //  x^2   y^2
    838         //  --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits)
    839         //  a^2   b^2
    840         // Do the math and it should be clear.
    841 
    842         float swingLimit = m_swingSpan1; // if xEllipse == 0, just use axis b (1)
    843         if (fabs(xEllipse) > SIMD_EPSILON)
    844         {
    845                 btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse);
    846                 btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2);
    847                 norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1);
    848                 btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
    849                 swingLimit = sqrt(swingLimit2);
    850         }
    851 
    852         // convert into point in constraint space:
    853         // note: twist is x-axis, swing 1 and 2 are along the z and y axes respectively
    854         btVector3 vSwingAxis(0, xEllipse, -yEllipse);
    855         btQuaternion qSwing(vSwingAxis, swingLimit);
    856         btVector3 vPointInConstraintSpace(fLength,0,0);
    857         return quatRotate(qSwing, vPointInConstraintSpace);
    858 }
    859 
    860 // given a twist rotation in constraint space, (pre: cone must already be removed)
    861 // this method computes its corresponding angle and axis.
    862 void btConeTwistConstraint::computeTwistLimitInfo(const btQuaternion& qTwist,
    863                                                                                                   btScalar& twistAngle, // out
    864                                                                                                   btVector3& vTwistAxis) // out
    865 {
    866         btQuaternion qMinTwist = qTwist;
    867         twistAngle = qTwist.getAngle();
    868 
    869         if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate.
    870         {
    871                 qMinTwist = operator-(qTwist);
    872                 twistAngle = qMinTwist.getAngle();
    873         }
    874         if (twistAngle < 0)
    875         {
    876                 // this should never happen
    877                 int wtf = 0; wtf = wtf;                 
    878         }
    879 
    880         vTwistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z());
    881         if (twistAngle > SIMD_EPSILON)
    882                 vTwistAxis.normalize();
    883 }
    884 
    885 
    886 void btConeTwistConstraint::adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const
    887 {
    888         // the swing axis is computed as the "twist-free" cone rotation,
    889         // but the cone limit is not circular, but elliptical (if swingspan1 != swingspan2).
    890         // so, if we're outside the limits, the closest way back inside the cone isn't
    891         // along the vector back to the center. better (and more stable) to use the ellipse normal.
    892 
    893         // convert swing axis to direction from center to surface of ellipse
    894         // (ie. rotate 2D vector by PI/2)
    895         btScalar y = -vSwingAxis.z();
    896         btScalar z =  vSwingAxis.y();
    897 
    898         // do the math...
    899         if (fabs(z) > SIMD_EPSILON) // avoid division by 0. and we don't need an update if z == 0.
    900         {
    901                 // compute gradient/normal of ellipse surface at current "point"
    902                 btScalar grad = y/z;
    903                 grad *= m_swingSpan2 / m_swingSpan1;
    904 
    905                 // adjust y/z to represent normal at point (instead of vector to point)
    906                 if (y > 0)
    907                         y =  fabs(grad * z);
    908                 else
    909                         y = -fabs(grad * z);
    910 
    911                 // convert ellipse direction back to swing axis
    912                 vSwingAxis.setZ(-y);
    913                 vSwingAxis.setY( z);
    914                 vSwingAxis.normalize();
    915         }
    916 }
    917 
    918 
    919 
    920 void btConeTwistConstraint::setMotorTarget(const btQuaternion &q)
    921 {
    922         btTransform trACur = m_rbA.getCenterOfMassTransform();
    923         btTransform trBCur = m_rbB.getCenterOfMassTransform();
    924         btTransform trABCur = trBCur.inverse() * trACur;
    925         btQuaternion qABCur = trABCur.getRotation();
    926         btTransform trConstraintCur = (trBCur * m_rbBFrame).inverse() * (trACur * m_rbAFrame);
    927         btQuaternion qConstraintCur = trConstraintCur.getRotation();
    928 
    929         btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * q * m_rbAFrame.getRotation();
    930         setMotorTargetInConstraintSpace(qConstraint);
    931 }
    932 
    933 
    934 void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion &q)
    935 {
    936         m_qTarget = q;
    937 
    938         // clamp motor target to within limits
    939         {
    940                 btScalar softness = 1.f;//m_limitSoftness;
    941 
    942                 // split into twist and cone
    943                 btVector3 vTwisted = quatRotate(m_qTarget, vTwist);
    944                 btQuaternion qTargetCone  = shortestArcQuat(vTwist, vTwisted); qTargetCone.normalize();
    945                 btQuaternion qTargetTwist = qTargetCone.inverse() * m_qTarget; qTargetTwist.normalize();
    946 
    947                 // clamp cone
    948                 if (m_swingSpan1 >= btScalar(0.05f) && m_swingSpan2 >= btScalar(0.05f))
    949                 {
    950                         btScalar swingAngle, swingLimit; btVector3 swingAxis;
    951                         computeConeLimitInfo(qTargetCone, swingAngle, swingAxis, swingLimit);
    952 
    953                         if (fabs(swingAngle) > SIMD_EPSILON)
    954                         {
    955                                 if (swingAngle > swingLimit*softness)
    956                                         swingAngle = swingLimit*softness;
    957                                 else if (swingAngle < -swingLimit*softness)
    958                                         swingAngle = -swingLimit*softness;
    959                                 qTargetCone = btQuaternion(swingAxis, swingAngle);
    960                         }
    961                 }
    962 
    963                 // clamp twist
    964                 if (m_twistSpan >= btScalar(0.05f))
    965                 {
    966                         btScalar twistAngle; btVector3 twistAxis;
    967                         computeTwistLimitInfo(qTargetTwist, twistAngle, twistAxis);
    968 
    969                         if (fabs(twistAngle) > SIMD_EPSILON)
    970                         {
    971                                 // eddy todo: limitSoftness used here???
    972                                 if (twistAngle > m_twistSpan*softness)
    973                                         twistAngle = m_twistSpan*softness;
    974                                 else if (twistAngle < -m_twistSpan*softness)
    975                                         twistAngle = -m_twistSpan*softness;
    976                                 qTargetTwist = btQuaternion(twistAxis, twistAngle);
    977                         }
    978                 }
    979 
    980                 m_qTarget = qTargetCone * qTargetTwist;
    981         }
    982 }
    983 
    984 
    985 //-----------------------------------------------------------------------------
    986 //-----------------------------------------------------------------------------
    987 //-----------------------------------------------------------------------------
    988 
    989 
     202        }
     203}
     204
     205void    btConeTwistConstraint::solveConstraint(btScalar timeStep)
     206{
     207
     208        btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
     209        btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
     210
     211        btScalar tau = btScalar(0.3);
     212
     213        //linear part
     214        if (!m_angularOnly)
     215        {
     216                btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
     217                btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
     218
     219                btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
     220                btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
     221                btVector3 vel = vel1 - vel2;
     222
     223                for (int i=0;i<3;i++)
     224                {               
     225                        const btVector3& normal = m_jac[i].m_linearJointAxis;
     226                        btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
     227
     228                        btScalar rel_vel;
     229                        rel_vel = normal.dot(vel);
     230                        //positional error (zeroth order error)
     231                        btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
     232                        btScalar impulse = depth*tau/timeStep  * jacDiagABInv -  rel_vel * jacDiagABInv;
     233                        m_appliedImpulse += impulse;
     234                        btVector3 impulse_vector = normal * impulse;
     235                        m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
     236                        m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
     237                }
     238        }
     239       
     240        {
     241                ///solve angular part
     242                const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
     243                const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
     244
     245                // solve swing limit
     246                if (m_solveSwingLimit)
     247                {
     248                        btScalar amplitude = ((angVelB - angVelA).dot( m_swingAxis )*m_relaxationFactor*m_relaxationFactor + m_swingCorrection*(btScalar(1.)/timeStep)*m_biasFactor);
     249                        btScalar impulseMag = amplitude * m_kSwing;
     250
     251                        // Clamp the accumulated impulse
     252                        btScalar temp = m_accSwingLimitImpulse;
     253                        m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) );
     254                        impulseMag = m_accSwingLimitImpulse - temp;
     255
     256                        btVector3 impulse = m_swingAxis * impulseMag;
     257
     258                        m_rbA.applyTorqueImpulse(impulse);
     259                        m_rbB.applyTorqueImpulse(-impulse);
     260
     261                }
     262
     263                // solve twist limit
     264                if (m_solveTwistLimit)
     265                {
     266                        btScalar amplitude = ((angVelB - angVelA).dot( m_twistAxis )*m_relaxationFactor*m_relaxationFactor + m_twistCorrection*(btScalar(1.)/timeStep)*m_biasFactor );
     267                        btScalar impulseMag = amplitude * m_kTwist;
     268
     269                        // Clamp the accumulated impulse
     270                        btScalar temp = m_accTwistLimitImpulse;
     271                        m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) );
     272                        impulseMag = m_accTwistLimitImpulse - temp;
     273
     274                        btVector3 impulse = m_twistAxis * impulseMag;
     275
     276                        m_rbA.applyTorqueImpulse(impulse);
     277                        m_rbB.applyTorqueImpulse(-impulse);
     278
     279                }
     280       
     281        }
     282
     283}
     284
     285void    btConeTwistConstraint::updateRHS(btScalar       timeStep)
     286{
     287        (void)timeStep;
     288
     289}

  • code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h

    r2907 r2908  
    4343        btScalar        m_relaxationFactor;
    4444
    45         btScalar        m_damping;
    46 
    4745        btScalar        m_swingSpan1;
    4846        btScalar        m_swingSpan2;
    4947        btScalar        m_twistSpan;
    50 
    51         btScalar        m_fixThresh;
    5248
    5349        btVector3   m_swingAxis;
    … …  
    6157        btScalar        m_twistCorrection;
    6258
    63         btScalar        m_twistAngle;
    64 
    6559        btScalar        m_accSwingLimitImpulse;
    6660        btScalar        m_accTwistLimitImpulse;
    … …  
    7064        bool            m_solveSwingLimit;
    7165
    72         bool    m_useSolveConstraintObsolete;
    73 
    74         // not yet used...
    75         btScalar        m_swingLimitRatio;
    76         btScalar        m_twistLimitRatio;
    77         btVector3   m_twistAxisA;
    78 
    79         // motor
    80         bool             m_bMotorEnabled;
    81         bool             m_bNormalizedMotorStrength;
    82         btQuaternion m_qTarget;
    83         btScalar         m_maxMotorImpulse;
    84         btVector3        m_accMotorImpulse;
    8566       
    8667public:
    … …  
    9475        virtual void    buildJacobian();
    9576
    96         virtual void getInfo1 (btConstraintInfo1* info);
    97        
    98         virtual void getInfo2 (btConstraintInfo2* info);
    99        
    100 
    101         virtual void    solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar        timeStep);
     77        virtual void    solveConstraint(btScalar        timeStep);
    10278
    10379        void    updateRHS(btScalar      timeStep);
    … …  
    11793        }
    11894
    119         void    setLimit(int limitIndex,btScalar limitValue)
    120         {
    121                 switch (limitIndex)
    122                 {
    123                 case 3:
    124                         {
    125                                 m_twistSpan = limitValue;
    126                                 break;
    127                         }
    128                 case 4:
    129                         {
    130                                 m_swingSpan2 = limitValue;
    131                                 break;
    132                         }
    133                 case 5:
    134                         {
    135                                 m_swingSpan1 = limitValue;
    136                                 break;
    137                         }
    138                 default:
    139                         {
    140                         }
    141                 };
    142         }
    143 
    144         void    setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan,  btScalar _softness = 1.f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
     95        void    setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan,  btScalar _softness = 0.8f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
    14596        {
    14697                m_swingSpan1 = _swingSpan1;
    … …  
    171122        }
    172123
    173         void calcAngleInfo();
    174         void calcAngleInfo2();
    175 
    176         inline btScalar getSwingSpan1()
    177         {
    178                 return m_swingSpan1;
    179         }
    180         inline btScalar getSwingSpan2()
    181         {
    182                 return m_swingSpan2;
    183         }
    184         inline btScalar getTwistSpan()
    185         {
    186                 return m_twistSpan;
    187         }
    188         inline btScalar getTwistAngle()
    189         {
    190                 return m_twistAngle;
    191         }
    192         bool isPastSwingLimit() { return m_solveSwingLimit; }
    193 
    194 
    195         void setDamping(btScalar damping) { m_damping = damping; }
    196 
    197         void enableMotor(bool b) { m_bMotorEnabled = b; }
    198         void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = false; }
    199         void setMaxMotorImpulseNormalized(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = true; }
    200 
    201         btScalar getFixThresh() { return m_fixThresh; }
    202         void setFixThresh(btScalar fixThresh) { m_fixThresh = fixThresh; }
    203 
    204         // setMotorTarget:
    205         // q: the desired rotation of bodyA wrt bodyB.
    206         // note: if q violates the joint limits, the internal target is clamped to avoid conflicting impulses (very bad for stability)
    207         // note: don't forget to enableMotor()
    208         void setMotorTarget(const btQuaternion &q);
    209 
    210         // same as above, but q is the desired rotation of frameA wrt frameB in constraint space
    211         void setMotorTargetInConstraintSpace(const btQuaternion &q);
    212 
    213         btVector3 GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const;
    214 
    215 
    216 
    217 protected:
    218         void init();
    219 
    220         void computeConeLimitInfo(const btQuaternion& qCone, // in
    221                 btScalar& swingAngle, btVector3& vSwingAxis, btScalar& swingLimit); // all outs
    222 
    223         void computeTwistLimitInfo(const btQuaternion& qTwist, // in
    224                 btScalar& twistAngle, btVector3& vTwistAxis); // all outs
    225 
    226         void adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const;
    227124};
    228125

  • code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btContactConstraint.cpp

    r2907 r2908  
    2323#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
    2424
    25 #define ASSERT2 btAssert
     25#define ASSERT2 assert
    2626
    2727#define USE_INTERNAL_APPLY_IMPULSE 1
    … …  
    5353       
    5454
    55            btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
     55          btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
    5656                body2.getCenterOfMassTransform().getBasis().transpose(),
    5757                rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(),
    … …  
    115115
    116116        btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
    117         btAssert(cpd);
     117        assert(cpd);
    118118        btScalar distance = cpd->m_penetration;
    119119        btScalar positionalError = Kcor *-distance;
    … …  
    167167       
    168168        btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
    169         btAssert(cpd);
     169        assert(cpd);
    170170
    171171        btScalar combinedFriction = cpd->m_friction;
    … …  
    256256       
    257257        btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
    258         btAssert(cpd);
     258        assert(cpd);
    259259
    260260        btScalar combinedFriction = cpd->m_friction;
    … …  
    338338
    339339        btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
    340         btAssert(cpd);
     340        assert(cpd);
    341341        btScalar distance = cpd->m_penetration;
    342342        btScalar positionalError = Kcor *-distance;

  • code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h

    r2907 r2908  
    2323        SOLVER_USE_WARMSTARTING = 4,
    2424        SOLVER_USE_FRICTION_WARMSTARTING = 8,
    25         SOLVER_USE_2_FRICTION_DIRECTIONS = 16,
    26         SOLVER_ENABLE_FRICTION_DIRECTION_CACHING = 32,
    27         SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION = 64,
    28         SOLVER_CACHE_FRIENDLY = 128,
    29         SOLVER_SIMD = 256,      //enabled for Windows, the solver innerloop is branchless SIMD, 40% faster than FPU/scalar version
    30         SOLVER_CUDA = 512       //will be open sourced during Game Developers Conference 2009. Much faster.
     25        SOLVER_CACHE_FRIENDLY = 16
    3126};
    3227
    … …  
    4540        btScalar        m_erp;//used as Baumgarte factor
    4641        btScalar        m_erp2;//used in Split Impulse
    47         btScalar        m_globalCfm;//constraint force mixing
    4842        int                     m_splitImpulse;
    4943        btScalar        m_splitImpulsePenetrationThreshold;
    … …  
    7266                m_erp = btScalar(0.2);
    7367                m_erp2 = btScalar(0.1);
    74                 m_globalCfm = btScalar(0.);
    75                 m_sor = btScalar(1.);
     68                m_sor = btScalar(1.3);
    7669                m_splitImpulse = false;
    7770                m_splitImpulsePenetrationThreshold = -0.02f;
    7871                m_linearSlop = btScalar(0.0);
    7972                m_warmstartingFactor=btScalar(0.85);
    80                 m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD ;//SOLVER_RANDMIZE_ORDER
     73                m_solverMode = SOLVER_CACHE_FRIENDLY |  SOLVER_RANDMIZE_ORDER |  SOLVER_USE_WARMSTARTING;
    8174                m_restingContactRestitutionThreshold = 2;//resting contact lifetime threshold to disable restitution
    8275        }

  • code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp

    r2907 r2908  
    2020*/
    2121
     22
    2223#include "btGeneric6DofConstraint.h"
    2324#include "BulletDynamics/Dynamics/btRigidBody.h"
    … …  
    2627
    2728
    28 #define D6_USE_OBSOLETE_METHOD false
    29 //-----------------------------------------------------------------------------
    30 
    31 btGeneric6DofConstraint::btGeneric6DofConstraint()
    32 :btTypedConstraint(D6_CONSTRAINT_TYPE),
    33 m_useLinearReferenceFrameA(true),
    34 m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
    35 {
    36 }
    37 
    38 //-----------------------------------------------------------------------------
    39 
    40 btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
    41 : btTypedConstraint(D6_CONSTRAINT_TYPE, rbA, rbB)
    42 , m_frameInA(frameInA)
    43 , m_frameInB(frameInB),
    44 m_useLinearReferenceFrameA(useLinearReferenceFrameA),
    45 m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
    46 {
    47 
    48 }
    49 //-----------------------------------------------------------------------------
    50 
    51 
    5229#define GENERIC_D6_DISABLE_WARMSTARTING 1
    53 
    54 //-----------------------------------------------------------------------------
    5530
    5631btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
    … …  
    6237}
    6338
    64 //-----------------------------------------------------------------------------
    65 
    6639///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
    6740bool    matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz);
    6841bool    matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz)
    6942{
    70         //      // rot =  cy*cz          -cy*sz           sy
    71         //      //        cz*sx*sy+cx*sz  cx*cz-sx*sy*sz -cy*sx
    72         //      //       -cx*cz*sy+sx*sz  cz*sx+cx*sy*sz  cx*cy
    73         //
    74 
    75         btScalar fi = btGetMatrixElem(mat,2);
    76         if (fi < btScalar(1.0f))
    77         {
    78                 if (fi > btScalar(-1.0f))
     43//      // rot =  cy*cz          -cy*sz           sy
     44//      //        cz*sx*sy+cx*sz  cx*cz-sx*sy*sz -cy*sx
     45//      //       -cx*cz*sy+sx*sz  cz*sx+cx*sy*sz  cx*cy
     46//
     47
     48                if (btGetMatrixElem(mat,2) < btScalar(1.0))
    7949                {
    80                         xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8));
    81                         xyz[1] = btAsin(btGetMatrixElem(mat,2));
    82                         xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
    83                         return true;
     50                        if (btGetMatrixElem(mat,2) > btScalar(-1.0))
     51                        {
     52                                xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8));
     53                                xyz[1] = btAsin(btGetMatrixElem(mat,2));
     54                                xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
     55                                return true;
     56                        }
     57                        else
     58                        {
     59                                // WARNING.  Not unique.  XA - ZA = -atan2(r10,r11)
     60                                xyz[0] = -btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
     61                                xyz[1] = -SIMD_HALF_PI;
     62                                xyz[2] = btScalar(0.0);
     63                                return false;
     64                        }
    8465                }
    8566                else
    8667                {
    87                         // WARNING.  Not unique.  XA - ZA = -atan2(r10,r11)
    88                         xyz[0] = -btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
    89                         xyz[1] = -SIMD_HALF_PI;
    90                         xyz[2] = btScalar(0.0);
    91                         return false;
     68                        // WARNING.  Not unique.  XAngle + ZAngle = atan2(r10,r11)
     69                        xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
     70                        xyz[1] = SIMD_HALF_PI;
     71                        xyz[2] = 0.0;
     72
    9273                }
    93         }
    94         else
    95         {
    96                 // WARNING.  Not unique.  XAngle + ZAngle = atan2(r10,r11)
    97                 xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
    98                 xyz[1] = SIMD_HALF_PI;
    99                 xyz[2] = 0.0;
    100         }
     74
     75
    10176        return false;
    10277}
    10378
     79
     80
    10481//////////////////////////// btRotationalLimitMotor ////////////////////////////////////
     82
    10583
    10684int btRotationalLimitMotor::testLimitValue(btScalar test_value)
    … …  
    127105        m_currentLimit = 0;//Free from violation
    128106        return 0;
    129 
    130 }
    131 
    132 //-----------------------------------------------------------------------------
     107       
     108}
     109
    133110
    134111btScalar btRotationalLimitMotor::solveAngularLimits(
    135         btScalar timeStep,btVector3& axis,btScalar jacDiagABInv,
    136         btRigidBody * body0, btSolverBody& bodyA, btRigidBody * body1, btSolverBody& bodyB)
    137 {
    138         if (needApplyTorques()==false) return 0.0f;
    139 
    140         btScalar target_velocity = m_targetVelocity;
    141         btScalar maxMotorForce = m_maxMotorForce;
     112                btScalar timeStep,btVector3& axis,btScalar jacDiagABInv,
     113                btRigidBody * body0, btRigidBody * body1)
     114{
     115    if (needApplyTorques()==false) return 0.0f;
     116
     117    btScalar target_velocity = m_targetVelocity;
     118    btScalar maxMotorForce = m_maxMotorForce;
    142119
    143120        //current error correction
    144         if (m_currentLimit!=0)
    145         {
    146                 target_velocity = -m_ERP*m_currentLimitError/(timeStep);
    147                 maxMotorForce = m_maxLimitForce;
    148         }
    149 
    150         maxMotorForce *= timeStep;
    151 
    152         // current velocity difference
    153 
    154         btVector3 angVelA;
    155         bodyA.getAngularVelocity(angVelA);
    156         btVector3 angVelB;
    157         bodyB.getAngularVelocity(angVelB);
    158 
    159         btVector3 vel_diff;
    160         vel_diff = angVelA-angVelB;
    161 
    162 
    163 
    164         btScalar rel_vel = axis.dot(vel_diff);
     121    if (m_currentLimit!=0)
     122    {
     123        target_velocity = -m_ERP*m_currentLimitError/(timeStep);
     124        maxMotorForce = m_maxLimitForce;
     125    }
     126
     127    maxMotorForce *= timeStep;
     128
     129    // current velocity difference
     130    btVector3 vel_diff = body0->getAngularVelocity();
     131    if (body1)
     132    {
     133        vel_diff -= body1->getAngularVelocity();
     134    }
     135
     136
     137
     138    btScalar rel_vel = axis.dot(vel_diff);
    165139
    166140        // correction velocity
    167         btScalar motor_relvel = m_limitSoftness*(target_velocity  - m_damping*rel_vel);
    168 
    169 
    170         if ( motor_relvel < SIMD_EPSILON && motor_relvel > -SIMD_EPSILON  )
    171         {
    172                 return 0.0f;//no need for applying force
    173         }
     141    btScalar motor_relvel = m_limitSoftness*(target_velocity  - m_damping*rel_vel);
     142
     143
     144    if ( motor_relvel < SIMD_EPSILON && motor_relvel > -SIMD_EPSILON  )
     145    {
     146        return 0.0f;//no need for applying force
     147    }
    174148
    175149
    176150        // correction impulse
    177         btScalar unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv;
     151    btScalar unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv;
    178152
    179153        // clip correction impulse
    180         btScalar clippedMotorImpulse;
    181 
    182         ///@todo: should clip against accumulated impulse
    183         if (unclippedMotorImpulse>0.0f)
    184         {
    185                 clippedMotorImpulse =  unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse;
    186         }
    187         else
    188         {
    189                 clippedMotorImpulse =  unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse;
    190         }
     154    btScalar clippedMotorImpulse;
     155
     156    ///@todo: should clip against accumulated impulse
     157    if (unclippedMotorImpulse>0.0f)
     158    {
     159        clippedMotorImpulse =  unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse;
     160    }
     161    else
     162    {
     163        clippedMotorImpulse =  unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse;
     164    }
    191165
    192166
    193167        // sort with accumulated impulses
    194         btScalar        lo = btScalar(-1e30);
    195         btScalar        hi = btScalar(1e30);
    196 
    197         btScalar oldaccumImpulse = m_accumulatedImpulse;
    198         btScalar sum = oldaccumImpulse + clippedMotorImpulse;
    199         m_accumulatedImpulse = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
    200 
    201         clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
    202 
    203         btVector3 motorImp = clippedMotorImpulse * axis;
    204 
    205         //body0->applyTorqueImpulse(motorImp);
    206         //body1->applyTorqueImpulse(-motorImp);
    207 
    208         bodyA.applyImpulse(btVector3(0,0,0), body0->getInvInertiaTensorWorld()*axis,clippedMotorImpulse);
    209         bodyB.applyImpulse(btVector3(0,0,0), body1->getInvInertiaTensorWorld()*axis,-clippedMotorImpulse);
    210 
    211 
    212         return clippedMotorImpulse;
     168    btScalar    lo = btScalar(-1e30);
     169    btScalar    hi = btScalar(1e30);
     170
     171    btScalar oldaccumImpulse = m_accumulatedImpulse;
     172    btScalar sum = oldaccumImpulse + clippedMotorImpulse;
     173    m_accumulatedImpulse = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
     174
     175    clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
     176
     177
     178
     179    btVector3 motorImp = clippedMotorImpulse * axis;
     180
     181
     182    body0->applyTorqueImpulse(motorImp);
     183    if (body1) body1->applyTorqueImpulse(-motorImp);
     184
     185    return clippedMotorImpulse;
    213186
    214187
    … …  
    217190//////////////////////////// End btRotationalLimitMotor ////////////////////////////////////
    218191
    219 
    220 
    221 
    222192//////////////////////////// btTranslationalLimitMotor ////////////////////////////////////
    223 
    224 
    225 int btTranslationalLimitMotor::testLimitValue(int limitIndex, btScalar test_value)
    226 {
    227         btScalar loLimit = m_lowerLimit[limitIndex];
    228         btScalar hiLimit = m_upperLimit[limitIndex];
    229         if(loLimit > hiLimit)
    230         {
    231                 m_currentLimit[limitIndex] = 0;//Free from violation
    232                 m_currentLimitError[limitIndex] = btScalar(0.f);
    233                 return 0;
    234         }
    235 
    236         if (test_value < loLimit)
    237         {
    238                 m_currentLimit[limitIndex] = 2;//low limit violation
    239                 m_currentLimitError[limitIndex] =  test_value - loLimit;
    240                 return 2;
    241         }
    242         else if (test_value> hiLimit)
    243         {
    244                 m_currentLimit[limitIndex] = 1;//High limit violation
    245                 m_currentLimitError[limitIndex] = test_value - hiLimit;
    246                 return 1;
    247         };
    248 
    249         m_currentLimit[limitIndex] = 0;//Free from violation
    250         m_currentLimitError[limitIndex] = btScalar(0.f);
    251         return 0;
    252 } // btTranslationalLimitMotor::testLimitValue()
    253 
    254 //-----------------------------------------------------------------------------
    255 
    256193btScalar btTranslationalLimitMotor::solveLinearAxis(
    257         btScalar timeStep,
    258         btScalar jacDiagABInv,
    259         btRigidBody& body1,btSolverBody& bodyA,const btVector3 &pointInA,
    260         btRigidBody& body2,btSolverBody& bodyB,const btVector3 &pointInB,
    261         int limit_index,
    262         const btVector3 & axis_normal_on_a,
    263         const btVector3 & anchorPos)
    264 {
    265 
    266         ///find relative velocity
    267         //    btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition();
    268         //    btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition();
    269         btVector3 rel_pos1 = anchorPos - body1.getCenterOfMassPosition();
    270         btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition();
    271 
    272         btVector3 vel1;
    273         bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
    274         btVector3 vel2;
    275         bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
    276         btVector3 vel = vel1 - vel2;
    277 
    278         btScalar rel_vel = axis_normal_on_a.dot(vel);
    279 
    280 
    281 
    282         /// apply displacement correction
    283 
    284         //positional error (zeroth order error)
    285         btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a);
    286         btScalar        lo = btScalar(-1e30);
    287         btScalar        hi = btScalar(1e30);
    288 
    289         btScalar minLimit = m_lowerLimit[limit_index];
    290         btScalar maxLimit = m_upperLimit[limit_index];
    291 
    292         //handle the limits
    293         if (minLimit < maxLimit)
    294         {
    295                 {
    296                         if (depth > maxLimit)
    297                         {
    298                                 depth -= maxLimit;
    299                                 lo = btScalar(0.);
    300 
    301                         }
    302                         else
    303                         {
    304                                 if (depth < minLimit)
    305                                 {
    306                                         depth -= minLimit;
    307                                         hi = btScalar(0.);
    308                                 }
    309                                 else
    310                                 {
    311                                         return 0.0f;
    312                                 }
    313                         }
    314                 }
    315         }
    316 
    317         btScalar normalImpulse= m_limitSoftness*(m_restitution*depth/timeStep - m_damping*rel_vel) * jacDiagABInv;
    318 
    319 
    320 
    321 
    322         btScalar oldNormalImpulse = m_accumulatedImpulse[limit_index];
    323         btScalar sum = oldNormalImpulse + normalImpulse;
    324         m_accumulatedImpulse[limit_index] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
    325         normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
    326 
    327         btVector3 impulse_vector = axis_normal_on_a * normalImpulse;
    328         //body1.applyImpulse( impulse_vector, rel_pos1);
    329         //body2.applyImpulse(-impulse_vector, rel_pos2);
    330 
    331         btVector3 ftorqueAxis1 = rel_pos1.cross(axis_normal_on_a);
    332         btVector3 ftorqueAxis2 = rel_pos2.cross(axis_normal_on_a);
    333         bodyA.applyImpulse(axis_normal_on_a*body1.getInvMass(), body1.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse);
    334         bodyB.applyImpulse(axis_normal_on_a*body2.getInvMass(), body2.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse);
    335 
    336 
    337 
    338 
    339         return normalImpulse;
     194                btScalar timeStep,
     195        btScalar jacDiagABInv,
     196        btRigidBody& body1,const btVector3 &pointInA,
     197        btRigidBody& body2,const btVector3 &pointInB,
     198        int limit_index,
     199        const btVector3 & axis_normal_on_a,
     200                const btVector3 & anchorPos)
     201{
     202
     203///find relative velocity
     204//    btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition();
     205//    btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition();
     206    btVector3 rel_pos1 = anchorPos - body1.getCenterOfMassPosition();
     207    btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition();
     208
     209    btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
     210    btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
     211    btVector3 vel = vel1 - vel2;
     212
     213    btScalar rel_vel = axis_normal_on_a.dot(vel);
     214
     215
     216
     217/// apply displacement correction
     218
     219//positional error (zeroth order error)
     220    btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a);
     221    btScalar    lo = btScalar(-1e30);
     222    btScalar    hi = btScalar(1e30);
     223
     224    btScalar minLimit = m_lowerLimit[limit_index];
     225    btScalar maxLimit = m_upperLimit[limit_index];
     226
     227    //handle the limits
     228    if (minLimit < maxLimit)
     229    {
     230        {
     231            if (depth > maxLimit)
     232            {
     233                depth -= maxLimit;
     234                lo = btScalar(0.);
     235
     236            }
     237            else
     238            {
     239                if (depth < minLimit)
     240                {
     241                    depth -= minLimit;
     242                    hi = btScalar(0.);
     243                }
     244                else
     245                {
     246                    return 0.0f;
     247                }
     248            }
     249        }
     250    }
     251
     252    btScalar normalImpulse= m_limitSoftness*(m_restitution*depth/timeStep - m_damping*rel_vel) * jacDiagABInv;
     253
     254
     255
     256
     257    btScalar oldNormalImpulse = m_accumulatedImpulse[limit_index];
     258    btScalar sum = oldNormalImpulse + normalImpulse;
     259    m_accumulatedImpulse[limit_index] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
     260    normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
     261
     262    btVector3 impulse_vector = axis_normal_on_a * normalImpulse;
     263    body1.applyImpulse( impulse_vector, rel_pos1);
     264    body2.applyImpulse(-impulse_vector, rel_pos2);
     265    return normalImpulse;
    340266}
    341267
    342268//////////////////////////// btTranslationalLimitMotor ////////////////////////////////////
    343269
     270
     271btGeneric6DofConstraint::btGeneric6DofConstraint()
     272        :btTypedConstraint(D6_CONSTRAINT_TYPE),
     273                m_useLinearReferenceFrameA(true)
     274{
     275}
     276
     277btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
     278        : btTypedConstraint(D6_CONSTRAINT_TYPE, rbA, rbB)
     279        , m_frameInA(frameInA)
     280        , m_frameInB(frameInB),
     281                m_useLinearReferenceFrameA(useLinearReferenceFrameA)
     282{
     283
     284}
     285
     286
     287
     288
     289
    344290void btGeneric6DofConstraint::calculateAngleInfo()
    345291{
    346292        btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis();
     293
    347294        matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff);
     295
     296
     297
    348298        // in euler angle mode we do not actually constrain the angular velocity
    349         // along the axes axis[0] and axis[2] (although we do use axis[1]) :
    350         //
    351         //    to get                    constrain w2-w1 along           ...not
    352         //    ------                    ---------------------           ------
    353         //    d(angle[0])/dt = 0        ax[1] x ax[2]                   ax[0]
    354         //    d(angle[1])/dt = 0        ax[1]
    355         //    d(angle[2])/dt = 0        ax[0] x ax[1]                   ax[2]
    356         //
    357         // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
    358         // to prove the result for angle[0], write the expression for angle[0] from
    359         // GetInfo1 then take the derivative. to prove this for angle[2] it is
    360         // easier to take the euler rate expression for d(angle[2])/dt with respect
    361         // to the components of w and set that to 0.
     299  // along the axes axis[0] and axis[2] (although we do use axis[1]) :
     300  //
     301  //    to get                  constrain w2-w1 along           ...not
     302  //    ------                  ---------------------           ------
     303  //    d(angle[0])/dt = 0      ax[1] x ax[2]                   ax[0]
     304  //    d(angle[1])/dt = 0      ax[1]
     305  //    d(angle[2])/dt = 0      ax[0] x ax[1]                   ax[2]
     306  //
     307  // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
     308  // to prove the result for angle[0], write the expression for angle[0] from
     309  // GetInfo1 then take the derivative. to prove this for angle[2] it is
     310  // easier to take the euler rate expression for d(angle[2])/dt with respect
     311  // to the components of w and set that to 0.
     312
    362313        btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0);
    363314        btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2);
    … …  
    367318        m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
    368319
    369         m_calculatedAxis[0].normalize();
    370         m_calculatedAxis[1].normalize();
    371         m_calculatedAxis[2].normalize();
    372 
    373 }
    374 
    375 //-----------------------------------------------------------------------------
     320
     321//    if(m_debugDrawer)
     322//    {
     323//
     324//      char buff[300];
     325//              sprintf(buff,"\n X: %.2f ; Y: %.2f ; Z: %.2f ",
     326//              m_calculatedAxisAngleDiff[0],
     327//              m_calculatedAxisAngleDiff[1],
     328//              m_calculatedAxisAngleDiff[2]);
     329//      m_debugDrawer->reportErrorWarning(buff);
     330//    }
     331
     332}
    376333
    377334void btGeneric6DofConstraint::calculateTransforms()
    378335{
    379         m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA;
    380         m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB;
    381         calculateLinearInfo();
    382         calculateAngleInfo();
    383 }
    384 
    385 //-----------------------------------------------------------------------------
     336    m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA;
     337    m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB;
     338
     339    calculateAngleInfo();
     340}
     341
    386342
    387343void btGeneric6DofConstraint::buildLinearJacobian(
    388         btJacobianEntry & jacLinear,const btVector3 & normalWorld,
    389         const btVector3 & pivotAInW,const btVector3 & pivotBInW)
    390 {
    391         new (&jacLinear) btJacobianEntry(
     344    btJacobianEntry & jacLinear,const btVector3 & normalWorld,
     345    const btVector3 & pivotAInW,const btVector3 & pivotBInW)
     346{
     347    new (&jacLinear) btJacobianEntry(
    392348        m_rbA.getCenterOfMassTransform().getBasis().transpose(),
    393349        m_rbB.getCenterOfMassTransform().getBasis().transpose(),
    … …  
    399355        m_rbB.getInvInertiaDiagLocal(),
    400356        m_rbB.getInvMass());
    401 }
    402 
    403 //-----------------------------------------------------------------------------
     357
     358}
    404359
    405360void btGeneric6DofConstraint::buildAngularJacobian(
    406         btJacobianEntry & jacAngular,const btVector3 & jointAxisW)
    407 {
    408         new (&jacAngular)      btJacobianEntry(jointAxisW,
     361    btJacobianEntry & jacAngular,const btVector3 & jointAxisW)
     362{
     363    new (&jacAngular)   btJacobianEntry(jointAxisW,
    409364                                      m_rbA.getCenterOfMassTransform().getBasis().transpose(),
    410365                                      m_rbB.getCenterOfMassTransform().getBasis().transpose(),
    … …  
    414369}
    415370
    416 //-----------------------------------------------------------------------------
    417 
    418371bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index)
    419372{
    420         btScalar angle = m_calculatedAxisAngleDiff[axis_index];
    421         //test limits
    422         m_angularLimits[axis_index].testLimitValue(angle);
    423         return m_angularLimits[axis_index].needApplyTorques();
    424 }
    425 
    426 //-----------------------------------------------------------------------------
     373    btScalar angle = m_calculatedAxisAngleDiff[axis_index];
     374
     375    //test limits
     376    m_angularLimits[axis_index].testLimitValue(angle);
     377    return m_angularLimits[axis_index].needApplyTorques();
     378}
    427379
    428380void btGeneric6DofConstraint::buildJacobian()
    429381{
    430         if (m_useSolveConstraintObsolete)
    431         {
    432 
    433                 // Clear accumulated impulses for the next simulation step
    434                 m_linearLimits.m_accumulatedImpulse.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
    435                 int i;
    436                 for(i = 0; i < 3; i++)
    437                 {
    438                         m_angularLimits[i].m_accumulatedImpulse = btScalar(0.);
    439                 }
    440                 //calculates transform
    441                 calculateTransforms();
    442 
    443                 //  const btVector3& pivotAInW = m_calculatedTransformA.getOrigin();
    444                 //  const btVector3& pivotBInW = m_calculatedTransformB.getOrigin();
    445                 calcAnchorPos();
    446                 btVector3 pivotAInW = m_AnchorPos;
    447                 btVector3 pivotBInW = m_AnchorPos;
    448 
    449                 // not used here
    450                 //    btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
    451                 //    btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
    452 
    453                 btVector3 normalWorld;
    454                 //linear part
    455                 for (i=0;i<3;i++)
    456                 {
    457                         if (m_linearLimits.isLimited(i))
    458                         {
    459                                 if (m_useLinearReferenceFrameA)
    460                                         normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
    461                                 else
    462                                         normalWorld = m_calculatedTransformB.getBasis().getColumn(i);
    463 
    464                                 buildLinearJacobian(
    465                                         m_jacLinear[i],normalWorld ,
    466                                         pivotAInW,pivotBInW);
    467 
    468                         }
    469                 }
    470 
    471                 // angular part
    472                 for (i=0;i<3;i++)
    473                 {
    474                         //calculates error angle
    475                         if (testAngularLimitMotor(i))
    476                         {
    477                                 normalWorld = this->getAxis(i);
    478                                 // Create angular atom
    479                                 buildAngularJacobian(m_jacAng[i],normalWorld);
    480                         }
    481                 }
    482 
    483         }
    484 }
    485 
    486 //-----------------------------------------------------------------------------
    487 
    488 void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info)
    489 {
    490         if (m_useSolveConstraintObsolete)
    491         {
    492                 info->m_numConstraintRows = 0;
    493                 info->nub = 0;
    494         } else
    495         {
    496                 //prepare constraint
    497                 calculateTransforms();
    498                 info->m_numConstraintRows = 0;
    499                 info->nub = 6;
    500                 int i;
    501                 //test linear limits
    502                 for(i = 0; i < 3; i++)
    503                 {
    504                         if(m_linearLimits.needApplyForce(i))
    505                         {
    506                                 info->m_numConstraintRows++;
    507                                 info->nub--;
    508                         }
    509                 }
    510                 //test angular limits
    511                 for (i=0;i<3 ;i++ )
    512                 {
    513                         if(testAngularLimitMotor(i))
    514                         {
    515                                 info->m_numConstraintRows++;
    516                                 info->nub--;
    517                         }
    518                 }
    519         }
    520 }
    521 
    522 //-----------------------------------------------------------------------------
    523 
    524 void btGeneric6DofConstraint::getInfo2 (btConstraintInfo2* info)
    525 {
    526         btAssert(!m_useSolveConstraintObsolete);
    527         int row = setLinearLimits(info);
    528         setAngularLimits(info, row);
    529 }
    530 
    531 //-----------------------------------------------------------------------------
    532 
    533 int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info)
    534 {
    535         btGeneric6DofConstraint * d6constraint = this;
    536         int row = 0;
    537         //solve linear limits
    538         btRotationalLimitMotor limot;
    539         for (int i=0;i<3 ;i++ )
    540         {
    541                 if(m_linearLimits.needApplyForce(i))
    542                 { // re-use rotational motor code
    543                         limot.m_bounce = btScalar(0.f);
    544                         limot.m_currentLimit = m_linearLimits.m_currentLimit[i];
    545                         limot.m_currentLimitError  = m_linearLimits.m_currentLimitError[i];
    546                         limot.m_damping  = m_linearLimits.m_damping;
    547                         limot.m_enableMotor  = m_linearLimits.m_enableMotor[i];
    548                         limot.m_ERP  = m_linearLimits.m_restitution;
    549                         limot.m_hiLimit  = m_linearLimits.m_upperLimit[i];
    550                         limot.m_limitSoftness  = m_linearLimits.m_limitSoftness;
    551                         limot.m_loLimit  = m_linearLimits.m_lowerLimit[i];
    552                         limot.m_maxLimitForce  = btScalar(0.f);
    553                         limot.m_maxMotorForce  = m_linearLimits.m_maxMotorForce[i];
    554                         limot.m_targetVelocity  = m_linearLimits.m_targetVelocity[i];
    555                         btVector3 axis = m_calculatedTransformA.getBasis().getColumn(i);
    556                         row += get_limit_motor_info2(&limot, &m_rbA, &m_rbB, info, row, axis, 0);
    557                 }
    558         }
    559         return row;
    560 }
    561 
    562 //-----------------------------------------------------------------------------
    563 
    564 int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_offset)
    565 {
    566         btGeneric6DofConstraint * d6constraint = this;
    567         int row = row_offset;
    568         //solve angular limits
    569         for (int i=0;i<3 ;i++ )
    570         {
    571                 if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques())
    572                 {
    573                         btVector3 axis = d6constraint->getAxis(i);
    574                         row += get_limit_motor_info2(
    575                                 d6constraint->getRotationalLimitMotor(i),
    576                                 &m_rbA,
    577                                 &m_rbB,
    578                                 info,row,axis,1);
    579                 }
    580         }
    581 
    582         return row;
    583 }
    584 
    585 //-----------------------------------------------------------------------------
    586 
    587 void btGeneric6DofConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar  timeStep)
    588 {
    589         if (m_useSolveConstraintObsolete)
    590         {
    591 
    592 
    593                 m_timeStep = timeStep;
    594 
    595                 //calculateTransforms();
    596 
    597                 int i;
    598 
    599                 // linear
    600 
    601                 btVector3 pointInA = m_calculatedTransformA.getOrigin();
    602                 btVector3 pointInB = m_calculatedTransformB.getOrigin();
    603 
    604                 btScalar jacDiagABInv;
    605                 btVector3 linear_axis;
    606                 for (i=0;i<3;i++)
    607                 {
    608                         if (m_linearLimits.isLimited(i))
    609                         {
    610                                 jacDiagABInv = btScalar(1.) / m_jacLinear[i].getDiagonal();
    611 
    612                                 if (m_useLinearReferenceFrameA)
    613                                         linear_axis = m_calculatedTransformA.getBasis().getColumn(i);
    614                                 else
    615                                         linear_axis = m_calculatedTransformB.getBasis().getColumn(i);
    616 
    617                                 m_linearLimits.solveLinearAxis(
    618                                         m_timeStep,
    619                                         jacDiagABInv,
    620                                         m_rbA,bodyA,pointInA,
    621                                         m_rbB,bodyB,pointInB,
    622                                         i,linear_axis, m_AnchorPos);
    623 
    624                         }
    625                 }
    626 
    627                 // angular
    628                 btVector3 angular_axis;
    629                 btScalar angularJacDiagABInv;
    630                 for (i=0;i<3;i++)
    631                 {
    632                         if (m_angularLimits[i].needApplyTorques())
    633                         {
    634 
    635                                 // get axis
    636                                 angular_axis = getAxis(i);
    637 
    638                                 angularJacDiagABInv = btScalar(1.) / m_jacAng[i].getDiagonal();
    639 
    640                                 m_angularLimits[i].solveAngularLimits(m_timeStep,angular_axis,angularJacDiagABInv, &m_rbA,bodyA,&m_rbB,bodyB);
    641                         }
    642                 }
    643         }
    644 }
    645 
    646 //-----------------------------------------------------------------------------
     382
     383        // Clear accumulated impulses for the next simulation step
     384    m_linearLimits.m_accumulatedImpulse.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
     385    int i;
     386    for(i = 0; i < 3; i++)
     387    {
     388        m_angularLimits[i].m_accumulatedImpulse = btScalar(0.);
     389    }
     390    //calculates transform
     391    calculateTransforms();
     392
     393//  const btVector3& pivotAInW = m_calculatedTransformA.getOrigin();
     394//  const btVector3& pivotBInW = m_calculatedTransformB.getOrigin();
     395        calcAnchorPos();
     396        btVector3 pivotAInW = m_AnchorPos;
     397        btVector3 pivotBInW = m_AnchorPos;
     398
     399// not used here
     400//    btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
     401//    btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
     402
     403    btVector3 normalWorld;
     404    //linear part
     405    for (i=0;i<3;i++)
     406    {
     407        if (m_linearLimits.isLimited(i))
     408        {
     409                        if (m_useLinearReferenceFrameA)
     410                    normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
     411                        else
     412                    normalWorld = m_calculatedTransformB.getBasis().getColumn(i);
     413
     414            buildLinearJacobian(
     415                m_jacLinear[i],normalWorld ,
     416                pivotAInW,pivotBInW);
     417
     418        }
     419    }
     420
     421    // angular part
     422    for (i=0;i<3;i++)
     423    {
     424        //calculates error angle
     425        if (testAngularLimitMotor(i))
     426        {
     427            normalWorld = this->getAxis(i);
     428            // Create angular atom
     429            buildAngularJacobian(m_jacAng[i],normalWorld);
     430        }
     431    }
     432
     433
     434}
     435
     436
     437void btGeneric6DofConstraint::solveConstraint(btScalar  timeStep)
     438{
     439    m_timeStep = timeStep;
     440
     441    //calculateTransforms();
     442
     443    int i;
     444
     445    // linear
     446
     447    btVector3 pointInA = m_calculatedTransformA.getOrigin();
     448        btVector3 pointInB = m_calculatedTransformB.getOrigin();
     449
     450        btScalar jacDiagABInv;
     451        btVector3 linear_axis;
     452    for (i=0;i<3;i++)
     453    {
     454        if (m_linearLimits.isLimited(i))
     455        {
     456            jacDiagABInv = btScalar(1.) / m_jacLinear[i].getDiagonal();
     457
     458                        if (m_useLinearReferenceFrameA)
     459                    linear_axis = m_calculatedTransformA.getBasis().getColumn(i);
     460                        else
     461                    linear_axis = m_calculatedTransformB.getBasis().getColumn(i);
     462
     463            m_linearLimits.solveLinearAxis(
     464                m_timeStep,
     465                jacDiagABInv,
     466                m_rbA,pointInA,
     467                m_rbB,pointInB,
     468                i,linear_axis, m_AnchorPos);
     469
     470        }
     471    }
     472
     473    // angular
     474    btVector3 angular_axis;
     475    btScalar angularJacDiagABInv;
     476    for (i=0;i<3;i++)
     477    {
     478        if (m_angularLimits[i].needApplyTorques())
     479        {
     480
     481                        // get axis
     482                        angular_axis = getAxis(i);
     483
     484                        angularJacDiagABInv = btScalar(1.) / m_jacAng[i].getDiagonal();
     485
     486                        m_angularLimits[i].solveAngularLimits(m_timeStep,angular_axis,angularJacDiagABInv, &m_rbA,&m_rbB);
     487        }
     488    }
     489}
    647490
    648491void    btGeneric6DofConstraint::updateRHS(btScalar     timeStep)
    649492{
    650         (void)timeStep;
    651 
    652 }
    653 
    654 //-----------------------------------------------------------------------------
     493    (void)timeStep;
     494
     495}
    655496
    656497btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const
    657498{
    658         return m_calculatedAxis[axis_index];
    659 }
    660 
    661 //-----------------------------------------------------------------------------
     499    return m_calculatedAxis[axis_index];
     500}
    662501
    663502btScalar btGeneric6DofConstraint::getAngle(int axis_index) const
    664503{
    665         return m_calculatedAxisAngleDiff[axis_index];
    666 }
    667 
    668 //-----------------------------------------------------------------------------
     504    return m_calculatedAxisAngleDiff[axis_index];
     505}
    669506
    670507void btGeneric6DofConstraint::calcAnchorPos(void)
    … …  
    687524} // btGeneric6DofConstraint::calcAnchorPos()
    688525
    689 //-----------------------------------------------------------------------------
    690 
    691 void btGeneric6DofConstraint::calculateLinearInfo()
    692 {
    693         m_calculatedLinearDiff = m_calculatedTransformB.getOrigin() - m_calculatedTransformA.getOrigin();
    694         m_calculatedLinearDiff = m_calculatedTransformA.getBasis().inverse() * m_calculatedLinearDiff;
    695         for(int i = 0; i < 3; i++)
    696         {
    697                 m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]);
    698         }
    699 } // btGeneric6DofConstraint::calculateLinearInfo()
    700 
    701 //-----------------------------------------------------------------------------
    702 
    703 int btGeneric6DofConstraint::get_limit_motor_info2(
    704         btRotationalLimitMotor * limot,
    705         btRigidBody * body0, btRigidBody * body1,
    706         btConstraintInfo2 *info, int row, btVector3& ax1, int rotational)
    707 {
    708     int srow = row * info->rowskip;
    709     int powered = limot->m_enableMotor;
    710     int limit = limot->m_currentLimit;
    711     if (powered || limit)
    712     {   // if the joint is powered, or has joint limits, add in the extra row
    713         btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
    714         btScalar *J2 = rotational ? info->m_J2angularAxis : 0;
    715         J1[srow+0] = ax1[0];
    716         J1[srow+1] = ax1[1];
    717         J1[srow+2] = ax1[2];
    718         if(rotational)
    719         {
    720             J2[srow+0] = -ax1[0];
    721             J2[srow+1] = -ax1[1];
    722             J2[srow+2] = -ax1[2];
    723         }
    724         if((!rotational) && limit)
    725         {
    726                         btVector3 ltd;  // Linear Torque Decoupling vector
    727                         btVector3 c = m_calculatedTransformB.getOrigin() - body0->getCenterOfMassPosition();
    728                         ltd = c.cross(ax1);
    729             info->m_J1angularAxis[srow+0] = ltd[0];
    730             info->m_J1angularAxis[srow+1] = ltd[1];
    731             info->m_J1angularAxis[srow+2] = ltd[2];
    732 
    733                         c = m_calculatedTransformB.getOrigin() - body1->getCenterOfMassPosition();
    734                         ltd = -c.cross(ax1);
    735                         info->m_J2angularAxis[srow+0] = ltd[0];
    736             info->m_J2angularAxis[srow+1] = ltd[1];
    737             info->m_J2angularAxis[srow+2] = ltd[2];
    738         }
    739         // if we're limited low and high simultaneously, the joint motor is
    740         // ineffective
    741         if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = 0;
    742         info->m_constraintError[srow] = btScalar(0.f);
    743         if (powered)
    744         {
    745             info->cfm[srow] = 0.0f;
    746             if(!limit)
    747             {
    748                 info->m_constraintError[srow] += limot->m_targetVelocity;
    749                 info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
    750                 info->m_upperLimit[srow] = limot->m_maxMotorForce;
    751             }
    752         }
    753         if(limit)
    754         {
    755             btScalar k = info->fps * limot->m_ERP;
    756                         if(!rotational)
    757                         {
    758                                 info->m_constraintError[srow] += k * limot->m_currentLimitError;
    759                         }
    760                         else
    761                         {
    762                                 info->m_constraintError[srow] += -k * limot->m_currentLimitError;
    763                         }
    764             info->cfm[srow] = 0.0f;
    765             if (limot->m_loLimit == limot->m_hiLimit)
    766             {   // limited low and high simultaneously
    767                 info->m_lowerLimit[srow] = -SIMD_INFINITY;
    768                 info->m_upperLimit[srow] = SIMD_INFINITY;
    769             }
    770             else
    771             {
    772                 if (limit == 1)
    773                 {
    774                     info->m_lowerLimit[srow] = 0;
    775                     info->m_upperLimit[srow] = SIMD_INFINITY;
    776                 }
    777                 else
    778                 {
    779                     info->m_lowerLimit[srow] = -SIMD_INFINITY;
    780                     info->m_upperLimit[srow] = 0;
    781                 }
    782                 // deal with bounce
    783                 if (limot->m_bounce > 0)
    784                 {
    785                     // calculate joint velocity
    786                     btScalar vel;
    787                     if (rotational)
    788                     {
    789                         vel = body0->getAngularVelocity().dot(ax1);
    790                         if (body1)
    791                             vel -= body1->getAngularVelocity().dot(ax1);
    792                     }
    793                     else
    794                     {
    795                         vel = body0->getLinearVelocity().dot(ax1);
    796                         if (body1)
    797                             vel -= body1->getLinearVelocity().dot(ax1);
    798                     }
    799                     // only apply bounce if the velocity is incoming, and if the
    800                     // resulting c[] exceeds what we already have.
    801                     if (limit == 1)
    802                     {
    803                         if (vel < 0)
    804                         {
    805                             btScalar newc = -limot->m_bounce* vel;
    806                             if (newc > info->m_constraintError[srow])
    807                                                                 info->m_constraintError[srow] = newc;
    808                         }
    809                     }
    810                     else
    811                     {
    812                         if (vel > 0)
    813                         {
    814                             btScalar newc = -limot->m_bounce * vel;
    815                             if (newc < info->m_constraintError[srow])
    816                                                                 info->m_constraintError[srow] = newc;
    817                         }
    818                     }
    819                 }
    820             }
    821         }
    822         return 1;
    823     }
    824     else return 0;
    825 }
    826 
    827 //-----------------------------------------------------------------------------
    828 //-----------------------------------------------------------------------------
    829 //-----------------------------------------------------------------------------

  • code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h

    r2907 r2908  
    2929
    3030class btRigidBody;
    31 
    32 
    3331
    3432
    … …  
    9593    bool isLimited()
    9694    {
    97         if(m_loLimit > m_hiLimit) return false;
     95        if(m_loLimit>=m_hiLimit) return false;
    9896        return true;
    9997    }
    … …  
    113111
    114112        //! apply the correction impulses for two bodies
    115     btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btSolverBody& bodyA,btRigidBody * body1,btSolverBody& bodyB);
     113    btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btRigidBody * body1);
     114
    116115
    117116};
    … …  
    131130    btScalar    m_restitution;//! Bounce parameter for linear limit
    132131    //!@}
    133         bool            m_enableMotor[3];
    134     btVector3   m_targetVelocity;//!< target motor velocity
    135     btVector3   m_maxMotorForce;//!< max force on motor
    136     btVector3   m_currentLimitError;//!  How much is violated this limit
    137     int                 m_currentLimit[3];//!< 0=free, 1=at lower limit, 2=at upper limit
    138132
    139133    btTranslationalLimitMotor()
    … …  
    146140        m_damping = btScalar(1.0f);
    147141        m_restitution = btScalar(0.5f);
    148                 for(int i=0; i < 3; i++)
    149                 {
    150                         m_enableMotor[i] = false;
    151                         m_targetVelocity[i] = btScalar(0.f);
    152                         m_maxMotorForce[i] = btScalar(0.f);
    153                 }
    154142    }
    155143
    … …  
    163151        m_damping = other.m_damping;
    164152        m_restitution = other.m_restitution;
    165                 for(int i=0; i < 3; i++)
    166                 {
    167                         m_enableMotor[i] = other.m_enableMotor[i];
    168                         m_targetVelocity[i] = other.m_targetVelocity[i];
    169                         m_maxMotorForce[i] = other.m_maxMotorForce[i];
    170                 }
    171153    }
    172154
    … …  
    182164       return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
    183165    }
    184     inline bool needApplyForce(int limitIndex)
    185     {
    186         if(m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false;
    187         return true;
    188     }
    189         int testLimitValue(int limitIndex, btScalar test_value);
    190166
    191167
    … …  
    193169        btScalar timeStep,
    194170        btScalar jacDiagABInv,
    195         btRigidBody& body1,btSolverBody& bodyA,const btVector3 &pointInA,
    196         btRigidBody& body2,btSolverBody& bodyB,const btVector3 &pointInB,
     171        btRigidBody& body1,const btVector3 &pointInA,
     172        btRigidBody& body2,const btVector3 &pointInB,
    197173        int limit_index,
    198174        const btVector3 & axis_normal_on_a,
    … …  
    272248    btVector3 m_calculatedAxisAngleDiff;
    273249    btVector3 m_calculatedAxis[3];
    274     btVector3 m_calculatedLinearDiff;
    275250   
    276251        btVector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes
    … …  
    288263
    289264
    290         int setAngularLimits(btConstraintInfo2 *info, int row_offset);
    291 
    292         int setLinearLimits(btConstraintInfo2 *info);
    293265
    294266    void buildLinearJacobian(
    … …  
    298270    void buildAngularJacobian(btJacobianEntry & jacAngular,const btVector3 & jointAxisW);
    299271
    300         // tests linear limits
    301         void calculateLinearInfo();
    302272
    303273        //! calcs the euler angles between the two bodies.
    … …  
    307277
    308278public:
    309 
    310         ///for backwards compatibility during the transition to 'getInfo/getInfo2'
    311         bool            m_useSolveConstraintObsolete;
    312 
    313279    btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
    314280
    … …  
    365331    virtual void        buildJacobian();
    366332
    367         virtual void getInfo1 (btConstraintInfo1* info);
    368 
    369         virtual void getInfo2 (btConstraintInfo2* info);
    370 
    371     virtual     void    solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar        timeStep);
     333    virtual     void    solveConstraint(btScalar        timeStep);
    372334
    373335    void        updateRHS(btScalar      timeStep);
    … …  
    471433        virtual void calcAnchorPos(void); // overridable
    472434
    473         int get_limit_motor_info2(      btRotationalLimitMotor * limot,
    474                                                                 btRigidBody * body0, btRigidBody * body1,
    475                                                                 btConstraintInfo2 *info, int row, btVector3& ax1, int rotational);
    476 
    477 
    478435};
    479436

  • code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp

    r2907 r2908  
    2020#include "LinearMath/btMinMax.h"
    2121#include <new>
    22 #include "btSolverBody.h"
    23 
    24 //-----------------------------------------------------------------------------
    25 
    26 #define HINGE_USE_OBSOLETE_SOLVER false
    27 
    28 //-----------------------------------------------------------------------------
    2922
    3023
    3124btHingeConstraint::btHingeConstraint()
    3225: btTypedConstraint (HINGE_CONSTRAINT_TYPE),
    33 m_enableAngularMotor(false),
    34 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
    35 m_useReferenceFrameA(false)
    36 {
    37         m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
    38 }
    39 
    40 //-----------------------------------------------------------------------------
     26m_enableAngularMotor(false)
     27{
     28}
    4129
    4230btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,
    43                                                                          btVector3& axisInA,btVector3& axisInB, bool useReferenceFrameA)
     31                                                                         btVector3& axisInA,btVector3& axisInB)
    4432                                                                         :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),
    4533                                                                         m_angularOnly(false),
    46                                                                          m_enableAngularMotor(false),
    47                                                                          m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
    48                                                                          m_useReferenceFrameA(useReferenceFrameA)
     34                                                                         m_enableAngularMotor(false)
    4935{
    5036        m_rbAFrame.getOrigin() = pivotInA;
    … …  
    7561       
    7662        m_rbBFrame.getOrigin() = pivotInB;
    77         m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
    78                                                                         rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
    79                                                                         rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
     63        m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),-axisInB.getX(),
     64                                                                        rbAxisB1.getY(),rbAxisB2.getY(),-axisInB.getY(),
     65                                                                        rbAxisB1.getZ(),rbAxisB2.getZ(),-axisInB.getZ() );
    8066       
    8167        //start with free
    … …  
    8672        m_limitSoftness = 0.9f;
    8773        m_solveLimit = false;
    88         m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
    89 }
    90 
    91 //-----------------------------------------------------------------------------
    92 
    93 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA, bool useReferenceFrameA)
    94 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false),
    95 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
    96 m_useReferenceFrameA(useReferenceFrameA)
     74
     75}
     76
     77
     78
     79btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA)
     80:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false)
    9781{
    9882
    … …  
    10791                                                                        rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
    10892
    109         btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * axisInA;
     93        btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * -axisInA;
    11094
    11195        btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
    … …  
    126110        m_limitSoftness = 0.9f;
    127111        m_solveLimit = false;
    128         m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
    129 }
    130 
    131 //-----------------------------------------------------------------------------
     112}
    132113
    133114btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB,
    134                                                                      const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA)
     115                                                                     const btTransform& rbAFrame, const btTransform& rbBFrame)
    135116:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
    136117m_angularOnly(false),
    137 m_enableAngularMotor(false),
    138 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
    139 m_useReferenceFrameA(useReferenceFrameA)
    140 {
     118m_enableAngularMotor(false)
     119{
     120        // flip axis
     121        m_rbBFrame.getBasis()[0][2] *= btScalar(-1.);
     122        m_rbBFrame.getBasis()[1][2] *= btScalar(-1.);
     123        m_rbBFrame.getBasis()[2][2] *= btScalar(-1.);
     124
    141125        //start with free
    142126        m_lowerLimit = btScalar(1e30);
    … …  
    146130        m_limitSoftness = 0.9f;
    147131        m_solveLimit = false;
    148         m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
    149132}                       
    150133
    151 //-----------------------------------------------------------------------------
    152 
    153 btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame, bool useReferenceFrameA)
     134
     135
     136btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame)
    154137:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame),
    155138m_angularOnly(false),
    156 m_enableAngularMotor(false),
    157 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
    158 m_useReferenceFrameA(useReferenceFrameA)
     139m_enableAngularMotor(false)
    159140{
    160141        ///not providing rigidbody B means implicitly using worldspace for body B
     142
     143        // flip axis
     144        m_rbBFrame.getBasis()[0][2] *= btScalar(-1.);
     145        m_rbBFrame.getBasis()[1][2] *= btScalar(-1.);
     146        m_rbBFrame.getBasis()[2][2] *= btScalar(-1.);
    161147
    162148        m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(m_rbAFrame.getOrigin());
    … …  
    169155        m_limitSoftness = 0.9f;
    170156        m_solveLimit = false;
    171         m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
    172 }
    173 
    174 //-----------------------------------------------------------------------------
     157}
    175158
    176159void    btHingeConstraint::buildJacobian()
    177160{
    178         if (m_useSolveConstraintObsolete)
     161        m_appliedImpulse = btScalar(0.);
     162
     163        if (!m_angularOnly)
    179164        {
    180                 m_appliedImpulse = btScalar(0.);
    181 
    182                 if (!m_angularOnly)
    183                 {
    184                         btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
    185                         btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
    186                         btVector3 relPos = pivotBInW - pivotAInW;
    187 
    188                         btVector3 normal[3];
    189                         if (relPos.length2() > SIMD_EPSILON)
    190                         {
    191                                 normal[0] = relPos.normalized();
    192                         }
    193                         else
    194                         {
    195                                 normal[0].setValue(btScalar(1.0),0,0);
    196                         }
    197 
    198                         btPlaneSpace1(normal[0], normal[1], normal[2]);
    199 
    200                         for (int i=0;i<3;i++)
    201                         {
    202                                 new (&m_jac[i]) btJacobianEntry(
     165                btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
     166                btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
     167                btVector3 relPos = pivotBInW - pivotAInW;
     168
     169                btVector3 normal[3];
     170                if (relPos.length2() > SIMD_EPSILON)
     171                {
     172                        normal[0] = relPos.normalized();
     173                }
     174                else
     175                {
     176                        normal[0].setValue(btScalar(1.0),0,0);
     177                }
     178
     179                btPlaneSpace1(normal[0], normal[1], normal[2]);
     180
     181                for (int i=0;i<3;i++)
     182                {
     183                        new (&m_jac[i]) btJacobianEntry(
    203184                                m_rbA.getCenterOfMassTransform().getBasis().transpose(),
    204185                                m_rbB.getCenterOfMassTransform().getBasis().transpose(),
    … …  
    210191                                m_rbB.getInvInertiaDiagLocal(),
    211192                                m_rbB.getInvMass());
     193                }
     194        }
     195
     196        //calculate two perpendicular jointAxis, orthogonal to hingeAxis
     197        //these two jointAxis require equal angular velocities for both bodies
     198
     199        //this is unused for now, it's a todo
     200        btVector3 jointAxis0local;
     201        btVector3 jointAxis1local;
     202       
     203        btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local);
     204
     205        getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
     206        btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local;
     207        btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local;
     208        btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
     209               
     210        new (&m_jacAng[0])      btJacobianEntry(jointAxis0,
     211                m_rbA.getCenterOfMassTransform().getBasis().transpose(),
     212                m_rbB.getCenterOfMassTransform().getBasis().transpose(),
     213                m_rbA.getInvInertiaDiagLocal(),
     214                m_rbB.getInvInertiaDiagLocal());
     215
     216        new (&m_jacAng[1])      btJacobianEntry(jointAxis1,
     217                m_rbA.getCenterOfMassTransform().getBasis().transpose(),
     218                m_rbB.getCenterOfMassTransform().getBasis().transpose(),
     219                m_rbA.getInvInertiaDiagLocal(),
     220                m_rbB.getInvInertiaDiagLocal());
     221
     222        new (&m_jacAng[2])      btJacobianEntry(hingeAxisWorld,
     223                m_rbA.getCenterOfMassTransform().getBasis().transpose(),
     224                m_rbB.getCenterOfMassTransform().getBasis().transpose(),
     225                m_rbA.getInvInertiaDiagLocal(),
     226                m_rbB.getInvInertiaDiagLocal());
     227
     228
     229        // Compute limit information
     230        btScalar hingeAngle = getHingeAngle(); 
     231
     232        //set bias, sign, clear accumulator
     233        m_correction = btScalar(0.);
     234        m_limitSign = btScalar(0.);
     235        m_solveLimit = false;
     236        m_accLimitImpulse = btScalar(0.);
     237
     238//      if (m_lowerLimit < m_upperLimit)
     239        if (m_lowerLimit <= m_upperLimit)
     240        {
     241//              if (hingeAngle <= m_lowerLimit*m_limitSoftness)
     242                if (hingeAngle <= m_lowerLimit)
     243                {
     244                        m_correction = (m_lowerLimit - hingeAngle);
     245                        m_limitSign = 1.0f;
     246                        m_solveLimit = true;
     247                }
     248//              else if (hingeAngle >= m_upperLimit*m_limitSoftness)
     249                else if (hingeAngle >= m_upperLimit)
     250                {
     251                        m_correction = m_upperLimit - hingeAngle;
     252                        m_limitSign = -1.0f;
     253                        m_solveLimit = true;
     254                }
     255        }
     256
     257        //Compute K = J*W*J' for hinge axis
     258        btVector3 axisA =  getRigidBodyA().getCenterOfMassTransform().getBasis() *  m_rbAFrame.getBasis().getColumn(2);
     259        m_kHinge =   1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) +
     260                                     getRigidBodyB().computeAngularImpulseDenominator(axisA));
     261
     262}
     263
     264void    btHingeConstraint::solveConstraint(btScalar     timeStep)
     265{
     266
     267        btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
     268        btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
     269
     270        btScalar tau = btScalar(0.3);
     271
     272        //linear part
     273        if (!m_angularOnly)
     274        {
     275                btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
     276                btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
     277
     278                btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
     279                btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
     280                btVector3 vel = vel1 - vel2;
     281
     282                for (int i=0;i<3;i++)
     283                {               
     284                        const btVector3& normal = m_jac[i].m_linearJointAxis;
     285                        btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
     286
     287                        btScalar rel_vel;
     288                        rel_vel = normal.dot(vel);
     289                        //positional error (zeroth order error)
     290                        btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
     291                        btScalar impulse = depth*tau/timeStep  * jacDiagABInv -  rel_vel * jacDiagABInv;
     292                        m_appliedImpulse += impulse;
     293                        btVector3 impulse_vector = normal * impulse;
     294                        m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
     295                        m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
     296                }
     297        }
     298
     299       
     300        {
     301                ///solve angular part
     302
     303                // get axes in world space
     304                btVector3 axisA =  getRigidBodyA().getCenterOfMassTransform().getBasis() *  m_rbAFrame.getBasis().getColumn(2);
     305                btVector3 axisB =  getRigidBodyB().getCenterOfMassTransform().getBasis() *  m_rbBFrame.getBasis().getColumn(2);
     306
     307                const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
     308                const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
     309
     310                btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA);
     311                btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB);
     312
     313                btVector3 angAorthog = angVelA - angVelAroundHingeAxisA;
     314                btVector3 angBorthog = angVelB - angVelAroundHingeAxisB;
     315                btVector3 velrelOrthog = angAorthog-angBorthog;
     316                {
     317                        //solve orthogonal angular velocity correction
     318                        btScalar relaxation = btScalar(1.);
     319                        btScalar len = velrelOrthog.length();
     320                        if (len > btScalar(0.00001))
     321                        {
     322                                btVector3 normal = velrelOrthog.normalized();
     323                                btScalar denom = getRigidBodyA().computeAngularImpulseDenominator(normal) +
     324                                        getRigidBodyB().computeAngularImpulseDenominator(normal);
     325                                // scale for mass and relaxation
     326                                velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor;
    212327                        }
    213                 }
    214 
    215                 //calculate two perpendicular jointAxis, orthogonal to hingeAxis
    216                 //these two jointAxis require equal angular velocities for both bodies
    217 
    218                 //this is unused for now, it's a todo
    219                 btVector3 jointAxis0local;
    220                 btVector3 jointAxis1local;
    221                
    222                 btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local);
    223 
    224                 getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
    225                 btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local;
    226                 btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local;
    227                 btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
     328
     329                        //solve angular positional correction
     330                        btVector3 angularError = -axisA.cross(axisB) *(btScalar(1.)/timeStep);
     331                        btScalar len2 = angularError.length();
     332                        if (len2>btScalar(0.00001))
     333                        {
     334                                btVector3 normal2 = angularError.normalized();
     335                                btScalar denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) +
     336                                                getRigidBodyB().computeAngularImpulseDenominator(normal2);
     337                                angularError *= (btScalar(1.)/denom2) * relaxation;
     338                        }
     339
     340                        m_rbA.applyTorqueImpulse(-velrelOrthog+angularError);
     341                        m_rbB.applyTorqueImpulse(velrelOrthog-angularError);
     342
     343                        // solve limit
     344                        if (m_solveLimit)
     345                        {
     346                                btScalar amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (btScalar(1.)/timeStep)*m_biasFactor  ) * m_limitSign;
     347
     348                                btScalar impulseMag = amplitude * m_kHinge;
     349
     350                                // Clamp the accumulated impulse
     351                                btScalar temp = m_accLimitImpulse;
     352                                m_accLimitImpulse = btMax(m_accLimitImpulse + impulseMag, btScalar(0) );
     353                                impulseMag = m_accLimitImpulse - temp;
     354
     355
     356                                btVector3 impulse = axisA * impulseMag * m_limitSign;
     357                                m_rbA.applyTorqueImpulse(impulse);
     358                                m_rbB.applyTorqueImpulse(-impulse);
     359                        }
     360                }
     361
     362                //apply motor
     363                if (m_enableAngularMotor)
     364                {
     365                        //todo: add limits too
     366                        btVector3 angularLimit(0,0,0);
     367
     368                        btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB;
     369                        btScalar projRelVel = velrel.dot(axisA);
     370
     371                        btScalar desiredMotorVel = m_motorTargetVelocity;
     372                        btScalar motor_relvel = desiredMotorVel - projRelVel;
     373
     374                        btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;;
     375                        //todo: should clip against accumulated impulse
     376                        btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse;
     377                        clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse;
     378                        btVector3 motorImp = clippedMotorImpulse * axisA;
     379
     380                        m_rbA.applyTorqueImpulse(motorImp+angularLimit);
     381                        m_rbB.applyTorqueImpulse(-motorImp-angularLimit);
    228382                       
    229                 new (&m_jacAng[0])      btJacobianEntry(jointAxis0,
    230                         m_rbA.getCenterOfMassTransform().getBasis().transpose(),
    231                         m_rbB.getCenterOfMassTransform().getBasis().transpose(),
    232                         m_rbA.getInvInertiaDiagLocal(),
    233                         m_rbB.getInvInertiaDiagLocal());
    234 
    235                 new (&m_jacAng[1])      btJacobianEntry(jointAxis1,
    236                         m_rbA.getCenterOfMassTransform().getBasis().transpose(),
    237                         m_rbB.getCenterOfMassTransform().getBasis().transpose(),
    238                         m_rbA.getInvInertiaDiagLocal(),
    239                         m_rbB.getInvInertiaDiagLocal());
    240 
    241                 new (&m_jacAng[2])      btJacobianEntry(hingeAxisWorld,
    242                         m_rbA.getCenterOfMassTransform().getBasis().transpose(),
    243                         m_rbB.getCenterOfMassTransform().getBasis().transpose(),
    244                         m_rbA.getInvInertiaDiagLocal(),
    245                         m_rbB.getInvInertiaDiagLocal());
    246 
    247                         // clear accumulator
    248                         m_accLimitImpulse = btScalar(0.);
    249 
    250                         // test angular limit
    251                         testLimit();
    252 
    253                 //Compute K = J*W*J' for hinge axis
    254                 btVector3 axisA =  getRigidBodyA().getCenterOfMassTransform().getBasis() *  m_rbAFrame.getBasis().getColumn(2);
    255                 m_kHinge =   1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) +
    256                                                          getRigidBodyB().computeAngularImpulseDenominator(axisA));
    257 
     383                }
    258384        }
    259 }
    260 
    261 //-----------------------------------------------------------------------------
    262 
    263 void btHingeConstraint::getInfo1(btConstraintInfo1* info)
    264 {
    265         if (m_useSolveConstraintObsolete)
    266         {
    267                 info->m_numConstraintRows = 0;
    268                 info->nub = 0;
    269         }
    270         else
    271         {
    272                 info->m_numConstraintRows = 5; // Fixed 3 linear + 2 angular
    273                 info->nub = 1;
    274                 //prepare constraint
    275                 testLimit();
    276                 if(getSolveLimit() || getEnableAngularMotor())
    277                 {
    278                         info->m_numConstraintRows++; // limit 3rd anguar as well
    279                         info->nub--;
    280                 }
    281         }
    282 } // btHingeConstraint::getInfo1 ()
    283 
    284 //-----------------------------------------------------------------------------
    285 
    286 void btHingeConstraint::getInfo2 (btConstraintInfo2* info)
    287 {
    288         btAssert(!m_useSolveConstraintObsolete);
    289         int i, s = info->rowskip;
    290         // transforms in world space
    291         btTransform trA = m_rbA.getCenterOfMassTransform()*m_rbAFrame;
    292         btTransform trB = m_rbB.getCenterOfMassTransform()*m_rbBFrame;
    293         // pivot point
    294         btVector3 pivotAInW = trA.getOrigin();
    295         btVector3 pivotBInW = trB.getOrigin();
    296         // linear (all fixed)
    297     info->m_J1linearAxis[0] = 1;
    298     info->m_J1linearAxis[s + 1] = 1;
    299     info->m_J1linearAxis[2 * s + 2] = 1;
    300         btVector3 a1 = pivotAInW - m_rbA.getCenterOfMassTransform().getOrigin();
    301         {
    302                 btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
    303                 btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + s);
    304                 btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * s);
    305                 btVector3 a1neg = -a1;
    306                 a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
    307         }
    308         btVector3 a2 = pivotBInW - m_rbB.getCenterOfMassTransform().getOrigin();
    309         {
    310                 btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
    311                 btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + s);
    312                 btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * s);
    313                 a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
    314         }
    315         // linear RHS
    316     btScalar k = info->fps * info->erp;
    317         for(i = 0; i < 3; i++)
    318     {
    319         info->m_constraintError[i * s] = k * (pivotBInW[i] - pivotAInW[i]);
    320     }
    321         // make rotations around X and Y equal
    322         // the hinge axis should be the only unconstrained
    323         // rotational axis, the angular velocity of the two bodies perpendicular to
    324         // the hinge axis should be equal. thus the constraint equations are
    325         //    p*w1 - p*w2 = 0
    326         //    q*w1 - q*w2 = 0
    327         // where p and q are unit vectors normal to the hinge axis, and w1 and w2
    328         // are the angular velocity vectors of the two bodies.
    329         // get hinge axis (Z)
    330         btVector3 ax1 = trA.getBasis().getColumn(2);
    331         // get 2 orthos to hinge axis (X, Y)
    332         btVector3 p = trA.getBasis().getColumn(0);
    333         btVector3 q = trA.getBasis().getColumn(1);
    334         // set the two hinge angular rows
    335     int s3 = 3 * info->rowskip;
    336     int s4 = 4 * info->rowskip;
    337 
    338         info->m_J1angularAxis[s3 + 0] = p[0];
    339         info->m_J1angularAxis[s3 + 1] = p[1];
    340         info->m_J1angularAxis[s3 + 2] = p[2];
    341         info->m_J1angularAxis[s4 + 0] = q[0];
    342         info->m_J1angularAxis[s4 + 1] = q[1];
    343         info->m_J1angularAxis[s4 + 2] = q[2];
    344 
    345         info->m_J2angularAxis[s3 + 0] = -p[0];
    346         info->m_J2angularAxis[s3 + 1] = -p[1];
    347         info->m_J2angularAxis[s3 + 2] = -p[2];
    348         info->m_J2angularAxis[s4 + 0] = -q[0];
    349         info->m_J2angularAxis[s4 + 1] = -q[1];
    350         info->m_J2angularAxis[s4 + 2] = -q[2];
    351     // compute the right hand side of the constraint equation. set relative
    352     // body velocities along p and q to bring the hinge back into alignment.
    353     // if ax1,ax2 are the unit length hinge axes as computed from body1 and
    354     // body2, we need to rotate both bodies along the axis u = (ax1 x ax2).
    355     // if `theta' is the angle between ax1 and ax2, we need an angular velocity
    356     // along u to cover angle erp*theta in one step :
    357     //   |angular_velocity| = angle/time = erp*theta / stepsize
    358     //                      = (erp*fps) * theta
    359     //    angular_velocity  = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
    360     //                      = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
    361     // ...as ax1 and ax2 are unit length. if theta is smallish,
    362     // theta ~= sin(theta), so
    363     //    angular_velocity  = (erp*fps) * (ax1 x ax2)
    364     // ax1 x ax2 is in the plane space of ax1, so we project the angular
    365     // velocity to p and q to find the right hand side.
    366     btVector3 ax2 = trB.getBasis().getColumn(2);
    367         btVector3 u = ax1.cross(ax2);
    368         info->m_constraintError[s3] = k * u.dot(p);
    369         info->m_constraintError[s4] = k * u.dot(q);
    370         // check angular limits
    371         int nrow = 4; // last filled row
    372         int srow;
    373         btScalar limit_err = btScalar(0.0);
    374         int limit = 0;
    375         if(getSolveLimit())
    376         {
    377                 limit_err = m_correction * m_referenceSign;
    378                 limit = (limit_err > btScalar(0.0)) ? 1 : 2;
    379         }
    380         // if the hinge has joint limits or motor, add in the extra row
    381         int powered = 0;
    382         if(getEnableAngularMotor())
    383         {
    384                 powered = 1;
    385         }
    386         if(limit || powered)
    387         {
    388                 nrow++;
    389                 srow = nrow * info->rowskip;
    390                 info->m_J1angularAxis[srow+0] = ax1[0];
    391                 info->m_J1angularAxis[srow+1] = ax1[1];
    392                 info->m_J1angularAxis[srow+2] = ax1[2];
    393 
    394                 info->m_J2angularAxis[srow+0] = -ax1[0];
    395                 info->m_J2angularAxis[srow+1] = -ax1[1];
    396                 info->m_J2angularAxis[srow+2] = -ax1[2];
    397 
    398                 btScalar lostop = getLowerLimit();
    399                 btScalar histop = getUpperLimit();
    400                 if(limit && (lostop == histop))
    401                 {  // the joint motor is ineffective
    402                         powered = 0;
    403                 }
    404                 info->m_constraintError[srow] = btScalar(0.0f);
    405                 if(powered)
    406                 {
    407             info->cfm[srow] = btScalar(0.0);
    408                         btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * info->erp);
    409                         info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
    410                         info->m_lowerLimit[srow] = - m_maxMotorImpulse;
    411                         info->m_upperLimit[srow] =   m_maxMotorImpulse;
    412                 }
    413                 if(limit)
    414                 {
    415                         k = info->fps * info->erp;
    416                         info->m_constraintError[srow] += k * limit_err;
    417                         info->cfm[srow] = btScalar(0.0);
    418                         if(lostop == histop)
    419                         {
    420                                 // limited low and high simultaneously
    421                                 info->m_lowerLimit[srow] = -SIMD_INFINITY;
    422                                 info->m_upperLimit[srow] = SIMD_INFINITY;
    423                         }
    424                         else if(limit == 1)
    425                         { // low limit
    426                                 info->m_lowerLimit[srow] = 0;
    427                                 info->m_upperLimit[srow] = SIMD_INFINITY;
    428                         }
    429                         else
    430                         { // high limit
    431                                 info->m_lowerLimit[srow] = -SIMD_INFINITY;
    432                                 info->m_upperLimit[srow] = 0;
    433                         }
    434                         // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
    435                         btScalar bounce = m_relaxationFactor;
    436                         if(bounce > btScalar(0.0))
    437                         {
    438                                 btScalar vel = m_rbA.getAngularVelocity().dot(ax1);
    439                                 vel -= m_rbB.getAngularVelocity().dot(ax1);
    440                                 // only apply bounce if the velocity is incoming, and if the
    441                                 // resulting c[] exceeds what we already have.
    442                                 if(limit == 1)
    443                                 {       // low limit
    444                                         if(vel < 0)
    445                                         {
    446                                                 btScalar newc = -bounce * vel;
    447                                                 if(newc > info->m_constraintError[srow])
    448                                                 {
    449                                                         info->m_constraintError[srow] = newc;
    450                                                 }
    451                                         }
    452                                 }
    453                                 else
    454                                 {       // high limit - all those computations are reversed
    455                                         if(vel > 0)
    456                                         {
    457                                                 btScalar newc = -bounce * vel;
    458                                                 if(newc < info->m_constraintError[srow])
    459                                                 {
    460                                                         info->m_constraintError[srow] = newc;
    461                                                 }
    462                                         }
    463                                 }
    464                         }
    465                         info->m_constraintError[srow] *= m_biasFactor;
    466                 } // if(limit)
    467         } // if angular limit or powered
    468 }
    469 
    470 //-----------------------------------------------------------------------------
    471 
    472 void    btHingeConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar     timeStep)
    473 {
    474 
    475         ///for backwards compatibility during the transition to 'getInfo/getInfo2'
    476         if (m_useSolveConstraintObsolete)
    477         {
    478 
    479                 btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
    480                 btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
    481 
    482                 btScalar tau = btScalar(0.3);
    483 
    484                 //linear part
    485                 if (!m_angularOnly)
    486                 {
    487                         btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
    488                         btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
    489 
    490                         btVector3 vel1,vel2;
    491                         bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
    492                         bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
    493                         btVector3 vel = vel1 - vel2;
    494 
    495                         for (int i=0;i<3;i++)
    496                         {               
    497                                 const btVector3& normal = m_jac[i].m_linearJointAxis;
    498                                 btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
    499 
    500                                 btScalar rel_vel;
    501                                 rel_vel = normal.dot(vel);
    502                                 //positional error (zeroth order error)
    503                                 btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
    504                                 btScalar impulse = depth*tau/timeStep  * jacDiagABInv -  rel_vel * jacDiagABInv;
    505                                 m_appliedImpulse += impulse;
    506                                 btVector3 impulse_vector = normal * impulse;
    507                                 btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
    508                                 btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
    509                                 bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse);
    510                                 bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse);
    511                         }
    512                 }
    513 
    514                
    515                 {
    516                         ///solve angular part
    517 
    518                         // get axes in world space
    519                         btVector3 axisA =  getRigidBodyA().getCenterOfMassTransform().getBasis() *  m_rbAFrame.getBasis().getColumn(2);
    520                         btVector3 axisB =  getRigidBodyB().getCenterOfMassTransform().getBasis() *  m_rbBFrame.getBasis().getColumn(2);
    521 
    522                         btVector3 angVelA;
    523                         bodyA.getAngularVelocity(angVelA);
    524                         btVector3 angVelB;
    525                         bodyB.getAngularVelocity(angVelB);
    526 
    527                         btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA);
    528                         btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB);
    529 
    530                         btVector3 angAorthog = angVelA - angVelAroundHingeAxisA;
    531                         btVector3 angBorthog = angVelB - angVelAroundHingeAxisB;
    532                         btVector3 velrelOrthog = angAorthog-angBorthog;
    533                         {
    534                                
    535 
    536                                 //solve orthogonal angular velocity correction
    537                                 btScalar relaxation = btScalar(1.);
    538                                 btScalar len = velrelOrthog.length();
    539                                 if (len > btScalar(0.00001))
    540                                 {
    541                                         btVector3 normal = velrelOrthog.normalized();
    542                                         btScalar denom = getRigidBodyA().computeAngularImpulseDenominator(normal) +
    543                                                 getRigidBodyB().computeAngularImpulseDenominator(normal);
    544                                         // scale for mass and relaxation
    545                                         //velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor;
    546 
    547                                         bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*velrelOrthog,-(btScalar(1.)/denom));
    548                                         bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*velrelOrthog,(btScalar(1.)/denom));
    549 
    550                                 }
    551 
    552                                 //solve angular positional correction
    553                                 btVector3 angularError =  axisA.cross(axisB) *(btScalar(1.)/timeStep);
    554                                 btScalar len2 = angularError.length();
    555                                 if (len2>btScalar(0.00001))
    556                                 {
    557                                         btVector3 normal2 = angularError.normalized();
    558                                         btScalar denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) +
    559                                                         getRigidBodyB().computeAngularImpulseDenominator(normal2);
    560                                         //angularError *= (btScalar(1.)/denom2) * relaxation;
    561                                        
    562                                         bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*angularError,(btScalar(1.)/denom2));
    563                                         bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*angularError,-(btScalar(1.)/denom2));
    564 
    565                                 }
    566                                
    567                                
    568 
    569 
    570 
    571                                 // solve limit
    572                                 if (m_solveLimit)
    573                                 {
    574                                         btScalar amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (btScalar(1.)/timeStep)*m_biasFactor  ) * m_limitSign;
    575 
    576                                         btScalar impulseMag = amplitude * m_kHinge;
    577 
    578                                         // Clamp the accumulated impulse
    579                                         btScalar temp = m_accLimitImpulse;
    580                                         m_accLimitImpulse = btMax(m_accLimitImpulse + impulseMag, btScalar(0) );
    581                                         impulseMag = m_accLimitImpulse - temp;
    582 
    583 
    584                                        
    585                                         bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,impulseMag * m_limitSign);
    586                                         bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-(impulseMag * m_limitSign));
    587 
    588                                 }
    589                         }
    590 
    591                         //apply motor
    592                         if (m_enableAngularMotor)
    593                         {
    594                                 //todo: add limits too
    595                                 btVector3 angularLimit(0,0,0);
    596 
    597                                 btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB;
    598                                 btScalar projRelVel = velrel.dot(axisA);
    599 
    600                                 btScalar desiredMotorVel = m_motorTargetVelocity;
    601                                 btScalar motor_relvel = desiredMotorVel - projRelVel;
    602 
    603                                 btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;;
    604                                 //todo: should clip against accumulated impulse
    605                                 btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse;
    606                                 clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse;
    607                                 btVector3 motorImp = clippedMotorImpulse * axisA;
    608                        
    609                                 bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,clippedMotorImpulse);
    610                                 bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-clippedMotorImpulse);
    611                                
    612                         }
    613                 }
    614         }
    615 
    616 }
    617 
    618 //-----------------------------------------------------------------------------
     385
     386}
    619387
    620388void    btHingeConstraint::updateRHS(btScalar   timeStep)
    … …  
    623391
    624392}
    625 
    626 //-----------------------------------------------------------------------------
    627393
    628394btScalar btHingeConstraint::getHingeAngle()
    … …  
    631397        const btVector3 refAxis1  = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1);
    632398        const btVector3 swingAxis = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(1);
    633         btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
    634         return m_referenceSign * angle;
    635 }
    636 
    637 //-----------------------------------------------------------------------------
    638 
    639 void btHingeConstraint::testLimit()
    640 {
    641         // Compute limit information
    642         m_hingeAngle = getHingeAngle(); 
    643         m_correction = btScalar(0.);
    644         m_limitSign = btScalar(0.);
    645         m_solveLimit = false;
    646         if (m_lowerLimit <= m_upperLimit)
    647         {
    648                 if (m_hingeAngle <= m_lowerLimit)
    649                 {
    650                         m_correction = (m_lowerLimit - m_hingeAngle);
    651                         m_limitSign = 1.0f;
    652                         m_solveLimit = true;
    653                 }
    654                 else if (m_hingeAngle >= m_upperLimit)
    655                 {
    656                         m_correction = m_upperLimit - m_hingeAngle;
    657                         m_limitSign = -1.0f;
    658                         m_solveLimit = true;
    659                 }
    660         }
    661         return;
    662 } // btHingeConstraint::testLimit()
    663 
    664 //-----------------------------------------------------------------------------
    665 //-----------------------------------------------------------------------------
    666 //-----------------------------------------------------------------------------
     399
     400        return btAtan2Fast( swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)  );
     401}
     402

  • code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.h

    r2907 r2908  
    5454
    5555        btScalar        m_accLimitImpulse;
    56         btScalar        m_hingeAngle;
    57         btScalar    m_referenceSign;
    5856
    5957        bool            m_angularOnly;
    6058        bool            m_enableAngularMotor;
    6159        bool            m_solveLimit;
    62         bool            m_useSolveConstraintObsolete;
    63         bool            m_useReferenceFrameA;
    6460
    6561       
    6662public:
    6763
    68         btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, btVector3& axisInA,btVector3& axisInB, bool useReferenceFrameA = false);
     64        btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, btVector3& axisInA,btVector3& axisInB);
    6965
    70         btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA, bool useReferenceFrameA = false);
     66        btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA);
    7167       
    72         btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false);
     68        btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame);
    7369
    74         btHingeConstraint(btRigidBody& rbA,const btTransform& rbAFrame, bool useReferenceFrameA = false);
     70        btHingeConstraint(btRigidBody& rbA,const btTransform& rbAFrame);
    7571
    7672        btHingeConstraint();
    … …  
    7874        virtual void    buildJacobian();
    7975
    80         virtual void getInfo1 (btConstraintInfo1* info);
    81 
    82         virtual void getInfo2 (btConstraintInfo2* info);
    83        
    84         virtual void    solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar        timeStep);
     76        virtual void    solveConstraint(btScalar        timeStep);
    8577
    8678        void    updateRHS(btScalar      timeStep);
    … …  
    9587        }
    9688
    97         btRigidBody& getRigidBodyA()   
    98         {               
    99                 return m_rbA;   
    100         }       
    101 
    102         btRigidBody& getRigidBodyB()   
    103         {               
    104                 return m_rbB;   
    105         }       
    106        
    10789        void    setAngularOnly(bool angularOnly)
    10890        {
    … …  
    141123        btScalar getHingeAngle();
    142124
    143         void testLimit();
    144 
    145125
    146126        const btTransform& getAFrame() { return m_rbAFrame; }; 

  • code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp

    r2907 r2908  
    2222
    2323btPoint2PointConstraint::btPoint2PointConstraint()
    24 :btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE),
    25 m_useSolveConstraintObsolete(false)
     24:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE)
    2625{
    2726}
    2827
    2928btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB)
    30 :btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB),
    31 m_useSolveConstraintObsolete(false)
     29:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB)
    3230{
    3331
    … …  
    3634
    3735btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA)
    38 :btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)),
    39 m_useSolveConstraintObsolete(false)
     36:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA))
    4037{
    4138       
    … …  
    4441void    btPoint2PointConstraint::buildJacobian()
    4542{
    46         ///we need it for both methods
     43        m_appliedImpulse = btScalar(0.);
     44
     45        btVector3       normal(0,0,0);
     46
     47        for (int i=0;i<3;i++)
    4748        {
    48                 m_appliedImpulse = btScalar(0.);
    49 
    50                 btVector3       normal(0,0,0);
    51 
    52                 for (int i=0;i<3;i++)
    53                 {
    54                         normal[i] = 1;
    55                         new (&m_jac[i]) btJacobianEntry(
     49                normal[i] = 1;
     50                new (&m_jac[i]) btJacobianEntry(
    5651                        m_rbA.getCenterOfMassTransform().getBasis().transpose(),
    5752                        m_rbB.getCenterOfMassTransform().getBasis().transpose(),
    … …  
    6459                        m_rbB.getInvMass());
    6560                normal[i] = 0;
    66                 }
    6761        }
    6862
    6963}
    7064
     65void    btPoint2PointConstraint::solveConstraint(btScalar       timeStep)
     66{
     67        btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
     68        btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
    7169
    72 void btPoint2PointConstraint::getInfo1 (btConstraintInfo1* info)
    73 {
    74         if (m_useSolveConstraintObsolete)
    75         {
    76                 info->m_numConstraintRows = 0;
    77                 info->nub = 0;
    78         } else
    79         {
    80                 info->m_numConstraintRows = 3;
    81                 info->nub = 3;
    82         }
    83 }
    8470
    85 void btPoint2PointConstraint::getInfo2 (btConstraintInfo2* info)
    86 {
    87         btAssert(!m_useSolveConstraintObsolete);
     71        btVector3 normal(0,0,0);
     72       
    8873
    89          //retrieve matrices
    90         btTransform body0_trans;
    91         body0_trans = m_rbA.getCenterOfMassTransform();
    92     btTransform body1_trans;
    93         body1_trans = m_rbB.getCenterOfMassTransform();
     74//      btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
     75//      btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
    9476
    95         // anchor points in global coordinates with respect to body PORs.
    96    
    97     // set jacobian
    98     info->m_J1linearAxis[0] = 1;
    99     info->m_J1linearAxis[info->rowskip+1] = 1;
    100     info->m_J1linearAxis[2*info->rowskip+2] = 1;
     77        for (int i=0;i<3;i++)
     78        {               
     79                normal[i] = 1;
     80                btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
    10181
    102         btVector3 a1 = body0_trans.getBasis()*getPivotInA();
    103         {
    104                 btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
    105                 btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
    106                 btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip);
    107                 btVector3 a1neg = -a1;
    108                 a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
    109         }
    110    
    111         /*info->m_J2linearAxis[0] = -1;
    112     info->m_J2linearAxis[s+1] = -1;
    113     info->m_J2linearAxis[2*s+2] = -1;
     82                btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
     83                btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
     84                //this jacobian entry could be re-used for all iterations
     85               
     86                btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
     87                btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
     88                btVector3 vel = vel1 - vel2;
     89               
     90                btScalar rel_vel;
     91                rel_vel = normal.dot(vel);
     92
     93        /*
     94                //velocity error (first order error)
     95                btScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
     96                                                                                                                m_rbB.getLinearVelocity(),angvelB);
    11497        */
    11598       
    116         btVector3 a2 = body1_trans.getBasis()*getPivotInB();
    117    
    118         {
    119                 btVector3 a2n = -a2;
    120                 btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
    121                 btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
    122                 btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip);
    123                 a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
    124         }
    125    
     99                //positional error (zeroth order error)
     100                btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
     101               
     102                btScalar impulse = depth*m_setting.m_tau/timeStep  * jacDiagABInv -  m_setting.m_damping * rel_vel * jacDiagABInv;
    126103
     104                btScalar impulseClamp = m_setting.m_impulseClamp;
     105                if (impulseClamp > 0)
     106                {
     107                        if (impulse < -impulseClamp)
     108                                impulse = -impulseClamp;
     109                        if (impulse > impulseClamp)
     110                                impulse = impulseClamp;
     111                }
    127112
    128     // set right hand side
    129     btScalar k = info->fps * info->erp;
    130     int j;
    131 
    132         for (j=0; j<3; j++)
    133     {
    134         info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] -                     a1[j] - body0_trans.getOrigin()[j]);
    135                 //printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
    136     }
    137 
    138         btScalar impulseClamp = m_setting.m_impulseClamp;//
    139         for (j=0; j<3; j++)
    140     {
    141                 if (m_setting.m_impulseClamp > 0)
    142                 {
    143                         info->m_lowerLimit[j*info->rowskip] = -impulseClamp;
    144                         info->m_upperLimit[j*info->rowskip] = impulseClamp;
    145                 }
    146         }
    147        
    148 }
    149 
    150 
    151 void    btPoint2PointConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar       timeStep)
    152 {
    153         if (m_useSolveConstraintObsolete)
    154         {
    155                 btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
    156                 btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
    157 
    158 
    159                 btVector3 normal(0,0,0);
     113                m_appliedImpulse+=impulse;
     114                btVector3 impulse_vector = normal * impulse;
     115                m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
     116                m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
    160117               
    161 
    162         //      btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
    163         //      btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
    164 
    165                 for (int i=0;i<3;i++)
    166                 {               
    167                         normal[i] = 1;
    168                         btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
    169 
    170                         btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
    171                         btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
    172                         //this jacobian entry could be re-used for all iterations
    173                        
    174                         btVector3 vel1,vel2;
    175                         bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
    176                         bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
    177                         btVector3 vel = vel1 - vel2;
    178                        
    179                         btScalar rel_vel;
    180                         rel_vel = normal.dot(vel);
    181 
    182                 /*
    183                         //velocity error (first order error)
    184                         btScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
    185                                                                                                                         m_rbB.getLinearVelocity(),angvelB);
    186                 */
    187                
    188                         //positional error (zeroth order error)
    189                         btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
    190                        
    191                         btScalar deltaImpulse = depth*m_setting.m_tau/timeStep  * jacDiagABInv -  m_setting.m_damping * rel_vel * jacDiagABInv;
    192 
    193                         btScalar impulseClamp = m_setting.m_impulseClamp;
    194                        
    195                         const btScalar sum = btScalar(m_appliedImpulse) + deltaImpulse;
    196                         if (sum < -impulseClamp)
    197                         {
    198                                 deltaImpulse = -impulseClamp-m_appliedImpulse;
    199                                 m_appliedImpulse = -impulseClamp;
    200                         }
    201                         else if (sum > impulseClamp)
    202                         {
    203                                 deltaImpulse = impulseClamp-m_appliedImpulse;
    204                                 m_appliedImpulse = impulseClamp;
    205                         }
    206                         else
    207                         {
    208                                 m_appliedImpulse = sum;
    209                         }
    210 
    211                        
    212                         btVector3 impulse_vector = normal * deltaImpulse;
    213                        
    214                         btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
    215                         btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
    216                         bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,deltaImpulse);
    217                         bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-deltaImpulse);
    218 
    219 
    220                         normal[i] = 0;
    221                 }
     118                normal[i] = 0;
    222119        }
    223120}

  • code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h

    r2907 r2908  
    5151public:
    5252
    53         ///for backwards compatibility during the transition to 'getInfo/getInfo2'
    54         bool            m_useSolveConstraintObsolete;
    55 
    5653        btConstraintSetting     m_setting;
    5754
    … …  
    6461        virtual void    buildJacobian();
    6562
    66         virtual void getInfo1 (btConstraintInfo1* info);
    6763
    68         virtual void getInfo2 (btConstraintInfo2* info);
    69 
    70 
    71         virtual void    solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar        timeStep);
     64        virtual void    solveConstraint(btScalar        timeStep);
    7265
    7366        void    updateRHS(btScalar      timeStep);

  • code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

    r2907 r2908  
    1616//#define COMPUTE_IMPULSE_DENOM 1
    1717//It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms.
     18//#define FORCE_REFESH_CONTACT_MANIFOLDS 1
    1819
    1920#include "btSequentialImpulseConstraintSolver.h"
    … …  
    3233#include "btSolverBody.h"
    3334#include "btSolverConstraint.h"
     35
     36
    3437#include "LinearMath/btAlignedObjectArray.h"
    35 #include <string.h> //for memset
     38
     39
     40int totalCpd = 0;
     41
     42int     gTotalContactPoints = 0;
     43
     44struct  btOrderIndex
     45{
     46        int     m_manifoldIndex;
     47        int     m_pointIndex;
     48};
     49
     50
     51
     52#define SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS 16384
     53static btOrderIndex     gOrder[SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS];
     54
     55
     56unsigned long btSequentialImpulseConstraintSolver::btRand2()
     57{
     58  m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff;
     59  return m_btSeed2;
     60}
     61
     62
     63
     64//See ODE: adam's all-int straightforward(?) dRandInt (0..n-1)
     65int btSequentialImpulseConstraintSolver::btRandInt2 (int n)
     66{
     67  // seems good; xor-fold and modulus
     68  const unsigned long un = static_cast<unsigned long>(n);
     69  unsigned long r = btRand2();
     70
     71  // note: probably more aggressive than it needs to be -- might be
     72  //       able to get away without one or two of the innermost branches.
     73  if (un <= 0x00010000UL) {
     74    r ^= (r >> 16);
     75    if (un <= 0x00000100UL) {
     76      r ^= (r >> 8);
     77      if (un <= 0x00000010UL) {
     78        r ^= (r >> 4);
     79        if (un <= 0x00000004UL) {
     80          r ^= (r >> 2);
     81          if (un <= 0x00000002UL) {
     82            r ^= (r >> 1);
     83          }
     84        }
     85     }
     86    }
     87   }
     88
     89  return (int) (r % un);
     90}
     91
     92
     93
     94
     95bool  MyContactDestroyedCallback(void* userPersistentData);
     96bool  MyContactDestroyedCallback(void* userPersistentData)
     97{
     98        assert (userPersistentData);
     99        btConstraintPersistentData* cpd = (btConstraintPersistentData*)userPersistentData;
     100        btAlignedFree(cpd);
     101        totalCpd--;
     102        //printf("totalCpd = %i. DELETED Ptr %x\n",totalCpd,userPersistentData);
     103        return true;
     104}
     105
     106
    36107
    37108btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
    38109:m_btSeed2(0)
    39110{
    40 
     111        gContactDestroyedCallback = &MyContactDestroyedCallback;
     112
     113        //initialize default friction/contact funcs
     114        int i,j;
     115        for (i=0;i<MAX_CONTACT_SOLVER_TYPES;i++)
     116                for (j=0;j<MAX_CONTACT_SOLVER_TYPES;j++)
     117                {
     118
     119                        m_contactDispatch[i][j] = resolveSingleCollision;
     120                        m_frictionDispatch[i][j] = resolveSingleFriction;
     121                }
    41122}
    42123
    43124btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
    44125{
    45 }
    46 
    47 #ifdef USE_SIMD
    48 #include <emmintrin.h>
    49 #define vec_splat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e,e,e,e))
    50 static inline __m128 _vmathVfDot3( __m128 vec0, __m128 vec1 )
    51 {
    52         __m128 result = _mm_mul_ps( vec0, vec1);
    53         return _mm_add_ps( vec_splat( result, 0 ), _mm_add_ps( vec_splat( result, 1 ), vec_splat( result, 2 ) ) );
    54 }
    55 #endif//USE_SIMD
    56 
    57 // Project Gauss Seidel or the equivalent Sequential Impulse
    58 void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
    59 {
    60 #ifdef USE_SIMD
    61         __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
    62         __m128  lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
    63         __m128  upperLimit1 = _mm_set1_ps(c.m_upperLimit);
    64         __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm)));
    65         __m128 deltaVel1Dotn    =       _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.m_deltaLinearVelocity.mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.m_deltaAngularVelocity.mVec128));
    66         __m128 deltaVel2Dotn    =       _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.m_deltaAngularVelocity.mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.m_deltaLinearVelocity.mVec128));
    67         deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
    68         deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
    69         btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
    70         btSimdScalar resultLowerLess,resultUpperLess;
    71         resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
    72         resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
    73         __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
    74         deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
    75         c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
    76         __m128 upperMinApplied = _mm_sub_ps(upperLimit1,cpAppliedImp);
    77         deltaImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied) );
    78         c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1) );
    79         __m128  linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,_mm_set1_ps(body1.m_invMass));
    80         __m128  linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,_mm_set1_ps(body2.m_invMass));
    81         __m128 impulseMagnitude = deltaImpulse;
    82         body1.m_deltaLinearVelocity.mVec128 = _mm_add_ps(body1.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
    83         body1.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body1.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
    84         body2.m_deltaLinearVelocity.mVec128 = _mm_sub_ps(body2.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
    85         body2.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body2.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
    86 #else
    87         resolveSingleConstraintRowGeneric(body1,body2,c);
    88 #endif
    89 }
    90 
    91 // Project Gauss Seidel or the equivalent Sequential Impulse
    92  void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
    93 {
    94         btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
    95         const btScalar deltaVel1Dotn    =       c.m_contactNormal.dot(body1.m_deltaLinearVelocity)      + c.m_relpos1CrossNormal.dot(body1.m_deltaAngularVelocity);
    96         const btScalar deltaVel2Dotn    =       -c.m_contactNormal.dot(body2.m_deltaLinearVelocity) + c.m_relpos2CrossNormal.dot(body2.m_deltaAngularVelocity);
    97 
    98         const btScalar delta_rel_vel    =       deltaVel1Dotn-deltaVel2Dotn;
    99         deltaImpulse    -=      deltaVel1Dotn*c.m_jacDiagABInv;
    100         deltaImpulse    -=      deltaVel2Dotn*c.m_jacDiagABInv;
    101 
    102         const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
    103         if (sum < c.m_lowerLimit)
    104         {
    105                 deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
    106                 c.m_appliedImpulse = c.m_lowerLimit;
    107         }
    108         else if (sum > c.m_upperLimit)
    109         {
    110                 deltaImpulse = c.m_upperLimit-c.m_appliedImpulse;
    111                 c.m_appliedImpulse = c.m_upperLimit;
    112         }
    113         else
    114         {
    115                 c.m_appliedImpulse = sum;
    116         }
    117         if (body1.m_invMass)
    118                 body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse);
    119         if (body2.m_invMass)
    120                 body2.applyImpulse(-c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,deltaImpulse);
    121 }
    122 
    123  void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
    124 {
    125 #ifdef USE_SIMD
    126         __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
    127         __m128  lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
    128         __m128  upperLimit1 = _mm_set1_ps(c.m_upperLimit);
    129         __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm)));
    130         __m128 deltaVel1Dotn    =       _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.m_deltaLinearVelocity.mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.m_deltaAngularVelocity.mVec128));
    131         __m128 deltaVel2Dotn    =       _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.m_deltaAngularVelocity.mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.m_deltaLinearVelocity.mVec128));
    132         deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
    133         deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
    134         btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
    135         btSimdScalar resultLowerLess,resultUpperLess;
    136         resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
    137         resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
    138         __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
    139         deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
    140         c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
    141         __m128  linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,_mm_set1_ps(body1.m_invMass));
    142         __m128  linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,_mm_set1_ps(body2.m_invMass));
    143         __m128 impulseMagnitude = deltaImpulse;
    144         body1.m_deltaLinearVelocity.mVec128 = _mm_add_ps(body1.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
    145         body1.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body1.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
    146         body2.m_deltaLinearVelocity.mVec128 = _mm_sub_ps(body2.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
    147         body2.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body2.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
    148 #else
    149         resolveSingleConstraintRowLowerLimit(body1,body2,c);
    150 #endif
    151 }
    152 
    153 // Project Gauss Seidel or the equivalent Sequential Impulse
    154  void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
    155 {
    156         btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
    157         const btScalar deltaVel1Dotn    =       c.m_contactNormal.dot(body1.m_deltaLinearVelocity)      + c.m_relpos1CrossNormal.dot(body1.m_deltaAngularVelocity);
    158         const btScalar deltaVel2Dotn    =       -c.m_contactNormal.dot(body2.m_deltaLinearVelocity) + c.m_relpos2CrossNormal.dot(body2.m_deltaAngularVelocity);
    159 
    160         deltaImpulse    -=      deltaVel1Dotn*c.m_jacDiagABInv;
    161         deltaImpulse    -=      deltaVel2Dotn*c.m_jacDiagABInv;
    162         const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
    163         if (sum < c.m_lowerLimit)
    164         {
    165                 deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
    166                 c.m_appliedImpulse = c.m_lowerLimit;
    167         }
    168         else
    169         {
    170                 c.m_appliedImpulse = sum;
    171         }
    172         if (body1.m_invMass)
    173                 body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse);
    174         if (body2.m_invMass)
    175                 body2.applyImpulse(-c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,deltaImpulse);
    176 }
    177 
    178 
    179 
    180 unsigned long btSequentialImpulseConstraintSolver::btRand2()
    181 {
    182         m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff;
    183         return m_btSeed2;
    184 }
    185 
    186 
    187 
    188 //See ODE: adam's all-int straightforward(?) dRandInt (0..n-1)
    189 int btSequentialImpulseConstraintSolver::btRandInt2 (int n)
    190 {
    191         // seems good; xor-fold and modulus
    192         const unsigned long un = static_cast<unsigned long>(n);
    193         unsigned long r = btRand2();
    194 
    195         // note: probably more aggressive than it needs to be -- might be
    196         //       able to get away without one or two of the innermost branches.
    197         if (un <= 0x00010000UL) {
    198                 r ^= (r >> 16);
    199                 if (un <= 0x00000100UL) {
    200                         r ^= (r >> 8);
    201                         if (un <= 0x00000010UL) {
    202                                 r ^= (r >> 4);
    203                                 if (un <= 0x00000004UL) {
    204                                         r ^= (r >> 2);
    205                                         if (un <= 0x00000002UL) {
    206                                                 r ^= (r >> 1);
    207                                         }
    208                                 }
    209                         }
    210                 }
    211         }
    212 
    213         return (int) (r % un);
    214 }
    215 
    216 
    217 
    218 void    btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject)
    219 {
    220         btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0;
    221 
    222         solverBody->m_deltaLinearVelocity.setValue(0.f,0.f,0.f);
    223         solverBody->m_deltaAngularVelocity.setValue(0.f,0.f,0.f);
    224 
     126
     127}
     128
     129void    initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject);
     130void    initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject)
     131{
     132        btRigidBody* rb = btRigidBody::upcast(collisionObject);
    225133        if (rb)
    226134        {
     135                solverBody->m_angularVelocity = rb->getAngularVelocity() ;
     136                solverBody->m_centerOfMassPosition = collisionObject->getWorldTransform().getOrigin();
     137                solverBody->m_friction = collisionObject->getFriction();
    227138                solverBody->m_invMass = rb->getInvMass();
     139                solverBody->m_linearVelocity = rb->getLinearVelocity();
    228140                solverBody->m_originalBody = rb;
    229141                solverBody->m_angularFactor = rb->getAngularFactor();
    230142        } else
    231143        {
     144                solverBody->m_angularVelocity.setValue(0,0,0);
     145                solverBody->m_centerOfMassPosition = collisionObject->getWorldTransform().getOrigin();
     146                solverBody->m_friction = collisionObject->getFriction();
    232147                solverBody->m_invMass = 0.f;
     148                solverBody->m_linearVelocity.setValue(0,0,0);
    233149                solverBody->m_originalBody = 0;
    234150                solverBody->m_angularFactor = 1.f;
    235151        }
     152        solverBody->m_pushVelocity.setValue(0.f,0.f,0.f);
     153        solverBody->m_turnVelocity.setValue(0.f,0.f,0.f);
    236154}
    237155
    … …  
    239157int             gNumSplitImpulseRecoveries = 0;
    240158
    241 btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution)
     159btScalar restitutionCurve(btScalar rel_vel, btScalar restitution);
     160btScalar restitutionCurve(btScalar rel_vel, btScalar restitution)
    242161{
    243162        btScalar rest = restitution * -rel_vel;
    … …  
    246165
    247166
    248 
    249 void    applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection);
    250 void    applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection)
    251 {
    252         if (colObj && colObj->hasAnisotropicFriction())
    253         {
    254                 // transform to local coordinates
    255                 btVector3 loc_lateral = frictionDirection * colObj->getWorldTransform().getBasis();
    256                 const btVector3& friction_scaling = colObj->getAnisotropicFriction();
    257                 //apply anisotropic friction
    258                 loc_lateral *= friction_scaling;
    259                 // ... and transform it back to global coordinates
    260                 frictionDirection = colObj->getWorldTransform().getBasis() * loc_lateral;
    261         }
    262 }
     167void    resolveSplitPenetrationImpulseCacheFriendly(
     168        btSolverBody& body1,
     169        btSolverBody& body2,
     170        const btSolverConstraint& contactConstraint,
     171        const btContactSolverInfo& solverInfo);
     172
     173//SIMD_FORCE_INLINE
     174void    resolveSplitPenetrationImpulseCacheFriendly(
     175        btSolverBody& body1,
     176        btSolverBody& body2,
     177        const btSolverConstraint& contactConstraint,
     178        const btContactSolverInfo& solverInfo)
     179{
     180        (void)solverInfo;
     181
     182                if (contactConstraint.m_penetration < solverInfo.m_splitImpulsePenetrationThreshold)
     183        {
     184
     185                                gNumSplitImpulseRecoveries++;
     186                btScalar normalImpulse;
     187
     188                //  Optimized version of projected relative velocity, use precomputed cross products with normal
     189                //      body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
     190                //      body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
     191                //      btVector3 vel = vel1 - vel2;
     192                //      btScalar  rel_vel = contactConstraint.m_contactNormal.dot(vel);
     193
     194                btScalar rel_vel;
     195                btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_pushVelocity)
     196                + contactConstraint.m_relpos1CrossNormal.dot(body1.m_turnVelocity);
     197                btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_pushVelocity)
     198                + contactConstraint.m_relpos2CrossNormal.dot(body2.m_turnVelocity);
     199
     200                rel_vel = vel1Dotn-vel2Dotn;
     201
     202
     203                                btScalar positionalError = -contactConstraint.m_penetration * solverInfo.m_erp2/solverInfo.m_timeStep;
     204                //      btScalar positionalError = contactConstraint.m_penetration;
     205
     206                btScalar velocityError = contactConstraint.m_restitution - rel_vel;// * damping;
     207
     208                btScalar penetrationImpulse = positionalError * contactConstraint.m_jacDiagABInv;
     209                btScalar        velocityImpulse = velocityError * contactConstraint.m_jacDiagABInv;
     210                normalImpulse = penetrationImpulse+velocityImpulse;
     211
     212                // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
     213                btScalar oldNormalImpulse = contactConstraint.m_appliedPushImpulse;
     214                btScalar sum = oldNormalImpulse + normalImpulse;
     215                contactConstraint.m_appliedPushImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
     216
     217                normalImpulse = contactConstraint.m_appliedPushImpulse - oldNormalImpulse;
     218
     219                                body1.internalApplyPushImpulse(contactConstraint.m_contactNormal*body1.m_invMass, contactConstraint.m_angularComponentA,normalImpulse);
     220               
     221                                body2.internalApplyPushImpulse(contactConstraint.m_contactNormal*body2.m_invMass, contactConstraint.m_angularComponentB,-normalImpulse);
     222               
     223        }
     224
     225}
     226
     227
     228//velocity + friction
     229//response  between two dynamic objects with friction
     230
     231btScalar resolveSingleCollisionCombinedCacheFriendly(
     232        btSolverBody& body1,
     233        btSolverBody& body2,
     234        const btSolverConstraint& contactConstraint,
     235        const btContactSolverInfo& solverInfo);
     236
     237//SIMD_FORCE_INLINE
     238btScalar resolveSingleCollisionCombinedCacheFriendly(
     239        btSolverBody& body1,
     240        btSolverBody& body2,
     241        const btSolverConstraint& contactConstraint,
     242        const btContactSolverInfo& solverInfo)
     243{
     244        (void)solverInfo;
     245
     246        btScalar normalImpulse;
     247
     248        {
     249
     250               
     251                //  Optimized version of projected relative velocity, use precomputed cross products with normal
     252                //      body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
     253                //      body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
     254                //      btVector3 vel = vel1 - vel2;
     255                //      btScalar  rel_vel = contactConstraint.m_contactNormal.dot(vel);
     256
     257                btScalar rel_vel;
     258                btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity)
     259                                        + contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity);
     260                btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity)
     261                                        + contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity);
     262
     263                rel_vel = vel1Dotn-vel2Dotn;
     264
     265                btScalar positionalError = 0.f;
     266                if (!solverInfo.m_splitImpulse || (contactConstraint.m_penetration > solverInfo.m_splitImpulsePenetrationThreshold))
     267                {
     268                        positionalError = -contactConstraint.m_penetration * solverInfo.m_erp/solverInfo.m_timeStep;
     269                }
     270
     271                btScalar velocityError = contactConstraint.m_restitution - rel_vel;// * damping;
     272
     273                btScalar penetrationImpulse = positionalError * contactConstraint.m_jacDiagABInv;
     274                btScalar        velocityImpulse = velocityError * contactConstraint.m_jacDiagABInv;
     275                normalImpulse = penetrationImpulse+velocityImpulse;
     276               
     277               
     278                // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
     279                btScalar oldNormalImpulse = contactConstraint.m_appliedImpulse;
     280                btScalar sum = oldNormalImpulse + normalImpulse;
     281                contactConstraint.m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
     282
     283                normalImpulse = contactConstraint.m_appliedImpulse - oldNormalImpulse;
     284
     285                body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,
     286                                contactConstraint.m_angularComponentA,normalImpulse);
     287               
     288                body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,
     289                                contactConstraint.m_angularComponentB,-normalImpulse);
     290        }
     291
     292        return normalImpulse;
     293}
     294
     295
     296#ifndef NO_FRICTION_TANGENTIALS
     297
     298btScalar resolveSingleFrictionCacheFriendly(
     299        btSolverBody& body1,
     300        btSolverBody& body2,
     301        const btSolverConstraint& contactConstraint,
     302        const btContactSolverInfo& solverInfo,
     303        btScalar appliedNormalImpulse);
     304
     305//SIMD_FORCE_INLINE
     306btScalar resolveSingleFrictionCacheFriendly(
     307        btSolverBody& body1,
     308        btSolverBody& body2,
     309        const btSolverConstraint& contactConstraint,
     310        const btContactSolverInfo& solverInfo,
     311        btScalar appliedNormalImpulse)
     312{
     313        (void)solverInfo;
     314
     315       
     316        const btScalar combinedFriction = contactConstraint.m_friction;
     317       
     318        const btScalar limit = appliedNormalImpulse * combinedFriction;
     319       
     320        if (appliedNormalImpulse>btScalar(0.))
     321        //friction
     322        {
     323               
     324                btScalar j1;
     325                {
     326
     327                        btScalar rel_vel;
     328                        const btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity)
     329                                                + contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity);
     330                        const btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity)
     331                                + contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity);
     332                        rel_vel = vel1Dotn-vel2Dotn;
     333
     334                        // calculate j that moves us to zero relative velocity
     335                        j1 = -rel_vel * contactConstraint.m_jacDiagABInv;
     336#define CLAMP_ACCUMULATED_FRICTION_IMPULSE 1
     337#ifdef CLAMP_ACCUMULATED_FRICTION_IMPULSE
     338                        btScalar oldTangentImpulse = contactConstraint.m_appliedImpulse;
     339                        contactConstraint.m_appliedImpulse = oldTangentImpulse + j1;
     340                       
     341                        if (limit < contactConstraint.m_appliedImpulse)
     342                        {
     343                                contactConstraint.m_appliedImpulse = limit;
     344                        } else
     345                        {
     346                                if (contactConstraint.m_appliedImpulse < -limit)
     347                                        contactConstraint.m_appliedImpulse = -limit;
     348                        }
     349                        j1 = contactConstraint.m_appliedImpulse - oldTangentImpulse;
     350#else
     351                        if (limit < j1)
     352                        {
     353                                j1 = limit;
     354                        } else
     355                        {
     356                                if (j1 < -limit)
     357                                        j1 = -limit;
     358                        }
     359
     360#endif //CLAMP_ACCUMULATED_FRICTION_IMPULSE
     361
     362                        //GEN_set_min(contactConstraint.m_appliedImpulse, limit);
     363                        //GEN_set_max(contactConstraint.m_appliedImpulse, -limit);
     364
     365                       
     366
     367                }
     368       
     369                body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,contactConstraint.m_angularComponentA,j1);
     370               
     371                body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,contactConstraint.m_angularComponentB,-j1);
     372
     373        }
     374        return 0.f;
     375}
     376
     377
     378#else
     379
     380//velocity + friction
     381//response  between two dynamic objects with friction
     382btScalar resolveSingleFrictionCacheFriendly(
     383        btSolverBody& body1,
     384        btSolverBody& body2,
     385        btSolverConstraint& contactConstraint,
     386        const btContactSolverInfo& solverInfo)
     387{
     388
     389        btVector3 vel1;
     390        btVector3 vel2;
     391        btScalar normalImpulse(0.f);
     392
     393        {
     394                const btVector3& normal = contactConstraint.m_contactNormal;
     395                if (contactConstraint.m_penetration < 0.f)
     396                        return 0.f;
     397
     398
     399                body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
     400                body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
     401                btVector3 vel = vel1 - vel2;
     402                btScalar rel_vel;
     403                rel_vel = normal.dot(vel);
     404
     405                btVector3 lat_vel = vel - normal * rel_vel;
     406                btScalar lat_rel_vel = lat_vel.length2();
     407
     408                btScalar combinedFriction = contactConstraint.m_friction;
     409                const btVector3& rel_pos1 = contactConstraint.m_rel_posA;
     410                const btVector3& rel_pos2 = contactConstraint.m_rel_posB;
     411
     412
     413                if (lat_rel_vel > SIMD_EPSILON*SIMD_EPSILON)
     414                {
     415                        lat_rel_vel = btSqrt(lat_rel_vel);
     416
     417                        lat_vel /= lat_rel_vel;
     418                        btVector3 temp1 = body1.m_invInertiaWorld * rel_pos1.cross(lat_vel);
     419                        btVector3 temp2 = body2.m_invInertiaWorld * rel_pos2.cross(lat_vel);
     420                        btScalar friction_impulse = lat_rel_vel /
     421                                (body1.m_invMass + body2.m_invMass + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
     422                        btScalar normal_impulse = contactConstraint.m_appliedImpulse * combinedFriction;
     423
     424                        GEN_set_min(friction_impulse, normal_impulse);
     425                        GEN_set_max(friction_impulse, -normal_impulse);
     426                        body1.applyImpulse(lat_vel * -friction_impulse, rel_pos1);
     427                        body2.applyImpulse(lat_vel * friction_impulse, rel_pos2);
     428                }
     429        }
     430
     431        return normalImpulse;
     432}
     433
     434#endif //NO_FRICTION_TANGENTIALS
     435
     436
    263437
    264438
    … …  
    266440btSolverConstraint&     btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation)
    267441{
    268 
    269442
    270443        btRigidBody* body0=btRigidBody::upcast(colObj0);
    271444        btRigidBody* body1=btRigidBody::upcast(colObj1);
    272445
    273         btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expand();
    274         memset(&solverConstraint,0xff,sizeof(btSolverConstraint));
     446        btSolverConstraint& solverConstraint = m_tmpSolverFrictionConstraintPool.expand();
    275447        solverConstraint.m_contactNormal = normalAxis;
    276448
    277449        solverConstraint.m_solverBodyIdA = solverBodyIdA;
    278450        solverConstraint.m_solverBodyIdB = solverBodyIdB;
     451        solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_FRICTION_1D;
    279452        solverConstraint.m_frictionIndex = frictionIndex;
    280453
    … …  
    282455        solverConstraint.m_originalContactPoint = 0;
    283456
    284         solverConstraint.m_appliedImpulse = 0.f;
    285         //      solverConstraint.m_appliedPushImpulse = 0.f;
    286 
     457        solverConstraint.m_appliedImpulse = btScalar(0.);
     458        solverConstraint.m_appliedPushImpulse = 0.f;
     459        solverConstraint.m_penetration = 0.f;
    287460        {
    288461                btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal);
    289462                solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
    290                 solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor() : btVector3(0,0,0);
    291         }
    292         {
    293                 btVector3 ftorqueAxis1 = rel_pos2.cross(-solverConstraint.m_contactNormal);
     463                solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1 : btVector3(0,0,0);
     464        }
     465        {
     466                btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal);
    294467                solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
    295                 solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor() : btVector3(0,0,0);
     468                solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1 : btVector3(0,0,0);
    296469        }
    297470
    … …  
    310483        if (body1)
    311484        {
    312                 vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
     485                vec = ( solverConstraint.m_angularComponentB).cross(rel_pos2);
    313486                denom1 = body1->getInvMass() + normalAxis.dot(vec);
    314487        }
    … …  
    319492        solverConstraint.m_jacDiagABInv = denom;
    320493
    321 #ifdef _USE_JACOBIAN
    322         solverConstraint.m_jac =  btJacobianEntry (
    323                 rel_pos1,rel_pos2,solverConstraint.m_contactNormal,
    324                 body0->getInvInertiaDiagLocal(),
    325                 body0->getInvMass(),
    326                 body1->getInvInertiaDiagLocal(),
    327                 body1->getInvMass());
    328 #endif //_USE_JACOBIAN
    329 
    330 
    331         {
    332                 btScalar rel_vel;
    333                 btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0?body0->getLinearVelocity():btVector3(0,0,0))
    334                         + solverConstraint.m_relpos1CrossNormal.dot(body0?body0->getAngularVelocity():btVector3(0,0,0));
    335                 btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(body1?body1->getLinearVelocity():btVector3(0,0,0))
    336                         + solverConstraint.m_relpos2CrossNormal.dot(body1?body1->getAngularVelocity():btVector3(0,0,0));
    337 
    338                 rel_vel = vel1Dotn+vel2Dotn;
    339 
    340                 btScalar positionalError = 0.f;
    341 
    342                 btSimdScalar velocityError =  - rel_vel;
    343                 btSimdScalar    velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
    344                 solverConstraint.m_rhs = velocityImpulse;
    345                 solverConstraint.m_cfm = 0.f;
    346                 solverConstraint.m_lowerLimit = 0;
    347                 solverConstraint.m_upperLimit = 1e10f;
    348         }
    349 
    350494        return solverConstraint;
    351495}
    352496
    353 int     btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body)
    354 {
    355         int solverBodyIdA = -1;
    356 
    357         if (body.getCompanionId() >= 0)
    358         {
    359                 //body has already been converted
    360                 solverBodyIdA = body.getCompanionId();
    361         } else
    362         {
    363                 btRigidBody* rb = btRigidBody::upcast(&body);
    364                 if (rb && rb->getInvMass())
    365                 {
    366                         solverBodyIdA = m_tmpSolverBodyPool.size();
    367                         btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
    368                         initSolverBody(&solverBody,&body);
    369                         body.setCompanionId(solverBodyIdA);
    370                 } else
    371                 {
    372                         return 0;//assume first one is a fixed solver body
    373                 }
    374         }
    375         return solverBodyIdA;
    376 }
    377 #include <stdio.h>
    378 
    379 
    380 
    381 void    btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal)
    382 {
     497
     498
     499btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** /*bodies */,int /*numBodies */,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
     500{
     501        BT_PROFILE("solveGroupCacheFriendlySetup");
     502        (void)stackAlloc;
     503        (void)debugDrawer;
     504
     505
     506        if (!(numConstraints + numManifolds))
     507        {
     508//              printf("empty\n");
     509                return 0.f;
     510        }
     511        btPersistentManifold* manifold = 0;
    383512        btCollisionObject* colObj0=0,*colObj1=0;
    384513
    385         colObj0 = (btCollisionObject*)manifold->getBody0();
    386         colObj1 = (btCollisionObject*)manifold->getBody1();
    387 
    388         int solverBodyIdA=-1;
    389         int solverBodyIdB=-1;
    390 
    391         if (manifold->getNumContacts())
    392         {
    393                 solverBodyIdA = getOrInitSolverBody(*colObj0);
    394                 solverBodyIdB = getOrInitSolverBody(*colObj1);
    395         }
    396 
    397         ///avoid collision response between two static objects
    398         if (!solverBodyIdA && !solverBodyIdB)
    399                 return;
    400 
    401         btVector3 rel_pos1;
    402         btVector3 rel_pos2;
    403         btScalar relaxation;
    404 
    405         for (int j=0;j<manifold->getNumContacts();j++)
    406         {
    407 
    408                 btManifoldPoint& cp = manifold->getContactPoint(j);
    409 
    410                 if (cp.getDistance() <= manifold->getContactProcessingThreshold())
    411                 {
    412 
    413                         const btVector3& pos1 = cp.getPositionWorldOnA();
    414                         const btVector3& pos2 = cp.getPositionWorldOnB();
    415 
    416                         rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
    417                         rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
    418 
    419 
    420                         relaxation = 1.f;
    421                         btScalar rel_vel;
    422                         btVector3 vel;
    423 
    424                         int frictionIndex = m_tmpSolverContactConstraintPool.size();
    425 
    426                         {
    427                                 btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expand();
    428                                 btRigidBody* rb0 = btRigidBody::upcast(colObj0);
    429                                 btRigidBody* rb1 = btRigidBody::upcast(colObj1);
    430 
    431                                 solverConstraint.m_solverBodyIdA = solverBodyIdA;
    432                                 solverConstraint.m_solverBodyIdB = solverBodyIdB;
    433 
    434                                 solverConstraint.m_originalContactPoint = &cp;
    435 
    436                                 btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
    437                                 solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
    438                                 btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);           
    439                                 solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
    440                                 {
    441 #ifdef COMPUTE_IMPULSE_DENOM
    442                                         btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
    443                                         btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
    444 #else                                                   
    445                                         btVector3 vec;
    446                                         btScalar denom0 = 0.f;
    447                                         btScalar denom1 = 0.f;
    448                                         if (rb0)
     514        //btRigidBody* rb0=0,*rb1=0;
     515
     516
     517#ifdef FORCE_REFESH_CONTACT_MANIFOLDS
     518
     519        BEGIN_PROFILE("refreshManifolds");
     520
     521        int i;
     522       
     523       
     524
     525        for (i=0;i<numManifolds;i++)
     526        {
     527                manifold = manifoldPtr[i];
     528                rb1 = (btRigidBody*)manifold->getBody1();
     529                rb0 = (btRigidBody*)manifold->getBody0();
     530               
     531                manifold->refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform());
     532
     533        }
     534
     535        END_PROFILE("refreshManifolds");
     536#endif //FORCE_REFESH_CONTACT_MANIFOLDS
     537
     538       
     539
     540
     541
     542        //int sizeofSB = sizeof(btSolverBody);
     543        //int sizeofSC = sizeof(btSolverConstraint);
     544
     545
     546        //if (1)
     547        {
     548                //if m_stackAlloc, try to pack bodies/constraints to speed up solving
     549//              btBlock*                                        sablock;
     550//              sablock = stackAlloc->beginBlock();
     551
     552        //      int memsize = 16;
     553//              unsigned char* stackMemory = stackAlloc->allocate(memsize);
     554
     555               
     556                //todo: use stack allocator for this temp memory
     557//              int minReservation = numManifolds*2;
     558
     559                //m_tmpSolverBodyPool.reserve(minReservation);
     560
     561                //don't convert all bodies, only the one we need so solver the constraints
     562/*
     563                {
     564                        for (int i=0;i<numBodies;i++)
     565                        {
     566                                btRigidBody* rb = btRigidBody::upcast(bodies[i]);
     567                                if (rb &&       (rb->getIslandTag() >= 0))
     568                                {
     569                                        btAssert(rb->getCompanionId() < 0);
     570                                        int solverBodyId = m_tmpSolverBodyPool.size();
     571                                        btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
     572                                        initSolverBody(&solverBody,rb);
     573                                        rb->setCompanionId(solverBodyId);
     574                                }
     575                        }
     576                }
     577*/
     578               
     579                //m_tmpSolverConstraintPool.reserve(minReservation);
     580                //m_tmpSolverFrictionConstraintPool.reserve(minReservation);
     581
     582                {
     583                        int i;
     584
     585                        for (i=0;i<numManifolds;i++)
     586                        {
     587                                manifold = manifoldPtr[i];
     588                                colObj0 = (btCollisionObject*)manifold->getBody0();
     589                                colObj1 = (btCollisionObject*)manifold->getBody1();
     590                       
     591                                int solverBodyIdA=-1;
     592                                int solverBodyIdB=-1;
     593
     594                                if (manifold->getNumContacts())
     595                                {
     596
     597                                       
     598
     599                                        if (colObj0->getIslandTag() >= 0)
    449600                                        {
    450                                                 vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
    451                                                 denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
    452                                         }
    453                                         if (rb1)
    454                                         {
    455                                                 vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
    456                                                 denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
    457                                         }
    458 #endif //COMPUTE_IMPULSE_DENOM         
    459 
    460                                         btScalar denom = relaxation/(denom0+denom1);
    461                                         solverConstraint.m_jacDiagABInv = denom;
    462                                 }
    463 
    464                                 solverConstraint.m_contactNormal = cp.m_normalWorldOnB;
    465                                 solverConstraint.m_relpos1CrossNormal = rel_pos1.cross(cp.m_normalWorldOnB);
    466                                 solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(-cp.m_normalWorldOnB);
    467 
    468 
    469                                 btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
    470                                 btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
    471 
    472                                 vel  = vel1 - vel2;
    473 
    474                                 rel_vel = cp.m_normalWorldOnB.dot(vel);
    475 
    476                                 btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop;
    477 
    478 
    479                                 solverConstraint.m_friction = cp.m_combinedFriction;
    480 
    481                                 btScalar restitution = 0.f;
    482                                
    483                                 if (cp.m_lifeTime>infoGlobal.m_restingContactRestitutionThreshold)
    484                                 {
    485                                         restitution = 0.f;
    486                                 } else
    487                                 {
    488                                         restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution);
    489                                         if (restitution <= btScalar(0.))
    490                                         {
    491                                                 restitution = 0.f;
    492                                         };
    493                                 }
    494 
    495 
    496                                 ///warm starting (or zero if disabled)
    497                                 if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
    498                                 {
    499                                         solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
    500                                         if (rb0)
    501                                                 m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
    502                                         if (rb1)
    503                                                 m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass(),-solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse);
    504                                 } else
    505                                 {
    506                                         solverConstraint.m_appliedImpulse = 0.f;
    507                                 }
    508 
    509                                 //                                                      solverConstraint.m_appliedPushImpulse = 0.f;
    510 
    511                                 {
    512                                         btScalar rel_vel;
    513                                         btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rb0?rb0->getLinearVelocity():btVector3(0,0,0))
    514                                                 + solverConstraint.m_relpos1CrossNormal.dot(rb0?rb0->getAngularVelocity():btVector3(0,0,0));
    515                                         btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rb1?rb1->getLinearVelocity():btVector3(0,0,0))
    516                                                 + solverConstraint.m_relpos2CrossNormal.dot(rb1?rb1->getAngularVelocity():btVector3(0,0,0));
    517 
    518                                         rel_vel = vel1Dotn+vel2Dotn;
    519 
    520                                         btScalar positionalError = 0.f;
    521                                         positionalError = -penetration * infoGlobal.m_erp/infoGlobal.m_timeStep;
    522                                         btScalar        velocityError = restitution - rel_vel;// * damping;
    523                                         btScalar  penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
    524                                         btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
    525                                         solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
    526                                         solverConstraint.m_cfm = 0.f;
    527                                         solverConstraint.m_lowerLimit = 0;
    528                                         solverConstraint.m_upperLimit = 1e10f;
    529                                 }
    530 
    531 
    532                                 /////setup the friction constraints
    533 
    534 
    535 
    536                                 if (1)
    537                                 {
    538                                         solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
    539                                         if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
    540                                         {
    541                                                 cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
    542                                                 btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
    543                                                 if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
     601                                                if (colObj0->getCompanionId() >= 0)
    544602                                                {
    545                                                         cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel);
    546                                                         applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
    547                                                         applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
    548                                                         addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
    549                                                         if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
    550                                                         {
    551                                                                 cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
    552                                                                 cp.m_lateralFrictionDir2.normalize();//??
    553                                                                 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
    554                                                                 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
    555                                                                 addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
    556                                                         }
    557                                                         cp.m_lateralFrictionInitialized = true;
     603                                                        //body has already been converted
     604                                                        solverBodyIdA = colObj0->getCompanionId();
    558605                                                } else
    559606                                                {
    560                                                         //re-calculate friction direction every frame, todo: check if this is really needed
    561                                                         btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
    562                                                         applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
    563                                                         applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
    564 
    565                                                         addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
    566                                                         if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
    567                                                         {
    568                                                                 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
    569                                                                 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
    570                                                                 addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
    571                                                         }
    572                                                         cp.m_lateralFrictionInitialized = true;
    573                                                 }
    574 
    575                                         } else
    576                                         {
    577                                                 addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
    578                                                 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
    579                                                         addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
    580                                         }
    581 
    582                                         if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
    583                                         {
    584                                                 {
    585                                                         btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
    586                                                         if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
    587                                                         {
    588                                                                 frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
    589                                                                 if (rb0)
    590                                                                         m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
    591                                                                 if (rb1)
    592                                                                         m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass(),-frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse);
    593                                                         } else
    594                                                         {
    595                                                                 frictionConstraint1.m_appliedImpulse = 0.f;
    596                                                         }
    597                                                 }
    598 
    599                                                 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
    600                                                 {
    601                                                         btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
    602                                                         if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
    603                                                         {
    604                                                                 frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
    605                                                                 if (rb0)
    606                                                                         m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
    607                                                                 if (rb1)
    608                                                                         m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse);
    609                                                         } else
    610                                                         {
    611                                                                 frictionConstraint2.m_appliedImpulse = 0.f;
    612                                                         }
     607                                                        solverBodyIdA = m_tmpSolverBodyPool.size();
     608                                                        btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
     609                                                        initSolverBody(&solverBody,colObj0);
     610                                                        colObj0->setCompanionId(solverBodyIdA);
    613611                                                }
    614612                                        } else
    615613                                        {
    616                                                 btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
    617                                                 frictionConstraint1.m_appliedImpulse = 0.f;
    618                                                 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
     614                                                //create a static body
     615                                                solverBodyIdA = m_tmpSolverBodyPool.size();
     616                                                btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
     617                                                initSolverBody(&solverBody,colObj0);
     618                                        }
     619
     620                                        if (colObj1->getIslandTag() >= 0)
     621                                        {
     622                                                if (colObj1->getCompanionId() >= 0)
    619623                                                {
    620                                                         btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
    621                                                         frictionConstraint2.m_appliedImpulse = 0.f;
     624                                                        solverBodyIdB = colObj1->getCompanionId();
     625                                                } else
     626                                                {
     627                                                        solverBodyIdB = m_tmpSolverBodyPool.size();
     628                                                        btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
     629                                                        initSolverBody(&solverBody,colObj1);
     630                                                        colObj1->setCompanionId(solverBodyIdB);
    622631                                                }
     632                                        } else
     633                                        {
     634                                                //create a static body
     635                                                solverBodyIdB = m_tmpSolverBodyPool.size();
     636                                                btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
     637                                                initSolverBody(&solverBody,colObj1);
    623638                                        }
    624639                                }
    625                         }
    626 
    627 
    628                 }
    629         }
    630 }
    631 
    632 
    633 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** /*bodies */,int /*numBodies */,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
    634 {
    635         BT_PROFILE("solveGroupCacheFriendlySetup");
    636         (void)stackAlloc;
    637         (void)debugDrawer;
    638 
    639 
    640         if (!(numConstraints + numManifolds))
    641         {
    642                 //              printf("empty\n");
    643                 return 0.f;
    644         }
    645 
    646         if (1)
     640
     641                                btVector3 rel_pos1;
     642                                btVector3 rel_pos2;
     643                                btScalar relaxation;
     644
     645                                for (int j=0;j<manifold->getNumContacts();j++)
     646                                {
     647                                       
     648                                        btManifoldPoint& cp = manifold->getContactPoint(j);
     649                                       
     650                                        if (cp.getDistance() <= btScalar(0.))
     651                                        {
     652                                               
     653                                                const btVector3& pos1 = cp.getPositionWorldOnA();
     654                                                const btVector3& pos2 = cp.getPositionWorldOnB();
     655
     656                                                 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
     657                                                 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
     658
     659                                               
     660                                                relaxation = 1.f;
     661                                                btScalar rel_vel;
     662                                                btVector3 vel;
     663
     664                                                int frictionIndex = m_tmpSolverConstraintPool.size();
     665
     666                                                {
     667                                                        btSolverConstraint& solverConstraint = m_tmpSolverConstraintPool.expand();
     668                                                        btRigidBody* rb0 = btRigidBody::upcast(colObj0);
     669                                                        btRigidBody* rb1 = btRigidBody::upcast(colObj1);
     670
     671                                                        solverConstraint.m_solverBodyIdA = solverBodyIdA;
     672                                                        solverConstraint.m_solverBodyIdB = solverBodyIdB;
     673                                                        solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_CONTACT_1D;
     674
     675                                                        solverConstraint.m_originalContactPoint = &cp;
     676
     677                                                        btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
     678                                                        solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0 : btVector3(0,0,0);
     679                                                        btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);           
     680                                                        solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*torqueAxis1 : btVector3(0,0,0);
     681                                                        {
     682#ifdef COMPUTE_IMPULSE_DENOM
     683                                                                btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
     684                                                                btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
     685#else                                                   
     686                                                                btVector3 vec;
     687                                                                btScalar denom0 = 0.f;
     688                                                                btScalar denom1 = 0.f;
     689                                                                if (rb0)
     690                                                                {
     691                                                                        vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
     692                                                                        denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
     693                                                                }
     694                                                                if (rb1)
     695                                                                {
     696                                                                        vec = ( solverConstraint.m_angularComponentB).cross(rel_pos2);
     697                                                                        denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
     698                                                                }
     699#endif //COMPUTE_IMPULSE_DENOM         
     700                                                               
     701                                                                btScalar denom = relaxation/(denom0+denom1);
     702                                                                solverConstraint.m_jacDiagABInv = denom;
     703                                                        }
     704
     705                                                        solverConstraint.m_contactNormal = cp.m_normalWorldOnB;
     706                                                        solverConstraint.m_relpos1CrossNormal = rel_pos1.cross(cp.m_normalWorldOnB);
     707                                                        solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(cp.m_normalWorldOnB);
     708
     709
     710                                                        btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
     711                                                        btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
     712                               
     713                                                        vel  = vel1 - vel2;
     714                                                       
     715                                                        rel_vel = cp.m_normalWorldOnB.dot(vel);
     716                                                       
     717                                                        solverConstraint.m_penetration = btMin(cp.getDistance()+infoGlobal.m_linearSlop,btScalar(0.));
     718                                                        //solverConstraint.m_penetration = cp.getDistance();
     719
     720                                                        solverConstraint.m_friction = cp.m_combinedFriction;
     721
     722                                                       
     723                                                        if (cp.m_lifeTime>infoGlobal.m_restingContactRestitutionThreshold)
     724                                                        {
     725                                                                solverConstraint.m_restitution = 0.f;
     726                                                        } else
     727                                                        {
     728                                                                solverConstraint.m_restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution);
     729                                                                if (solverConstraint.m_restitution <= btScalar(0.))
     730                                                                {
     731                                                                        solverConstraint.m_restitution = 0.f;
     732                                                                };
     733                                                        }
     734
     735                                                       
     736                                                        btScalar penVel = -solverConstraint.m_penetration/infoGlobal.m_timeStep;
     737
     738                                                       
     739
     740                                                        if (solverConstraint.m_restitution > penVel)
     741                                                        {
     742                                                                solverConstraint.m_penetration = btScalar(0.);
     743                                                        }
     744                                                         
     745                                                       
     746                                                       
     747                                                        ///warm starting (or zero if disabled)
     748                                                        if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
     749                                                        {
     750                                                                solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
     751                                                                if (rb0)
     752                                                                        m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
     753                                                                if (rb1)
     754                                                                        m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass(),solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse);
     755                                                        } else
     756                                                        {
     757                                                                solverConstraint.m_appliedImpulse = 0.f;
     758                                                        }
     759
     760                                                        solverConstraint.m_appliedPushImpulse = 0.f;
     761                                                       
     762                                                        solverConstraint.m_frictionIndex = m_tmpSolverFrictionConstraintPool.size();
     763                                                        if (!cp.m_lateralFrictionInitialized)
     764                                                        {
     765                                                                cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
     766                                                                btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
     767                                                                if (lat_rel_vel > SIMD_EPSILON)//0.0f)
     768                                                                {
     769                                                                        cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel);
     770                                                                        addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
     771                                                                        if(infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
     772                                                                        {
     773                                                                                cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
     774                                                                                cp.m_lateralFrictionDir2.normalize();//??
     775                                                                                addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
     776                                                                                cp.m_lateralFrictionInitialized = true;
     777                                                                        }
     778                                                                } else
     779                                                                {
     780                                                                        //re-calculate friction direction every frame, todo: check if this is really needed
     781                                                                        btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
     782                                                                        addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
     783                                                                        addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
     784                                                                        if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
     785                                                                        {
     786                                                                                cp.m_lateralFrictionInitialized = true;
     787                                                                        }
     788                                                                }
     789                                                               
     790                                                        } else
     791                                                        {
     792                                                                addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
     793                                                                if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
     794                                                                        addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
     795                                                        }
     796
     797                                                        if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
     798                                                        {
     799                                                                {
     800                                                                        btSolverConstraint& frictionConstraint1 = m_tmpSolverFrictionConstraintPool[solverConstraint.m_frictionIndex];
     801                                                                        if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
     802                                                                        {
     803                                                                                frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
     804                                                                                if (rb0)
     805                                                                                        m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
     806                                                                                if (rb1)
     807                                                                                        m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass(),frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse);
     808                                                                        } else
     809                                                                        {
     810                                                                                frictionConstraint1.m_appliedImpulse = 0.f;
     811                                                                        }
     812                                                                }
     813                                                                {
     814                                                                        btSolverConstraint& frictionConstraint2 = m_tmpSolverFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
     815                                                                        if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
     816                                                                        {
     817                                                                                frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
     818                                                                                if (rb0)
     819                                                                                        m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
     820                                                                                if (rb1)
     821                                                                                        m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse);
     822                                                                        } else
     823                                                                        {
     824                                                                                frictionConstraint2.m_appliedImpulse = 0.f;
     825                                                                        }
     826                                                                }
     827                                                        }
     828                                                }
     829
     830
     831                                        }
     832                                }
     833                        }
     834                }
     835        }
     836       
     837        btContactSolverInfo info = infoGlobal;
     838
    647839        {
    648840                int j;
    … …  
    653845                }
    654846        }
    655 
    656         btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
    657         initSolverBody(&fixedBody,0);
    658 
    659         //btRigidBody* rb0=0,*rb1=0;
    660 
    661         //if (1)
    662         {
    663                 {
    664 
    665                         int totalNumRows = 0;
    666                         int i;
    667                         //calculate the total number of contraint rows
    668                         for (i=0;i<numConstraints;i++)
    669                         {
    670 
    671                                 btTypedConstraint::btConstraintInfo1 info1;
    672                                 constraints[i]->getInfo1(&info1);
    673                                 totalNumRows += info1.m_numConstraintRows;
    674                         }
    675                         m_tmpSolverNonContactConstraintPool.resize(totalNumRows);
    676 
    677                         btTypedConstraint::btConstraintInfo1 info1;
    678                         info1.m_numConstraintRows = 0;
    679 
    680 
    681                         ///setup the btSolverConstraints
    682                         int currentRow = 0;
    683 
    684                         for (i=0;i<numConstraints;i++,currentRow+=info1.m_numConstraintRows)
    685                         {
    686                                 constraints[i]->getInfo1(&info1);
    687                                 if (info1.m_numConstraintRows)
    688                                 {
    689                                         btAssert(currentRow<totalNumRows);
    690 
    691                                         btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];
    692                                         btTypedConstraint* constraint = constraints[i];
    693 
    694 
    695 
    696                                         btRigidBody& rbA = constraint->getRigidBodyA();
    697                                         btRigidBody& rbB = constraint->getRigidBodyB();
    698 
    699                                         int solverBodyIdA = getOrInitSolverBody(rbA);
    700                                         int solverBodyIdB = getOrInitSolverBody(rbB);
    701 
    702                                         btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
    703                                         btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
    704 
    705                                         int j;
    706                                         for ( j=0;j<info1.m_numConstraintRows;j++)
    707                                         {
    708                                                 memset(&currentConstraintRow[j],0,sizeof(btSolverConstraint));
    709                                                 currentConstraintRow[j].m_lowerLimit = -FLT_MAX;
    710                                                 currentConstraintRow[j].m_upperLimit = FLT_MAX;
    711                                                 currentConstraintRow[j].m_appliedImpulse = 0.f;
    712                                                 currentConstraintRow[j].m_appliedPushImpulse = 0.f;
    713                                                 currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA;
    714                                                 currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB;
    715                                         }
    716 
    717                                         bodyAPtr->m_deltaLinearVelocity.setValue(0.f,0.f,0.f);
    718                                         bodyAPtr->m_deltaAngularVelocity.setValue(0.f,0.f,0.f);
    719                                         bodyBPtr->m_deltaLinearVelocity.setValue(0.f,0.f,0.f);
    720                                         bodyBPtr->m_deltaAngularVelocity.setValue(0.f,0.f,0.f);
    721 
    722 
    723 
    724                                         btTypedConstraint::btConstraintInfo2 info2;
    725                                         info2.fps = 1.f/infoGlobal.m_timeStep;
    726                                         info2.erp = infoGlobal.m_erp;
    727                                         info2.m_J1linearAxis = currentConstraintRow->m_contactNormal;
    728                                         info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal;
    729                                         info2.m_J2linearAxis = 0;
    730                                         info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal;
    731                                         info2.rowskip = sizeof(btSolverConstraint)/sizeof(btScalar);//check this
    732                                         ///the size of btSolverConstraint needs be a multiple of btScalar
    733                                         btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint));
    734                                         info2.m_constraintError = &currentConstraintRow->m_rhs;
    735                                         info2.cfm = &currentConstraintRow->m_cfm;
    736                                         info2.m_lowerLimit = &currentConstraintRow->m_lowerLimit;
    737                                         info2.m_upperLimit = &currentConstraintRow->m_upperLimit;
    738                                         constraints[i]->getInfo2(&info2);
    739 
    740                                         ///finalize the constraint setup
    741                                         for ( j=0;j<info1.m_numConstraintRows;j++)
    742                                         {
    743                                                 btSolverConstraint& solverConstraint = currentConstraintRow[j];
    744 
    745                                                 {
    746                                                         const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
    747                                                         solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld()*ftorqueAxis1*constraint->getRigidBodyA().getAngularFactor();
    748                                                 }
    749                                                 {
    750                                                         const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal;
    751                                                         solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld()*ftorqueAxis2*constraint->getRigidBodyB().getAngularFactor();
    752                                                 }
    753 
    754                                                 {
    755                                                         btVector3 iMJlA = solverConstraint.m_contactNormal*rbA.getInvMass();
    756                                                         btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal;
    757                                                         btVector3 iMJlB = solverConstraint.m_contactNormal*rbB.getInvMass();//sign of normal?
    758                                                         btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal;
    759 
    760                                                         btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal);
    761                                                         sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
    762                                                         sum += iMJlB.dot(solverConstraint.m_contactNormal);
    763                                                         sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
    764 
    765                                                         solverConstraint.m_jacDiagABInv = btScalar(1.)/sum;
    766                                                 }
    767 
    768 
    769                                                 ///fix rhs
    770                                                 ///todo: add force/torque accelerators
    771                                                 {
    772                                                         btScalar rel_vel;
    773                                                         btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rbA.getLinearVelocity()) + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity());
    774                                                         btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rbB.getLinearVelocity()) + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity());
    775 
    776                                                         rel_vel = vel1Dotn+vel2Dotn;
    777 
    778                                                         btScalar restitution = 0.f;
    779                                                         btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2
    780                                                         btScalar        velocityError = restitution - rel_vel;// * damping;
    781                                                         btScalar        penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
    782                                                         btScalar        velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
    783                                                         solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
    784                                                         solverConstraint.m_appliedImpulse = 0.f;
    785 
    786                                                 }
    787                                         }
    788                                 }
    789                         }
    790                 }
    791 
    792                 {
    793                         int i;
    794                         btPersistentManifold* manifold = 0;
    795                         btCollisionObject* colObj0=0,*colObj1=0;
    796 
    797 
    798                         for (i=0;i<numManifolds;i++)
    799                         {
    800                                 manifold = manifoldPtr[i];
    801                                 convertContact(manifold,infoGlobal);
    802                         }
    803                 }
    804         }
    805 
    806         btContactSolverInfo info = infoGlobal;
    807 
    808 
    809 
    810         int numConstraintPool = m_tmpSolverContactConstraintPool.size();
    811         int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
     847       
     848       
     849
     850        int numConstraintPool = m_tmpSolverConstraintPool.size();
     851        int numFrictionPool = m_tmpSolverFrictionConstraintPool.size();
    812852
    813853        ///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
    … …  
    826866        }
    827867
     868
     869
    828870        return 0.f;
    829871
    … …  
    833875{
    834876        BT_PROFILE("solveGroupCacheFriendlyIterations");
    835 
    836         int numConstraintPool = m_tmpSolverContactConstraintPool.size();
    837         int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
     877        int numConstraintPool = m_tmpSolverConstraintPool.size();
     878        int numFrictionPool = m_tmpSolverFrictionConstraintPool.size();
    838879
    839880        //should traverse the contacts random order...
    … …  
    863904                        }
    864905
    865                         if (infoGlobal.m_solverMode & SOLVER_SIMD)
    866                         {
    867                                 ///solve all joint constraints, using SIMD, if available
    868                                 for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
    869                                 {
    870                                         btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
    871                                         resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
    872                                 }
    873 
    874                                 for (j=0;j<numConstraints;j++)
    875                                 {
    876                                         int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA());
    877                                         int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB());
    878                                         btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
    879                                         btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
    880                                         constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
    881                                 }
    882 
    883                                 ///solve all contact constraints using SIMD, if available
    884                                 int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
     906                        for (j=0;j<numConstraints;j++)
     907                        {
     908                                btTypedConstraint* constraint = constraints[j];
     909                                ///todo: use solver bodies, so we don't need to copy from/to btRigidBody
     910
     911                                if ((constraint->getRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0))
     912                                {
     913                                        m_tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].writebackVelocity();
     914                                }
     915                                if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0))
     916                                {
     917                                        m_tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].writebackVelocity();
     918                                }
     919
     920                                constraint->solveConstraint(infoGlobal.m_timeStep);
     921
     922                                if ((constraint->getRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0))
     923                                {
     924                                        m_tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].readVelocity();
     925                                }
     926                                if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0))
     927                                {
     928                                        m_tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].readVelocity();
     929                                }
     930
     931                        }
     932
     933                        {
     934                                int numPoolConstraints = m_tmpSolverConstraintPool.size();
    885935                                for (j=0;j<numPoolConstraints;j++)
    886936                                {
    887                                         const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
    888                                         resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
    889 
    890                                 }
    891                                 ///solve all friction constraints, using SIMD, if available
    892                                 int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
    893                                 for (j=0;j<numFrictionPoolConstraints;j++)
    894                                 {
    895                                         btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
    896                                         btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
    897 
    898                                         if (totalImpulse>btScalar(0))
     937                                       
     938                                        const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[m_orderTmpConstraintPool[j]];
     939                                        resolveSingleCollisionCombinedCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
     940                                                m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal);
     941                                }
     942                        }
     943
     944                        {
     945                                 int numFrictionPoolConstraints = m_tmpSolverFrictionConstraintPool.size();
     946                               
     947                                 for (j=0;j<numFrictionPoolConstraints;j++)
     948                                {
     949                                        const btSolverConstraint& solveManifold = m_tmpSolverFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
     950                                        btScalar totalImpulse = m_tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse+
     951                                                                m_tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedPushImpulse;                 
     952
     953                                                resolveSingleFrictionCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
     954                                                        m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal,
     955                                                        totalImpulse);
     956                                }
     957                        }
     958                       
     959
     960
     961                }
     962       
     963                if (infoGlobal.m_splitImpulse)
     964                {
     965                       
     966                        for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
     967                        {
     968                                {
     969                                        int numPoolConstraints = m_tmpSolverConstraintPool.size();
     970                                        int j;
     971                                        for (j=0;j<numPoolConstraints;j++)
    899972                                        {
    900                                                 solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
    901                                                 solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
    902 
    903                                                 resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],       m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
     973                                                const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[m_orderTmpConstraintPool[j]];
     974
     975                                                resolveSplitPenetrationImpulseCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
     976                                                        m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal);
    904977                                        }
    905978                                }
    906                         } else
    907                         {
    908 
    909                                 ///solve all joint constraints
    910                                 for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
    911                                 {
    912                                         btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
    913                                         resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
    914                                 }
    915 
    916                                 for (j=0;j<numConstraints;j++)
    917                                 {
    918                                         int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA());
    919                                         int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB());
    920                                         btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
    921                                         btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
    922 
    923                                         constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
    924                                 }
    925 
    926                                 ///solve all contact constraints
    927                                 int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
    928                                 for (j=0;j<numPoolConstraints;j++)
    929                                 {
    930                                         const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
    931                                         resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
    932                                 }
    933                                 ///solve all friction constraints
    934                                 int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
    935                                 for (j=0;j<numFrictionPoolConstraints;j++)
    936                                 {
    937                                         btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
    938                                         btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
    939 
    940                                         if (totalImpulse>btScalar(0))
    941                                         {
    942                                                 solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
    943                                                 solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
    944 
    945                                                 resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],                                                   m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
    946                                         }
    947                                 }
    948                         }
    949 
    950 
    951 
    952                 }
    953         }
     979                        }
     980
     981                }
     982
     983        }
     984
    954985        return 0.f;
    955986}
    956987
    957988
    958 
    959 /// btSequentialImpulseConstraintSolver Sequentially applies impulses
    960 btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/)
    961 {
    962 
    963        
    964 
    965         BT_PROFILE("solveGroup");
    966         //we only implement SOLVER_CACHE_FRIENDLY now
    967         //you need to provide at least some bodies
    968         btAssert(bodies);
    969         btAssert(numBodies);
    970 
     989btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
     990{
    971991        int i;
    972992
    … …  
    974994        solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
    975995
    976         int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
     996        int numPoolConstraints = m_tmpSolverConstraintPool.size();
    977997        int j;
    978 
    979998        for (j=0;j<numPoolConstraints;j++)
    980999        {
    981 
    982                 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j];
     1000               
     1001                const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[j];
    9831002                btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
    9841003                btAssert(pt);
    … …  
    9861005                if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
    9871006                {
    988                         pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
    989                         pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
     1007                        pt->m_appliedImpulseLateral1 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
     1008                        pt->m_appliedImpulseLateral2 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
    9901009                }
    9911010
    9921011                //do a callback here?
     1012
    9931013        }
    9941014
    … …  
    10021022        {
    10031023                for ( i=0;i<m_tmpSolverBodyPool.size();i++)
    1004                 {
    1005                         m_tmpSolverBodyPool[i].writebackVelocity();
    1006                 }
    1007         }
    1008 
     1024        {
     1025                m_tmpSolverBodyPool[i].writebackVelocity();
     1026        }
     1027        }
     1028
     1029//      printf("m_tmpSolverConstraintPool.size() = %i\n",m_tmpSolverConstraintPool.size());
     1030
     1031/*
     1032        printf("m_tmpSolverBodyPool.size() = %i\n",m_tmpSolverBodyPool.size());
     1033        printf("m_tmpSolverConstraintPool.size() = %i\n",m_tmpSolverConstraintPool.size());
     1034        printf("m_tmpSolverFrictionConstraintPool.size() = %i\n",m_tmpSolverFrictionConstraintPool.size());
     1035
     1036       
     1037        printf("m_tmpSolverBodyPool.capacity() = %i\n",m_tmpSolverBodyPool.capacity());
     1038        printf("m_tmpSolverConstraintPool.capacity() = %i\n",m_tmpSolverConstraintPool.capacity());
     1039        printf("m_tmpSolverFrictionConstraintPool.capacity() = %i\n",m_tmpSolverFrictionConstraintPool.capacity());
     1040*/
    10091041
    10101042        m_tmpSolverBodyPool.resize(0);
    1011         m_tmpSolverContactConstraintPool.resize(0);
    1012         m_tmpSolverNonContactConstraintPool.resize(0);
    1013         m_tmpSolverContactFrictionConstraintPool.resize(0);
     1043        m_tmpSolverConstraintPool.resize(0);
     1044        m_tmpSolverFrictionConstraintPool.resize(0);
     1045
    10141046
    10151047        return 0.f;
    10161048}
    10171049
    1018 
    1019 
    1020 
    1021 
    1022 
    1023 
     1050/// btSequentialImpulseConstraintSolver Sequentially applies impulses
     1051btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/)
     1052{
     1053        BT_PROFILE("solveGroup");
     1054        if (infoGlobal.m_solverMode & SOLVER_CACHE_FRIENDLY)
     1055        {
     1056                //you need to provide at least some bodies
     1057                //btSimpleDynamicsWorld needs to switch off SOLVER_CACHE_FRIENDLY
     1058                btAssert(bodies);
     1059                btAssert(numBodies);
     1060                return solveGroupCacheFriendly(bodies,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
     1061        }
     1062
     1063       
     1064
     1065        btContactSolverInfo info = infoGlobal;
     1066
     1067        int numiter = infoGlobal.m_numIterations;
     1068
     1069        int totalPoints = 0;
     1070
     1071
     1072        {
     1073                short j;
     1074                for (j=0;j<numManifolds;j++)
     1075                {
     1076                        btPersistentManifold* manifold = manifoldPtr[j];
     1077                        prepareConstraints(manifold,info,debugDrawer);
     1078
     1079                        for (short p=0;p<manifoldPtr[j]->getNumContacts();p++)
     1080                        {
     1081                                gOrder[totalPoints].m_manifoldIndex = j;
     1082                                gOrder[totalPoints].m_pointIndex = p;
     1083                                totalPoints++;
     1084                        }
     1085                }
     1086        }
     1087
     1088        {
     1089                int j;
     1090                for (j=0;j<numConstraints;j++)
     1091                {
     1092                        btTypedConstraint* constraint = constraints[j];
     1093                        constraint->buildJacobian();
     1094                }
     1095        }
     1096       
     1097
     1098        //should traverse the contacts random order...
     1099        int iteration;
     1100
     1101        {
     1102                for ( iteration = 0;iteration<numiter;iteration++)
     1103                {
     1104                        int j;
     1105                        if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
     1106                        {
     1107                                if ((iteration & 7) == 0) {
     1108                                        for (j=0; j<totalPoints; ++j) {
     1109                                                btOrderIndex tmp = gOrder[j];
     1110                                                int swapi = btRandInt2(j+1);
     1111                                                gOrder[j] = gOrder[swapi];
     1112                                                gOrder[swapi] = tmp;
     1113                                        }
     1114                                }
     1115                        }
     1116
     1117                        for (j=0;j<numConstraints;j++)
     1118                        {
     1119                                btTypedConstraint* constraint = constraints[j];
     1120                                constraint->solveConstraint(info.m_timeStep);
     1121                        }
     1122
     1123                        for (j=0;j<totalPoints;j++)
     1124                        {
     1125                                btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
     1126                                solve( (btRigidBody*)manifold->getBody0(),
     1127                                                                        (btRigidBody*)manifold->getBody1()
     1128                                ,manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer);
     1129                        }
     1130               
     1131                        for (j=0;j<totalPoints;j++)
     1132                        {
     1133                                btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
     1134                                solveFriction((btRigidBody*)manifold->getBody0(),
     1135                                        (btRigidBody*)manifold->getBody1(),manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer);
     1136                        }
     1137                       
     1138                }
     1139        }
     1140               
     1141
     1142
     1143
     1144        return btScalar(0.);
     1145}
     1146
     1147
     1148
     1149
     1150
     1151
     1152
     1153void    btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer)
     1154{
     1155
     1156        (void)debugDrawer;
     1157
     1158        btRigidBody* body0 = (btRigidBody*)manifoldPtr->getBody0();
     1159        btRigidBody* body1 = (btRigidBody*)manifoldPtr->getBody1();
     1160
     1161
     1162        //only necessary to refresh the manifold once (first iteration). The integration is done outside the loop
     1163        {
     1164#ifdef FORCE_REFESH_CONTACT_MANIFOLDS
     1165                manifoldPtr->refreshContactPoints(body0->getCenterOfMassTransform(),body1->getCenterOfMassTransform());
     1166#endif //FORCE_REFESH_CONTACT_MANIFOLDS         
     1167                int numpoints = manifoldPtr->getNumContacts();
     1168
     1169                gTotalContactPoints += numpoints;
     1170
     1171               
     1172                for (int i=0;i<numpoints ;i++)
     1173                {
     1174                        btManifoldPoint& cp = manifoldPtr->getContactPoint(i);
     1175                        if (cp.getDistance() <= btScalar(0.))
     1176                        {
     1177                                const btVector3& pos1 = cp.getPositionWorldOnA();
     1178                                const btVector3& pos2 = cp.getPositionWorldOnB();
     1179
     1180                                btVector3 rel_pos1 = pos1 - body0->getCenterOfMassPosition();
     1181                                btVector3 rel_pos2 = pos2 - body1->getCenterOfMassPosition();
     1182                               
     1183
     1184                                //this jacobian entry is re-used for all iterations
     1185                                btJacobianEntry jac(body0->getCenterOfMassTransform().getBasis().transpose(),
     1186                                        body1->getCenterOfMassTransform().getBasis().transpose(),
     1187                                        rel_pos1,rel_pos2,cp.m_normalWorldOnB,body0->getInvInertiaDiagLocal(),body0->getInvMass(),
     1188                                        body1->getInvInertiaDiagLocal(),body1->getInvMass());
     1189
     1190                               
     1191                                btScalar jacDiagAB = jac.getDiagonal();
     1192
     1193                                btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
     1194                                if (cpd)
     1195                                {
     1196                                        //might be invalid
     1197                                        cpd->m_persistentLifeTime++;
     1198                                        if (cpd->m_persistentLifeTime != cp.getLifeTime())
     1199                                        {
     1200                                                //printf("Invalid: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
     1201                                                new (cpd) btConstraintPersistentData;
     1202                                                cpd->m_persistentLifeTime = cp.getLifeTime();
     1203
     1204                                        } else
     1205                                        {
     1206                                                //printf("Persistent: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
     1207                                               
     1208                                        }
     1209                                } else
     1210                                {
     1211                                               
     1212                                        //todo: should this be in a pool?
     1213                                        void* mem = btAlignedAlloc(sizeof(btConstraintPersistentData),16);
     1214                                        cpd = new (mem)btConstraintPersistentData;
     1215                                        assert(cpd);
     1216
     1217                                        totalCpd ++;
     1218                                        //printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd);
     1219                                        cp.m_userPersistentData = cpd;
     1220                                        cpd->m_persistentLifeTime = cp.getLifeTime();
     1221                                        //printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.getLifeTime());
     1222                                       
     1223                                }
     1224                                assert(cpd);
     1225
     1226                                cpd->m_jacDiagABInv = btScalar(1.) / jacDiagAB;
     1227
     1228                                //Dependent on Rigidbody A and B types, fetch the contact/friction response func
     1229                                //perhaps do a similar thing for friction/restutution combiner funcs...
     1230                               
     1231                                cpd->m_frictionSolverFunc = m_frictionDispatch[body0->m_frictionSolverType][body1->m_frictionSolverType];
     1232                                cpd->m_contactSolverFunc = m_contactDispatch[body0->m_contactSolverType][body1->m_contactSolverType];
     1233                               
     1234                                btVector3 vel1 = body0->getVelocityInLocalPoint(rel_pos1);
     1235                                btVector3 vel2 = body1->getVelocityInLocalPoint(rel_pos2);
     1236                                btVector3 vel = vel1 - vel2;
     1237                                btScalar rel_vel;
     1238                                rel_vel = cp.m_normalWorldOnB.dot(vel);
     1239                               
     1240                                btScalar combinedRestitution = cp.m_combinedRestitution;
     1241                               
     1242                                cpd->m_penetration = cp.getDistance();///btScalar(info.m_numIterations);
     1243                                cpd->m_friction = cp.m_combinedFriction;
     1244                                if (cp.m_lifeTime>info.m_restingContactRestitutionThreshold)
     1245                                {
     1246                                        cpd->m_restitution = 0.f;
     1247                                } else
     1248                                {
     1249                                        cpd->m_restitution = restitutionCurve(rel_vel, combinedRestitution);
     1250                                        if (cpd->m_restitution <= btScalar(0.))
     1251                                        {
     1252                                                cpd->m_restitution = btScalar(0.0);
     1253                                        };
     1254                                }
     1255                               
     1256                                //restitution and penetration work in same direction so
     1257                                //rel_vel
     1258                               
     1259                                btScalar penVel = -cpd->m_penetration/info.m_timeStep;
     1260
     1261                                if (cpd->m_restitution > penVel)
     1262                                {
     1263                                        cpd->m_penetration = btScalar(0.);
     1264                                }                               
     1265                               
     1266
     1267                                btScalar relaxation = info.m_damping;
     1268                                if (info.m_solverMode & SOLVER_USE_WARMSTARTING)
     1269                                {
     1270                                        cpd->m_appliedImpulse *= relaxation;
     1271                                } else
     1272                                {
     1273                                        cpd->m_appliedImpulse =btScalar(0.);
     1274                                }
     1275       
     1276                                //for friction
     1277                                cpd->m_prevAppliedImpulse = cpd->m_appliedImpulse;
     1278                               
     1279                                //re-calculate friction direction every frame, todo: check if this is really needed
     1280                                btPlaneSpace1(cp.m_normalWorldOnB,cpd->m_frictionWorldTangential0,cpd->m_frictionWorldTangential1);
     1281
     1282
     1283#define NO_FRICTION_WARMSTART 1
     1284
     1285        #ifdef NO_FRICTION_WARMSTART
     1286                                cpd->m_accumulatedTangentImpulse0 = btScalar(0.);
     1287                                cpd->m_accumulatedTangentImpulse1 = btScalar(0.);
     1288        #endif //NO_FRICTION_WARMSTART
     1289                                btScalar denom0 = body0->computeImpulseDenominator(pos1,cpd->m_frictionWorldTangential0);
     1290                                btScalar denom1 = body1->computeImpulseDenominator(pos2,cpd->m_frictionWorldTangential0);
     1291                                btScalar denom = relaxation/(denom0+denom1);
     1292                                cpd->m_jacDiagABInvTangent0 = denom;
     1293
     1294
     1295                                denom0 = body0->computeImpulseDenominator(pos1,cpd->m_frictionWorldTangential1);
     1296                                denom1 = body1->computeImpulseDenominator(pos2,cpd->m_frictionWorldTangential1);
     1297                                denom = relaxation/(denom0+denom1);
     1298                                cpd->m_jacDiagABInvTangent1 = denom;
     1299
     1300                                btVector3 totalImpulse =
     1301        #ifndef NO_FRICTION_WARMSTART
     1302                                        cpd->m_frictionWorldTangential0*cpd->m_accumulatedTangentImpulse0+
     1303                                        cpd->m_frictionWorldTangential1*cpd->m_accumulatedTangentImpulse1+
     1304        #endif //NO_FRICTION_WARMSTART
     1305                                        cp.m_normalWorldOnB*cpd->m_appliedImpulse;
     1306
     1307
     1308
     1309                                ///
     1310                                {
     1311                                btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
     1312                                cpd->m_angularComponentA = body0->getInvInertiaTensorWorld()*torqueAxis0;
     1313                                btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);           
     1314                                cpd->m_angularComponentB = body1->getInvInertiaTensorWorld()*torqueAxis1;
     1315                                }
     1316                                {
     1317                                        btVector3 ftorqueAxis0 = rel_pos1.cross(cpd->m_frictionWorldTangential0);
     1318                                        cpd->m_frictionAngularComponent0A = body0->getInvInertiaTensorWorld()*ftorqueAxis0;
     1319                                }
     1320                                {
     1321                                        btVector3 ftorqueAxis1 = rel_pos1.cross(cpd->m_frictionWorldTangential1);
     1322                                        cpd->m_frictionAngularComponent1A = body0->getInvInertiaTensorWorld()*ftorqueAxis1;
     1323                                }
     1324                                {
     1325                                        btVector3 ftorqueAxis0 = rel_pos2.cross(cpd->m_frictionWorldTangential0);
     1326                                        cpd->m_frictionAngularComponent0B = body1->getInvInertiaTensorWorld()*ftorqueAxis0;
     1327                                }
     1328                                {
     1329                                        btVector3 ftorqueAxis1 = rel_pos2.cross(cpd->m_frictionWorldTangential1);
     1330                                        cpd->m_frictionAngularComponent1B = body1->getInvInertiaTensorWorld()*ftorqueAxis1;
     1331                                }
     1332                               
     1333                                ///
     1334
     1335
     1336
     1337                                //apply previous frames impulse on both bodies
     1338                                body0->applyImpulse(totalImpulse, rel_pos1);
     1339                                body1->applyImpulse(-totalImpulse, rel_pos2);
     1340                        }
     1341                       
     1342                }
     1343        }
     1344}
     1345
     1346
     1347btScalar btSequentialImpulseConstraintSolver::solveCombinedContactFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
     1348{
     1349        btScalar maxImpulse = btScalar(0.);
     1350
     1351        {
     1352
     1353               
     1354                {
     1355                        if (cp.getDistance() <= btScalar(0.))
     1356                        {
     1357
     1358                               
     1359
     1360                                {
     1361
     1362                                        //btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
     1363                                        btScalar impulse = resolveSingleCollisionCombined(
     1364                                                *body0,*body1,
     1365                                                cp,
     1366                                                info);
     1367
     1368                                        if (maxImpulse < impulse)
     1369                                                maxImpulse  = impulse;
     1370
     1371                                }
     1372                        }
     1373                }
     1374        }
     1375        return maxImpulse;
     1376}
     1377
     1378
     1379
     1380btScalar btSequentialImpulseConstraintSolver::solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
     1381{
     1382
     1383        btScalar maxImpulse = btScalar(0.);
     1384
     1385        {
     1386
     1387               
     1388                {
     1389                        if (cp.getDistance() <= btScalar(0.))
     1390                        {
     1391
     1392                               
     1393
     1394                                {
     1395
     1396                                        btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
     1397                                        btScalar impulse = cpd->m_contactSolverFunc(
     1398                                                *body0,*body1,
     1399                                                cp,
     1400                                                info);
     1401
     1402                                        if (maxImpulse < impulse)
     1403                                                maxImpulse  = impulse;
     1404
     1405                                }
     1406                        }
     1407                }
     1408        }
     1409        return maxImpulse;
     1410}
     1411
     1412btScalar btSequentialImpulseConstraintSolver::solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
     1413{
     1414
     1415        (void)debugDrawer;
     1416        (void)iter;
     1417
     1418
     1419        {
     1420
     1421               
     1422                {
     1423                       
     1424                        if (cp.getDistance() <= btScalar(0.))
     1425                        {
     1426
     1427                                btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
     1428                                cpd->m_frictionSolverFunc(
     1429                                        *body0,*body1,
     1430                                        cp,
     1431                                        info);
     1432
     1433                               
     1434                        }
     1435                }
     1436
     1437       
     1438        }
     1439        return btScalar(0.);
     1440}
    10241441
    10251442

  • code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h

    r2907 r2908  
    2424
    2525
    26 
    27 ///The btSequentialImpulseConstraintSolver is a fast SIMD implementation of the Projected Gauss Seidel (iterative LCP) method.
     26///The btSequentialImpulseConstraintSolver uses a Propagation Method and Sequentially applies impulses
     27///The approach is the 3D version of Erin Catto's GDC 2006 tutorial. See http://www.gphysics.com
     28///Although Sequential Impulse is more intuitive, it is mathematically equivalent to Projected Successive Overrelaxation (iterative LCP)
     29///Applies impulses for combined restitution and penetration recovery and to simulate friction
    2830class btSequentialImpulseConstraintSolver : public btConstraintSolver
    2931{
    30 protected:
    3132
    3233        btAlignedObjectArray<btSolverBody>      m_tmpSolverBodyPool;
    33         btConstraintArray                       m_tmpSolverContactConstraintPool;
    34         btConstraintArray                       m_tmpSolverNonContactConstraintPool;
    35         btConstraintArray                       m_tmpSolverContactFrictionConstraintPool;
     34        btAlignedObjectArray<btSolverConstraint>        m_tmpSolverConstraintPool;
     35        btAlignedObjectArray<btSolverConstraint>        m_tmpSolverFrictionConstraintPool;
    3636        btAlignedObjectArray<int>       m_orderTmpConstraintPool;
    3737        btAlignedObjectArray<int>       m_orderFrictionConstraintPool;
    3838
     39
     40protected:
     41        btScalar solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
     42        btScalar solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
     43        void  prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer);
    3944        btSolverConstraint&     addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation);
     45
     46        ContactSolverFunc m_contactDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES];
     47        ContactSolverFunc m_frictionDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES];
     48
    4049       
    4150        ///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
    4251        unsigned long   m_btSeed2;
    4352
    44         void    initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject);
    45         btScalar restitutionCurve(btScalar rel_vel, btScalar restitution);
    46 
    47         void    convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal);
    48 
    49         void    resolveSplitPenetrationImpulseCacheFriendly(
    50         btSolverBody& body1,
    51         btSolverBody& body2,
    52         const btSolverConstraint& contactConstraint,
    53         const btContactSolverInfo& solverInfo);
    54 
    55         //internal method
    56         int     getOrInitSolverBody(btCollisionObject& body);
    57 
    58         void    resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint);
    59 
    60         void    resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint);
    61        
    62         void    resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint);
    63        
    64         void    resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint);
    65                
    6653public:
    6754
    6855       
    6956        btSequentialImpulseConstraintSolver();
     57
     58        ///Advanced: Override the default contact solving function for contacts, for certain types of rigidbody
     59        ///See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType
     60        void    setContactSolverFunc(ContactSolverFunc func,int type0,int type1)
     61        {
     62                m_contactDispatch[type0][type1] = func;
     63        }
     64       
     65        ///Advanced: Override the default friction solving function for contacts, for certain types of rigidbody
     66        ///See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType
     67        void    SetFrictionSolverFunc(ContactSolverFunc func,int type0,int type1)
     68        {
     69                m_frictionDispatch[type0][type1] = func;
     70        }
     71
    7072        virtual ~btSequentialImpulseConstraintSolver();
     73       
     74        virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher);
    7175
    72         virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher);
    73        
     76        virtual btScalar solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
     77        btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
    7478        btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
    75         btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
     79
    7680
    7781        ///clear internal cached data and reset random seed
    7882        virtual void    reset();
     83
     84        btScalar solveCombinedContactFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
     85
     86
    7987       
    8088        unsigned long btRand2();

  • code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp

    r2907 r2908  
    6969btSliderConstraint::btSliderConstraint()
    7070        :btTypedConstraint(SLIDER_CONSTRAINT_TYPE),
    71                 m_useLinearReferenceFrameA(true),
    72                 m_useSolveConstraintObsolete(false)
    73 //              m_useSolveConstraintObsolete(true)
     71                m_useLinearReferenceFrameA(true)
    7472{
    7573        initParams();
    … …  
    8280        , m_frameInA(frameInA)
    8381        , m_frameInB(frameInB),
    84                 m_useLinearReferenceFrameA(useLinearReferenceFrameA),
    85                 m_useSolveConstraintObsolete(false)
    86 //              m_useSolveConstraintObsolete(true)
     82                m_useLinearReferenceFrameA(useLinearReferenceFrameA)
    8783{
    8884        initParams();
    … …  
    9389void btSliderConstraint::buildJacobian()
    9490{
    95         if (!m_useSolveConstraintObsolete)
    96         {
    97                 return;
    98         }
    9991        if(m_useLinearReferenceFrameA)
    10092        {
    … …  
    164156//-----------------------------------------------------------------------------
    165157
    166 void btSliderConstraint::getInfo1(btConstraintInfo1* info)
    167 {
    168         if (m_useSolveConstraintObsolete)
    169         {
    170                 info->m_numConstraintRows = 0;
    171                 info->nub = 0;
     158void btSliderConstraint::solveConstraint(btScalar timeStep)
     159{
     160    m_timeStep = timeStep;
     161        if(m_useLinearReferenceFrameA)
     162        {
     163                solveConstraintInt(m_rbA, m_rbB);
    172164        }
    173165        else
    174166        {
    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 {
    198         btAssert(!m_useSolveConstraintObsolete);
    199         int i, s = info->rowskip;
    200         const btTransform& trA = getCalculatedTransformA();
    201         const btTransform& trB = getCalculatedTransformB();
    202         btScalar signFact = m_useLinearReferenceFrameA ? btScalar(1.0f) : btScalar(-1.0f);
    203         // make rotations around Y and Z equal
    204         // the slider axis should be the only unconstrained
    205         // rotational axis, the angular velocity of the two bodies perpendicular to
    206         // the slider axis should be equal. thus the constraint equations are
    207         //    p*w1 - p*w2 = 0
    208         //    q*w1 - q*w2 = 0
    209         // where p and q are unit vectors normal to the slider axis, and w1 and w2
    210         // 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
    217         info->m_J1angularAxis[0] = p[0];
    218         info->m_J1angularAxis[1] = p[1];
    219         info->m_J1angularAxis[2] = p[2];
    220         info->m_J1angularAxis[s+0] = q[0];
    221         info->m_J1angularAxis[s+1] = q[1];
    222         info->m_J1angularAxis[s+2] = q[2];
    223 
    224         info->m_J2angularAxis[0] = -p[0];
    225         info->m_J2angularAxis[1] = -p[1];
    226         info->m_J2angularAxis[2] = -p[2];
    227         info->m_J2angularAxis[s+0] = -q[0];
    228         info->m_J2angularAxis[s+1] = -q[1];
    229         info->m_J2angularAxis[s+2] = -q[2];
    230         // compute the right hand side of the constraint equation. set relative
    231         // 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).
    234         // if "theta" is the angle between ax1 and ax2, we need an angular velocity
    235         // along u to cover angle erp*theta in one step :
    236         //   |angular_velocity| = angle/time = erp*theta / stepsize
    237         //                      = (erp*fps) * theta
    238         //    angular_velocity  = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
    239         //                      = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
    240         // ...as ax1 and ax2 are unit length. if theta is smallish,
    241         // theta ~= sin(theta), so
    242         //    angular_velocity  = (erp*fps) * (ax1 x ax2)
    243         // ax1 x ax2 is in the plane space of ax1, so we project the angular
    244         // 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);
    248         info->m_constraintError[0] = k * u.dot(p);
    249         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
    293         int srow;
    294         // check linear limits linear
    295         btScalar limit_err = btScalar(0.0);
    296         int limit = 0;
    297         if(getSolveLinLimit())
    298         {
    299                 limit_err = getLinDepth() *  signFact;
    300                 limit = (limit_err > btScalar(0.0)) ? 2 : 1;
    301         }
    302         int powered = 0;
    303         if(getPoweredLinMotor())
    304         {
    305                 powered = 1;
    306         }
    307         // if the slider has joint limits or motor, add in the extra row
    308         if (limit || powered)
    309         {
    310                 nrow++;
    311                 srow = nrow * info->rowskip;
    312                 info->m_J1linearAxis[srow+0] = ax1[0];
    313                 info->m_J1linearAxis[srow+1] = ax1[1];
    314                 info->m_J1linearAxis[srow+2] = ax1[2];
    315                 // linear torque decoupling step:
    316                 //
    317                 // we have to be careful that the linear constraint forces (+/- ax1) applied to the two bodies
    318                 // do not create a torque couple. in other words, the points that the
    319                 // constraint force is applied at must lie along the same ax1 axis.
    320                 // a torque couple will result in limited slider-jointed free
    321                 // 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];
    332                 // right-hand part
    333                 btScalar lostop = getLowerLinLimit();
    334                 btScalar histop = getUpperLinLimit();
    335                 if(limit && (lostop == histop))
    336                 {  // the joint motor is ineffective
    337                         powered = 0;
    338                 }
    339                 info->m_constraintError[srow] = 0.;
    340                 info->m_lowerLimit[srow] = 0.;
    341                 info->m_upperLimit[srow] = 0.;
    342                 if(powered)
    343                 {
    344             info->cfm[nrow] = btScalar(0.0);
    345                         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();
    348                         info->m_constraintError[srow] -= signFact * mot_fact * getTargetLinMotorVelocity();
    349                         info->m_lowerLimit[srow] += -getMaxLinMotorForce() * info->fps;
    350                         info->m_upperLimit[srow] += getMaxLinMotorForce() * info->fps;
    351                 }
    352                 if(limit)
    353                 {
    354                         k = info->fps * info->erp;
    355                         info->m_constraintError[srow] += k * limit_err;
    356                         info->cfm[srow] = btScalar(0.0); // stop_cfm;
    357                         if(lostop == histop)
    358                         {       // limited low and high simultaneously
    359                                 info->m_lowerLimit[srow] = -SIMD_INFINITY;
    360                                 info->m_upperLimit[srow] = SIMD_INFINITY;
    361                         }
    362                         else if(limit == 1)
    363                         { // low limit
    364                                 info->m_lowerLimit[srow] = -SIMD_INFINITY;
    365                                 info->m_upperLimit[srow] = 0;
    366                         }
    367                         else
    368                         { // high limit
    369                                 info->m_lowerLimit[srow] = 0;
    370                                 info->m_upperLimit[srow] = SIMD_INFINITY;
    371                         }
    372                         // bounce (we'll use slider parameter abs(1.0 - m_dampingLimLin) for that)
    373                         btScalar bounce = btFabs(btScalar(1.0) - getDampingLimLin());
    374                         if(bounce > btScalar(0.0))
    375                         {
    376                                 btScalar vel = m_rbA.getLinearVelocity().dot(ax1);
    377                                 vel -= m_rbB.getLinearVelocity().dot(ax1);
    378                                 vel *= signFact;
    379                                 // only apply bounce if the velocity is incoming, and if the
    380                                 // resulting c[] exceeds what we already have.
    381                                 if(limit == 1)
    382                                 {       // low limit
    383                                         if(vel < 0)
    384                                         {
    385                                                 btScalar newc = -bounce * vel;
    386                                                 if (newc > info->m_constraintError[srow])
    387                                                 {
    388                                                         info->m_constraintError[srow] = newc;
    389                                                 }
    390                                         }
    391                                 }
    392                                 else
    393                                 { // high limit - all those computations are reversed
    394                                         if(vel > 0)
    395                                         {
    396                                                 btScalar newc = -bounce * vel;
    397                                                 if(newc < info->m_constraintError[srow])
    398                                                 {
    399                                                         info->m_constraintError[srow] = newc;
    400                                                 }
    401                                         }
    402                                 }
    403                         }
    404                         info->m_constraintError[srow] *= getSoftnessLimLin();
    405                 } // if(limit)
    406         } // if linear limit
    407         // check angular limits
    408         limit_err = btScalar(0.0);
    409         limit = 0;
    410         if(getSolveAngLimit())
    411         {
    412                 limit_err = getAngDepth();
    413                 limit = (limit_err > btScalar(0.0)) ? 1 : 2;
    414         }
    415         // if the slider has joint limits, add in the extra row
    416         powered = 0;
    417         if(getPoweredAngMotor())
    418         {
    419                 powered = 1;
    420         }
    421         if(limit || powered)
    422         {
    423                 nrow++;
    424                 srow = nrow * info->rowskip;
    425                 info->m_J1angularAxis[srow+0] = ax1[0];
    426                 info->m_J1angularAxis[srow+1] = ax1[1];
    427                 info->m_J1angularAxis[srow+2] = ax1[2];
    428 
    429                 info->m_J2angularAxis[srow+0] = -ax1[0];
    430                 info->m_J2angularAxis[srow+1] = -ax1[1];
    431                 info->m_J2angularAxis[srow+2] = -ax1[2];
    432 
    433                 btScalar lostop = getLowerAngLimit();
    434                 btScalar histop = getUpperAngLimit();
    435                 if(limit && (lostop == histop))
    436                 {  // the joint motor is ineffective
    437                         powered = 0;
    438                 }
    439                 if(powered)
    440                 {
    441             info->cfm[srow] = btScalar(0.0);
    442                         btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * info->erp);
    443                         info->m_constraintError[srow] = mot_fact * getTargetAngMotorVelocity();
    444                         info->m_lowerLimit[srow] = -getMaxAngMotorForce() * info->fps;
    445                         info->m_upperLimit[srow] = getMaxAngMotorForce() * info->fps;
    446                 }
    447                 if(limit)
    448                 {
    449                         k = info->fps * info->erp;
    450                         info->m_constraintError[srow] += k * limit_err;
    451                         info->cfm[srow] = btScalar(0.0); // stop_cfm;
    452                         if(lostop == histop)
    453                         {
    454                                 // limited low and high simultaneously
    455                                 info->m_lowerLimit[srow] = -SIMD_INFINITY;
    456                                 info->m_upperLimit[srow] = SIMD_INFINITY;
    457                         }
    458                         else if(limit == 1)
    459                         { // low limit
    460                                 info->m_lowerLimit[srow] = 0;
    461                                 info->m_upperLimit[srow] = SIMD_INFINITY;
    462                         }
    463                         else
    464                         { // high limit
    465                                 info->m_lowerLimit[srow] = -SIMD_INFINITY;
    466                                 info->m_upperLimit[srow] = 0;
    467                         }
    468                         // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
    469                         btScalar bounce = btFabs(btScalar(1.0) - getDampingLimAng());
    470                         if(bounce > btScalar(0.0))
    471                         {
    472                                 btScalar vel = m_rbA.getAngularVelocity().dot(ax1);
    473                                 vel -= m_rbB.getAngularVelocity().dot(ax1);
    474                                 // only apply bounce if the velocity is incoming, and if the
    475                                 // resulting c[] exceeds what we already have.
    476                                 if(limit == 1)
    477                                 {       // low limit
    478                                         if(vel < 0)
    479                                         {
    480                                                 btScalar newc = -bounce * vel;
    481                                                 if(newc > info->m_constraintError[srow])
    482                                                 {
    483                                                         info->m_constraintError[srow] = newc;
    484                                                 }
    485                                         }
    486                                 }
    487                                 else
    488                                 {       // high limit - all those computations are reversed
    489                                         if(vel > 0)
    490                                         {
    491                                                 btScalar newc = -bounce * vel;
    492                                                 if(newc < info->m_constraintError[srow])
    493                                                 {
    494                                                         info->m_constraintError[srow] = newc;
    495                                                 }
    496                                         }
    497                                 }
    498                         }
    499                         info->m_constraintError[srow] *= getSoftnessLimAng();
    500                 } // if(limit)
    501         } // 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);
    514                 }
    515                 else
    516                 {
    517                         solveConstraintInt(m_rbB,bodyB, m_rbA,bodyA);
    518                 }
     167                solveConstraintInt(m_rbB, m_rbA);
    519168        }
    520169} // btSliderConstraint::solveConstraint()
    … …  
    522171//-----------------------------------------------------------------------------
    523172
    524 void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB)
     173void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB)
    525174{
    526175    int i;
    527176    // linear
    528     btVector3 velA;
    529         bodyA.getVelocityInLocalPointObsolete(m_relPosA,velA);
    530     btVector3 velB;
    531         bodyB.getVelocityInLocalPointObsolete(m_relPosB,velB);
     177    btVector3 velA = rbA.getVelocityInLocalPoint(m_relPosA);
     178    btVector3 velB = rbB.getVelocityInLocalPoint(m_relPosB);
    532179    btVector3 vel = velA - velB;
    533180        for(i = 0; i < 3; i++)
    … …  
    544191                btScalar normalImpulse = softness * (restitution * depth / m_timeStep - damping * rel_vel) * m_jacLinDiagABInv[i];
    545192                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 
     193                rbA.applyImpulse( impulse_vector, m_relPosA);
     194                rbB.applyImpulse(-impulse_vector, m_relPosB);
    558195                if(m_poweredLinMotor && (!i))
    559196                { // apply linear motor
    … …  
    581218                                // apply clamped impulse
    582219                                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 
     220                                rbA.applyImpulse( impulse_vector, m_relPosA);
     221                                rbB.applyImpulse(-impulse_vector, m_relPosB);
    595222                        }
    596223                }
    … …  
    601228        btVector3 axisB =  m_calculatedTransformB.getBasis().getColumn(0);
    602229
    603         btVector3 angVelA;
    604         bodyA.getAngularVelocity(angVelA);
    605         btVector3 angVelB;
    606         bodyB.getAngularVelocity(angVelB);
     230        const btVector3& angVelA = rbA.getAngularVelocity();
     231        const btVector3& angVelB = rbB.getAngularVelocity();
    607232
    608233        btVector3 angVelAroundAxisA = axisA * axisA.dot(angVelA);
    … …  
    614239        //solve orthogonal angular velocity correction
    615240        btScalar len = velrelOrthog.length();
    616         btScalar orthorImpulseMag = 0.f;
    617 
    618241        if (len > btScalar(0.00001))
    619242        {
    620243                btVector3 normal = velrelOrthog.normalized();
    621244                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;
     245                velrelOrthog *= (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng;
    624246        }
    625247        //solve angular positional correction
    626248        btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/m_timeStep);
    627         btVector3 angularAxis = angularError;
    628         btScalar angularImpulseMag = 0;
    629 
    630249        btScalar len2 = angularError.length();
    631250        if (len2>btScalar(0.00001))
    … …  
    633252                btVector3 normal2 = angularError.normalized();
    634253                btScalar denom2 = rbA.computeAngularImpulseDenominator(normal2) + rbB.computeAngularImpulseDenominator(normal2);
    635                 angularImpulseMag = (btScalar(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng;
    636                 angularError *= angularImpulseMag;
     254                angularError *= (btScalar(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng;
    637255        }
    638256        // 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 
     257        rbA.applyTorqueImpulse(-velrelOrthog+angularError);
     258        rbB.applyTorqueImpulse(velrelOrthog-angularError);
    648259        btScalar impulseMag;
    649260        //solve angular limits
    … …  
    659270        }
    660271        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 
     272        rbA.applyTorqueImpulse(impulse);
     273        rbB.applyTorqueImpulse(-impulse);
    669274        //apply angular motor
    670275        if(m_poweredAngMotor)
    … …  
    697302                        // apply clamped impulse
    698303                        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);
     304                        m_rbA.applyTorqueImpulse(motorImp);
     305                        m_rbB.applyTorqueImpulse(-motorImp);
    704306                }
    705307        }
    … …  
    711313
    712314void btSliderConstraint::calculateTransforms(void){
    713         if(m_useLinearReferenceFrameA || (!m_useSolveConstraintObsolete))
     315        if(m_useLinearReferenceFrameA)
    714316        {
    715317                m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA;
    … …  
    724326        m_realPivotBInW = m_calculatedTransformB.getOrigin();
    725327        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         }
     328        m_delta = m_realPivotBInW - m_realPivotAInW;
    734329        m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis;
    735330    btVector3 normalWorld;
    … …  
    773368
    774369//-----------------------------------------------------------------------------
     370 
    775371
    776372void btSliderConstraint::testAngLimits(void)
    … …  
    784380                const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1);
    785381                btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0)); 
    786                 m_angPos = rot;
    787382                if(rot < m_lowerAngLimit)
    788383                {
    … …  
    797392        }
    798393} // btSliderConstraint::testAngLimits()
     394
    799395       
    800396//-----------------------------------------------------------------------------
     397
     398
    801399
    802400btVector3 btSliderConstraint::getAncorInA(void)

  • code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.h

    r2907 r2908  
    4747{
    4848protected:
    49         ///for backwards compatibility during the transition to 'getInfo/getInfo2'
    50         bool            m_useSolveConstraintObsolete;
    5149        btTransform     m_frameInA;
    5250    btTransform m_frameInB;
    … …  
    107105
    108106        btScalar m_linPos;
    109         btScalar m_angPos;
    110107
    111108        btScalar m_angDepth;
    … …  
    130127        // overrides
    131128    virtual void        buildJacobian();
    132     virtual void getInfo1 (btConstraintInfo1* info);
    133        
    134         virtual void getInfo2 (btConstraintInfo2* info);
    135 
    136     virtual     void    solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar        timeStep);
    137        
    138 
     129    virtual     void    solveConstraint(btScalar        timeStep);
    139130        // access
    140131    const btRigidBody& getRigidBodyA() const { return m_rbA; }
    … …  
    204195        btScalar getMaxAngMotorForce() { return m_maxAngMotorForce; }
    205196        btScalar getLinearPos() { return m_linPos; }
    206        
    207197
    208198        // access for ODE solver
    … …  
    213203        // internal
    214204    void        buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB);
    215     void        solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB);
     205    void        solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB);
    216206        // shared code used by ODE solver
    217207        void    calculateTransforms(void);
    218208        void    testLinLimits(void);
    219         void    testLinLimits2(btConstraintInfo2* info);
    220209        void    testAngLimits(void);
    221210        // access for PE Solver

  • code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btSolverBody.h

    r2907 r2908  
    2424#include "LinearMath/btTransformUtil.h"
    2525
    26 ///Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later, and not double precision
    27 #ifdef BT_USE_SSE
    28 #define USE_SIMD 1
    29 #endif //
    30 
    31 
    32 #ifdef USE_SIMD
    33 
    34 struct  btSimdScalar
    35 {
    36         SIMD_FORCE_INLINE       btSimdScalar()
    37         {
    38 
    39         }
    40 
    41         SIMD_FORCE_INLINE       btSimdScalar(float      fl)
    42         :m_vec128 (_mm_set1_ps(fl))
    43         {
    44         }
    45 
    46         SIMD_FORCE_INLINE       btSimdScalar(__m128 v128)
    47                 :m_vec128(v128)
    48         {
    49         }
    50         union
    51         {
    52                 __m128          m_vec128;
    53                 float           m_floats[4];
    54                 int                     m_ints[4];
    55                 btScalar        m_unusedPadding;
    56         };
    57         SIMD_FORCE_INLINE       __m128  get128()
    58         {
    59                 return m_vec128;
    60         }
    61 
    62         SIMD_FORCE_INLINE       const __m128    get128() const
    63         {
    64                 return m_vec128;
    65         }
    66 
    67         SIMD_FORCE_INLINE       void    set128(__m128 v128)
    68         {
    69                 m_vec128 = v128;
    70         }
    71 
    72         SIMD_FORCE_INLINE       operator       __m128()       
    73         {
    74                 return m_vec128;
    75         }
    76         SIMD_FORCE_INLINE       operator const __m128() const
    77         {
    78                 return m_vec128;
    79         }
    80        
    81         SIMD_FORCE_INLINE       operator float() const
    82         {
    83                 return m_floats[0];
    84         }
    85 
    86 };
    87 
    88 ///@brief Return the elementwise product of two btSimdScalar
    89 SIMD_FORCE_INLINE btSimdScalar
    90 operator*(const btSimdScalar& v1, const btSimdScalar& v2)
    91 {
    92         return btSimdScalar(_mm_mul_ps(v1.get128(),v2.get128()));
    93 }
    94 
    95 ///@brief Return the elementwise product of two btSimdScalar
    96 SIMD_FORCE_INLINE btSimdScalar
    97 operator+(const btSimdScalar& v1, const btSimdScalar& v2)
    98 {
    99         return btSimdScalar(_mm_add_ps(v1.get128(),v2.get128()));
    100 }
    101 
    102 
    103 #else
    104 #define btSimdScalar btScalar
    105 #endif
    10626
    10727///The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
    … …  
    10929{
    11030        BT_DECLARE_ALIGNED_ALLOCATOR();
    111         btVector3               m_deltaLinearVelocity;
    112         btVector3               m_deltaAngularVelocity;
    113         btScalar                m_angularFactor;
    114         btScalar                m_invMass;
    115         btScalar                m_friction;
     31       
     32        btMatrix3x3             m_worldInvInertiaTensor;
     33
     34        btVector3               m_angularVelocity;
     35        btVector3               m_linearVelocity;
     36        btVector3               m_centerOfMassPosition;
     37        btVector3               m_pushVelocity;
     38        btVector3               m_turnVelocity;
     39
     40        float                   m_angularFactor;
     41        float                   m_invMass;
     42        float                   m_friction;
    11643        btRigidBody*    m_originalBody;
    117         btVector3               m_pushVelocity;
    118         //btVector3             m_turnVelocity;
    119 
    12044       
    121         SIMD_FORCE_INLINE void  getVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const
     45       
     46        SIMD_FORCE_INLINE void  getVelocityInLocalPoint(const btVector3& rel_pos, btVector3& velocity ) const
    12247        {
    123                 if (m_originalBody)
    124                         velocity = m_originalBody->getLinearVelocity()+m_deltaLinearVelocity + (m_originalBody->getAngularVelocity()+m_deltaAngularVelocity).cross(rel_pos);
    125                 else
    126                         velocity.setValue(0,0,0);
     48                velocity = m_linearVelocity + m_angularVelocity.cross(rel_pos);
    12749        }
    12850
    129         SIMD_FORCE_INLINE void  getAngularVelocity(btVector3& angVel) const
     51        //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
     52        SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
    13053        {
    131                 if (m_originalBody)
    132                         angVel = m_originalBody->getAngularVelocity()+m_deltaAngularVelocity;
    133                 else
    134                         angVel.setValue(0,0,0);
     54                if (m_invMass)
     55                {
     56                        m_linearVelocity += linearComponent*impulseMagnitude;
     57                        m_angularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
     58                }
    13559        }
    136 
    137 
    138         //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
    139         SIMD_FORCE_INLINE void applyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude)
     60       
     61        SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
    14062        {
    141                 //if (m_invMass)
     63                if (m_invMass)
    14264                {
    143                         m_deltaLinearVelocity += linearComponent*impulseMagnitude;
    144                         m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
     65                        m_pushVelocity += linearComponent*impulseMagnitude;
     66                        m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
    14567                }
    14668        }
    14769
    148        
    149 /*
    15070       
    15171        void    writebackVelocity()
    … …  
    15373                if (m_invMass)
    15474                {
    155                         m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+ m_deltaLinearVelocity);
    156                         m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity);
     75                        m_originalBody->setLinearVelocity(m_linearVelocity);
     76                        m_originalBody->setAngularVelocity(m_angularVelocity);
    15777                       
    15878                        //m_originalBody->setCompanionId(-1);
    15979                }
    16080        }
    161         */
     81       
    16282
    163         void    writebackVelocity(btScalar timeStep=0)
     83        void    writebackVelocity(btScalar timeStep)
    16484        {
    16585                if (m_invMass)
    16686                {
    167                         m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+m_deltaLinearVelocity);
    168                         m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity);
     87                        m_originalBody->setLinearVelocity(m_linearVelocity);
     88                        m_originalBody->setAngularVelocity(m_angularVelocity);
     89                       
     90                        //correct the position/orientation based on push/turn recovery
     91                        btTransform newTransform;
     92                        btTransformUtil::integrateTransform(m_originalBody->getWorldTransform(),m_pushVelocity,m_turnVelocity,timeStep,newTransform);
     93                        m_originalBody->setWorldTransform(newTransform);
     94                       
    16995                        //m_originalBody->setCompanionId(-1);
    17096                }
    17197        }
     98
     99        void    readVelocity()
     100        {
     101                if (m_invMass)
     102                {
     103                        m_linearVelocity = m_originalBody->getLinearVelocity();
     104                        m_angularVelocity = m_originalBody->getAngularVelocity();
     105                }
     106        }
     107
    172108       
    173109

  • code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btSolverConstraint.h

    r2907 r2908  
    2020#include "LinearMath/btVector3.h"
    2121#include "LinearMath/btMatrix3x3.h"
    22 #include "btJacobianEntry.h"
    2322
    2423//#define NO_FRICTION_TANGENTIALS 1
    25 #include "btSolverBody.h"
    26 
    2724
    2825///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
    … …  
    3128        BT_DECLARE_ALIGNED_ALLOCATOR();
    3229
    33         btVector3               m_relpos1CrossNormal;
    34         btVector3               m_contactNormal;
     30        btVector3       m_relpos1CrossNormal;
     31        btVector3       m_contactNormal;
    3532
    36         btVector3               m_relpos2CrossNormal;
    37         //btVector3             m_contactNormal2;//usually m_contactNormal2 == -m_contactNormal
     33        btVector3       m_relpos2CrossNormal;
     34        btVector3       m_angularComponentA;
    3835
    39         btVector3               m_angularComponentA;
    40         btVector3               m_angularComponentB;
     36        btVector3       m_angularComponentB;
     37
     38        mutable btScalar        m_appliedPushImpulse;
    4139       
    42         mutable btSimdScalar    m_appliedPushImpulse;
    43         mutable btSimdScalar    m_appliedImpulse;
    44        
     40        mutable btScalar        m_appliedImpulse;
     41        int                     m_solverBodyIdA;
     42        int                     m_solverBodyIdB;
    4543       
    4644        btScalar        m_friction;
     45        btScalar        m_restitution;
    4746        btScalar        m_jacDiagABInv;
    48         union
    49         {
    50                 int     m_numConsecutiveRowsPerKernel;
    51                 btScalar        m_unusedPadding0;
    52         };
     47        btScalar        m_penetration;
     48       
    5349
    54         union
    55         {
    56                 int                     m_frictionIndex;
    57                 btScalar        m_unusedPadding1;
    58         };
    59         union
    60         {
    61                 int                     m_solverBodyIdA;
    62                 btScalar        m_unusedPadding2;
    63         };
    64         union
    65         {
    66                 int                     m_solverBodyIdB;
    67                 btScalar        m_unusedPadding3;
    68         };
    6950       
    70         union
    71         {
    72                 void*           m_originalContactPoint;
    73                 btScalar        m_unusedPadding4;
    74         };
     51        int                     m_constraintType;
     52        int                     m_frictionIndex;
     53        void*           m_originalContactPoint;
     54        int                     m_unusedPadding[1];
    7555
    76         btScalar                m_rhs;
    77         btScalar                m_cfm;
    78         btScalar                m_lowerLimit;
    79         btScalar                m_upperLimit;
    8056
    8157        enum            btSolverConstraintType
    … …  
    8662};
    8763
    88 typedef btAlignedObjectArray<btSolverConstraint>        btConstraintArray;
     64
     65
     66
    8967
    9068

  • code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp

    r2907 r2908  
    2020static btRigidBody s_fixed(0, 0,0);
    2121
    22 #define DEFAULT_DEBUGDRAW_SIZE btScalar(0.3f)
    23 
    2422btTypedConstraint::btTypedConstraint(btTypedConstraintType type)
    2523:m_userConstraintType(-1),
    … …  
    2826m_rbA(s_fixed),
    2927m_rbB(s_fixed),
    30 m_appliedImpulse(btScalar(0.)),
    31 m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE)
     28m_appliedImpulse(btScalar(0.))
    3229{
    3330        s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
    … …  
    3936m_rbA(rbA),
    4037m_rbB(s_fixed),
    41 m_appliedImpulse(btScalar(0.)),
    42 m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE)
     38m_appliedImpulse(btScalar(0.))
    4339{
    4440                s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
    … …  
    5349m_rbA(rbA),
    5450m_rbB(rbB),
    55 m_appliedImpulse(btScalar(0.)),
    56 m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE)
     51m_appliedImpulse(btScalar(0.))
    5752{
    5853                s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
    … …  
    6055}
    6156
    62 
    63 //-----------------------------------------------------------------------------
    64 
    65 btScalar btTypedConstraint::getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact)
    66 {
    67         if(lowLim > uppLim)
    68         {
    69                 return btScalar(1.0f);
    70         }
    71         else if(lowLim == uppLim)
    72         {
    73                 return btScalar(0.0f);
    74         }
    75         btScalar lim_fact = btScalar(1.0f);
    76         btScalar delta_max = vel / timeFact;
    77         if(delta_max < btScalar(0.0f))
    78         {
    79                 if((pos >= lowLim) && (pos < (lowLim - delta_max)))
    80                 {
    81                         lim_fact = (lowLim - pos) / delta_max;
    82                 }
    83                 else if(pos  < lowLim)
    84                 {
    85                         lim_fact = btScalar(0.0f);
    86                 }
    87                 else
    88                 {
    89                         lim_fact = btScalar(1.0f);
    90                 }
    91         }
    92         else if(delta_max > btScalar(0.0f))
    93         {
    94                 if((pos <= uppLim) && (pos > (uppLim - delta_max)))
    95                 {
    96                         lim_fact = (uppLim - pos) / delta_max;
    97                 }
    98                 else if(pos  > uppLim)
    99                 {
    100                         lim_fact = btScalar(0.0f);
    101                 }
    102                 else
    103                 {
    104                         lim_fact = btScalar(1.0f);
    105                 }
    106         }
    107         else
    108         {
    109                         lim_fact = btScalar(0.0f);
    110         }
    111         return lim_fact;
    112 } // btTypedConstraint::getMotorFactor()
    113 
    114 

  • code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btTypedConstraint.h

    r2907 r2908  
    1919class btRigidBody;
    2020#include "LinearMath/btScalar.h"
    21 #include "btSolverConstraint.h"
    22 struct  btSolverBody;
    23 
    24 
    25 
    2621
    2722enum btTypedConstraintType
    … …  
    3126        CONETWIST_CONSTRAINT_TYPE,
    3227        D6_CONSTRAINT_TYPE,
     28        VEHICLE_CONSTRAINT_TYPE,
    3329        SLIDER_CONSTRAINT_TYPE
    3430};
    … …  
    5349        btRigidBody&    m_rbB;
    5450        btScalar        m_appliedImpulse;
    55         btScalar        m_dbgDrawSize;
    5651
    5752
    … …  
    6156        virtual ~btTypedConstraint() {};
    6257        btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA);
     58
    6359        btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB);
    64 
    65         struct btConstraintInfo1 {
    66                 int m_numConstraintRows,nub;
    67         };
    68 
    69         struct btConstraintInfo2 {
    70                 // integrator parameters: frames per second (1/stepsize), default error
    71                 // reduction parameter (0..1).
    72                 btScalar fps,erp;
    73 
    74                 // for the first and second body, pointers to two (linear and angular)
    75                 // n*3 jacobian sub matrices, stored by rows. these matrices will have
    76                 // been initialized to 0 on entry. if the second body is zero then the
    77                 // J2xx pointers may be 0.
    78                 btScalar *m_J1linearAxis,*m_J1angularAxis,*m_J2linearAxis,*m_J2angularAxis;
    79 
    80                 // elements to jump from one row to the next in J's
    81                 int rowskip;
    82 
    83                 // right hand sides of the equation J*v = c + cfm * lambda. cfm is the
    84                 // "constraint force mixing" vector. c is set to zero on entry, cfm is
    85                 // set to a constant value (typically very small or zero) value on entry.
    86                 btScalar *m_constraintError,*cfm;
    87 
    88                 // lo and hi limits for variables (set to -/+ infinity on entry).
    89                 btScalar *m_lowerLimit,*m_upperLimit;
    90 
    91                 // findex vector for variables. see the LCP solver interface for a
    92                 // description of what this does. this is set to -1 on entry.
    93                 // note that the returned indexes are relative to the first index of
    94                 // the constraint.
    95                 int *findex;
    96         };
    97 
    9860
    9961        virtual void    buildJacobian() = 0;
    10062
    101         virtual void    setupSolverConstraint(btConstraintArray& ca, int solverBodyA,int solverBodyB, btScalar timeStep)
    102         {
    103         }
    104         virtual void getInfo1 (btConstraintInfo1* info)=0;
     63        virtual void    solveConstraint(btScalar        timeStep) = 0;
    10564
    106         virtual void getInfo2 (btConstraintInfo2* info)=0;
    107 
    108         virtual void    solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar        timeStep) = 0;
    109 
    110         btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact);
    111        
    11265        const btRigidBody& getRigidBodyA() const
    11366        {
    … …  
    14295                m_userConstraintId = uid;
    14396        }
    144 
     97       
    14598        int getUserConstraintId() const
    14699        {
    … …  
    162115                return m_constraintType;
    163116        }
    164        
    165         void setDbgDrawSize(btScalar dbgDrawSize)
    166         {
    167                 m_dbgDrawSize = dbgDrawSize;
    168         }
    169         btScalar getDbgDrawSize()
    170         {
    171                 return m_dbgDrawSize;
    172         }
    173        
     117
    174118};
    175119
Note: See TracChangeset for help on using the changeset viewer.