Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

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

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

Notes for updating

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

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

Key new features:

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

There are also some regressions:

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

Legend:

Unmodified
Added
Removed

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

    r5781 r8351  
    2323#include <new>
    2424
    25 //-----------------------------------------------------------------------------
    26 
     25
     26
     27//#define CONETWIST_USE_OBSOLETE_SOLVER true
    2728#define CONETWIST_USE_OBSOLETE_SOLVER false
    2829#define CONETWIST_DEF_FIX_THRESH btScalar(.05f)
    2930
    30 //-----------------------------------------------------------------------------
    31 
    32 btConeTwistConstraint::btConeTwistConstraint()
    33 :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE),
    34 m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
    35 {
    36 }
     31
     32SIMD_FORCE_INLINE btScalar computeAngularImpulseDenominator(const btVector3& axis, const btMatrix3x3& invInertiaWorld)
     33{
     34        btVector3 vec = axis * invInertiaWorld;
     35        return axis.dot(vec);
     36}
     37
     38
    3739
    3840
    … …  
    6466        m_maxMotorImpulse = btScalar(-1);
    6567
    66         setLimit(btScalar(1e30), btScalar(1e30), btScalar(1e30));
     68        setLimit(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
    6769        m_damping = btScalar(0.01);
    6870        m_fixThresh = CONETWIST_DEF_FIX_THRESH;
    69 }
    70 
    71 
    72 //-----------------------------------------------------------------------------
     71        m_flags = 0;
     72        m_linCFM = btScalar(0.f);
     73        m_linERP = btScalar(0.7f);
     74        m_angCFM = btScalar(0.f);
     75}
     76
    7377
    7478void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info)
    … …  
    8387                info->m_numConstraintRows = 3;
    8488                info->nub = 3;
    85                 calcAngleInfo2();
     89                calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
    8690                if(m_solveSwingLimit)
    8791                {
    … …  
    100104                }
    101105        }
    102 } // btConeTwistConstraint::getInfo1()
     106}
     107
     108void btConeTwistConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
     109{
     110        //always reserve 6 rows: object transform is not available on SPU
     111        info->m_numConstraintRows = 6;
     112        info->nub = 0;
     113               
     114}
    103115       
    104 //-----------------------------------------------------------------------------
    105116
    106117void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info)
    107118{
     119        getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
     120}
     121
     122void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB)
     123{
     124        calcAngleInfo2(transA,transB,invInertiaWorldA,invInertiaWorldB);
     125       
    108126        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();
    114127    // set jacobian
    115128    info->m_J1linearAxis[0] = 1;
    116129    info->m_J1linearAxis[info->rowskip+1] = 1;
    117130    info->m_J1linearAxis[2*info->rowskip+2] = 1;
    118         btVector3 a1 = body0_trans.getBasis() * m_rbAFrame.getOrigin();
     131        btVector3 a1 = transA.getBasis() * m_rbAFrame.getOrigin();
    119132        {
    120133                btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
    … …  
    124137                a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
    125138        }
    126         btVector3 a2 = body1_trans.getBasis() * m_rbBFrame.getOrigin();
     139        btVector3 a2 = transB.getBasis() * m_rbBFrame.getOrigin();
    127140        {
    128141                btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
    … …  
    132145        }
    133146    // set right hand side
    134     btScalar k = info->fps * info->erp;
     147        btScalar linERP = (m_flags & BT_CONETWIST_FLAGS_LIN_ERP) ? m_linERP : info->erp;
     148    btScalar k = info->fps * linERP;
    135149    int j;
    136150        for (j=0; j<3; j++)
    137151    {
    138         info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
     152        info->m_constraintError[j*info->rowskip] = k * (a2[j] + transB.getOrigin()[j] - a1[j] - transA.getOrigin()[j]);
    139153                info->m_lowerLimit[j*info->rowskip] = -SIMD_INFINITY;
    140154                info->m_upperLimit[j*info->rowskip] = SIMD_INFINITY;
     155                if(m_flags & BT_CONETWIST_FLAGS_LIN_CFM)
     156                {
     157                        info->cfm[j*info->rowskip] = m_linCFM;
     158                }
    141159    }
    142160        int row = 3;
    … …  
    150168                if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
    151169                {
    152                         btTransform trA = m_rbA.getCenterOfMassTransform()*m_rbAFrame;
     170                        btTransform trA = transA*m_rbAFrame;
    153171                        btVector3 p = trA.getBasis().getColumn(1);
    154172                        btVector3 q = trA.getBasis().getColumn(2);
    … …  
    187205
    188206                        info->m_constraintError[srow] = k * m_swingCorrection;
    189                         info->cfm[srow] = 0.0f;
     207                        if(m_flags & BT_CONETWIST_FLAGS_ANG_CFM)
     208                        {
     209                                info->cfm[srow] = m_angCFM;
     210                        }
    190211                        // m_swingCorrection is always positive or 0
    191212                        info->m_lowerLimit[srow] = 0;
    … …  
    207228                btScalar k = info->fps * m_biasFactor;
    208229                info->m_constraintError[srow] = k * m_twistCorrection;
    209                 info->cfm[srow] = 0.0f;
     230                if(m_flags & BT_CONETWIST_FLAGS_ANG_CFM)
     231                {
     232                        info->cfm[srow] = m_angCFM;
     233                }
    210234                if(m_twistSpan > 0.0f)
    211235                {
    … …  
    231255}
    232256       
    233 //-----------------------------------------------------------------------------
     257
    234258
    235259void    btConeTwistConstraint::buildJacobian()
    … …  
    240264                m_accTwistLimitImpulse = btScalar(0.);
    241265                m_accSwingLimitImpulse = btScalar(0.);
     266                m_accMotorImpulse = btVector3(0.,0.,0.);
    242267
    243268                if (!m_angularOnly)
    … …  
    274299                }
    275300
    276                 calcAngleInfo2();
    277         }
    278 }
    279 
    280 //-----------------------------------------------------------------------------
    281 
    282 void    btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
    283 {
     301                calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
     302        }
     303}
     304
     305
     306
     307void    btConeTwistConstraint::solveConstraintObsolete(btRigidBody& bodyA,btRigidBody& bodyB,btScalar   timeStep)
     308{
     309        #ifndef __SPU__
    284310        if (m_useSolveConstraintObsolete)
    285311        {
    … …  
    296322
    297323                        btVector3 vel1;
    298                         bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
     324                        bodyA.internalGetVelocityInLocalPointObsolete(rel_pos1,vel1);
    299325                        btVector3 vel2;
    300                         bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
     326                        bodyB.internalGetVelocityInLocalPointObsolete(rel_pos2,vel2);
    301327                        btVector3 vel = vel1 - vel2;
    302328
    … …  
    315341                                btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
    316342                                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);
     343                                bodyA.internalApplyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse);
     344                                bodyB.internalApplyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse);
    319345               
    320346                        }
    … …  
    327353                        btTransform trACur = m_rbA.getCenterOfMassTransform();
    328354                        btTransform trBCur = m_rbB.getCenterOfMassTransform();
    329                         btVector3 omegaA; bodyA.getAngularVelocity(omegaA);
    330                         btVector3 omegaB; bodyB.getAngularVelocity(omegaB);
     355                        btVector3 omegaA; bodyA.internalGetAngularVelocity(omegaA);
     356                        btVector3 omegaB; bodyB.internalGetAngularVelocity(omegaB);
    331357                        btTransform trAPred; trAPred.setIdentity();
    332358                        btVector3 zerovec(0,0,0);
    … …  
    402428                                btVector3 impulseAxis =  impulse / impulseMag;
    403429
    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();
     430                                bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
     431                                bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
     432
     433                        }
     434                }
     435                else if (m_damping > SIMD_EPSILON) // no motor: do a little damping
     436                {
     437                        btVector3 angVelA; bodyA.internalGetAngularVelocity(angVelA);
     438                        btVector3 angVelB; bodyB.internalGetAngularVelocity(angVelB);
    413439                        btVector3 relVel = angVelB - angVelA;
    414440                        if (relVel.length2() > SIMD_EPSILON)
    … …  
    422448                                btScalar  impulseMag  = impulse.length();
    423449                                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);
     450                                bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
     451                                bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
    426452                        }
    427453                }
    … …  
    431457                        ///solve angular part
    432458                        btVector3 angVelA;
    433                         bodyA.getAngularVelocity(angVelA);
     459                        bodyA.internalGetAngularVelocity(angVelA);
    434460                        btVector3 angVelB;
    435                         bodyB.getAngularVelocity(angVelB);
     461                        bodyB.internalGetAngularVelocity(angVelB);
    436462
    437463                        // solve swing limit
    … …  
    462488                                btVector3 noTwistSwingAxis = impulse / impulseMag;
    463489
    464                                 bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*noTwistSwingAxis, impulseMag);
    465                                 bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*noTwistSwingAxis, -impulseMag);
     490                                bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*noTwistSwingAxis, impulseMag);
     491                                bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*noTwistSwingAxis, -impulseMag);
    466492                        }
    467493
    … …  
    483509                                btVector3 impulse = m_twistAxis * impulseMag;
    484510
    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);
     511                                bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_twistAxis,impulseMag);
     512                                bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_twistAxis,-impulseMag);
    487513                        }               
    488514                }
    489515        }
    490 
    491 }
    492 
    493 //-----------------------------------------------------------------------------
     516#else
     517btAssert(0);
     518#endif //__SPU__
     519}
     520
     521
     522
    494523
    495524void    btConeTwistConstraint::updateRHS(btScalar       timeStep)
    … …  
    499528}
    500529
    501 //-----------------------------------------------------------------------------
    502 
     530
     531#ifndef __SPU__
    503532void btConeTwistConstraint::calcAngleInfo()
    504533{
    … …  
    585614                }
    586615        }
    587 } // btConeTwistConstraint::calcAngleInfo()
    588 
     616}
     617#endif //__SPU__
    589618
    590619static btVector3 vTwist(1,0,0); // twist axis in constraint's space
    591620
    592 //-----------------------------------------------------------------------------
    593 
    594 void btConeTwistConstraint::calcAngleInfo2()
     621
     622
     623void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB)
    595624{
    596625        m_swingCorrection = btScalar(0.);
    … …  
    598627        m_solveTwistLimit = false;
    599628        m_solveSwingLimit = false;
     629        // compute rotation of A wrt B (in constraint space)
     630        if (m_bMotorEnabled && (!m_useSolveConstraintObsolete))
     631        {       // it is assumed that setMotorTarget() was alredy called
     632                // and motor target m_qTarget is within constraint limits
     633                // TODO : split rotation to pure swing and pure twist
     634                // compute desired transforms in world
     635                btTransform trPose(m_qTarget);
     636                btTransform trA = transA * m_rbAFrame;
     637                btTransform trB = transB * m_rbBFrame;
     638                btTransform trDeltaAB = trB * trPose * trA.inverse();
     639                btQuaternion qDeltaAB = trDeltaAB.getRotation();
     640                btVector3 swingAxis =   btVector3(qDeltaAB.x(), qDeltaAB.y(), qDeltaAB.z());
     641                m_swingAxis = swingAxis;
     642                m_swingAxis.normalize();
     643                m_swingCorrection = qDeltaAB.getAngle();
     644                if(!btFuzzyZero(m_swingCorrection))
     645                {
     646                        m_solveSwingLimit = true;
     647                }
     648                return;
     649        }
     650
    600651
    601652        {
    602653                // 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();
     654                btQuaternion qA = transA.getRotation() * m_rbAFrame.getRotation();
     655                btQuaternion qB = transB.getRotation() * m_rbBFrame.getRotation();
    605656                btQuaternion qAB = qB.inverse() * qA;
    606 
    607657                // split rotation into cone and twist
    608658                // (all this is done from B's perspective. Maybe I should be averaging axes...)
    … …  
    642692
    643693                                m_kSwing =  btScalar(1.) /
    644                                         (getRigidBodyA().computeAngularImpulseDenominator(m_swingAxis) +
    645                                          getRigidBodyB().computeAngularImpulseDenominator(m_swingAxis));
     694                                        (computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldA) +
     695                                         computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldB));
    646696                        }
    647697                }
    … …  
    651701                        // or you're trying to set at least one of the swing limits too small. (if so, do you really want a conetwist constraint?)
    652702                        // 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);
     703                        btVector3 ivA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0);
     704                        btVector3 jvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1);
     705                        btVector3 kvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(2);
     706                        btVector3 ivB = transB.getBasis() * m_rbBFrame.getBasis().getColumn(0);
    657707                        btVector3 target;
    658708                        btScalar x = ivB.dot(ivA);
    … …  
    745795
    746796                                m_kTwist = btScalar(1.) /
    747                                         (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
    748                                          getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));
     797                                        (computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldA) +
     798                                         computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldB));
    749799                        }
    750800
    … …  
    757807                }
    758808        }
    759 } // btConeTwistConstraint::calcAngleInfo2()
     809}
    760810
    761811
    … …  
    9821032}
    9831033
    984 
    985 //-----------------------------------------------------------------------------
    986 //-----------------------------------------------------------------------------
    987 //-----------------------------------------------------------------------------
    988 
    989 
     1034///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
     1035///If no axis is provided, it uses the default axis for this constraint.
     1036void btConeTwistConstraint::setParam(int num, btScalar value, int axis)
     1037{
     1038        switch(num)
     1039        {
     1040                case BT_CONSTRAINT_ERP :
     1041                case BT_CONSTRAINT_STOP_ERP :
     1042                        if((axis >= 0) && (axis < 3))
     1043                        {
     1044                                m_linERP = value;
     1045                                m_flags |= BT_CONETWIST_FLAGS_LIN_ERP;
     1046                        }
     1047                        else
     1048                        {
     1049                                m_biasFactor = value;
     1050                        }
     1051                        break;
     1052                case BT_CONSTRAINT_CFM :
     1053                case BT_CONSTRAINT_STOP_CFM :
     1054                        if((axis >= 0) && (axis < 3))
     1055                        {
     1056                                m_linCFM = value;
     1057                                m_flags |= BT_CONETWIST_FLAGS_LIN_CFM;
     1058                        }
     1059                        else
     1060                        {
     1061                                m_angCFM = value;
     1062                                m_flags |= BT_CONETWIST_FLAGS_ANG_CFM;
     1063                        }
     1064                        break;
     1065                default:
     1066                        btAssertConstrParams(0);
     1067                        break;
     1068        }
     1069}
     1070
     1071///return the local value of parameter
     1072btScalar btConeTwistConstraint::getParam(int num, int axis) const
     1073{
     1074        btScalar retVal = 0;
     1075        switch(num)
     1076        {
     1077                case BT_CONSTRAINT_ERP :
     1078                case BT_CONSTRAINT_STOP_ERP :
     1079                        if((axis >= 0) && (axis < 3))
     1080                        {
     1081                                btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_LIN_ERP);
     1082                                retVal = m_linERP;
     1083                        }
     1084                        else if((axis >= 3) && (axis < 6))
     1085                        {
     1086                                retVal = m_biasFactor;
     1087                        }
     1088                        else
     1089                        {
     1090                                btAssertConstrParams(0);
     1091                        }
     1092                        break;
     1093                case BT_CONSTRAINT_CFM :
     1094                case BT_CONSTRAINT_STOP_CFM :
     1095                        if((axis >= 0) && (axis < 3))
     1096                        {
     1097                                btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_LIN_CFM);
     1098                                retVal = m_linCFM;
     1099                        }
     1100                        else if((axis >= 3) && (axis < 6))
     1101                        {
     1102                                btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_ANG_CFM);
     1103                                retVal = m_angCFM;
     1104                        }
     1105                        else
     1106                        {
     1107                                btAssertConstrParams(0);
     1108                        }
     1109                        break;
     1110                default :
     1111                        btAssertConstrParams(0);
     1112        }
     1113        return retVal;
     1114}
     1115
     1116
     1117

  • code/trunk/src/external/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h

    r5781 r8351  
    1818
    1919
     20/*
     21Overview:
     22
     23btConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc).
     24It is a fixed translation, 3 degree-of-freedom (DOF) rotational "joint".
     25It divides the 3 rotational DOFs into swing (movement within a cone) and twist.
     26Swing is divided into swing1 and swing2 which can have different limits, giving an elliptical shape.
     27(Note: the cone's base isn't flat, so this ellipse is "embedded" on the surface of a sphere.)
     28
     29In the contraint's frame of reference:
     30twist is along the x-axis,
     31and swing 1 and 2 are along the z and y axes respectively.
     32*/
     33
     34
     35
    2036#ifndef CONETWISTCONSTRAINT_H
    2137#define CONETWISTCONSTRAINT_H
    … …  
    2743class btRigidBody;
    2844
     45enum btConeTwistFlags
     46{
     47        BT_CONETWIST_FLAGS_LIN_CFM = 1,
     48        BT_CONETWIST_FLAGS_LIN_ERP = 2,
     49        BT_CONETWIST_FLAGS_ANG_CFM = 4
     50};
    2951
    3052///btConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc)
    … …  
    84106        btVector3        m_accMotorImpulse;
    85107       
     108        // parameters
     109        int                     m_flags;
     110        btScalar        m_linCFM;
     111        btScalar        m_linERP;
     112        btScalar        m_angCFM;
     113       
     114protected:
     115
     116        void init();
     117
     118        void computeConeLimitInfo(const btQuaternion& qCone, // in
     119                btScalar& swingAngle, btVector3& vSwingAxis, btScalar& swingLimit); // all outs
     120
     121        void computeTwistLimitInfo(const btQuaternion& qTwist, // in
     122                btScalar& twistAngle, btVector3& vTwistAxis); // all outs
     123
     124        void adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const;
     125
     126
    86127public:
    87128
    … …  
    90131        btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame);
    91132
    92         btConeTwistConstraint();
    93 
    94133        virtual void    buildJacobian();
    95134
    96135        virtual void getInfo1 (btConstraintInfo1* info);
     136
     137        void    getInfo1NonVirtual(btConstraintInfo1* info);
    97138       
    98139        virtual void getInfo2 (btConstraintInfo2* info);
    99140       
    100 
    101         virtual void    solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar        timeStep);
     141        void    getInfo2NonVirtual(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB);
     142
     143        virtual void    solveConstraintObsolete(btRigidBody& bodyA,btRigidBody& bodyB,btScalar  timeStep);
    102144
    103145        void    updateRHS(btScalar      timeStep);
    … …  
    142184        }
    143185
    144         void    setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan,  btScalar _softness = 1.f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
     186        // setLimit(), a few notes:
     187        // _softness:
     188        //              0->1, recommend ~0.8->1.
     189        //              describes % of limits where movement is free.
     190        //              beyond this softness %, the limit is gradually enforced until the "hard" (1.0) limit is reached.
     191        // _biasFactor:
     192        //              0->1?, recommend 0.3 +/-0.3 or so.
     193        //              strength with which constraint resists zeroth order (angular, not angular velocity) limit violation.
     194        // __relaxationFactor:
     195        //              0->1, recommend to stay near 1.
     196        //              the lower the value, the less the constraint will fight velocities which violate the angular limits.
     197        void    setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan, btScalar _softness = 1.f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
    145198        {
    146199                m_swingSpan1 = _swingSpan1;
    … …  
    172225
    173226        void calcAngleInfo();
    174         void calcAngleInfo2();
     227        void calcAngleInfo2(const btTransform& transA, const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB);
    175228
    176229        inline btScalar getSwingSpan1()
    … …  
    213266        btVector3 GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const;
    214267
    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;
     268        ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
     269        ///If no axis is provided, it uses the default axis for this constraint.
     270        virtual void setParam(int num, btScalar value, int axis = -1);
     271        ///return the local value of parameter
     272        virtual btScalar getParam(int num, int axis = -1) const;
     273
     274        virtual int     calculateSerializeBufferSize() const;
     275
     276        ///fills the dataBuffer and returns the struct name (and 0 on failure)
     277        virtual const char*     serialize(void* dataBuffer, btSerializer* serializer) const;
     278
    227279};
    228280
     281///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
     282struct  btConeTwistConstraintData
     283{
     284        btTypedConstraintData   m_typeConstraintData;
     285        btTransformFloatData m_rbAFrame;
     286        btTransformFloatData m_rbBFrame;
     287
     288        //limits
     289        float   m_swingSpan1;
     290        float   m_swingSpan2;
     291        float   m_twistSpan;
     292        float   m_limitSoftness;
     293        float   m_biasFactor;
     294        float   m_relaxationFactor;
     295
     296        float   m_damping;
     297               
     298        char m_pad[4];
     299
     300};
     301       
     302
     303
     304SIMD_FORCE_INLINE int   btConeTwistConstraint::calculateSerializeBufferSize() const
     305{
     306        return sizeof(btConeTwistConstraintData);
     307
     308}
     309
     310
     311        ///fills the dataBuffer and returns the struct name (and 0 on failure)
     312SIMD_FORCE_INLINE const char*   btConeTwistConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
     313{
     314        btConeTwistConstraintData* cone = (btConeTwistConstraintData*) dataBuffer;
     315        btTypedConstraint::serialize(&cone->m_typeConstraintData,serializer);
     316
     317        m_rbAFrame.serializeFloat(cone->m_rbAFrame);
     318        m_rbBFrame.serializeFloat(cone->m_rbBFrame);
     319       
     320        cone->m_swingSpan1 = float(m_swingSpan1);
     321        cone->m_swingSpan2 = float(m_swingSpan2);
     322        cone->m_twistSpan = float(m_twistSpan);
     323        cone->m_limitSoftness = float(m_limitSoftness);
     324        cone->m_biasFactor = float(m_biasFactor);
     325        cone->m_relaxationFactor = float(m_relaxationFactor);
     326        cone->m_damping = float(m_damping);
     327
     328        return "btConeTwistConstraintData";
     329}
     330
     331
    229332#endif //CONETWISTCONSTRAINT_H

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

    r5781 r8351  
    13133. This notice may not be removed or altered from any source distribution.
    1414*/
     15
     16
     17#include "btContactConstraint.h"
     18#include "BulletDynamics/Dynamics/btRigidBody.h"
     19#include "LinearMath/btVector3.h"
     20#include "btJacobianEntry.h"
     21#include "btContactSolverInfo.h"
     22#include "LinearMath/btMinMax.h"
     23#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
     24
     25
     26
     27btContactConstraint::btContactConstraint(btPersistentManifold* contactManifold,btRigidBody& rbA,btRigidBody& rbB)
     28:btTypedConstraint(CONTACT_CONSTRAINT_TYPE,rbA,rbB),
     29        m_contactManifold(*contactManifold)
     30{
     31
     32}
     33
     34btContactConstraint::~btContactConstraint()
     35{
     36
     37}
     38
     39void    btContactConstraint::setContactManifold(btPersistentManifold* contactManifold)
     40{
     41        m_contactManifold = *contactManifold;
     42}
     43
     44void btContactConstraint::getInfo1 (btConstraintInfo1* info)
     45{
     46
     47}
     48
     49void btContactConstraint::getInfo2 (btConstraintInfo2* info)
     50{
     51
     52}
     53
     54void    btContactConstraint::buildJacobian()
     55{
     56
     57}
     58
     59
     60
    1561
    1662
    … …  
    86132
    87133
    88 //response  between two dynamic objects with friction
    89 btScalar resolveSingleCollision(
    90         btRigidBody& body1,
    91         btRigidBody& body2,
    92         btManifoldPoint& contactPoint,
    93         const btContactSolverInfo& solverInfo)
    94 {
    95134
    96         const btVector3& pos1_ = contactPoint.getPositionWorldOnA();
    97         const btVector3& pos2_ = contactPoint.getPositionWorldOnB();
    98         const btVector3& normal = contactPoint.m_normalWorldOnB;
    99 
    100         //constant over all iterations
    101         btVector3 rel_pos1 = pos1_ - body1.getCenterOfMassPosition();
    102         btVector3 rel_pos2 = pos2_ - body2.getCenterOfMassPosition();
    103        
    104         btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
    105         btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
    106         btVector3 vel = vel1 - vel2;
    107         btScalar rel_vel;
    108         rel_vel = normal.dot(vel);
    109        
    110         btScalar Kfps = btScalar(1.) / solverInfo.m_timeStep ;
    111 
    112         // btScalar damping = solverInfo.m_damping ;
    113         btScalar Kerp = solverInfo.m_erp;
    114         btScalar Kcor = Kerp *Kfps;
    115 
    116         btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
    117         btAssert(cpd);
    118         btScalar distance = cpd->m_penetration;
    119         btScalar positionalError = Kcor *-distance;
    120         btScalar velocityError = cpd->m_restitution - rel_vel;// * damping;
    121 
    122         btScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv;
    123 
    124         btScalar        velocityImpulse = velocityError * cpd->m_jacDiagABInv;
    125 
    126         btScalar normalImpulse = penetrationImpulse+velocityImpulse;
    127        
    128         // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
    129         btScalar oldNormalImpulse = cpd->m_appliedImpulse;
    130         btScalar sum = oldNormalImpulse + normalImpulse;
    131         cpd->m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
    132 
    133         normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse;
    134 
    135 #ifdef USE_INTERNAL_APPLY_IMPULSE
    136         if (body1.getInvMass())
    137         {
    138                 body1.internalApplyImpulse(contactPoint.m_normalWorldOnB*body1.getInvMass(),cpd->m_angularComponentA,normalImpulse);
    139         }
    140         if (body2.getInvMass())
    141         {
    142                 body2.internalApplyImpulse(contactPoint.m_normalWorldOnB*body2.getInvMass(),cpd->m_angularComponentB,-normalImpulse);
    143         }
    144 #else //USE_INTERNAL_APPLY_IMPULSE
    145         body1.applyImpulse(normal*(normalImpulse), rel_pos1);
    146         body2.applyImpulse(-normal*(normalImpulse), rel_pos2);
    147 #endif //USE_INTERNAL_APPLY_IMPULSE
    148 
    149         return normalImpulse;
    150 }
    151 
    152 
    153 btScalar resolveSingleFriction(
    154         btRigidBody& body1,
    155         btRigidBody& body2,
    156         btManifoldPoint& contactPoint,
    157         const btContactSolverInfo& solverInfo)
    158 {
    159 
    160         (void)solverInfo;
    161 
    162         const btVector3& pos1 = contactPoint.getPositionWorldOnA();
    163         const btVector3& pos2 = contactPoint.getPositionWorldOnB();
    164 
    165         btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
    166         btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
    167        
    168         btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
    169         btAssert(cpd);
    170 
    171         btScalar combinedFriction = cpd->m_friction;
    172        
    173         btScalar limit = cpd->m_appliedImpulse * combinedFriction;
    174        
    175         if (cpd->m_appliedImpulse>btScalar(0.))
    176         //friction
    177         {
    178                 //apply friction in the 2 tangential directions
    179                
    180                 // 1st tangent
    181                 btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
    182                 btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
    183                 btVector3 vel = vel1 - vel2;
    184        
    185                 btScalar j1,j2;
    186 
    187                 {
    188                                
    189                         btScalar vrel = cpd->m_frictionWorldTangential0.dot(vel);
    190 
    191                         // calculate j that moves us to zero relative velocity
    192                         j1 = -vrel * cpd->m_jacDiagABInvTangent0;
    193                         btScalar oldTangentImpulse = cpd->m_accumulatedTangentImpulse0;
    194                         cpd->m_accumulatedTangentImpulse0 = oldTangentImpulse + j1;
    195                         btSetMin(cpd->m_accumulatedTangentImpulse0, limit);
    196                         btSetMax(cpd->m_accumulatedTangentImpulse0, -limit);
    197                         j1 = cpd->m_accumulatedTangentImpulse0 - oldTangentImpulse;
    198 
    199                 }
    200                 {
    201                         // 2nd tangent
    202 
    203                         btScalar vrel = cpd->m_frictionWorldTangential1.dot(vel);
    204                        
    205                         // calculate j that moves us to zero relative velocity
    206                         j2 = -vrel * cpd->m_jacDiagABInvTangent1;
    207                         btScalar oldTangentImpulse = cpd->m_accumulatedTangentImpulse1;
    208                         cpd->m_accumulatedTangentImpulse1 = oldTangentImpulse + j2;
    209                         btSetMin(cpd->m_accumulatedTangentImpulse1, limit);
    210                         btSetMax(cpd->m_accumulatedTangentImpulse1, -limit);
    211                         j2 = cpd->m_accumulatedTangentImpulse1 - oldTangentImpulse;
    212                 }
    213 
    214 #ifdef USE_INTERNAL_APPLY_IMPULSE
    215         if (body1.getInvMass())
    216         {
    217                 body1.internalApplyImpulse(cpd->m_frictionWorldTangential0*body1.getInvMass(),cpd->m_frictionAngularComponent0A,j1);
    218                 body1.internalApplyImpulse(cpd->m_frictionWorldTangential1*body1.getInvMass(),cpd->m_frictionAngularComponent1A,j2);
    219         }
    220         if (body2.getInvMass())
    221         {
    222                 body2.internalApplyImpulse(cpd->m_frictionWorldTangential0*body2.getInvMass(),cpd->m_frictionAngularComponent0B,-j1);
    223                 body2.internalApplyImpulse(cpd->m_frictionWorldTangential1*body2.getInvMass(),cpd->m_frictionAngularComponent1B,-j2);   
    224         }
    225 #else //USE_INTERNAL_APPLY_IMPULSE
    226         body1.applyImpulse((j1 * cpd->m_frictionWorldTangential0)+(j2 * cpd->m_frictionWorldTangential1), rel_pos1);
    227         body2.applyImpulse((j1 * -cpd->m_frictionWorldTangential0)+(j2 * -cpd->m_frictionWorldTangential1), rel_pos2);
    228 #endif //USE_INTERNAL_APPLY_IMPULSE
    229 
    230 
    231         }
    232         return cpd->m_appliedImpulse;
    233 }
    234 
    235 
    236 btScalar resolveSingleFrictionOriginal(
    237         btRigidBody& body1,
    238         btRigidBody& body2,
    239         btManifoldPoint& contactPoint,
    240         const btContactSolverInfo& solverInfo);
    241 
    242 btScalar resolveSingleFrictionOriginal(
    243         btRigidBody& body1,
    244         btRigidBody& body2,
    245         btManifoldPoint& contactPoint,
    246         const btContactSolverInfo& solverInfo)
    247 {
    248 
    249         (void)solverInfo;
    250 
    251         const btVector3& pos1 = contactPoint.getPositionWorldOnA();
    252         const btVector3& pos2 = contactPoint.getPositionWorldOnB();
    253 
    254         btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
    255         btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
    256        
    257         btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
    258         btAssert(cpd);
    259 
    260         btScalar combinedFriction = cpd->m_friction;
    261        
    262         btScalar limit = cpd->m_appliedImpulse * combinedFriction;
    263         //if (contactPoint.m_appliedImpulse>btScalar(0.))
    264         //friction
    265         {
    266                 //apply friction in the 2 tangential directions
    267                
    268                 {
    269                         // 1st tangent
    270                         btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
    271                         btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
    272                         btVector3 vel = vel1 - vel2;
    273                        
    274                         btScalar vrel = cpd->m_frictionWorldTangential0.dot(vel);
    275 
    276                         // calculate j that moves us to zero relative velocity
    277                         btScalar j = -vrel * cpd->m_jacDiagABInvTangent0;
    278                         btScalar total = cpd->m_accumulatedTangentImpulse0 + j;
    279                         btSetMin(total, limit);
    280                         btSetMax(total, -limit);
    281                         j = total - cpd->m_accumulatedTangentImpulse0;
    282                         cpd->m_accumulatedTangentImpulse0 = total;
    283                         body1.applyImpulse(j * cpd->m_frictionWorldTangential0, rel_pos1);
    284                         body2.applyImpulse(j * -cpd->m_frictionWorldTangential0, rel_pos2);
    285                 }
    286 
    287                                
    288                 {
    289                         // 2nd tangent
    290                         btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
    291                         btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
    292                         btVector3 vel = vel1 - vel2;
    293 
    294                         btScalar vrel = cpd->m_frictionWorldTangential1.dot(vel);
    295                        
    296                         // calculate j that moves us to zero relative velocity
    297                         btScalar j = -vrel * cpd->m_jacDiagABInvTangent1;
    298                         btScalar total = cpd->m_accumulatedTangentImpulse1 + j;
    299                         btSetMin(total, limit);
    300                         btSetMax(total, -limit);
    301                         j = total - cpd->m_accumulatedTangentImpulse1;
    302                         cpd->m_accumulatedTangentImpulse1 = total;
    303                         body1.applyImpulse(j * cpd->m_frictionWorldTangential1, rel_pos1);
    304                         body2.applyImpulse(j * -cpd->m_frictionWorldTangential1, rel_pos2);
    305                 }
    306         }
    307         return cpd->m_appliedImpulse;
    308 }
    309 
    310 
    311 //velocity + friction
    312 //response  between two dynamic objects with friction
    313 btScalar resolveSingleCollisionCombined(
    314         btRigidBody& body1,
    315         btRigidBody& body2,
    316         btManifoldPoint& contactPoint,
    317         const btContactSolverInfo& solverInfo)
    318 {
    319 
    320         const btVector3& pos1 = contactPoint.getPositionWorldOnA();
    321         const btVector3& pos2 = contactPoint.getPositionWorldOnB();
    322         const btVector3& normal = contactPoint.m_normalWorldOnB;
    323 
    324         btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
    325         btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
    326        
    327         btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
    328         btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
    329         btVector3 vel = vel1 - vel2;
    330         btScalar rel_vel;
    331         rel_vel = normal.dot(vel);
    332        
    333         btScalar Kfps = btScalar(1.) / solverInfo.m_timeStep ;
    334 
    335         //btScalar damping = solverInfo.m_damping ;
    336         btScalar Kerp = solverInfo.m_erp;
    337         btScalar Kcor = Kerp *Kfps;
    338 
    339         btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
    340         btAssert(cpd);
    341         btScalar distance = cpd->m_penetration;
    342         btScalar positionalError = Kcor *-distance;
    343         btScalar velocityError = cpd->m_restitution - rel_vel;// * damping;
    344 
    345         btScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv;
    346 
    347         btScalar        velocityImpulse = velocityError * cpd->m_jacDiagABInv;
    348 
    349         btScalar normalImpulse = penetrationImpulse+velocityImpulse;
    350        
    351         // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
    352         btScalar oldNormalImpulse = cpd->m_appliedImpulse;
    353         btScalar sum = oldNormalImpulse + normalImpulse;
    354         cpd->m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
    355 
    356         normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse;
    357 
    358 
    359 #ifdef USE_INTERNAL_APPLY_IMPULSE
    360         if (body1.getInvMass())
    361         {
    362                 body1.internalApplyImpulse(contactPoint.m_normalWorldOnB*body1.getInvMass(),cpd->m_angularComponentA,normalImpulse);
    363         }
    364         if (body2.getInvMass())
    365         {
    366                 body2.internalApplyImpulse(contactPoint.m_normalWorldOnB*body2.getInvMass(),cpd->m_angularComponentB,-normalImpulse);
    367         }
    368 #else //USE_INTERNAL_APPLY_IMPULSE
    369         body1.applyImpulse(normal*(normalImpulse), rel_pos1);
    370         body2.applyImpulse(-normal*(normalImpulse), rel_pos2);
    371 #endif //USE_INTERNAL_APPLY_IMPULSE
    372 
    373         {
    374                 //friction
    375                 btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
    376                 btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
    377                 btVector3 vel = vel1 - vel2;
    378          
    379                 rel_vel = normal.dot(vel);
    380 
    381 
    382                 btVector3 lat_vel = vel - normal * rel_vel;
    383                 btScalar lat_rel_vel = lat_vel.length();
    384 
    385                 btScalar combinedFriction = cpd->m_friction;
    386 
    387                 if (cpd->m_appliedImpulse > 0)
    388                 if (lat_rel_vel > SIMD_EPSILON)
    389                 {
    390                         lat_vel /= lat_rel_vel;
    391                         btVector3 temp1 = body1.getInvInertiaTensorWorld() * rel_pos1.cross(lat_vel);
    392                         btVector3 temp2 = body2.getInvInertiaTensorWorld() * rel_pos2.cross(lat_vel);
    393                         btScalar friction_impulse = lat_rel_vel /
    394                                 (body1.getInvMass() + body2.getInvMass() + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
    395                         btScalar normal_impulse = cpd->m_appliedImpulse * combinedFriction;
    396 
    397                         btSetMin(friction_impulse, normal_impulse);
    398                         btSetMax(friction_impulse, -normal_impulse);
    399                         body1.applyImpulse(lat_vel * -friction_impulse, rel_pos1);
    400                         body2.applyImpulse(lat_vel * friction_impulse, rel_pos2);
    401                 }
    402         }
    403 
    404 
    405 
    406         return normalImpulse;
    407 }
    408 
    409 btScalar resolveSingleFrictionEmpty(
    410         btRigidBody& body1,
    411         btRigidBody& body2,
    412         btManifoldPoint& contactPoint,
    413         const btContactSolverInfo& solverInfo);
    414 
    415 btScalar resolveSingleFrictionEmpty(
    416         btRigidBody& body1,
    417         btRigidBody& body2,
    418         btManifoldPoint& contactPoint,
    419         const btContactSolverInfo& solverInfo)
    420 {
    421         (void)contactPoint;
    422         (void)body1;
    423         (void)body2;
    424         (void)solverInfo;
    425 
    426 
    427         return btScalar(0.);
    428 }
    429 

  • code/trunk/src/external/bullet/BulletDynamics/ConstraintSolver/btContactConstraint.h

    r5781 r8351  
    1717#define CONTACT_CONSTRAINT_H
    1818
    19 ///@todo: make into a proper class working with the iterative constraint solver
     19#include "LinearMath/btVector3.h"
     20#include "btJacobianEntry.h"
     21#include "btTypedConstraint.h"
     22#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
    2023
    21 class btRigidBody;
    22 #include "LinearMath/btVector3.h"
    23 #include "LinearMath/btScalar.h"
    24 struct btContactSolverInfo;
    25 class btManifoldPoint;
     24///btContactConstraint can be automatically created to solve contact constraints using the unified btTypedConstraint interface
     25ATTRIBUTE_ALIGNED16(class) btContactConstraint : public btTypedConstraint
     26{
     27protected:
    2628
    27 enum {
    28         DEFAULT_CONTACT_SOLVER_TYPE=0,
    29         CONTACT_SOLVER_TYPE1,
    30         CONTACT_SOLVER_TYPE2,
    31         USER_CONTACT_SOLVER_TYPE1,
    32         MAX_CONTACT_SOLVER_TYPES
     29        btPersistentManifold m_contactManifold;
     30
     31public:
     32
     33
     34        btContactConstraint(btPersistentManifold* contactManifold,btRigidBody& rbA,btRigidBody& rbB);
     35
     36        void    setContactManifold(btPersistentManifold* contactManifold);
     37
     38        btPersistentManifold* getContactManifold()
     39        {
     40                return &m_contactManifold;
     41        }
     42
     43        const btPersistentManifold* getContactManifold() const
     44        {
     45                return &m_contactManifold;
     46        }
     47
     48        virtual ~btContactConstraint();
     49
     50        virtual void getInfo1 (btConstraintInfo1* info);
     51
     52        virtual void getInfo2 (btConstraintInfo2* info);
     53
     54        ///obsolete methods
     55        virtual void    buildJacobian();
     56
     57
    3358};
    3459
    3560
    36 typedef btScalar (*ContactSolverFunc)(btRigidBody& body1,
    37                                                                          btRigidBody& body2,
    38                                                                          class btManifoldPoint& contactPoint,
    39                                                                          const btContactSolverInfo& info);
    40 
    41 ///stores some extra information to each contact point. It is not in the contact point, because that want to keep the collision detection independent from the constraint solver.
    42 struct btConstraintPersistentData
    43 {
    44         inline btConstraintPersistentData()
    45         :m_appliedImpulse(btScalar(0.)),
    46         m_prevAppliedImpulse(btScalar(0.)),
    47         m_accumulatedTangentImpulse0(btScalar(0.)),
    48         m_accumulatedTangentImpulse1(btScalar(0.)),
    49         m_jacDiagABInv(btScalar(0.)),
    50         m_persistentLifeTime(0),
    51         m_restitution(btScalar(0.)),
    52         m_friction(btScalar(0.)),
    53         m_penetration(btScalar(0.)),
    54         m_contactSolverFunc(0),
    55         m_frictionSolverFunc(0)
    56         {
    57         }
    58        
    59                                        
    60                                 /// total applied impulse during most recent frame
    61                         btScalar        m_appliedImpulse;
    62                         btScalar        m_prevAppliedImpulse;
    63                         btScalar        m_accumulatedTangentImpulse0;
    64                         btScalar        m_accumulatedTangentImpulse1;
    65                        
    66                         btScalar        m_jacDiagABInv;
    67                         btScalar        m_jacDiagABInvTangent0;
    68                         btScalar        m_jacDiagABInvTangent1;
    69                         int             m_persistentLifeTime;
    70                         btScalar        m_restitution;
    71                         btScalar        m_friction;
    72                         btScalar        m_penetration;
    73                         btVector3       m_frictionWorldTangential0;
    74                         btVector3       m_frictionWorldTangential1;
    75 
    76                         btVector3       m_frictionAngularComponent0A;
    77                         btVector3       m_frictionAngularComponent0B;
    78                         btVector3       m_frictionAngularComponent1A;
    79                         btVector3       m_frictionAngularComponent1B;
    80 
    81                         //some data doesn't need to be persistent over frames: todo: clean/reuse this
    82                         btVector3       m_angularComponentA;
    83                         btVector3       m_angularComponentB;
    84                
    85                         ContactSolverFunc       m_contactSolverFunc;
    86                         ContactSolverFunc       m_frictionSolverFunc;
    87 
    88 };
    89 
    90 ///bilateral constraint between two dynamic objects
    91 ///positive distance = separation, negative distance = penetration
     61///resolveSingleBilateral is an obsolete methods used for vehicle friction between two dynamic objects
    9262void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
    9363                      btRigidBody& body2, const btVector3& pos2,
    … …  
    9565
    9666
    97 ///contact constraint resolution:
    98 ///calculate and apply impulse to satisfy non-penetration and non-negative relative velocity constraint
    99 ///positive distance = separation, negative distance = penetration
    100 btScalar resolveSingleCollision(
    101         btRigidBody& body1,
    102         btRigidBody& body2,
    103                 btManifoldPoint& contactPoint,
    104                  const btContactSolverInfo& info);
    105 
    106 btScalar resolveSingleFriction(
    107         btRigidBody& body1,
    108         btRigidBody& body2,
    109         btManifoldPoint& contactPoint,
    110         const btContactSolverInfo& solverInfo
    111                 );
    112 
    113 
    114 
    115 btScalar resolveSingleCollisionCombined(
    116         btRigidBody& body1,
    117         btRigidBody& body2,
    118         btManifoldPoint& contactPoint,
    119         const btContactSolverInfo& solverInfo
    120                 );
    12167
    12268#endif //CONTACT_CONSTRAINT_H

  • code/trunk/src/external/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h

    r5781 r8351  
    3636
    3737        btScalar        m_tau;
    38         btScalar        m_damping;
     38        btScalar        m_damping;//global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
    3939        btScalar        m_friction;
    4040        btScalar        m_timeStep;
    … …  
    5353        int                     m_solverMode;
    5454        int     m_restingContactRestitutionThreshold;
     55        int                     m_minimumSolverBatchSize;
    5556
    5657
    … …  
    7879                m_linearSlop = btScalar(0.0);
    7980                m_warmstartingFactor=btScalar(0.85);
    80                 m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD ;//SOLVER_RANDMIZE_ORDER
     81                m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD;// | SOLVER_RANDMIZE_ORDER;
    8182                m_restingContactRestitutionThreshold = 2;//resting contact lifetime threshold to disable restitution
     83                m_minimumSolverBatchSize = 128; //try to combine islands until the amount of constraints reaches this limit
    8284        }
    8385};

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

    r5781 r8351  
    2323#include "BulletDynamics/Dynamics/btRigidBody.h"
    2424#include "LinearMath/btTransformUtil.h"
     25#include "LinearMath/btTransformUtil.h"
    2526#include <new>
    2627
    2728
     29
    2830#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 //-----------------------------------------------------------------------------
     31#define D6_USE_FRAME_OFFSET true
     32
     33
     34
     35
     36
    3937
    4038btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
    … …  
    4341, m_frameInB(frameInB),
    4442m_useLinearReferenceFrameA(useLinearReferenceFrameA),
     43m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET),
     44m_flags(0),
    4545m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
    4646{
    47 
    48 }
    49 //-----------------------------------------------------------------------------
     47        calculateTransforms();
     48}
     49
     50
     51
     52btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB)
     53        : btTypedConstraint(D6_CONSTRAINT_TYPE, getFixedBody(), rbB),
     54                m_frameInB(frameInB),
     55                m_useLinearReferenceFrameA(useLinearReferenceFrameB),
     56                m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET),
     57                m_flags(0),
     58                m_useSolveConstraintObsolete(false)
     59{
     60        ///not providing rigidbody A means implicitly using worldspace for body A
     61        m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB;
     62        calculateTransforms();
     63}
     64
     65
    5066
    5167
    5268#define GENERIC_D6_DISABLE_WARMSTARTING 1
    5369
    54 //-----------------------------------------------------------------------------
     70
    5571
    5672btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
    … …  
    6278}
    6379
    64 //-----------------------------------------------------------------------------
     80
    6581
    6682///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
    … …  
    111127                return 0;
    112128        }
    113 
    114129        if (test_value < m_loLimit)
    115130        {
    … …  
    130145}
    131146
    132 //-----------------------------------------------------------------------------
     147
    133148
    134149btScalar btRotationalLimitMotor::solveAngularLimits(
    135150        btScalar timeStep,btVector3& axis,btScalar jacDiagABInv,
    136         btRigidBody * body0, btSolverBody& bodyA, btRigidBody * body1, btSolverBody& bodyB)
     151        btRigidBody * body0, btRigidBody * body1 )
    137152{
    138153        if (needApplyTorques()==false) return 0.0f;
    … …  
    144159        if (m_currentLimit!=0)
    145160        {
    146                 target_velocity = -m_ERP*m_currentLimitError/(timeStep);
     161                target_velocity = -m_stopERP*m_currentLimitError/(timeStep);
    147162                maxMotorForce = m_maxLimitForce;
    148163        }
    … …  
    153168
    154169        btVector3 angVelA;
    155         bodyA.getAngularVelocity(angVelA);
     170        body0->internalGetAngularVelocity(angVelA);
    156171        btVector3 angVelB;
    157         bodyB.getAngularVelocity(angVelB);
     172        body1->internalGetAngularVelocity(angVelB);
    158173
    159174        btVector3 vel_diff;
    … …  
    192207
    193208        // sort with accumulated impulses
    194         btScalar        lo = btScalar(-1e30);
    195         btScalar        hi = btScalar(1e30);
     209        btScalar        lo = btScalar(-BT_LARGE_FLOAT);
     210        btScalar        hi = btScalar(BT_LARGE_FLOAT);
    196211
    197212        btScalar oldaccumImpulse = m_accumulatedImpulse;
    … …  
    206221        //body1->applyTorqueImpulse(-motorImp);
    207222
    208         bodyA.applyImpulse(btVector3(0,0,0), body0->getInvInertiaTensorWorld()*axis,clippedMotorImpulse);
    209         bodyB.applyImpulse(btVector3(0,0,0), body1->getInvInertiaTensorWorld()*axis,-clippedMotorImpulse);
     223        body0->internalApplyImpulse(btVector3(0,0,0), body0->getInvInertiaTensorWorld()*axis,clippedMotorImpulse);
     224        body1->internalApplyImpulse(btVector3(0,0,0), body1->getInvInertiaTensorWorld()*axis,-clippedMotorImpulse);
    210225
    211226
    … …  
    250265        m_currentLimitError[limitIndex] = btScalar(0.f);
    251266        return 0;
    252 } // btTranslationalLimitMotor::testLimitValue()
    253 
    254 //-----------------------------------------------------------------------------
     267}
     268
     269
    255270
    256271btScalar btTranslationalLimitMotor::solveLinearAxis(
    257272        btScalar timeStep,
    258273        btScalar jacDiagABInv,
    259         btRigidBody& body1,btSolverBody& bodyA,const btVector3 &pointInA,
    260         btRigidBody& body2,btSolverBody& bodyB,const btVector3 &pointInB,
     274        btRigidBody& body1,const btVector3 &pointInA,
     275        btRigidBody& body2,const btVector3 &pointInB,
    261276        int limit_index,
    262277        const btVector3 & axis_normal_on_a,
    … …  
    271286
    272287        btVector3 vel1;
    273         bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
     288        body1.internalGetVelocityInLocalPointObsolete(rel_pos1,vel1);
    274289        btVector3 vel2;
    275         bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
     290        body2.internalGetVelocityInLocalPointObsolete(rel_pos2,vel2);
    276291        btVector3 vel = vel1 - vel2;
    277292
    … …  
    284299        //positional error (zeroth order error)
    285300        btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a);
    286         btScalar        lo = btScalar(-1e30);
    287         btScalar        hi = btScalar(1e30);
     301        btScalar        lo = btScalar(-BT_LARGE_FLOAT);
     302        btScalar        hi = btScalar(BT_LARGE_FLOAT);
    288303
    289304        btScalar minLimit = m_lowerLimit[limit_index];
    … …  
    331346        btVector3 ftorqueAxis1 = rel_pos1.cross(axis_normal_on_a);
    332347        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);
     348        body1.internalApplyImpulse(axis_normal_on_a*body1.getInvMass(), body1.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse);
     349        body2.internalApplyImpulse(axis_normal_on_a*body2.getInvMass(), body2.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse);
    335350
    336351
    … …  
    373388}
    374389
    375 //-----------------------------------------------------------------------------
    376 
    377390void btGeneric6DofConstraint::calculateTransforms()
    378391{
    379         m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA;
    380         m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB;
     392        calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
     393}
     394
     395void btGeneric6DofConstraint::calculateTransforms(const btTransform& transA,const btTransform& transB)
     396{
     397        m_calculatedTransformA = transA * m_frameInA;
     398        m_calculatedTransformB = transB * m_frameInB;
    381399        calculateLinearInfo();
    382400        calculateAngleInfo();
    383 }
    384 
    385 //-----------------------------------------------------------------------------
     401        if(m_useOffsetForConstraintFrame)
     402        {       //  get weight factors depending on masses
     403                btScalar miA = getRigidBodyA().getInvMass();
     404                btScalar miB = getRigidBodyB().getInvMass();
     405                m_hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
     406                btScalar miS = miA + miB;
     407                if(miS > btScalar(0.f))
     408                {
     409                        m_factA = miB / miS;
     410                }
     411                else
     412                {
     413                        m_factA = btScalar(0.5f);
     414                }
     415                m_factB = btScalar(1.0f) - m_factA;
     416        }
     417}
     418
     419
    386420
    387421void btGeneric6DofConstraint::buildLinearJacobian(
    … …  
    401435}
    402436
    403 //-----------------------------------------------------------------------------
     437
    404438
    405439void btGeneric6DofConstraint::buildAngularJacobian(
    … …  
    414448}
    415449
    416 //-----------------------------------------------------------------------------
     450
    417451
    418452bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index)
    419453{
    420454        btScalar angle = m_calculatedAxisAngleDiff[axis_index];
     455        angle = btAdjustAngleToLimits(angle, m_angularLimits[axis_index].m_loLimit, m_angularLimits[axis_index].m_hiLimit);
     456        m_angularLimits[axis_index].m_currentPosition = angle;
    421457        //test limits
    422458        m_angularLimits[axis_index].testLimitValue(angle);
    … …  
    424460}
    425461
    426 //-----------------------------------------------------------------------------
     462
    427463
    428464void btGeneric6DofConstraint::buildJacobian()
    429465{
     466#ifndef __SPU__
    430467        if (m_useSolveConstraintObsolete)
    431468        {
    … …  
    439476                }
    440477                //calculates transform
    441                 calculateTransforms();
     478                calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
    442479
    443480                //  const btVector3& pivotAInW = m_calculatedTransformA.getOrigin();
    … …  
    482519
    483520        }
    484 }
    485 
    486 //-----------------------------------------------------------------------------
     521#endif //__SPU__
     522
     523}
     524
    487525
    488526void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info)
    … …  
    495533        {
    496534                //prepare constraint
    497                 calculateTransforms();
     535                calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
    498536                info->m_numConstraintRows = 0;
    499537                info->nub = 6;
    … …  
    520558}
    521559
    522 //-----------------------------------------------------------------------------
     560void btGeneric6DofConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
     561{
     562        if (m_useSolveConstraintObsolete)
     563        {
     564                info->m_numConstraintRows = 0;
     565                info->nub = 0;
     566        } else
     567        {
     568                //pre-allocate all 6
     569                info->m_numConstraintRows = 6;
     570                info->nub = 0;
     571        }
     572}
     573
    523574
    524575void btGeneric6DofConstraint::getInfo2 (btConstraintInfo2* info)
    525576{
    526577        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;
     578
     579        const btTransform& transA = m_rbA.getCenterOfMassTransform();
     580        const btTransform& transB = m_rbB.getCenterOfMassTransform();
     581        const btVector3& linVelA = m_rbA.getLinearVelocity();
     582        const btVector3& linVelB = m_rbB.getLinearVelocity();
     583        const btVector3& angVelA = m_rbA.getAngularVelocity();
     584        const btVector3& angVelB = m_rbB.getAngularVelocity();
     585
     586        if(m_useOffsetForConstraintFrame)
     587        { // for stability better to solve angular limits first
     588                int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB);
     589                setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB);
     590        }
     591        else
     592        { // leave old version for compatibility
     593                int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB);
     594                setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB);
     595        }
     596
     597}
     598
     599
     600void btGeneric6DofConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
     601{
     602       
     603        btAssert(!m_useSolveConstraintObsolete);
     604        //prepare constraint
     605        calculateTransforms(transA,transB);
     606
     607        int i;
     608        for (i=0;i<3 ;i++ )
     609        {
     610                testAngularLimitMotor(i);
     611        }
     612
     613        if(m_useOffsetForConstraintFrame)
     614        { // for stability better to solve angular limits first
     615                int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB);
     616                setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB);
     617        }
     618        else
     619        { // leave old version for compatibility
     620                int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB);
     621                setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB);
     622        }
     623}
     624
     625
     626
     627int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
     628{
     629//      int row = 0;
    537630        //solve linear limits
    538631        btRotationalLimitMotor limot;
    … …  
    543636                        limot.m_bounce = btScalar(0.f);
    544637                        limot.m_currentLimit = m_linearLimits.m_currentLimit[i];
     638                        limot.m_currentPosition = m_linearLimits.m_currentLinearDiff[i];
    545639                        limot.m_currentLimitError  = m_linearLimits.m_currentLimitError[i];
    546640                        limot.m_damping  = m_linearLimits.m_damping;
    547641                        limot.m_enableMotor  = m_linearLimits.m_enableMotor[i];
    548                         limot.m_ERP  = m_linearLimits.m_restitution;
    549642                        limot.m_hiLimit  = m_linearLimits.m_upperLimit[i];
    550643                        limot.m_limitSoftness  = m_linearLimits.m_limitSoftness;
    … …  
    554647                        limot.m_targetVelocity  = m_linearLimits.m_targetVelocity[i];
    555648                        btVector3 axis = m_calculatedTransformA.getBasis().getColumn(i);
    556                         row += get_limit_motor_info2(&limot, &m_rbA, &m_rbB, info, row, axis, 0);
     649                        int flags = m_flags >> (i * BT_6DOF_FLAGS_AXIS_SHIFT);
     650                        limot.m_normalCFM       = (flags & BT_6DOF_FLAGS_CFM_NORM) ? m_linearLimits.m_normalCFM[i] : info->cfm[0];
     651                        limot.m_stopCFM         = (flags & BT_6DOF_FLAGS_CFM_STOP) ? m_linearLimits.m_stopCFM[i] : info->cfm[0];
     652                        limot.m_stopERP         = (flags & BT_6DOF_FLAGS_ERP_STOP) ? m_linearLimits.m_stopERP[i] : info->erp;
     653                        if(m_useOffsetForConstraintFrame)
     654                        {
     655                                int indx1 = (i + 1) % 3;
     656                                int indx2 = (i + 2) % 3;
     657                                int rotAllowed = 1; // rotations around orthos to current axis
     658                                if(m_angularLimits[indx1].m_currentLimit && m_angularLimits[indx2].m_currentLimit)
     659                                {
     660                                        rotAllowed = 0;
     661                                }
     662                                row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0, rotAllowed);
     663                        }
     664                        else
     665                        {
     666                                row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0);
     667                        }
    557668                }
    558669        }
    … …  
    560671}
    561672
    562 //-----------------------------------------------------------------------------
    563 
    564 int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_offset)
     673
     674
     675int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_offset, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
    565676{
    566677        btGeneric6DofConstraint * d6constraint = this;
    … …  
    572683                {
    573684                        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);
     685                        int flags = m_flags >> ((i + 3) * BT_6DOF_FLAGS_AXIS_SHIFT);
     686                        if(!(flags & BT_6DOF_FLAGS_CFM_NORM))
     687                        {
     688                                m_angularLimits[i].m_normalCFM = info->cfm[0];
     689                        }
     690                        if(!(flags & BT_6DOF_FLAGS_CFM_STOP))
     691                        {
     692                                m_angularLimits[i].m_stopCFM = info->cfm[0];
     693                        }
     694                        if(!(flags & BT_6DOF_FLAGS_ERP_STOP))
     695                        {
     696                                m_angularLimits[i].m_stopERP = info->erp;
     697                        }
     698                        row += get_limit_motor_info2(d6constraint->getRotationalLimitMotor(i),
     699                                                                                                transA,transB,linVelA,linVelB,angVelA,angVelB, info,row,axis,1);
    579700                }
    580701        }
    … …  
    583704}
    584705
    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 //-----------------------------------------------------------------------------
     706
     707
    647708
    648709void    btGeneric6DofConstraint::updateRHS(btScalar     timeStep)
    … …  
    652713}
    653714
    654 //-----------------------------------------------------------------------------
     715
    655716
    656717btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const
    … …  
    659720}
    660721
    661 //-----------------------------------------------------------------------------
    662 
    663 btScalar btGeneric6DofConstraint::getAngle(int axis_index) const
    664 {
    665         return m_calculatedAxisAngleDiff[axis_index];
    666 }
    667 
    668 //-----------------------------------------------------------------------------
     722
     723btScalar        btGeneric6DofConstraint::getRelativePivotPosition(int axisIndex) const
     724{
     725        return m_calculatedLinearDiff[axisIndex];
     726}
     727
     728
     729btScalar btGeneric6DofConstraint::getAngle(int axisIndex) const
     730{
     731        return m_calculatedAxisAngleDiff[axisIndex];
     732}
     733
     734
    669735
    670736void btGeneric6DofConstraint::calcAnchorPos(void)
    … …  
    685751        m_AnchorPos = pA * weight + pB * (btScalar(1.0) - weight);
    686752        return;
    687 } // btGeneric6DofConstraint::calcAnchorPos()
    688 
    689 //-----------------------------------------------------------------------------
     753}
     754
     755
    690756
    691757void btGeneric6DofConstraint::calculateLinearInfo()
    … …  
    695761        for(int i = 0; i < 3; i++)
    696762        {
     763                m_linearLimits.m_currentLinearDiff[i] = m_calculatedLinearDiff[i];
    697764                m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]);
    698765        }
    699 } // btGeneric6DofConstraint::calculateLinearInfo()
    700 
    701 //-----------------------------------------------------------------------------
     766}
     767
     768
    702769
    703770int btGeneric6DofConstraint::get_limit_motor_info2(
    704771        btRotationalLimitMotor * limot,
    705         btRigidBody * body0, btRigidBody * body1,
    706         btConstraintInfo2 *info, int row, btVector3& ax1, int rotational)
     772        const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
     773        btConstraintInfo2 *info, int row, btVector3& ax1, int rotational,int rotAllowed)
    707774{
    708775    int srow = row * info->rowskip;
    … …  
    722789            J2[srow+2] = -ax1[2];
    723790        }
    724         if((!rotational) && limit)
     791        if((!rotational))
    725792        {
    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];
     793                        if (m_useOffsetForConstraintFrame)
     794                        {
     795                                btVector3 tmpA, tmpB, relA, relB;
     796                                // get vector from bodyB to frameB in WCS
     797                                relB = m_calculatedTransformB.getOrigin() - transB.getOrigin();
     798                                // get its projection to constraint axis
     799                                btVector3 projB = ax1 * relB.dot(ax1);
     800                                // get vector directed from bodyB to constraint axis (and orthogonal to it)
     801                                btVector3 orthoB = relB - projB;
     802                                // same for bodyA
     803                                relA = m_calculatedTransformA.getOrigin() - transA.getOrigin();
     804                                btVector3 projA = ax1 * relA.dot(ax1);
     805                                btVector3 orthoA = relA - projA;
     806                                // get desired offset between frames A and B along constraint axis
     807                                btScalar desiredOffs = limot->m_currentPosition - limot->m_currentLimitError;
     808                                // desired vector from projection of center of bodyA to projection of center of bodyB to constraint axis
     809                                btVector3 totalDist = projA + ax1 * desiredOffs - projB;
     810                                // get offset vectors relA and relB
     811                                relA = orthoA + totalDist * m_factA;
     812                                relB = orthoB - totalDist * m_factB;
     813                                tmpA = relA.cross(ax1);
     814                                tmpB = relB.cross(ax1);
     815                                if(m_hasStaticBody && (!rotAllowed))
     816                                {
     817                                        tmpA *= m_factA;
     818                                        tmpB *= m_factB;
     819                                }
     820                                int i;
     821                                for (i=0; i<3; i++) info->m_J1angularAxis[srow+i] = tmpA[i];
     822                                for (i=0; i<3; i++) info->m_J2angularAxis[srow+i] = -tmpB[i];
     823                        } else
     824                        {
     825                                btVector3 ltd;  // Linear Torque Decoupling vector
     826                                btVector3 c = m_calculatedTransformB.getOrigin() - transA.getOrigin();
     827                                ltd = c.cross(ax1);
     828                                info->m_J1angularAxis[srow+0] = ltd[0];
     829                                info->m_J1angularAxis[srow+1] = ltd[1];
     830                                info->m_J1angularAxis[srow+2] = ltd[2];
     831
     832                                c = m_calculatedTransformB.getOrigin() - transB.getOrigin();
     833                                ltd = -c.cross(ax1);
     834                                info->m_J2angularAxis[srow+0] = ltd[0];
     835                                info->m_J2angularAxis[srow+1] = ltd[1];
     836                                info->m_J2angularAxis[srow+2] = ltd[2];
     837                        }
    738838        }
    739839        // if we're limited low and high simultaneously, the joint motor is
    … …  
    743843        if (powered)
    744844        {
    745             info->cfm[srow] = 0.0f;
     845                        info->cfm[srow] = limot->m_normalCFM;
    746846            if(!limit)
    747847            {
    748                 info->m_constraintError[srow] += limot->m_targetVelocity;
     848                                btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;
     849
     850                                btScalar mot_fact = getMotorFactor(     limot->m_currentPosition,
     851                                                                                                        limot->m_loLimit,
     852                                                                                                        limot->m_hiLimit,
     853                                                                                                        tag_vel,
     854                                                                                                        info->fps * limot->m_stopERP);
     855                                info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity;
    749856                info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
    750857                info->m_upperLimit[srow] = limot->m_maxMotorForce;
    … …  
    753860        if(limit)
    754861        {
    755             btScalar k = info->fps * limot->m_ERP;
     862            btScalar k = info->fps * limot->m_stopERP;
    756863                        if(!rotational)
    757864                        {
    … …  
    762869                                info->m_constraintError[srow] += -k * limot->m_currentLimitError;
    763870                        }
    764             info->cfm[srow] = 0.0f;
     871                        info->cfm[srow] = limot->m_stopCFM;
    765872            if (limot->m_loLimit == limot->m_hiLimit)
    766873            {   // limited low and high simultaneously
    … …  
    787894                    if (rotational)
    788895                    {
    789                         vel = body0->getAngularVelocity().dot(ax1);
    790                         if (body1)
    791                             vel -= body1->getAngularVelocity().dot(ax1);
     896                        vel = angVelA.dot(ax1);
     897//make sure that if no body -> angVelB == zero vec
     898//                        if (body1)
     899                            vel -= angVelB.dot(ax1);
    792900                    }
    793901                    else
    794902                    {
    795                         vel = body0->getLinearVelocity().dot(ax1);
    796                         if (body1)
    797                             vel -= body1->getLinearVelocity().dot(ax1);
     903                        vel = linVelA.dot(ax1);
     904//make sure that if no body -> angVelB == zero vec
     905//                        if (body1)
     906                            vel -= linVelB.dot(ax1);
    798907                    }
    799908                    // only apply bounce if the velocity is incoming, and if the
    … …  
    825934}
    826935
    827 //-----------------------------------------------------------------------------
    828 //-----------------------------------------------------------------------------
    829 //-----------------------------------------------------------------------------
     936
     937
     938
     939
     940
     941        ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
     942        ///If no axis is provided, it uses the default axis for this constraint.
     943void btGeneric6DofConstraint::setParam(int num, btScalar value, int axis)
     944{
     945        if((axis >= 0) && (axis < 3))
     946        {
     947                switch(num)
     948                {
     949                        case BT_CONSTRAINT_STOP_ERP :
     950                                m_linearLimits.m_stopERP[axis] = value;
     951                                m_flags |= BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
     952                                break;
     953                        case BT_CONSTRAINT_STOP_CFM :
     954                                m_linearLimits.m_stopCFM[axis] = value;
     955                                m_flags |= BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
     956                                break;
     957                        case BT_CONSTRAINT_CFM :
     958                                m_linearLimits.m_normalCFM[axis] = value;
     959                                m_flags |= BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
     960                                break;
     961                        default :
     962                                btAssertConstrParams(0);
     963                }
     964        }
     965        else if((axis >=3) && (axis < 6))
     966        {
     967                switch(num)
     968                {
     969                        case BT_CONSTRAINT_STOP_ERP :
     970                                m_angularLimits[axis - 3].m_stopERP = value;
     971                                m_flags |= BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
     972                                break;
     973                        case BT_CONSTRAINT_STOP_CFM :
     974                                m_angularLimits[axis - 3].m_stopCFM = value;
     975                                m_flags |= BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
     976                                break;
     977                        case BT_CONSTRAINT_CFM :
     978                                m_angularLimits[axis - 3].m_normalCFM = value;
     979                                m_flags |= BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
     980                                break;
     981                        default :
     982                                btAssertConstrParams(0);
     983                }
     984        }
     985        else
     986        {
     987                btAssertConstrParams(0);
     988        }
     989}
     990
     991        ///return the local value of parameter
     992btScalar btGeneric6DofConstraint::getParam(int num, int axis) const
     993{
     994        btScalar retVal = 0;
     995        if((axis >= 0) && (axis < 3))
     996        {
     997                switch(num)
     998                {
     999                        case BT_CONSTRAINT_STOP_ERP :
     1000                                btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
     1001                                retVal = m_linearLimits.m_stopERP[axis];
     1002                                break;
     1003                        case BT_CONSTRAINT_STOP_CFM :
     1004                                btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
     1005                                retVal = m_linearLimits.m_stopCFM[axis];
     1006                                break;
     1007                        case BT_CONSTRAINT_CFM :
     1008                                btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
     1009                                retVal = m_linearLimits.m_normalCFM[axis];
     1010                                break;
     1011                        default :
     1012                                btAssertConstrParams(0);
     1013                }
     1014        }
     1015        else if((axis >=3) && (axis < 6))
     1016        {
     1017                switch(num)
     1018                {
     1019                        case BT_CONSTRAINT_STOP_ERP :
     1020                                btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
     1021                                retVal = m_angularLimits[axis - 3].m_stopERP;
     1022                                break;
     1023                        case BT_CONSTRAINT_STOP_CFM :
     1024                                btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
     1025                                retVal = m_angularLimits[axis - 3].m_stopCFM;
     1026                                break;
     1027                        case BT_CONSTRAINT_CFM :
     1028                                btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
     1029                                retVal = m_angularLimits[axis - 3].m_normalCFM;
     1030                                break;
     1031                        default :
     1032                                btAssertConstrParams(0);
     1033                }
     1034        }
     1035        else
     1036        {
     1037                btAssertConstrParams(0);
     1038        }
     1039        return retVal;
     1040}

  • code/trunk/src/external/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h

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

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

    r5781 r8351  
    2222#include "btSolverBody.h"
    2323
    24 //-----------------------------------------------------------------------------
    25 
     24
     25
     26//#define HINGE_USE_OBSOLETE_SOLVER false
    2627#define HINGE_USE_OBSOLETE_SOLVER false
    2728
    28 //-----------------------------------------------------------------------------
    29 
    30 
    31 btHingeConstraint::btHingeConstraint()
    32 : 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 //-----------------------------------------------------------------------------
     29#define HINGE_USE_FRAME_OFFSET true
     30
     31#ifndef __SPU__
     32
     33
     34
     35
    4136
    4237btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,
    43                                                                          btVector3& axisInA,btVector3& axisInB, bool useReferenceFrameA)
     38                                                                         const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA)
    4439                                                                         :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),
    4540                                                                         m_angularOnly(false),
    4641                                                                         m_enableAngularMotor(false),
    4742                                                                         m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
    48                                                                          m_useReferenceFrameA(useReferenceFrameA)
     43                                                                         m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
     44                                                                         m_useReferenceFrameA(useReferenceFrameA),
     45                                                                         m_flags(0)
    4946{
    5047        m_rbAFrame.getOrigin() = pivotInA;
    … …  
    8077       
    8178        //start with free
    82         m_lowerLimit = btScalar(1e30);
    83         m_upperLimit = btScalar(-1e30);
     79        m_lowerLimit = btScalar(1.0f);
     80        m_upperLimit = btScalar(-1.0f);
    8481        m_biasFactor = 0.3f;
    8582        m_relaxationFactor = 1.0f;
    … …  
    8986}
    9087
    91 //-----------------------------------------------------------------------------
    92 
    93 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA, bool useReferenceFrameA)
     88
     89
     90btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA)
    9491:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false),
    9592m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
    96 m_useReferenceFrameA(useReferenceFrameA)
     93m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
     94m_useReferenceFrameA(useReferenceFrameA),
     95m_flags(0)
    9796{
    9897
    … …  
    120119       
    121120        //start with free
    122         m_lowerLimit = btScalar(1e30);
    123         m_upperLimit = btScalar(-1e30);
     121        m_lowerLimit = btScalar(1.0f);
     122        m_upperLimit = btScalar(-1.0f);
    124123        m_biasFactor = 0.3f;
    125124        m_relaxationFactor = 1.0f;
    … …  
    129128}
    130129
    131 //-----------------------------------------------------------------------------
     130
    132131
    133132btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB,
    … …  
    137136m_enableAngularMotor(false),
    138137m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
    139 m_useReferenceFrameA(useReferenceFrameA)
     138m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
     139m_useReferenceFrameA(useReferenceFrameA),
     140m_flags(0)
    140141{
    141142        //start with free
    142         m_lowerLimit = btScalar(1e30);
    143         m_upperLimit = btScalar(-1e30);
     143        m_lowerLimit = btScalar(1.0f);
     144        m_upperLimit = btScalar(-1.0f);
    144145        m_biasFactor = 0.3f;
    145146        m_relaxationFactor = 1.0f;
    … …  
    149150}                       
    150151
    151 //-----------------------------------------------------------------------------
     152
    152153
    153154btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame, bool useReferenceFrameA)
    … …  
    156157m_enableAngularMotor(false),
    157158m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
    158 m_useReferenceFrameA(useReferenceFrameA)
     159m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
     160m_useReferenceFrameA(useReferenceFrameA),
     161m_flags(0)
    159162{
    160163        ///not providing rigidbody B means implicitly using worldspace for body B
    … …  
    163166
    164167        //start with free
    165         m_lowerLimit = btScalar(1e30);
    166         m_upperLimit = btScalar(-1e30);
     168        m_lowerLimit = btScalar(1.0f);
     169        m_upperLimit = btScalar(-1.0f);
    167170        m_biasFactor = 0.3f;
    168171        m_relaxationFactor = 1.0f;
    … …  
    172175}
    173176
    174 //-----------------------------------------------------------------------------
     177
    175178
    176179void    btHingeConstraint::buildJacobian()
    … …  
    179182        {
    180183                m_appliedImpulse = btScalar(0.);
     184                m_accMotorImpulse = btScalar(0.);
    181185
    182186                if (!m_angularOnly)
    … …  
    222226                btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local);
    223227
    224                 getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
    225228                btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local;
    226229                btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local;
    … …  
    249252
    250253                        // test angular limit
    251                         testLimit();
     254                        testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
    252255
    253256                //Compute K = J*W*J' for hinge axis
    … …  
    259262}
    260263
    261 //-----------------------------------------------------------------------------
     264
     265#endif //__SPU__
     266
    262267
    263268void btHingeConstraint::getInfo1(btConstraintInfo1* info)
    … …  
    272277                info->m_numConstraintRows = 5; // Fixed 3 linear + 2 angular
    273278                info->nub = 1;
     279                //always add the row, to avoid computation (data is not available yet)
    274280                //prepare constraint
    275                 testLimit();
     281                testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
    276282                if(getSolveLimit() || getEnableAngularMotor())
    277283                {
    … …  
    279285                        info->nub--;
    280286                }
    281         }
    282 } // btHingeConstraint::getInfo1 ()
    283 
    284 //-----------------------------------------------------------------------------
     287
     288        }
     289}
     290
     291void btHingeConstraint::getInfo1NonVirtual(btConstraintInfo1* info)
     292{
     293        if (m_useSolveConstraintObsolete)
     294        {
     295                info->m_numConstraintRows = 0;
     296                info->nub = 0;
     297        }
     298        else
     299        {
     300                //always add the 'limit' row, to avoid computation (data is not available yet)
     301                info->m_numConstraintRows = 6; // Fixed 3 linear + 2 angular
     302                info->nub = 0;
     303        }
     304}
    285305
    286306void btHingeConstraint::getInfo2 (btConstraintInfo2* info)
    287307{
     308        if(m_useOffsetForConstraintFrame)
     309        {
     310                getInfo2InternalUsingFrameOffset(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getAngularVelocity(),m_rbB.getAngularVelocity());
     311        }
     312        else
     313        {
     314                getInfo2Internal(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getAngularVelocity(),m_rbB.getAngularVelocity());
     315        }
     316}
     317
     318
     319void    btHingeConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB)
     320{
     321        ///the regular (virtual) implementation getInfo2 already performs 'testLimit' during getInfo1, so we need to do it now
     322        testLimit(transA,transB);
     323
     324        getInfo2Internal(info,transA,transB,angVelA,angVelB);
     325}
     326
     327
     328void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB)
     329{
     330
    288331        btAssert(!m_useSolveConstraintObsolete);
    289         int i, s = info->rowskip;
     332        int i, skip = info->rowskip;
    290333        // transforms in world space
    291         btTransform trA = m_rbA.getCenterOfMassTransform()*m_rbAFrame;
    292         btTransform trB = m_rbB.getCenterOfMassTransform()*m_rbBFrame;
     334        btTransform trA = transA*m_rbAFrame;
     335        btTransform trB = transB*m_rbBFrame;
    293336        // pivot point
    294337        btVector3 pivotAInW = trA.getOrigin();
    295338        btVector3 pivotBInW = trB.getOrigin();
     339#if 0
     340        if (0)
     341        {
     342                for (i=0;i<6;i++)
     343                {
     344                        info->m_J1linearAxis[i*skip]=0;
     345                        info->m_J1linearAxis[i*skip+1]=0;
     346                        info->m_J1linearAxis[i*skip+2]=0;
     347
     348                        info->m_J1angularAxis[i*skip]=0;
     349                        info->m_J1angularAxis[i*skip+1]=0;
     350                        info->m_J1angularAxis[i*skip+2]=0;
     351
     352                        info->m_J2angularAxis[i*skip]=0;
     353                        info->m_J2angularAxis[i*skip+1]=0;
     354                        info->m_J2angularAxis[i*skip+2]=0;
     355
     356                        info->m_constraintError[i*skip]=0.f;
     357                }
     358        }
     359#endif //#if 0
    296360        // 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();
     361
     362        if (!m_angularOnly)
     363        {
     364                info->m_J1linearAxis[0] = 1;
     365                info->m_J1linearAxis[skip + 1] = 1;
     366                info->m_J1linearAxis[2 * skip + 2] = 1;
     367        }       
     368
     369
     370
     371
     372        btVector3 a1 = pivotAInW - transA.getOrigin();
    301373        {
    302374                btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
    303                 btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + s);
    304                 btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * s);
     375                btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + skip);
     376                btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * skip);
    305377                btVector3 a1neg = -a1;
    306378                a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
    307379        }
    308         btVector3 a2 = pivotBInW - m_rbB.getCenterOfMassTransform().getOrigin();
     380        btVector3 a2 = pivotBInW - transB.getOrigin();
    309381        {
    310382                btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
    311                 btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + s);
    312                 btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * s);
     383                btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + skip);
     384                btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * skip);
    313385                a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
    314386        }
    315387        // linear RHS
    316388    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     }
     389        if (!m_angularOnly)
     390        {
     391                for(i = 0; i < 3; i++)
     392                {
     393                        info->m_constraintError[i * skip] = k * (pivotBInW[i] - pivotAInW[i]);
     394                }
     395        }
    321396        // make rotations around X and Y equal
    322397        // the hinge axis should be the only unconstrained
    … …  
    403478                }
    404479                info->m_constraintError[srow] = btScalar(0.0f);
     480                btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp;
    405481                if(powered)
    406482                {
    407             info->cfm[srow] = btScalar(0.0);
    408                         btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * info->erp);
     483                        if(m_flags & BT_HINGE_FLAGS_CFM_NORM)
     484                        {
     485                                info->cfm[srow] = m_normalCFM;
     486                        }
     487                        btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP);
    409488                        info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
    410489                        info->m_lowerLimit[srow] = - m_maxMotorImpulse;
    … …  
    413492                if(limit)
    414493                {
    415                         k = info->fps * info->erp;
     494                        k = info->fps * currERP;
    416495                        info->m_constraintError[srow] += k * limit_err;
    417                         info->cfm[srow] = btScalar(0.0);
     496                        if(m_flags & BT_HINGE_FLAGS_CFM_STOP)
     497                        {
     498                                info->cfm[srow] = m_stopCFM;
     499                        }
    418500                        if(lostop == histop)
    419501                        {
    … …  
    436518                        if(bounce > btScalar(0.0))
    437519                        {
    438                                 btScalar vel = m_rbA.getAngularVelocity().dot(ax1);
    439                                 vel -= m_rbB.getAngularVelocity().dot(ax1);
     520                                btScalar vel = angVelA.dot(ax1);
     521                                vel -= angVelB.dot(ax1);
    440522                                // only apply bounce if the velocity is incoming, and if the
    441523                                // resulting c[] exceeds what we already have.
    … …  
    468550}
    469551
    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 //-----------------------------------------------------------------------------
     552
     553
     554
     555
    619556
    620557void    btHingeConstraint::updateRHS(btScalar   timeStep)
    … …  
    624561}
    625562
    626 //-----------------------------------------------------------------------------
    627563
    628564btScalar btHingeConstraint::getHingeAngle()
    629565{
    630         const btVector3 refAxis0  = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0);
    631         const btVector3 refAxis1  = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1);
    632         const btVector3 swingAxis = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(1);
    633         btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
     566        return getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
     567}
     568
     569btScalar btHingeConstraint::getHingeAngle(const btTransform& transA,const btTransform& transB)
     570{
     571        const btVector3 refAxis0  = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0);
     572        const btVector3 refAxis1  = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1);
     573        const btVector3 swingAxis = transB.getBasis() * m_rbBFrame.getBasis().getColumn(1);
     574//      btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
     575        btScalar angle = btAtan2(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
    634576        return m_referenceSign * angle;
    635577}
    636578
    637 //-----------------------------------------------------------------------------
    638 
     579
     580#if 0
    639581void btHingeConstraint::testLimit()
    640582{
    … …  
    660602        }
    661603        return;
    662 } // btHingeConstraint::testLimit()
    663 
    664 //-----------------------------------------------------------------------------
    665 //-----------------------------------------------------------------------------
    666 //-----------------------------------------------------------------------------
     604}
     605#else
     606
     607
     608void btHingeConstraint::testLimit(const btTransform& transA,const btTransform& transB)
     609{
     610        // Compute limit information
     611        m_hingeAngle = getHingeAngle(transA,transB);
     612        m_correction = btScalar(0.);
     613        m_limitSign = btScalar(0.);
     614        m_solveLimit = false;
     615        if (m_lowerLimit <= m_upperLimit)
     616        {
     617                m_hingeAngle = btAdjustAngleToLimits(m_hingeAngle, m_lowerLimit, m_upperLimit);
     618                if (m_hingeAngle <= m_lowerLimit)
     619                {
     620                        m_correction = (m_lowerLimit - m_hingeAngle);
     621                        m_limitSign = 1.0f;
     622                        m_solveLimit = true;
     623                }
     624                else if (m_hingeAngle >= m_upperLimit)
     625                {
     626                        m_correction = m_upperLimit - m_hingeAngle;
     627                        m_limitSign = -1.0f;
     628                        m_solveLimit = true;
     629                }
     630        }
     631        return;
     632}
     633#endif
     634
     635static btVector3 vHinge(0, 0, btScalar(1));
     636
     637void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt)
     638{
     639        // convert target from body to constraint space
     640        btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * qAinB * m_rbAFrame.getRotation();
     641        qConstraint.normalize();
     642
     643        // extract "pure" hinge component
     644        btVector3 vNoHinge = quatRotate(qConstraint, vHinge); vNoHinge.normalize();
     645        btQuaternion qNoHinge = shortestArcQuat(vHinge, vNoHinge);
     646        btQuaternion qHinge = qNoHinge.inverse() * qConstraint;
     647        qHinge.normalize();
     648
     649        // compute angular target, clamped to limits
     650        btScalar targetAngle = qHinge.getAngle();
     651        if (targetAngle > SIMD_PI) // long way around. flip quat and recalculate.
     652        {
     653                qHinge = operator-(qHinge);
     654                targetAngle = qHinge.getAngle();
     655        }
     656        if (qHinge.getZ() < 0)
     657                targetAngle = -targetAngle;
     658
     659        setMotorTarget(targetAngle, dt);
     660}
     661
     662void btHingeConstraint::setMotorTarget(btScalar targetAngle, btScalar dt)
     663{
     664        if (m_lowerLimit < m_upperLimit)
     665        {
     666                if (targetAngle < m_lowerLimit)
     667                        targetAngle = m_lowerLimit;
     668                else if (targetAngle > m_upperLimit)
     669                        targetAngle = m_upperLimit;
     670        }
     671
     672        // compute angular velocity
     673        btScalar curAngle  = getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
     674        btScalar dAngle = targetAngle - curAngle;
     675        m_motorTargetVelocity = dAngle / dt;
     676}
     677
     678
     679
     680void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB)
     681{
     682        btAssert(!m_useSolveConstraintObsolete);
     683        int i, s = info->rowskip;
     684        // transforms in world space
     685        btTransform trA = transA*m_rbAFrame;
     686        btTransform trB = transB*m_rbBFrame;
     687        // pivot point
     688        btVector3 pivotAInW = trA.getOrigin();
     689        btVector3 pivotBInW = trB.getOrigin();
     690#if 1
     691        // difference between frames in WCS
     692        btVector3 ofs = trB.getOrigin() - trA.getOrigin();
     693        // now get weight factors depending on masses
     694        btScalar miA = getRigidBodyA().getInvMass();
     695        btScalar miB = getRigidBodyB().getInvMass();
     696        bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
     697        btScalar miS = miA + miB;
     698        btScalar factA, factB;
     699        if(miS > btScalar(0.f))
     700        {
     701                factA = miB / miS;
     702        }
     703        else
     704        {
     705                factA = btScalar(0.5f);
     706        }
     707        factB = btScalar(1.0f) - factA;
     708        // get the desired direction of hinge axis
     709        // as weighted sum of Z-orthos of frameA and frameB in WCS
     710        btVector3 ax1A = trA.getBasis().getColumn(2);
     711        btVector3 ax1B = trB.getBasis().getColumn(2);
     712        btVector3 ax1 = ax1A * factA + ax1B * factB;
     713        ax1.normalize();
     714        // fill first 3 rows
     715        // we want: velA + wA x relA == velB + wB x relB
     716        btTransform bodyA_trans = transA;
     717        btTransform bodyB_trans = transB;
     718        int s0 = 0;
     719        int s1 = s;
     720        int s2 = s * 2;
     721        int nrow = 2; // last filled row
     722        btVector3 tmpA, tmpB, relA, relB, p, q;
     723        // get vector from bodyB to frameB in WCS
     724        relB = trB.getOrigin() - bodyB_trans.getOrigin();
     725        // get its projection to hinge axis
     726        btVector3 projB = ax1 * relB.dot(ax1);
     727        // get vector directed from bodyB to hinge axis (and orthogonal to it)
     728        btVector3 orthoB = relB - projB;
     729        // same for bodyA
     730        relA = trA.getOrigin() - bodyA_trans.getOrigin();
     731        btVector3 projA = ax1 * relA.dot(ax1);
     732        btVector3 orthoA = relA - projA;
     733        btVector3 totalDist = projA - projB;
     734        // get offset vectors relA and relB
     735        relA = orthoA + totalDist * factA;
     736        relB = orthoB - totalDist * factB;
     737        // now choose average ortho to hinge axis
     738        p = orthoB * factA + orthoA * factB;
     739        btScalar len2 = p.length2();
     740        if(len2 > SIMD_EPSILON)
     741        {
     742                p /= btSqrt(len2);
     743        }
     744        else
     745        {
     746                p = trA.getBasis().getColumn(1);
     747        }
     748        // make one more ortho
     749        q = ax1.cross(p);
     750        // fill three rows
     751        tmpA = relA.cross(p);
     752        tmpB = relB.cross(p);
     753    for (i=0; i<3; i++) info->m_J1angularAxis[s0+i] = tmpA[i];
     754    for (i=0; i<3; i++) info->m_J2angularAxis[s0+i] = -tmpB[i];
     755        tmpA = relA.cross(q);
     756        tmpB = relB.cross(q);
     757        if(hasStaticBody && getSolveLimit())
     758        { // to make constraint between static and dynamic objects more rigid
     759                // remove wA (or wB) from equation if angular limit is hit
     760                tmpB *= factB;
     761                tmpA *= factA;
     762        }
     763        for (i=0; i<3; i++) info->m_J1angularAxis[s1+i] = tmpA[i];
     764    for (i=0; i<3; i++) info->m_J2angularAxis[s1+i] = -tmpB[i];
     765        tmpA = relA.cross(ax1);
     766        tmpB = relB.cross(ax1);
     767        if(hasStaticBody)
     768        { // to make constraint between static and dynamic objects more rigid
     769                // remove wA (or wB) from equation
     770                tmpB *= factB;
     771                tmpA *= factA;
     772        }
     773        for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i];
     774    for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i];
     775
     776        btScalar k = info->fps * info->erp;
     777
     778        if (!m_angularOnly)
     779        {
     780                for (i=0; i<3; i++) info->m_J1linearAxis[s0+i] = p[i];
     781                for (i=0; i<3; i++) info->m_J1linearAxis[s1+i] = q[i];
     782                for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = ax1[i];
     783       
     784        // compute three elements of right hand side
     785       
     786                btScalar rhs = k * p.dot(ofs);
     787                info->m_constraintError[s0] = rhs;
     788                rhs = k * q.dot(ofs);
     789                info->m_constraintError[s1] = rhs;
     790                rhs = k * ax1.dot(ofs);
     791                info->m_constraintError[s2] = rhs;
     792        }
     793        // the hinge axis should be the only unconstrained
     794        // rotational axis, the angular velocity of the two bodies perpendicular to
     795        // the hinge axis should be equal. thus the constraint equations are
     796        //    p*w1 - p*w2 = 0
     797        //    q*w1 - q*w2 = 0
     798        // where p and q are unit vectors normal to the hinge axis, and w1 and w2
     799        // are the angular velocity vectors of the two bodies.
     800        int s3 = 3 * s;
     801        int s4 = 4 * s;
     802        info->m_J1angularAxis[s3 + 0] = p[0];
     803        info->m_J1angularAxis[s3 + 1] = p[1];
     804        info->m_J1angularAxis[s3 + 2] = p[2];
     805        info->m_J1angularAxis[s4 + 0] = q[0];
     806        info->m_J1angularAxis[s4 + 1] = q[1];
     807        info->m_J1angularAxis[s4 + 2] = q[2];
     808
     809        info->m_J2angularAxis[s3 + 0] = -p[0];
     810        info->m_J2angularAxis[s3 + 1] = -p[1];
     811        info->m_J2angularAxis[s3 + 2] = -p[2];
     812        info->m_J2angularAxis[s4 + 0] = -q[0];
     813        info->m_J2angularAxis[s4 + 1] = -q[1];
     814        info->m_J2angularAxis[s4 + 2] = -q[2];
     815        // compute the right hand side of the constraint equation. set relative
     816        // body velocities along p and q to bring the hinge back into alignment.
     817        // if ax1A,ax1B are the unit length hinge axes as computed from bodyA and
     818        // bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2).
     819        // if "theta" is the angle between ax1 and ax2, we need an angular velocity
     820        // along u to cover angle erp*theta in one step :
     821        //   |angular_velocity| = angle/time = erp*theta / stepsize
     822        //                      = (erp*fps) * theta
     823        //    angular_velocity  = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
     824        //                      = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
     825        // ...as ax1 and ax2 are unit length. if theta is smallish,
     826        // theta ~= sin(theta), so
     827        //    angular_velocity  = (erp*fps) * (ax1 x ax2)
     828        // ax1 x ax2 is in the plane space of ax1, so we project the angular
     829        // velocity to p and q to find the right hand side.
     830        k = info->fps * info->erp;
     831        btVector3 u = ax1A.cross(ax1B);
     832        info->m_constraintError[s3] = k * u.dot(p);
     833        info->m_constraintError[s4] = k * u.dot(q);
     834#endif
     835        // check angular limits
     836        nrow = 4; // last filled row
     837        int srow;
     838        btScalar limit_err = btScalar(0.0);
     839        int limit = 0;
     840        if(getSolveLimit())
     841        {
     842                limit_err = m_correction * m_referenceSign;
     843                limit = (limit_err > btScalar(0.0)) ? 1 : 2;
     844        }
     845        // if the hinge has joint limits or motor, add in the extra row
     846        int powered = 0;
     847        if(getEnableAngularMotor())
     848        {
     849                powered = 1;
     850        }
     851        if(limit || powered)
     852        {
     853                nrow++;
     854                srow = nrow * info->rowskip;
     855                info->m_J1angularAxis[srow+0] = ax1[0];
     856                info->m_J1angularAxis[srow+1] = ax1[1];
     857                info->m_J1angularAxis[srow+2] = ax1[2];
     858
     859                info->m_J2angularAxis[srow+0] = -ax1[0];
     860                info->m_J2angularAxis[srow+1] = -ax1[1];
     861                info->m_J2angularAxis[srow+2] = -ax1[2];
     862
     863                btScalar lostop = getLowerLimit();
     864                btScalar histop = getUpperLimit();
     865                if(limit && (lostop == histop))
     866                {  // the joint motor is ineffective
     867                        powered = 0;
     868                }
     869                info->m_constraintError[srow] = btScalar(0.0f);
     870                btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp;
     871                if(powered)
     872                {
     873                        if(m_flags & BT_HINGE_FLAGS_CFM_NORM)
     874                        {
     875                                info->cfm[srow] = m_normalCFM;
     876                        }
     877                        btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP);
     878                        info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
     879                        info->m_lowerLimit[srow] = - m_maxMotorImpulse;
     880                        info->m_upperLimit[srow] =   m_maxMotorImpulse;
     881                }
     882                if(limit)
     883                {
     884                        k = info->fps * currERP;
     885                        info->m_constraintError[srow] += k * limit_err;
     886                        if(m_flags & BT_HINGE_FLAGS_CFM_STOP)
     887                        {
     888                                info->cfm[srow] = m_stopCFM;
     889                        }
     890                        if(lostop == histop)
     891                        {
     892                                // limited low and high simultaneously
     893                                info->m_lowerLimit[srow] = -SIMD_INFINITY;
     894                                info->m_upperLimit[srow] = SIMD_INFINITY;
     895                        }
     896                        else if(limit == 1)
     897                        { // low limit
     898                                info->m_lowerLimit[srow] = 0;
     899                                info->m_upperLimit[srow] = SIMD_INFINITY;
     900                        }
     901                        else
     902                        { // high limit
     903                                info->m_lowerLimit[srow] = -SIMD_INFINITY;
     904                                info->m_upperLimit[srow] = 0;
     905                        }
     906                        // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
     907                        btScalar bounce = m_relaxationFactor;
     908                        if(bounce > btScalar(0.0))
     909                        {
     910                                btScalar vel = angVelA.dot(ax1);
     911                                vel -= angVelB.dot(ax1);
     912                                // only apply bounce if the velocity is incoming, and if the
     913                                // resulting c[] exceeds what we already have.
     914                                if(limit == 1)
     915                                {       // low limit
     916                                        if(vel < 0)
     917                                        {
     918                                                btScalar newc = -bounce * vel;
     919                                                if(newc > info->m_constraintError[srow])
     920                                                {
     921                                                        info->m_constraintError[srow] = newc;
     922                                                }
     923                                        }
     924                                }
     925                                else
     926                                {       // high limit - all those computations are reversed
     927                                        if(vel > 0)
     928                                        {
     929                                                btScalar newc = -bounce * vel;
     930                                                if(newc < info->m_constraintError[srow])
     931                                                {
     932                                                        info->m_constraintError[srow] = newc;
     933                                                }
     934                                        }
     935                                }
     936                        }
     937                        info->m_constraintError[srow] *= m_biasFactor;
     938                } // if(limit)
     939        } // if angular limit or powered
     940}
     941
     942
     943///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
     944///If no axis is provided, it uses the default axis for this constraint.
     945void btHingeConstraint::setParam(int num, btScalar value, int axis)
     946{
     947        if((axis == -1) || (axis == 5))
     948        {
     949                switch(num)
     950                {       
     951                        case BT_CONSTRAINT_STOP_ERP :
     952                                m_stopERP = value;
     953                                m_flags |= BT_HINGE_FLAGS_ERP_STOP;
     954                                break;
     955                        case BT_CONSTRAINT_STOP_CFM :
     956                                m_stopCFM = value;
     957                                m_flags |= BT_HINGE_FLAGS_CFM_STOP;
     958                                break;
     959                        case BT_CONSTRAINT_CFM :
     960                                m_normalCFM = value;
     961                                m_flags |= BT_HINGE_FLAGS_CFM_NORM;
     962                                break;
     963                        default :
     964                                btAssertConstrParams(0);
     965                }
     966        }
     967        else
     968        {
     969                btAssertConstrParams(0);
     970        }
     971}
     972
     973///return the local value of parameter
     974btScalar btHingeConstraint::getParam(int num, int axis) const
     975{
     976        btScalar retVal = 0;
     977        if((axis == -1) || (axis == 5))
     978        {
     979                switch(num)
     980                {       
     981                        case BT_CONSTRAINT_STOP_ERP :
     982                                btAssertConstrParams(m_flags & BT_HINGE_FLAGS_ERP_STOP);
     983                                retVal = m_stopERP;
     984                                break;
     985                        case BT_CONSTRAINT_STOP_CFM :
     986                                btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_STOP);
     987                                retVal = m_stopCFM;
     988                                break;
     989                        case BT_CONSTRAINT_CFM :
     990                                btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_NORM);
     991                                retVal = m_normalCFM;
     992                                break;
     993                        default :
     994                                btAssertConstrParams(0);
     995                }
     996        }
     997        else
     998        {
     999                btAssertConstrParams(0);
     1000        }
     1001        return retVal;
     1002}
     1003
     1004

  • code/trunk/src/external/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.h

    r5781 r8351  
    2525class btRigidBody;
    2626
     27#ifdef BT_USE_DOUBLE_PRECISION
     28#define btHingeConstraintData   btHingeConstraintDoubleData
     29#define btHingeConstraintDataName       "btHingeConstraintDoubleData"
     30#else
     31#define btHingeConstraintData   btHingeConstraintFloatData
     32#define btHingeConstraintDataName       "btHingeConstraintFloatData"
     33#endif //BT_USE_DOUBLE_PRECISION
     34
     35
     36enum btHingeFlags
     37{
     38        BT_HINGE_FLAGS_CFM_STOP = 1,
     39        BT_HINGE_FLAGS_ERP_STOP = 2,
     40        BT_HINGE_FLAGS_CFM_NORM = 4
     41};
     42
     43
    2744/// hinge constraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
    2845/// axis defines the orientation of the hinge axis
    29 class btHingeConstraint : public btTypedConstraint
     46ATTRIBUTE_ALIGNED16(class) btHingeConstraint : public btTypedConstraint
    3047{
    3148#ifdef IN_PARALLELL_SOLVER
    … …  
    6178        bool            m_solveLimit;
    6279        bool            m_useSolveConstraintObsolete;
     80        bool            m_useOffsetForConstraintFrame;
    6381        bool            m_useReferenceFrameA;
    6482
     83        btScalar        m_accMotorImpulse;
     84
     85        int                     m_flags;
     86        btScalar        m_normalCFM;
     87        btScalar        m_stopCFM;
     88        btScalar        m_stopERP;
     89
    6590       
    6691public:
    6792
    68         btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, btVector3& axisInA,btVector3& axisInB, bool useReferenceFrameA = false);
    69 
    70         btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA, bool useReferenceFrameA = false);
     93        btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA = false);
     94
     95        btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA = false);
    7196       
    7297        btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false);
    … …  
    7499        btHingeConstraint(btRigidBody& rbA,const btTransform& rbAFrame, bool useReferenceFrameA = false);
    75100
    76         btHingeConstraint();
    77101
    78102        virtual void    buildJacobian();
    … …  
    80104        virtual void getInfo1 (btConstraintInfo1* info);
    81105
     106        void getInfo1NonVirtual(btConstraintInfo1* info);
     107
    82108        virtual void getInfo2 (btConstraintInfo2* info);
    83        
    84         virtual void    solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar        timeStep);
     109
     110        void    getInfo2NonVirtual(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB);
     111
     112        void    getInfo2Internal(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB);
     113        void    getInfo2InternalUsingFrameOffset(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB);
     114               
    85115
    86116        void    updateRHS(btScalar      timeStep);
    … …  
    117147        }
    118148
     149        // extra motor API, including ability to set a target rotation (as opposed to angular velocity)
     150        // note: setMotorTarget sets angular velocity under the hood, so you must call it every tick to
     151        //       maintain a given angular target.
     152        void enableMotor(bool enableMotor)      { m_enableAngularMotor = enableMotor; }
     153        void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; }
     154        void setMotorTarget(const btQuaternion& qAinB, btScalar dt); // qAinB is rotation of body A wrt body B.
     155        void setMotorTarget(btScalar targetAngle, btScalar dt);
     156
     157
    119158        void    setLimit(btScalar low,btScalar high,btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
    120159        {
    121                 m_lowerLimit = low;
    122                 m_upperLimit = high;
     160                m_lowerLimit = btNormalizeAngle(low);
     161                m_upperLimit = btNormalizeAngle(high);
    123162
    124163                m_limitSoftness =  _softness;
    … …  
    128167        }
    129168
     169        void    setAxis(btVector3& axisInA)
     170        {
     171                btVector3 rbAxisA1, rbAxisA2;
     172                btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2);
     173                btVector3 pivotInA = m_rbAFrame.getOrigin();
     174//              m_rbAFrame.getOrigin() = pivotInA;
     175                m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
     176                                                                                rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
     177                                                                                rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
     178
     179                btVector3 axisInB = m_rbA.getCenterOfMassTransform().getBasis() * axisInA;
     180
     181                btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
     182                btVector3 rbAxisB1 =  quatRotate(rotationArc,rbAxisA1);
     183                btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
     184
     185
     186                m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(pivotInA);
     187                m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
     188                                                                                rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
     189                                                                                rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
     190        }
     191
    130192        btScalar        getLowerLimit() const
    131193        {
    … …  
    141203        btScalar getHingeAngle();
    142204
    143         void testLimit();
    144 
    145 
    146         const btTransform& getAFrame() { return m_rbAFrame; }; 
    147         const btTransform& getBFrame() { return m_rbBFrame; };
     205        btScalar getHingeAngle(const btTransform& transA,const btTransform& transB);
     206
     207        void testLimit(const btTransform& transA,const btTransform& transB);
     208
     209
     210        const btTransform& getAFrame() const { return m_rbAFrame; };   
     211        const btTransform& getBFrame() const { return m_rbBFrame; };
     212
     213        btTransform& getAFrame() { return m_rbAFrame; };       
     214        btTransform& getBFrame() { return m_rbBFrame; };
    148215
    149216        inline int getSolveLimit()
    … …  
    173240                return m_maxMotorImpulse;
    174241        }
     242        // access for UseFrameOffset
     243        bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
     244        void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
     245
     246
     247        ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
     248        ///If no axis is provided, it uses the default axis for this constraint.
     249        virtual void    setParam(int num, btScalar value, int axis = -1);
     250        ///return the local value of parameter
     251        virtual btScalar getParam(int num, int axis = -1) const;
     252
     253        virtual int     calculateSerializeBufferSize() const;
     254
     255        ///fills the dataBuffer and returns the struct name (and 0 on failure)
     256        virtual const char*     serialize(void* dataBuffer, btSerializer* serializer) const;
     257
    175258
    176259};
    177260
     261///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
     262struct  btHingeConstraintDoubleData
     263{
     264        btTypedConstraintData   m_typeConstraintData;
     265        btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
     266        btTransformDoubleData m_rbBFrame;
     267        int                     m_useReferenceFrameA;
     268        int                     m_angularOnly;
     269        int                     m_enableAngularMotor;
     270        float   m_motorTargetVelocity;
     271        float   m_maxMotorImpulse;
     272
     273        float   m_lowerLimit;
     274        float   m_upperLimit;
     275        float   m_limitSoftness;
     276        float   m_biasFactor;
     277        float   m_relaxationFactor;
     278
     279};
     280///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
     281struct  btHingeConstraintFloatData
     282{
     283        btTypedConstraintData   m_typeConstraintData;
     284        btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
     285        btTransformFloatData m_rbBFrame;
     286        int                     m_useReferenceFrameA;
     287        int                     m_angularOnly;
     288       
     289        int                     m_enableAngularMotor;
     290        float   m_motorTargetVelocity;
     291        float   m_maxMotorImpulse;
     292
     293        float   m_lowerLimit;
     294        float   m_upperLimit;
     295        float   m_limitSoftness;
     296        float   m_biasFactor;
     297        float   m_relaxationFactor;
     298
     299};
     300
     301
     302
     303SIMD_FORCE_INLINE       int     btHingeConstraint::calculateSerializeBufferSize() const
     304{
     305        return sizeof(btHingeConstraintData);
     306}
     307
     308        ///fills the dataBuffer and returns the struct name (and 0 on failure)
     309SIMD_FORCE_INLINE       const char*     btHingeConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
     310{
     311        btHingeConstraintData* hingeData = (btHingeConstraintData*)dataBuffer;
     312        btTypedConstraint::serialize(&hingeData->m_typeConstraintData,serializer);
     313
     314        m_rbAFrame.serialize(hingeData->m_rbAFrame);
     315        m_rbBFrame.serialize(hingeData->m_rbBFrame);
     316
     317        hingeData->m_angularOnly = m_angularOnly;
     318        hingeData->m_enableAngularMotor = m_enableAngularMotor;
     319        hingeData->m_maxMotorImpulse = float(m_maxMotorImpulse);
     320        hingeData->m_motorTargetVelocity = float(m_motorTargetVelocity);
     321        hingeData->m_useReferenceFrameA = m_useReferenceFrameA;
     322       
     323        hingeData->m_lowerLimit = float(m_lowerLimit);
     324        hingeData->m_upperLimit = float(m_upperLimit);
     325        hingeData->m_limitSoftness = float(m_limitSoftness);
     326        hingeData->m_biasFactor = float(m_biasFactor);
     327        hingeData->m_relaxationFactor = float(m_relaxationFactor);
     328
     329        return btHingeConstraintDataName;
     330}
     331
    178332#endif //HINGECONSTRAINT_H

  • code/trunk/src/external/bullet/BulletDynamics/ConstraintSolver/btJacobianEntry.h

    r5781 r8351  
    2929/// it can be used in combination with a constraint solver
    3030/// Can be used to relate the effect of an impulse to the constraint error
    31 class btJacobianEntry
     31ATTRIBUTE_ALIGNED16(class) btJacobianEntry
    3232{
    3333public:

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

    r5781 r8351  
    2121
    2222
    23 btPoint2PointConstraint::btPoint2PointConstraint()
    24 :btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE),
    25 m_useSolveConstraintObsolete(false)
    26 {
    27 }
     23
    2824
    2925btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB)
    3026:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB),
     27m_flags(0),
    3128m_useSolveConstraintObsolete(false)
    3229{
    … …  
    3734btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA)
    3835:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)),
     36m_flags(0),
    3937m_useSolveConstraintObsolete(false)
    4038{
    … …  
    4442void    btPoint2PointConstraint::buildJacobian()
    4543{
     44
    4645        ///we need it for both methods
    4746        {
    … …  
    6766        }
    6867
    69 }
    70 
     68
     69}
    7170
    7271void btPoint2PointConstraint::getInfo1 (btConstraintInfo1* info)
     72{
     73        getInfo1NonVirtual(info);
     74}
     75
     76void btPoint2PointConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
    7377{
    7478        if (m_useSolveConstraintObsolete)
    … …  
    8387}
    8488
     89
     90
     91
    8592void btPoint2PointConstraint::getInfo2 (btConstraintInfo2* info)
    8693{
     94        getInfo2NonVirtual(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
     95}
     96
     97void btPoint2PointConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& body0_trans, const btTransform& body1_trans)
     98{
    8799        btAssert(!m_useSolveConstraintObsolete);
    88100
    89101         //retrieve matrices
    90         btTransform body0_trans;
    91         body0_trans = m_rbA.getCenterOfMassTransform();
    92     btTransform body1_trans;
    93         body1_trans = m_rbB.getCenterOfMassTransform();
    94102
    95103        // anchor points in global coordinates with respect to body PORs.
    … …  
    97105    // set jacobian
    98106    info->m_J1linearAxis[0] = 1;
    99     info->m_J1linearAxis[info->rowskip+1] = 1;
    100     info->m_J1linearAxis[2*info->rowskip+2] = 1;
     107        info->m_J1linearAxis[info->rowskip+1] = 1;
     108        info->m_J1linearAxis[2*info->rowskip+2] = 1;
    101109
    102110        btVector3 a1 = body0_trans.getBasis()*getPivotInA();
    … …  
    127135
    128136    // set right hand side
    129     btScalar k = info->fps * info->erp;
     137        btScalar currERP = (m_flags & BT_P2P_FLAGS_ERP) ? m_erp : info->erp;
     138    btScalar k = info->fps * currERP;
    130139    int j;
    131 
    132140        for (j=0; j<3; j++)
    133141    {
    134         info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] -                     a1[j] - body0_trans.getOrigin()[j]);
     142        info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
    135143                //printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
    136144    }
     145        if(m_flags & BT_P2P_FLAGS_CFM)
     146        {
     147                for (j=0; j<3; j++)
     148                {
     149                        info->cfm[j*info->rowskip] = m_cfm;
     150                }
     151        }
    137152
    138153        btScalar impulseClamp = m_setting.m_impulseClamp;//
    … …  
    145160                }
    146161        }
    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);
    160                
    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                 }
    222         }
    223 }
     162        info->m_damping = m_setting.m_damping;
     163       
     164}
     165
     166
    224167
    225168void    btPoint2PointConstraint::updateRHS(btScalar     timeStep)
    … …  
    229172}
    230173
     174///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
     175///If no axis is provided, it uses the default axis for this constraint.
     176void btPoint2PointConstraint::setParam(int num, btScalar value, int axis)
     177{
     178        if(axis != -1)
     179        {
     180                btAssertConstrParams(0);
     181        }
     182        else
     183        {
     184                switch(num)
     185                {
     186                        case BT_CONSTRAINT_ERP :
     187                        case BT_CONSTRAINT_STOP_ERP :
     188                                m_erp = value;
     189                                m_flags |= BT_P2P_FLAGS_ERP;
     190                                break;
     191                        case BT_CONSTRAINT_CFM :
     192                        case BT_CONSTRAINT_STOP_CFM :
     193                                m_cfm = value;
     194                                m_flags |= BT_P2P_FLAGS_CFM;
     195                                break;
     196                        default:
     197                                btAssertConstrParams(0);
     198                }
     199        }
     200}
     201
     202///return the local value of parameter
     203btScalar btPoint2PointConstraint::getParam(int num, int axis) const
     204{
     205        btScalar retVal(SIMD_INFINITY);
     206        if(axis != -1)
     207        {
     208                btAssertConstrParams(0);
     209        }
     210        else
     211        {
     212                switch(num)
     213                {
     214                        case BT_CONSTRAINT_ERP :
     215                        case BT_CONSTRAINT_STOP_ERP :
     216                                btAssertConstrParams(m_flags & BT_P2P_FLAGS_ERP);
     217                                retVal = m_erp;
     218                                break;
     219                        case BT_CONSTRAINT_CFM :
     220                        case BT_CONSTRAINT_STOP_CFM :
     221                                btAssertConstrParams(m_flags & BT_P2P_FLAGS_CFM);
     222                                retVal = m_cfm;
     223                                break;
     224                        default:
     225                                btAssertConstrParams(0);
     226                }
     227        }
     228        return retVal;
     229}
     230       

  • code/trunk/src/external/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h

    r5781 r8351  
    2323class btRigidBody;
    2424
     25
     26#ifdef BT_USE_DOUBLE_PRECISION
     27#define btPoint2PointConstraintData     btPoint2PointConstraintDoubleData
     28#define btPoint2PointConstraintDataName "btPoint2PointConstraintDoubleData"
     29#else
     30#define btPoint2PointConstraintData     btPoint2PointConstraintFloatData
     31#define btPoint2PointConstraintDataName "btPoint2PointConstraintFloatData"
     32#endif //BT_USE_DOUBLE_PRECISION
     33
    2534struct  btConstraintSetting
    2635{
    … …  
    3645};
    3746
     47enum btPoint2PointFlags
     48{
     49        BT_P2P_FLAGS_ERP = 1,
     50        BT_P2P_FLAGS_CFM = 2
     51};
     52
    3853/// point to point constraint between two rigidbodies each with a pivotpoint that descibes the 'ballsocket' location in local space
    39 class btPoint2PointConstraint : public btTypedConstraint
     54ATTRIBUTE_ALIGNED16(class) btPoint2PointConstraint : public btTypedConstraint
    4055{
    4156#ifdef IN_PARALLELL_SOLVER
    … …  
    4762        btVector3       m_pivotInB;
    4863       
    49        
     64        int                     m_flags;
     65        btScalar        m_erp;
     66        btScalar        m_cfm;
    5067       
    5168public:
    … …  
    6077        btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA);
    6178
    62         btPoint2PointConstraint();
    6379
    6480        virtual void    buildJacobian();
    … …  
    6682        virtual void getInfo1 (btConstraintInfo1* info);
    6783
     84        void getInfo1NonVirtual (btConstraintInfo1* info);
     85
    6886        virtual void getInfo2 (btConstraintInfo2* info);
    6987
    70 
    71         virtual void    solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar        timeStep);
     88        void getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& body0_trans, const btTransform& body1_trans);
    7289
    7390        void    updateRHS(btScalar      timeStep);
    … …  
    93110        }
    94111
     112        ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
     113        ///If no axis is provided, it uses the default axis for this constraint.
     114        virtual void    setParam(int num, btScalar value, int axis = -1);
     115        ///return the local value of parameter
     116        virtual btScalar getParam(int num, int axis = -1) const;
     117
     118        virtual int     calculateSerializeBufferSize() const;
     119
     120        ///fills the dataBuffer and returns the struct name (and 0 on failure)
     121        virtual const char*     serialize(void* dataBuffer, btSerializer* serializer) const;
     122
    95123
    96124};
    97125
     126///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
     127struct  btPoint2PointConstraintFloatData
     128{
     129        btTypedConstraintData   m_typeConstraintData;
     130        btVector3FloatData      m_pivotInA;
     131        btVector3FloatData      m_pivotInB;
     132};
     133
     134///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
     135struct  btPoint2PointConstraintDoubleData
     136{
     137        btTypedConstraintData   m_typeConstraintData;
     138        btVector3DoubleData     m_pivotInA;
     139        btVector3DoubleData     m_pivotInB;
     140};
     141
     142
     143SIMD_FORCE_INLINE       int     btPoint2PointConstraint::calculateSerializeBufferSize() const
     144{
     145        return sizeof(btPoint2PointConstraintData);
     146
     147}
     148
     149        ///fills the dataBuffer and returns the struct name (and 0 on failure)
     150SIMD_FORCE_INLINE       const char*     btPoint2PointConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
     151{
     152        btPoint2PointConstraintData* p2pData = (btPoint2PointConstraintData*)dataBuffer;
     153
     154        btTypedConstraint::serialize(&p2pData->m_typeConstraintData,serializer);
     155        m_pivotInA.serialize(p2pData->m_pivotInA);
     156        m_pivotInB.serialize(p2pData->m_pivotInB);
     157
     158        return btPoint2PointConstraintDataName;
     159}
     160
    98161#endif //POINT2POINTCONSTRAINT_H

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

    r5781 r8351  
    3535#include <string.h> //for memset
    3636
     37int             gNumSplitImpulseRecoveries = 0;
     38
    3739btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
    3840:m_btSeed2(0)
    … …  
    5658
    5759// Project Gauss Seidel or the equivalent Sequential Impulse
    58 void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
     60void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
    5961{
    6062#ifdef USE_SIMD
    … …  
    6365        __m128  upperLimit1 = _mm_set1_ps(c.m_upperLimit);
    6466        __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        __m128 deltaVel1Dotn    =       _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
     68        __m128 deltaVel2Dotn    =       _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.internalGetDeltaLinearVelocity().mVec128));
    6769        deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
    6870        deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
    … …  
    7779        deltaImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied) );
    7880        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  linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128);
     82        __m128  linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128);
    8183        __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));
     84        body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
     85        body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
     86        body2.internalGetDeltaLinearVelocity().mVec128 = _mm_sub_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
     87        body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
    8688#else
    8789        resolveSingleConstraintRowGeneric(body1,body2,c);
    … …  
    9092
    9193// Project Gauss Seidel or the equivalent Sequential Impulse
    92  void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
     94 void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
    9395{
    9496        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;
     97        const btScalar deltaVel1Dotn    =       c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity())   + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
     98        const btScalar deltaVel2Dotn    =       -c.m_contactNormal.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
     99
     100//      const btScalar delta_rel_vel    =       deltaVel1Dotn-deltaVel2Dotn;
    99101        deltaImpulse    -=      deltaVel1Dotn*c.m_jacDiagABInv;
    100102        deltaImpulse    -=      deltaVel2Dotn*c.m_jacDiagABInv;
    … …  
    115117                c.m_appliedImpulse = sum;
    116118        }
    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)
     119                body1.internalApplyImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
     120                body2.internalApplyImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
     121}
     122
     123 void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
    124124{
    125125#ifdef USE_SIMD
    … …  
    128128        __m128  upperLimit1 = _mm_set1_ps(c.m_upperLimit);
    129129        __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));
     130        __m128 deltaVel1Dotn    =       _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
     131        __m128 deltaVel2Dotn    =       _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.internalGetDeltaLinearVelocity().mVec128));
    132132        deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
    133133        deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
    … …  
    139139        deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
    140140        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));
     141        __m128  linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128);
     142        __m128  linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128);
    143143        __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));
     144        body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
     145        body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
     146        body2.internalGetDeltaLinearVelocity().mVec128 = _mm_sub_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
     147        body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
    148148#else
    149149        resolveSingleConstraintRowLowerLimit(body1,body2,c);
    … …  
    152152
    153153// Project Gauss Seidel or the equivalent Sequential Impulse
    154  void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
     154 void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
    155155{
    156156        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);
     157        const btScalar deltaVel1Dotn    =       c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity())   + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
     158        const btScalar deltaVel2Dotn    =       -c.m_contactNormal.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
    159159
    160160        deltaImpulse    -=      deltaVel1Dotn*c.m_jacDiagABInv;
    … …  
    170170                c.m_appliedImpulse = sum;
    171171        }
    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);
     172        body1.internalApplyImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
     173        body2.internalApplyImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
     174}
     175
     176
     177void    btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFriendly(
     178        btRigidBody& body1,
     179        btRigidBody& body2,
     180        const btSolverConstraint& c)
     181{
     182                if (c.m_rhsPenetration)
     183        {
     184                        gNumSplitImpulseRecoveries++;
     185                        btScalar deltaImpulse = c.m_rhsPenetration-btScalar(c.m_appliedPushImpulse)*c.m_cfm;
     186                        const btScalar deltaVel1Dotn    =       c.m_contactNormal.dot(body1.internalGetPushVelocity())  + c.m_relpos1CrossNormal.dot(body1.internalGetTurnVelocity());
     187                        const btScalar deltaVel2Dotn    =       -c.m_contactNormal.dot(body2.internalGetPushVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetTurnVelocity());
     188
     189                        deltaImpulse    -=      deltaVel1Dotn*c.m_jacDiagABInv;
     190                        deltaImpulse    -=      deltaVel2Dotn*c.m_jacDiagABInv;
     191                        const btScalar sum = btScalar(c.m_appliedPushImpulse) + deltaImpulse;
     192                        if (sum < c.m_lowerLimit)
     193                        {
     194                                deltaImpulse = c.m_lowerLimit-c.m_appliedPushImpulse;
     195                                c.m_appliedPushImpulse = c.m_lowerLimit;
     196                        }
     197                        else
     198                        {
     199                                c.m_appliedPushImpulse = sum;
     200                        }
     201                        body1.internalApplyPushImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
     202                        body2.internalApplyPushImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
     203        }
     204}
     205
     206 void btSequentialImpulseConstraintSolver::resolveSplitPenetrationSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
     207{
     208#ifdef USE_SIMD
     209        if (!c.m_rhsPenetration)
     210                return;
     211
     212        gNumSplitImpulseRecoveries++;
     213
     214        __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedPushImpulse);
     215        __m128  lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
     216        __m128  upperLimit1 = _mm_set1_ps(c.m_upperLimit);
     217        __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhsPenetration), _mm_mul_ps(_mm_set1_ps(c.m_appliedPushImpulse),_mm_set1_ps(c.m_cfm)));
     218        __m128 deltaVel1Dotn    =       _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.internalGetPushVelocity().mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetTurnVelocity().mVec128));
     219        __m128 deltaVel2Dotn    =       _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetTurnVelocity().mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.internalGetPushVelocity().mVec128));
     220        deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
     221        deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
     222        btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
     223        btSimdScalar resultLowerLess,resultUpperLess;
     224        resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
     225        resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
     226        __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
     227        deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
     228        c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
     229        __m128  linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128);
     230        __m128  linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128);
     231        __m128 impulseMagnitude = deltaImpulse;
     232        body1.internalGetPushVelocity().mVec128 = _mm_add_ps(body1.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
     233        body1.internalGetTurnVelocity().mVec128 = _mm_add_ps(body1.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
     234        body2.internalGetPushVelocity().mVec128 = _mm_sub_ps(body2.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
     235        body2.internalGetTurnVelocity().mVec128 = _mm_add_ps(body2.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
     236#else
     237        resolveSplitPenetrationImpulseCacheFriendly(body1,body2,c);
     238#endif
    176239}
    177240
    … …  
    215278
    216279
    217 
     280#if 0
    218281void    btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject)
    219282{
    220283        btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0;
    221284
    222         solverBody->m_deltaLinearVelocity.setValue(0.f,0.f,0.f);
    223         solverBody->m_deltaAngularVelocity.setValue(0.f,0.f,0.f);
     285        solverBody->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
     286        solverBody->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
     287        solverBody->internalGetPushVelocity().setValue(0.f,0.f,0.f);
     288        solverBody->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
    224289
    225290        if (rb)
    226291        {
    227                 solverBody->m_invMass = rb->getInvMass();
     292                solverBody->internalGetInvMass() = btVector3(rb->getInvMass(),rb->getInvMass(),rb->getInvMass())*rb->getLinearFactor();
    228293                solverBody->m_originalBody = rb;
    229294                solverBody->m_angularFactor = rb->getAngularFactor();
    230295        } else
    231296        {
    232                 solverBody->m_invMass = 0.f;
     297                solverBody->internalGetInvMass().setValue(0,0,0);
    233298                solverBody->m_originalBody = 0;
    234                 solverBody->m_angularFactor = 1.f;
    235         }
    236 }
    237 
    238 
    239 int             gNumSplitImpulseRecoveries = 0;
     299                solverBody->m_angularFactor.setValue(1,1,1);
     300        }
     301}
     302#endif
     303
     304
     305
     306
    240307
    241308btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution)
    … …  
    263330
    264331
    265 
    266 btSolverConstraint&     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)
     332void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyB,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
    267333{
    268334
    … …  
    271337        btRigidBody* body1=btRigidBody::upcast(colObj1);
    272338
    273         btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expand();
    274         memset(&solverConstraint,0xff,sizeof(btSolverConstraint));
    275339        solverConstraint.m_contactNormal = normalAxis;
    276340
    277         solverConstraint.m_solverBodyIdA = solverBodyIdA;
    278         solverConstraint.m_solverBodyIdB = solverBodyIdB;
    279         solverConstraint.m_frictionIndex = frictionIndex;
     341        solverConstraint.m_solverBodyA = body0 ? body0 : &getFixedBody();
     342        solverConstraint.m_solverBodyB = body1 ? body1 : &getFixedBody();
    280343
    281344        solverConstraint.m_friction = cp.m_combinedFriction;
    … …  
    283346
    284347        solverConstraint.m_appliedImpulse = 0.f;
    285         //      solverConstraint.m_appliedPushImpulse = 0.f;
     348        solverConstraint.m_appliedPushImpulse = 0.f;
    286349
    287350        {
    … …  
    338401                rel_vel = vel1Dotn+vel2Dotn;
    339402
    340                 btScalar positionalError = 0.f;
    341 
    342                 btSimdScalar velocityError =  - rel_vel;
     403//              btScalar positionalError = 0.f;
     404
     405                btSimdScalar velocityError =  desiredVelocity - rel_vel;
    343406                btSimdScalar    velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
    344407                solverConstraint.m_rhs = velocityImpulse;
    345                 solverConstraint.m_cfm = 0.f;
     408                solverConstraint.m_cfm = cfmSlip;
    346409                solverConstraint.m_lowerLimit = 0;
    347410                solverConstraint.m_upperLimit = 1e10f;
    348411        }
    349 
     412}
     413
     414
     415
     416btSolverConstraint&     btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
     417{
     418        btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing();
     419        solverConstraint.m_frictionIndex = frictionIndex;
     420        setupFrictionConstraint(solverConstraint, normalAxis, solverBodyA, solverBodyB, cp, rel_pos1, rel_pos2,
     421                                                        colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
    350422        return solverConstraint;
    351423}
    … …  
    353425int     btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body)
    354426{
     427#if 0
    355428        int solverBodyIdA = -1;
    356429
    … …  
    374447        }
    375448        return solverBodyIdA;
     449#endif
     450        return 0;
    376451}
    377452#include <stdio.h>
    378453
    379454
    380 
    381 void    btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal)
    382 {
    383         btCollisionObject* colObj0=0,*colObj1=0;
    384 
    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                 {
     455void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint,
     456                                                                                                                                 btCollisionObject* colObj0, btCollisionObject* colObj1,
     457                                                                                                                                 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
     458                                                                                                                                 btVector3& vel, btScalar& rel_vel, btScalar& relaxation,
     459                                                                                                                                 btVector3& rel_pos1, btVector3& rel_pos2)
     460{
     461                        btRigidBody* rb0 = btRigidBody::upcast(colObj0);
     462                        btRigidBody* rb1 = btRigidBody::upcast(colObj1);
    412463
    413464                        const btVector3& pos1 = cp.getPositionWorldOnA();
    414465                        const btVector3& pos2 = cp.getPositionWorldOnB();
    415466
     467//                      btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
     468//                      btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
    416469                        rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
    417470                        rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
    418471
    419 
    420472                        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);
     473
     474                        btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
     475                        solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
     476                        btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);           
     477                        solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
     478
    440479                                {
    441480#ifdef COMPUTE_IMPULSE_DENOM
    … …  
    467506
    468507
    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);
     508
     509
     510                        btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
     511                        btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
     512                        vel  = vel1 - vel2;
     513                        rel_vel = cp.m_normalWorldOnB.dot(vel);
    475514
    476515                                btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop;
    … …  
    499538                                        solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
    500539                                        if (rb0)
    501                                                 m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
     540                                                rb0->internalApplyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
    502541                                        if (rb1)
    503                                                 m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass(),-solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse);
     542                                                rb1->internalApplyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
    504543                                } else
    505544                                {
    … …  
    507546                                }
    508547
    509                                 //                                                      solverConstraint.m_appliedPushImpulse = 0.f;
     548                                solverConstraint.m_appliedPushImpulse = 0.f;
    510549
    511550                                {
    … …  
    523562                                        btScalar  penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
    524563                                        btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
    525                                         solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
     564                                        if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
     565                                        {
     566                                                //combine position and velocity into rhs
     567                                                solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
     568                                                solverConstraint.m_rhsPenetration = 0.f;
     569                                        } else
     570                                        {
     571                                                //split position and velocity into rhs and m_rhsPenetration
     572                                                solverConstraint.m_rhs = velocityImpulse;
     573                                                solverConstraint.m_rhsPenetration = penetrationImpulse;
     574                                        }
    526575                                        solverConstraint.m_cfm = 0.f;
    527576                                        solverConstraint.m_lowerLimit = 0;
    … …  
    530579
    531580
    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)
    544                                                 {
    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;
    558                                                 } else
    559                                                 {
    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 
     581
     582
     583}
     584
     585
     586
     587void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolverConstraint& solverConstraint,
     588                                                                                                                                                btRigidBody* rb0, btRigidBody* rb1,
     589                                                                                                                                 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal)
     590{
    582591                                        if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
    583592                                        {
    … …  
    588597                                                                frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
    589598                                                                if (rb0)
    590                                                                         m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
     599                                                                        rb0->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
    591600                                                                if (rb1)
    592                                                                         m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass(),-frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse);
     601                                                                        rb1->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-frictionConstraint1.m_angularComponentB,-(btScalar)frictionConstraint1.m_appliedImpulse);
    593602                                                        } else
    594603                                                        {
    … …  
    604613                                                                frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
    605614                                                                if (rb0)
    606                                                                         m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
     615                                                                        rb0->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
    607616                                                                if (rb1)
    608                                                                         m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse);
     617                                                                        rb1->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-(btScalar)frictionConstraint2.m_appliedImpulse);
    609618                                                        } else
    610619                                                        {
    … …  
    622631                                                }
    623632                                        }
     633}
     634
     635
     636
     637
     638void    btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal)
     639{
     640        btCollisionObject* colObj0=0,*colObj1=0;
     641
     642        colObj0 = (btCollisionObject*)manifold->getBody0();
     643        colObj1 = (btCollisionObject*)manifold->getBody1();
     644
     645
     646        btRigidBody* solverBodyA = btRigidBody::upcast(colObj0);
     647        btRigidBody* solverBodyB = btRigidBody::upcast(colObj1);
     648
     649        ///avoid collision response between two static objects
     650        if ((!solverBodyA || !solverBodyA->getInvMass()) && (!solverBodyB || !solverBodyB->getInvMass()))
     651                return;
     652
     653        for (int j=0;j<manifold->getNumContacts();j++)
     654        {
     655
     656                btManifoldPoint& cp = manifold->getContactPoint(j);
     657
     658                if (cp.getDistance() <= manifold->getContactProcessingThreshold())
     659                {
     660                        btVector3 rel_pos1;
     661                        btVector3 rel_pos2;
     662                        btScalar relaxation;
     663                        btScalar rel_vel;
     664                        btVector3 vel;
     665
     666                        int frictionIndex = m_tmpSolverContactConstraintPool.size();
     667                        btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing();
     668                        btRigidBody* rb0 = btRigidBody::upcast(colObj0);
     669                        btRigidBody* rb1 = btRigidBody::upcast(colObj1);
     670                        solverConstraint.m_solverBodyA = rb0? rb0 : &getFixedBody();
     671                        solverConstraint.m_solverBodyB = rb1? rb1 : &getFixedBody();
     672                        solverConstraint.m_originalContactPoint = &cp;
     673
     674                        setupContactConstraint(solverConstraint, colObj0, colObj1, cp, infoGlobal, vel, rel_vel, relaxation, rel_pos1, rel_pos2);
     675
     676//                      const btVector3& pos1 = cp.getPositionWorldOnA();
     677//                      const btVector3& pos2 = cp.getPositionWorldOnB();
     678
     679                        /////setup the friction constraints
     680
     681                        solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
     682
     683                        if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
     684                        {
     685                                cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
     686                                btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
     687                                if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
     688                                {
     689                                        cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel);
     690                                        if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
     691                                        {
     692                                                cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
     693                                                cp.m_lateralFrictionDir2.normalize();//??
     694                                                applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
     695                                                applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
     696                                                addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
     697                                        }
     698
     699                                        applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
     700                                        applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
     701                                        addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
     702                                        cp.m_lateralFrictionInitialized = true;
     703                                } else
     704                                {
     705                                        //re-calculate friction direction every frame, todo: check if this is really needed
     706                                        btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
     707                                        if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
     708                                        {
     709                                                applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
     710                                                applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
     711                                                addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
     712                                        }
     713
     714                                        applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
     715                                        applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
     716                                        addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
     717
     718                                        cp.m_lateralFrictionInitialized = true;
    624719                                }
    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)
     720
     721                        } else
     722                        {
     723                                addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_contactCFM1);
     724                                if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
     725                                        addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2);
     726                        }
     727                       
     728                        setFrictionConstraintImpulse( solverConstraint, rb0, rb1, cp, infoGlobal);
     729
     730                }
     731        }
     732}
     733
     734
     735btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
    634736{
    635737        BT_PROFILE("solveGroupCacheFriendlySetup");
    … …  
    644746        }
    645747
     748        if (infoGlobal.m_splitImpulse)
     749        {
     750                for (int i = 0; i < numBodies; i++)
     751                {
     752                        btRigidBody* body = btRigidBody::upcast(bodies[i]);
     753                        if (body)
     754                        {       
     755                                body->internalGetDeltaLinearVelocity().setZero();
     756                                body->internalGetDeltaAngularVelocity().setZero();
     757                                body->internalGetPushVelocity().setZero();
     758                                body->internalGetTurnVelocity().setZero();
     759                        }
     760                }
     761        }
     762        else
     763        {
     764                for (int i = 0; i < numBodies; i++)
     765                {
     766                        btRigidBody* body = btRigidBody::upcast(bodies[i]);
     767                        if (body)
     768                        {       
     769                                body->internalGetDeltaLinearVelocity().setZero();
     770                                body->internalGetDeltaAngularVelocity().setZero();
     771                        }
     772                }
     773        }
     774
    646775        if (1)
    647776        {
    … …  
    653782                }
    654783        }
    655 
    656         btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
    657         initSolverBody(&fixedBody,0);
    658 
    659784        //btRigidBody* rb0=0,*rb1=0;
    660785
    … …  
    665790                        int totalNumRows = 0;
    666791                        int i;
     792                       
     793                        m_tmpConstraintSizesPool.resize(numConstraints);
    667794                        //calculate the total number of contraint rows
    668795                        for (i=0;i<numConstraints;i++)
    669796                        {
    670 
    671                                 btTypedConstraint::btConstraintInfo1 info1;
     797                                btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
    672798                                constraints[i]->getInfo1(&info1);
    673799                                totalNumRows += info1.m_numConstraintRows;
    … …  
    675801                        m_tmpSolverNonContactConstraintPool.resize(totalNumRows);
    676802
    677                         btTypedConstraint::btConstraintInfo1 info1;
    678                         info1.m_numConstraintRows = 0;
    679 
    680 
     803                       
    681804                        ///setup the btSolverConstraints
    682805                        int currentRow = 0;
    683806
    684                         for (i=0;i<numConstraints;i++,currentRow+=info1.m_numConstraintRows)
     807                        for (i=0;i<numConstraints;i++)
    685808                        {
    686                                 constraints[i]->getInfo1(&info1);
     809                                const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
     810                               
    687811                                if (info1.m_numConstraintRows)
    688812                                {
    … …  
    697821                                        btRigidBody& rbB = constraint->getRigidBodyB();
    698822
    699                                         int solverBodyIdA = getOrInitSolverBody(rbA);
    700                                         int solverBodyIdB = getOrInitSolverBody(rbB);
    701 
    702                                         btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
    703                                         btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
    704 
     823                                       
    705824                                        int j;
    706825                                        for ( j=0;j<info1.m_numConstraintRows;j++)
    … …  
    711830                                                currentConstraintRow[j].m_appliedImpulse = 0.f;
    712831                                                currentConstraintRow[j].m_appliedPushImpulse = 0.f;
    713                                                 currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA;
    714                                                 currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB;
     832                                                currentConstraintRow[j].m_solverBodyA = &rbA;
     833                                                currentConstraintRow[j].m_solverBodyB = &rbB;
    715834                                        }
    716835
    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);
     836                                        rbA.internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
     837                                        rbA.internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
     838                                        rbB.internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
     839                                        rbB.internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
    721840
    722841
    … …  
    733852                                        btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint));
    734853                                        info2.m_constraintError = &currentConstraintRow->m_rhs;
     854                                        currentConstraintRow->m_cfm = infoGlobal.m_globalCfm;
     855                                        info2.m_damping = infoGlobal.m_damping;
    735856                                        info2.cfm = &currentConstraintRow->m_cfm;
    736857                                        info2.m_lowerLimit = &currentConstraintRow->m_lowerLimit;
    737858                                        info2.m_upperLimit = &currentConstraintRow->m_upperLimit;
     859                                        info2.m_numIterations = infoGlobal.m_numIterations;
    738860                                        constraints[i]->getInfo2(&info2);
    739861
    … …  
    742864                                        {
    743865                                                btSolverConstraint& solverConstraint = currentConstraintRow[j];
     866                                                solverConstraint.m_originalContactPoint = constraint;
    744867
    745868                                                {
    … …  
    778901                                                        btScalar restitution = 0.f;
    779902                                                        btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2
    780                                                         btScalar        velocityError = restitution - rel_vel;// * damping;
     903                                                        btScalar        velocityError = restitution - rel_vel * info2.m_damping;
    781904                                                        btScalar        penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
    782905                                                        btScalar        velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
    … …  
    787910                                        }
    788911                                }
     912                                currentRow+=m_tmpConstraintSizesPool[i].m_numConstraintRows;
    789913                        }
    790914                }
    … …  
    793917                        int i;
    794918                        btPersistentManifold* manifold = 0;
    795                         btCollisionObject* colObj0=0,*colObj1=0;
     919//                      btCollisionObject* colObj0=0,*colObj1=0;
    796920
    797921
    … …  
    830954}
    831955
    832 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/)
    833 {
    834         BT_PROFILE("solveGroupCacheFriendlyIterations");
     956btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/)
     957{
    835958
    836959        int numConstraintPool = m_tmpSolverContactConstraintPool.size();
    837960        int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
    838961
     962        int j;
     963
     964        if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
     965        {
     966                if ((iteration & 7) == 0) {
     967                        for (j=0; j<numConstraintPool; ++j) {
     968                                int tmp = m_orderTmpConstraintPool[j];
     969                                int swapi = btRandInt2(j+1);
     970                                m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
     971                                m_orderTmpConstraintPool[swapi] = tmp;
     972                        }
     973
     974                        for (j=0; j<numFrictionPool; ++j) {
     975                                int tmp = m_orderFrictionConstraintPool[j];
     976                                int swapi = btRandInt2(j+1);
     977                                m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
     978                                m_orderFrictionConstraintPool[swapi] = tmp;
     979                        }
     980                }
     981        }
     982
     983        if (infoGlobal.m_solverMode & SOLVER_SIMD)
     984        {
     985                ///solve all joint constraints, using SIMD, if available
     986                for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
     987                {
     988                        btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
     989                        resolveSingleConstraintRowGenericSIMD(*constraint.m_solverBodyA,*constraint.m_solverBodyB,constraint);
     990                }
     991
     992                for (j=0;j<numConstraints;j++)
     993                {
     994                        constraints[j]->solveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
     995                }
     996
     997                ///solve all contact constraints using SIMD, if available
     998                int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
     999                for (j=0;j<numPoolConstraints;j++)
     1000                {
     1001                        const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
     1002                        resolveSingleConstraintRowLowerLimitSIMD(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
     1003
     1004                }
     1005                ///solve all friction constraints, using SIMD, if available
     1006                int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
     1007                for (j=0;j<numFrictionPoolConstraints;j++)
     1008                {
     1009                        btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
     1010                        btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
     1011
     1012                        if (totalImpulse>btScalar(0))
     1013                        {
     1014                                solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
     1015                                solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
     1016
     1017                                resolveSingleConstraintRowGenericSIMD(*solveManifold.m_solverBodyA,     *solveManifold.m_solverBodyB,solveManifold);
     1018                        }
     1019                }
     1020        } else
     1021        {
     1022
     1023                ///solve all joint constraints
     1024                for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
     1025                {
     1026                        btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
     1027                        resolveSingleConstraintRowGeneric(*constraint.m_solverBodyA,*constraint.m_solverBodyB,constraint);
     1028                }
     1029
     1030                for (j=0;j<numConstraints;j++)
     1031                {
     1032                        constraints[j]->solveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
     1033                }
     1034                ///solve all contact constraints
     1035                int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
     1036                for (j=0;j<numPoolConstraints;j++)
     1037                {
     1038                        const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
     1039                        resolveSingleConstraintRowLowerLimit(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
     1040                }
     1041                ///solve all friction constraints
     1042                int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
     1043                for (j=0;j<numFrictionPoolConstraints;j++)
     1044                {
     1045                        btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
     1046                        btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
     1047
     1048                        if (totalImpulse>btScalar(0))
     1049                        {
     1050                                solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
     1051                                solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
     1052
     1053                                resolveSingleConstraintRowGeneric(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
     1054                        }
     1055                }
     1056        }
     1057        return 0.f;
     1058}
     1059
     1060
     1061void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
     1062{
     1063        int iteration;
     1064        if (infoGlobal.m_splitImpulse)
     1065        {
     1066                if (infoGlobal.m_solverMode & SOLVER_SIMD)
     1067                {
     1068                        for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
     1069                        {
     1070                                {
     1071                                        int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
     1072                                        int j;
     1073                                        for (j=0;j<numPoolConstraints;j++)
     1074                                        {
     1075                                                const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
     1076
     1077                                                resolveSplitPenetrationSIMD(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
     1078                                        }
     1079                                }
     1080                        }
     1081                }
     1082                else
     1083                {
     1084                        for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
     1085                        {
     1086                                {
     1087                                        int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
     1088                                        int j;
     1089                                        for (j=0;j<numPoolConstraints;j++)
     1090                                        {
     1091                                                const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
     1092
     1093                                                resolveSplitPenetrationImpulseCacheFriendly(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
     1094                                        }
     1095                                }
     1096                        }
     1097                }
     1098        }
     1099}
     1100
     1101btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
     1102{
     1103        BT_PROFILE("solveGroupCacheFriendlyIterations");
     1104
     1105       
    8391106        //should traverse the contacts random order...
    8401107        int iteration;
    … …  
    8421109                for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
    8431110                {                       
    844 
    845                         int j;
    846                         if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
    847                         {
    848                                 if ((iteration & 7) == 0) {
    849                                         for (j=0; j<numConstraintPool; ++j) {
    850                                                 int tmp = m_orderTmpConstraintPool[j];
    851                                                 int swapi = btRandInt2(j+1);
    852                                                 m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
    853                                                 m_orderTmpConstraintPool[swapi] = tmp;
    854                                         }
    855 
    856                                         for (j=0; j<numFrictionPool; ++j) {
    857                                                 int tmp = m_orderFrictionConstraintPool[j];
    858                                                 int swapi = btRandInt2(j+1);
    859                                                 m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
    860                                                 m_orderFrictionConstraintPool[swapi] = tmp;
    861                                         }
    862                                 }
    863                         }
    864 
    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();
    885                                 for (j=0;j<numPoolConstraints;j++)
    886                                 {
    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))
    899                                         {
    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);
    904                                         }
    905                                 }
    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                 }
     1111                        solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
     1112                }
     1113               
     1114                solveGroupCacheFriendlySplitImpulseIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
    9531115        }
    9541116        return 0.f;
    9551117}
    9561118
    957 
    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 
    971         int i;
    972 
    973         solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
    974         solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
    975 
     1119btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies ,int numBodies,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** /*constraints*/,int /* numConstraints*/,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/)
     1120{
    9761121        int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
    977         int j;
     1122        int i,j;
    9781123
    9791124        for (j=0;j<numPoolConstraints;j++)
    … …  
    9931138        }
    9941139
     1140        numPoolConstraints = m_tmpSolverNonContactConstraintPool.size();
     1141        for (j=0;j<numPoolConstraints;j++)
     1142        {
     1143                const btSolverConstraint& solverConstr = m_tmpSolverNonContactConstraintPool[j];
     1144                btTypedConstraint* constr = (btTypedConstraint*)solverConstr.m_originalContactPoint;
     1145                btScalar sum = constr->internalGetAppliedImpulse();
     1146                sum += solverConstr.m_appliedImpulse;
     1147                constr->internalSetAppliedImpulse(sum);
     1148        }
     1149
     1150
    9951151        if (infoGlobal.m_splitImpulse)
    9961152        {               
    997                 for ( i=0;i<m_tmpSolverBodyPool.size();i++)
    998                 {
    999                         m_tmpSolverBodyPool[i].writebackVelocity(infoGlobal.m_timeStep);
     1153                for ( i=0;i<numBodies;i++)
     1154                {
     1155                        btRigidBody* body = btRigidBody::upcast(bodies[i]);
     1156                        if (body)
     1157                                body->internalWritebackVelocity(infoGlobal.m_timeStep);
    10001158                }
    10011159        } else
    10021160        {
    1003                 for ( i=0;i<m_tmpSolverBodyPool.size();i++)
    1004                 {
    1005                         m_tmpSolverBodyPool[i].writebackVelocity();
    1006                 }
    1007         }
    1008 
    1009 
    1010         m_tmpSolverBodyPool.resize(0);
     1161                for ( i=0;i<numBodies;i++)
     1162                {
     1163                        btRigidBody* body = btRigidBody::upcast(bodies[i]);
     1164                        if (body)
     1165                                body->internalWritebackVelocity();
     1166                }
     1167        }
     1168
     1169
    10111170        m_tmpSolverContactConstraintPool.resize(0);
    10121171        m_tmpSolverNonContactConstraintPool.resize(0);
    … …  
    10181177
    10191178
    1020 
    1021 
    1022 
    1023 
    1024 
     1179/// btSequentialImpulseConstraintSolver Sequentially applies impulses
     1180btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/)
     1181{
     1182
     1183        BT_PROFILE("solveGroup");
     1184        //you need to provide at least some bodies
     1185        btAssert(bodies);
     1186        btAssert(numBodies);
     1187
     1188        solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
     1189
     1190        solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
     1191
     1192        solveGroupCacheFriendlyFinish(bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
     1193       
     1194        return 0.f;
     1195}
    10251196
    10261197void    btSequentialImpulseConstraintSolver::reset()
    … …  
    10291200}
    10301201
    1031 
     1202btRigidBody& btSequentialImpulseConstraintSolver::getFixedBody()
     1203{
     1204        static btRigidBody s_fixed(0, 0,0);
     1205        s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
     1206        return s_fixed;
     1207}
     1208

  • code/trunk/src/external/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h

    r5781 r8351  
    2222#include "btSolverBody.h"
    2323#include "btSolverConstraint.h"
    24 
    25 
     24#include "btTypedConstraint.h"
     25#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
    2626
    2727///The btSequentialImpulseConstraintSolver is a fast SIMD implementation of the Projected Gauss Seidel (iterative LCP) method.
    … …  
    3030protected:
    3131
    32         btAlignedObjectArray<btSolverBody>      m_tmpSolverBodyPool;
    3332        btConstraintArray                       m_tmpSolverContactConstraintPool;
    3433        btConstraintArray                       m_tmpSolverNonContactConstraintPool;
    … …  
    3635        btAlignedObjectArray<int>       m_orderTmpConstraintPool;
    3736        btAlignedObjectArray<int>       m_orderFrictionConstraintPool;
     37        btAlignedObjectArray<btTypedConstraint::btConstraintInfo1> m_tmpConstraintSizesPool;
    3838
    39         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);
     39        void setupFrictionConstraint(   btSolverConstraint& solverConstraint, const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyIdB,
     40                                                                        btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,
     41                                                                        btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation,
     42                                                                        btScalar desiredVelocity=0., btScalar cfmSlip=0.);
     43
     44        btSolverConstraint&     addFrictionConstraint(const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0., btScalar cfmSlip=0.);
    4045       
     46        void setupContactConstraint(btSolverConstraint& solverConstraint, btCollisionObject* colObj0, btCollisionObject* colObj1, btManifoldPoint& cp,
     47                                                                const btContactSolverInfo& infoGlobal, btVector3& vel, btScalar& rel_vel, btScalar& relaxation,
     48                                                                btVector3& rel_pos1, btVector3& rel_pos2);
     49
     50        void setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, btRigidBody* rb0, btRigidBody* rb1,
     51                                                                                 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal);
     52
    4153        ///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
    4254        unsigned long   m_btSeed2;
    4355
    44         void    initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject);
     56//      void    initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject);
    4557        btScalar restitutionCurve(btScalar rel_vel, btScalar restitution);
    4658
    4759        void    convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal);
    4860
     61
     62        void    resolveSplitPenetrationSIMD(
     63        btRigidBody& body1,
     64        btRigidBody& body2,
     65        const btSolverConstraint& contactConstraint);
     66
    4967        void    resolveSplitPenetrationImpulseCacheFriendly(
    50         btSolverBody& body1,
    51         btSolverBody& body2,
    52         const btSolverConstraint& contactConstraint,
    53         const btContactSolverInfo& solverInfo);
     68        btRigidBody& body1,
     69        btRigidBody& body2,
     70        const btSolverConstraint& contactConstraint);
    5471
    5572        //internal method
    5673        int     getOrInitSolverBody(btCollisionObject& body);
    5774
    58         void    resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint);
     75        void    resolveSingleConstraintRowGeneric(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& contactConstraint);
    5976
    60         void    resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint);
     77        void    resolveSingleConstraintRowGenericSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& contactConstraint);
    6178       
    62         void    resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint);
     79        void    resolveSingleConstraintRowLowerLimit(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& contactConstraint);
    6380       
    64         void    resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint);
     81        void    resolveSingleConstraintRowLowerLimitSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& contactConstraint);
    6582               
     83protected:
     84        static btRigidBody& getFixedBody();
     85       
     86        virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
     87        virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
     88        btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
     89
     90        virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
     91        virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
     92
     93
    6694public:
    6795
    … …  
    72100        virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher);
    73101       
    74         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);
    76102
     103       
    77104        ///clear internal cached data and reset random seed
    78105        virtual void    reset();

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

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

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

    r5781 r8351  
    2626#define SLIDER_CONSTRAINT_H
    2727
    28 //-----------------------------------------------------------------------------
     28
    2929
    3030#include "LinearMath/btVector3.h"
    … …  
    3232#include "btTypedConstraint.h"
    3333
    34 //-----------------------------------------------------------------------------
     34
    3535
    3636class btRigidBody;
    3737
    38 //-----------------------------------------------------------------------------
     38
    3939
    4040#define SLIDER_CONSTRAINT_DEF_SOFTNESS          (btScalar(1.0))
    4141#define SLIDER_CONSTRAINT_DEF_DAMPING           (btScalar(1.0))
    4242#define SLIDER_CONSTRAINT_DEF_RESTITUTION       (btScalar(0.7))
    43 
    44 //-----------------------------------------------------------------------------
     43#define SLIDER_CONSTRAINT_DEF_CFM                       (btScalar(0.f))
     44
     45
     46enum btSliderFlags
     47{
     48        BT_SLIDER_FLAGS_CFM_DIRLIN = (1 << 0),
     49        BT_SLIDER_FLAGS_ERP_DIRLIN = (1 << 1),
     50        BT_SLIDER_FLAGS_CFM_DIRANG = (1 << 2),
     51        BT_SLIDER_FLAGS_ERP_DIRANG = (1 << 3),
     52        BT_SLIDER_FLAGS_CFM_ORTLIN = (1 << 4),
     53        BT_SLIDER_FLAGS_ERP_ORTLIN = (1 << 5),
     54        BT_SLIDER_FLAGS_CFM_ORTANG = (1 << 6),
     55        BT_SLIDER_FLAGS_ERP_ORTANG = (1 << 7),
     56        BT_SLIDER_FLAGS_CFM_LIMLIN = (1 << 8),
     57        BT_SLIDER_FLAGS_ERP_LIMLIN = (1 << 9),
     58        BT_SLIDER_FLAGS_CFM_LIMANG = (1 << 10),
     59        BT_SLIDER_FLAGS_ERP_LIMANG = (1 << 11)
     60};
     61
    4562
    4663class btSliderConstraint : public btTypedConstraint
    … …  
    4966        ///for backwards compatibility during the transition to 'getInfo/getInfo2'
    5067        bool            m_useSolveConstraintObsolete;
     68        bool            m_useOffsetForConstraintFrame;
    5169        btTransform     m_frameInA;
    5270    btTransform m_frameInB;
    … …  
    6886        btScalar m_restitutionDirLin;
    6987        btScalar m_dampingDirLin;
     88        btScalar m_cfmDirLin;
     89
    7090        btScalar m_softnessDirAng;
    7191        btScalar m_restitutionDirAng;
    7292        btScalar m_dampingDirAng;
     93        btScalar m_cfmDirAng;
     94
    7395        btScalar m_softnessLimLin;
    7496        btScalar m_restitutionLimLin;
    7597        btScalar m_dampingLimLin;
     98        btScalar m_cfmLimLin;
     99
    76100        btScalar m_softnessLimAng;
    77101        btScalar m_restitutionLimAng;
    78102        btScalar m_dampingLimAng;
     103        btScalar m_cfmLimAng;
     104
    79105        btScalar m_softnessOrthoLin;
    80106        btScalar m_restitutionOrthoLin;
    81107        btScalar m_dampingOrthoLin;
     108        btScalar m_cfmOrthoLin;
     109
    82110        btScalar m_softnessOrthoAng;
    83111        btScalar m_restitutionOrthoAng;
    84112        btScalar m_dampingOrthoAng;
     113        btScalar m_cfmOrthoAng;
    85114       
    86115        // for interlal use
    87116        bool m_solveLinLim;
    88117        bool m_solveAngLim;
     118
     119        int m_flags;
    89120
    90121        btJacobianEntry m_jacLin[3];
    … …  
    127158        // constructors
    128159    btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
    129     btSliderConstraint();
     160    btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameA);
     161
    130162        // overrides
    131     virtual void        buildJacobian();
     163
    132164    virtual void getInfo1 (btConstraintInfo1* info);
     165
     166        void getInfo1NonVirtual(btConstraintInfo1* info);
    133167       
    134168        virtual void getInfo2 (btConstraintInfo2* info);
    135169
    136     virtual     void    solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar        timeStep);
    137        
     170        void getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass);
     171
    138172
    139173        // access
    … …  
    151185    void setUpperLinLimit(btScalar upperLimit) { m_upperLinLimit = upperLimit; }
    152186    btScalar getLowerAngLimit() { return m_lowerAngLimit; }
    153     void setLowerAngLimit(btScalar lowerLimit) { m_lowerAngLimit = lowerLimit; }
     187    void setLowerAngLimit(btScalar lowerLimit) { m_lowerAngLimit = btNormalizeAngle(lowerLimit); }
    154188    btScalar getUpperAngLimit() { return m_upperAngLimit; }
    155     void setUpperAngLimit(btScalar upperLimit) { m_upperAngLimit = upperLimit; }
     189    void setUpperAngLimit(btScalar upperLimit) { m_upperAngLimit = btNormalizeAngle(upperLimit); }
    156190        bool getUseLinearReferenceFrameA() { return m_useLinearReferenceFrameA; }
    157191        btScalar getSoftnessDirLin() { return m_softnessDirLin; }
    … …  
    211245        bool getSolveAngLimit() { return m_solveAngLim; }
    212246        btScalar getAngDepth() { return m_angDepth; }
    213         // internal
    214     void        buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB);
    215     void        solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB);
    216247        // shared code used by ODE solver
    217         void    calculateTransforms(void);
    218         void    testLinLimits(void);
    219         void    testLinLimits2(btConstraintInfo2* info);
    220         void    testAngLimits(void);
     248        void    calculateTransforms(const btTransform& transA,const btTransform& transB);
     249        void    testLinLimits();
     250        void    testAngLimits();
    221251        // access for PE Solver
    222         btVector3 getAncorInA(void);
    223         btVector3 getAncorInB(void);
     252        btVector3 getAncorInA();
     253        btVector3 getAncorInB();
     254        // access for UseFrameOffset
     255        bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
     256        void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
     257
     258        ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
     259        ///If no axis is provided, it uses the default axis for this constraint.
     260        virtual void    setParam(int num, btScalar value, int axis = -1);
     261        ///return the local value of parameter
     262        virtual btScalar getParam(int num, int axis = -1) const;
     263
     264        virtual int     calculateSerializeBufferSize() const;
     265
     266        ///fills the dataBuffer and returns the struct name (and 0 on failure)
     267        virtual const char*     serialize(void* dataBuffer, btSerializer* serializer) const;
     268
     269
    224270};
    225271
    226 //-----------------------------------------------------------------------------
     272///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
     273struct btSliderConstraintData
     274{
     275        btTypedConstraintData   m_typeConstraintData;
     276        btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
     277        btTransformFloatData m_rbBFrame;
     278       
     279        float   m_linearUpperLimit;
     280        float   m_linearLowerLimit;
     281
     282        float   m_angularUpperLimit;
     283        float   m_angularLowerLimit;
     284
     285        int     m_useLinearReferenceFrameA;
     286        int m_useOffsetForConstraintFrame;
     287
     288};
     289
     290
     291SIMD_FORCE_INLINE               int     btSliderConstraint::calculateSerializeBufferSize() const
     292{
     293        return sizeof(btSliderConstraintData);
     294}
     295
     296        ///fills the dataBuffer and returns the struct name (and 0 on failure)
     297SIMD_FORCE_INLINE       const char*     btSliderConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
     298{
     299
     300        btSliderConstraintData* sliderData = (btSliderConstraintData*) dataBuffer;
     301        btTypedConstraint::serialize(&sliderData->m_typeConstraintData,serializer);
     302
     303        m_frameInA.serializeFloat(sliderData->m_rbAFrame);
     304        m_frameInB.serializeFloat(sliderData->m_rbBFrame);
     305
     306        sliderData->m_linearUpperLimit = float(m_upperLinLimit);
     307        sliderData->m_linearLowerLimit = float(m_lowerLinLimit);
     308
     309        sliderData->m_angularUpperLimit = float(m_upperAngLimit);
     310        sliderData->m_angularLowerLimit = float(m_lowerAngLimit);
     311
     312        sliderData->m_useLinearReferenceFrameA = m_useLinearReferenceFrameA;
     313        sliderData->m_useOffsetForConstraintFrame = m_useOffsetForConstraintFrame;
     314
     315        return "btSliderConstraintData";
     316}
     317
     318
    227319
    228320#endif //SLIDER_CONSTRAINT_H

  • code/trunk/src/external/bullet/BulletDynamics/ConstraintSolver/btSolverBody.h

    r5781 r8351  
    106106
    107107///The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
    108 ATTRIBUTE_ALIGNED16 (struct)    btSolverBody
     108ATTRIBUTE_ALIGNED64 (struct)    btSolverBodyObsolete
    109109{
    110110        BT_DECLARE_ALIGNED_ALLOCATOR();
    111111        btVector3               m_deltaLinearVelocity;
    112112        btVector3               m_deltaAngularVelocity;
    113         btScalar                m_angularFactor;
    114         btScalar                m_invMass;
    115         btScalar                m_friction;
     113        btVector3               m_angularFactor;
     114        btVector3               m_invMass;
    116115        btRigidBody*    m_originalBody;
    117116        btVector3               m_pushVelocity;
    118         //btVector3             m_turnVelocity;
     117        btVector3               m_turnVelocity;
    119118
    120119       
    … …  
    146145        }
    147146
    148        
    149 /*
     147        SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
     148        {
     149                if (m_originalBody)
     150                {
     151                        m_pushVelocity += linearComponent*impulseMagnitude;
     152                        m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
     153                }
     154        }
    150155       
    151156        void    writebackVelocity()
    152157        {
    153                 if (m_invMass)
     158                if (m_originalBody)
    154159                {
    155160                        m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+ m_deltaLinearVelocity);
    … …  
    159164                }
    160165        }
    161         */
    162166
    163         void    writebackVelocity(btScalar timeStep=0)
     167
     168        void    writebackVelocity(btScalar timeStep)
    164169        {
    165                 if (m_invMass)
     170        (void) timeStep;
     171                if (m_originalBody)
    166172                {
    167                         m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+m_deltaLinearVelocity);
     173                        m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+ m_deltaLinearVelocity);
    168174                        m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity);
     175                       
     176                        //correct the position/orientation based on push/turn recovery
     177                        btTransform newTransform;
     178                        btTransformUtil::integrateTransform(m_originalBody->getWorldTransform(),m_pushVelocity,m_turnVelocity,timeStep,newTransform);
     179                        m_originalBody->setWorldTransform(newTransform);
     180                       
    169181                        //m_originalBody->setCompanionId(-1);
    170182                }

  • code/trunk/src/external/bullet/BulletDynamics/ConstraintSolver/btSolverConstraint.h

    r5781 r8351  
    2727
    2828///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
    29 ATTRIBUTE_ALIGNED16 (struct)    btSolverConstraint
     29ATTRIBUTE_ALIGNED64 (struct)    btSolverConstraint
    3030{
    3131        BT_DECLARE_ALIGNED_ALLOCATOR();
    … …  
    5959        union
    6060        {
    61                 int                     m_solverBodyIdA;
    62                 btScalar        m_unusedPadding2;
     61                btRigidBody*    m_solverBodyA;
     62                int                             m_companionIdA;
    6363        };
    6464        union
    6565        {
    66                 int                     m_solverBodyIdB;
    67                 btScalar        m_unusedPadding3;
     66                btRigidBody*    m_solverBodyB;
     67                int                             m_companionIdB;
    6868        };
    6969       
    … …  
    7878        btScalar                m_lowerLimit;
    7979        btScalar                m_upperLimit;
     80
     81        btScalar                m_rhsPenetration;
    8082
    8183        enum            btSolverConstraintType

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

    r5781 r8351  
    1717#include "btTypedConstraint.h"
    1818#include "BulletDynamics/Dynamics/btRigidBody.h"
     19#include "LinearMath/btSerializer.h"
    1920
    20 static btRigidBody s_fixed(0, 0,0);
    2121
    2222#define DEFAULT_DEBUGDRAW_SIZE btScalar(0.3f)
    2323
    24 btTypedConstraint::btTypedConstraint(btTypedConstraintType type)
    25 :m_userConstraintType(-1),
     24btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA)
     25:btTypedObject(type),
     26m_userConstraintType(-1),
    2627m_userConstraintId(-1),
    27 m_constraintType (type),
    28 m_rbA(s_fixed),
    29 m_rbB(s_fixed),
     28m_needsFeedback(false),
     29m_rbA(rbA),
     30m_rbB(getFixedBody()),
    3031m_appliedImpulse(btScalar(0.)),
    3132m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE)
    3233{
    33         s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
    34 }
    35 btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA)
    36 :m_userConstraintType(-1),
    37 m_userConstraintId(-1),
    38 m_constraintType (type),
    39 m_rbA(rbA),
    40 m_rbB(s_fixed),
    41 m_appliedImpulse(btScalar(0.)),
    42 m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE)
    43 {
    44                 s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
    45 
    4634}
    4735
    4836
    4937btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB)
    50 :m_userConstraintType(-1),
     38:btTypedObject(type),
     39m_userConstraintType(-1),
    5140m_userConstraintId(-1),
    52 m_constraintType (type),
     41m_needsFeedback(false),
    5342m_rbA(rbA),
    5443m_rbB(rbB),
    … …  
    5645m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE)
    5746{
    58                 s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
    59 
    6047}
    6148
    6249
    63 //-----------------------------------------------------------------------------
     50
    6451
    6552btScalar btTypedConstraint::getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact)
    … …  
    11097        }
    11198        return lim_fact;
    112 } // btTypedConstraint::getMotorFactor()
     99}
    113100
     101///fills the dataBuffer and returns the struct name (and 0 on failure)
     102const char*     btTypedConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
     103{
     104        btTypedConstraintData* tcd = (btTypedConstraintData*) dataBuffer;
    114105
     106        tcd->m_rbA = (btRigidBodyData*)serializer->getUniquePointer(&m_rbA);
     107        tcd->m_rbB = (btRigidBodyData*)serializer->getUniquePointer(&m_rbB);
     108        char* name = (char*) serializer->findNameForPointer(this);
     109        tcd->m_name = (char*)serializer->getUniquePointer(name);
     110        if (tcd->m_name)
     111        {
     112                serializer->serializeName(name);
     113        }
     114
     115        tcd->m_objectType = m_objectType;
     116        tcd->m_needsFeedback = m_needsFeedback;
     117        tcd->m_userConstraintId =m_userConstraintId;
     118        tcd->m_userConstraintType =m_userConstraintType;
     119
     120        tcd->m_appliedImpulse = float(m_appliedImpulse);
     121        tcd->m_dbgDrawSize = float(m_dbgDrawSize );
     122
     123        tcd->m_disableCollisionsBetweenLinkedBodies = false;
     124
     125        int i;
     126        for (i=0;i<m_rbA.getNumConstraintRefs();i++)
     127                if (m_rbA.getConstraintRef(i) == this)
     128                        tcd->m_disableCollisionsBetweenLinkedBodies = true;
     129        for (i=0;i<m_rbB.getNumConstraintRefs();i++)
     130                if (m_rbB.getConstraintRef(i) == this)
     131                        tcd->m_disableCollisionsBetweenLinkedBodies = true;
     132
     133        return "btTypedConstraintData";
     134}
     135
     136btRigidBody& btTypedConstraint::getFixedBody()
     137{
     138        static btRigidBody s_fixed(0, 0,0);
     139        s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
     140        return s_fixed;
     141}
     142

  • code/trunk/src/external/bullet/BulletDynamics/ConstraintSolver/btTypedConstraint.h

    r5781 r8351  
    11/*
    22Bullet Continuous Collision Detection and Physics Library
    3 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
     3Copyright (c) 2003-2010 Erwin Coumans  http://continuousphysics.com/Bullet/
    44
    55This software is provided 'as-is', without any express or implied warranty.
    … …  
    2020#include "LinearMath/btScalar.h"
    2121#include "btSolverConstraint.h"
    22 struct  btSolverBody;
    23 
    24 
    25 
     22#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
     23
     24class btSerializer;
    2625
    2726enum btTypedConstraintType
    2827{
    29         POINT2POINT_CONSTRAINT_TYPE,
     28        POINT2POINT_CONSTRAINT_TYPE=MAX_CONTACT_MANIFOLD_TYPE+1,
    3029        HINGE_CONSTRAINT_TYPE,
    3130        CONETWIST_CONSTRAINT_TYPE,
    3231        D6_CONSTRAINT_TYPE,
    33         SLIDER_CONSTRAINT_TYPE
     32        SLIDER_CONSTRAINT_TYPE,
     33        CONTACT_CONSTRAINT_TYPE
    3434};
    3535
     36
     37enum btConstraintParams
     38{
     39        BT_CONSTRAINT_ERP=1,
     40        BT_CONSTRAINT_STOP_ERP,
     41        BT_CONSTRAINT_CFM,
     42        BT_CONSTRAINT_STOP_CFM
     43};
     44
     45#if 1
     46        #define btAssertConstrParams(_par) btAssert(_par)
     47#else
     48        #define btAssertConstrParams(_par)
     49#endif
     50
     51
    3652///TypedConstraint is the baseclass for Bullet constraints and vehicles
    37 class btTypedConstraint
     53class btTypedConstraint : public btTypedObject
    3854{
    3955        int     m_userConstraintType;
    40         int     m_userConstraintId;
    41 
    42         btTypedConstraintType m_constraintType;
     56
     57        union
     58        {
     59                int     m_userConstraintId;
     60                void* m_userConstraintPtr;
     61        };
     62
     63        bool m_needsFeedback;
    4364
    4465        btTypedConstraint&      operator=(btTypedConstraint&    other)
    … …  
    5576        btScalar        m_dbgDrawSize;
    5677
     78        ///internal method used by the constraint solver, don't use them directly
     79        btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact);
     80       
     81        static btRigidBody& getFixedBody();
    5782
    5883public:
    5984
    60         btTypedConstraint(btTypedConstraintType type);
    6185        virtual ~btTypedConstraint() {};
    6286        btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA);
    … …  
    94118                // the constraint.
    95119                int *findex;
     120                // number of solver iterations
     121                int m_numIterations;
     122
     123                //damping of the velocity
     124                btScalar        m_damping;
    96125        };
    97126
    98 
    99         virtual void    buildJacobian() = 0;
    100 
     127        ///internal method used by the constraint solver, don't use them directly
     128        virtual void    buildJacobian() {};
     129
     130        ///internal method used by the constraint solver, don't use them directly
    101131        virtual void    setupSolverConstraint(btConstraintArray& ca, int solverBodyA,int solverBodyB, btScalar timeStep)
    102132        {
    103         }
     133        (void)ca;
     134        (void)solverBodyA;
     135        (void)solverBodyB;
     136        (void)timeStep;
     137        }
     138       
     139        ///internal method used by the constraint solver, don't use them directly
    104140        virtual void getInfo1 (btConstraintInfo1* info)=0;
    105141
     142        ///internal method used by the constraint solver, don't use them directly
    106143        virtual void getInfo2 (btConstraintInfo2* info)=0;
    107144
    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);
     145        ///internal method used by the constraint solver, don't use them directly
     146        void    internalSetAppliedImpulse(btScalar appliedImpulse)
     147        {
     148                m_appliedImpulse = appliedImpulse;
     149        }
     150        ///internal method used by the constraint solver, don't use them directly
     151        btScalar        internalGetAppliedImpulse()
     152        {
     153                return m_appliedImpulse;
     154        }
     155
     156        ///internal method used by the constraint solver, don't use them directly
     157        virtual void    solveConstraintObsolete(btRigidBody& /*bodyA*/,btRigidBody& /*bodyB*/,btScalar  /*timeStep*/) {};
     158
    111159       
    112160        const btRigidBody& getRigidBodyA() const
    … …  
    148196        }
    149197
     198        void    setUserConstraintPtr(void* ptr)
     199        {
     200                m_userConstraintPtr = ptr;
     201        }
     202
     203        void*   getUserConstraintPtr()
     204        {
     205                return m_userConstraintPtr;
     206        }
     207
    150208        int getUid() const
    151209        {
    … …  
    153211        }
    154212
     213        bool    needsFeedback() const
     214        {
     215                return m_needsFeedback;
     216        }
     217
     218        ///enableFeedback will allow to read the applied linear and angular impulse
     219        ///use getAppliedImpulse, getAppliedLinearImpulse and getAppliedAngularImpulse to read feedback information
     220        void    enableFeedback(bool needsFeedback)
     221        {
     222                m_needsFeedback = needsFeedback;
     223        }
     224
     225        ///getAppliedImpulse is an estimated total applied impulse.
     226        ///This feedback could be used to determine breaking constraints or playing sounds.
    155227        btScalar        getAppliedImpulse() const
    156228        {
     229                btAssert(m_needsFeedback);
    157230                return m_appliedImpulse;
    158231        }
    … …  
    160233        btTypedConstraintType getConstraintType () const
    161234        {
    162                 return m_constraintType;
     235                return btTypedConstraintType(m_objectType);
    163236        }
    164237       
    … …  
    171244                return m_dbgDrawSize;
    172245        }
    173        
     246
     247        ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
     248        ///If no axis is provided, it uses the default axis for this constraint.
     249        virtual void    setParam(int num, btScalar value, int axis = -1) = 0;
     250
     251        ///return the local value of parameter
     252        virtual btScalar getParam(int num, int axis = -1) const = 0;
     253       
     254        virtual int     calculateSerializeBufferSize() const;
     255
     256        ///fills the dataBuffer and returns the struct name (and 0 on failure)
     257        virtual const char*     serialize(void* dataBuffer, btSerializer* serializer) const;
     258
    174259};
    175260
     261// returns angle in range [-SIMD_2_PI, SIMD_2_PI], closest to one of the limits
     262// all arguments should be normalized angles (i.e. in range [-SIMD_PI, SIMD_PI])
     263SIMD_FORCE_INLINE btScalar btAdjustAngleToLimits(btScalar angleInRadians, btScalar angleLowerLimitInRadians, btScalar angleUpperLimitInRadians)
     264{
     265        if(angleLowerLimitInRadians >= angleUpperLimitInRadians)
     266        {
     267                return angleInRadians;
     268        }
     269        else if(angleInRadians < angleLowerLimitInRadians)
     270        {
     271                btScalar diffLo = btFabs(btNormalizeAngle(angleLowerLimitInRadians - angleInRadians));
     272                btScalar diffHi = btFabs(btNormalizeAngle(angleUpperLimitInRadians - angleInRadians));
     273                return (diffLo < diffHi) ? angleInRadians : (angleInRadians + SIMD_2_PI);
     274        }
     275        else if(angleInRadians > angleUpperLimitInRadians)
     276        {
     277                btScalar diffHi = btFabs(btNormalizeAngle(angleInRadians - angleUpperLimitInRadians));
     278                btScalar diffLo = btFabs(btNormalizeAngle(angleInRadians - angleLowerLimitInRadians));
     279                return (diffLo < diffHi) ? (angleInRadians - SIMD_2_PI) : angleInRadians;
     280        }
     281        else
     282        {
     283                return angleInRadians;
     284        }
     285}
     286
     287///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
     288struct  btTypedConstraintData
     289{
     290        btRigidBodyData         *m_rbA;
     291        btRigidBodyData         *m_rbB;
     292        char    *m_name;
     293
     294        int     m_objectType;
     295        int     m_userConstraintType;
     296        int     m_userConstraintId;
     297        int     m_needsFeedback;
     298
     299        float   m_appliedImpulse;
     300        float   m_dbgDrawSize;
     301
     302        int     m_disableCollisionsBetweenLinkedBodies;
     303        char    m_pad4[4];
     304       
     305};
     306
     307SIMD_FORCE_INLINE       int     btTypedConstraint::calculateSerializeBufferSize() const
     308{
     309        return sizeof(btTypedConstraintData);
     310}
     311
     312
     313
     314
    176315#endif //TYPED_CONSTRAINT_H
Note: See TracChangeset for help on using the changeset viewer.