Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

Ignore:
Timestamp:
Mar 31, 2009, 8:05:51 PM (16 years ago)
Author:
rgrieder
Message:

Update from Bullet 2.73 to 2.74.

Location:
code/trunk/src/bullet/BulletDynamics
Files:
1 added
31 edited

Legend:

Unmodified
Added
Removed

  • code/trunk/src/bullet/BulletDynamics/CMakeLists.txt

    r2710 r2882  
    3636        ConstraintSolver/btTypedConstraint.h
    3737
     38        Dynamics/btActionInterface.h
    3839        Dynamics/btContinuousDynamicsWorld.h
    3940        Dynamics/btDiscreteDynamicsWorld.h

  • code/trunk/src/bullet/BulletDynamics/Character/btCharacterControllerInterface.h

    r2662 r2882  
    1818
    1919#include "LinearMath/btVector3.h"
     20#include "BulletDynamics/Dynamics/btActionInterface.h"
    2021
    2122class btCollisionShape;
    … …  
    2324class btCollisionWorld;
    2425
    25 class btCharacterControllerInterface
     26class btCharacterControllerInterface : public btActionInterface
    2627{
    2728public:

  • code/trunk/src/bullet/BulletDynamics/Character/btKinematicCharacterController.cpp

    r2662 r2882  
    2323#include "btKinematicCharacterController.h"
    2424
     25static btVector3 upAxisDirection[3] = { btVector3(1.0f, 0.0f, 0.0f), btVector3(0.0f, 1.0f, 0.0f), btVector3(0.0f, 0.0f, 1.0f) };
     26
    2527///@todo Interact with dynamic objects,
    2628///Ride kinematicly animated platforms properly
    … …  
    9496}
    9597
    96 btKinematicCharacterController::btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight)
    97 {
     98btKinematicCharacterController::btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis)
     99{
     100        m_upAxis = upAxis;
    98101        m_addedMargin = 0.02f;
    99102        m_walkDirection.setValue(0,0,0);
    … …  
    102105        m_stepHeight = stepHeight;
    103106        m_turnAngle = btScalar(0.0);
    104         m_convexShape=convexShape;
    105        
     107        m_convexShape=convexShape;     
    106108}
    107109
    … …  
    110112}
    111113
    112 
    113114btPairCachingGhostObject* btKinematicCharacterController::getGhostObject()
    114115{
    … …  
    116117}
    117118
    118 bool btKinematicCharacterController::recoverFromPenetration (btCollisionWorld* collisionWorld)
     119bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld* collisionWorld)
    119120{
    120121
    … …  
    173174        // phase 1: up
    174175        btTransform start, end;
    175         m_targetPosition = m_currentPosition + btVector3 (btScalar(0.0), m_stepHeight, btScalar(0.0));
     176        m_targetPosition = m_currentPosition + upAxisDirection[m_upAxis] * m_stepHeight;
    176177
    177178        start.setIdentity ();
    … …  
    179180
    180181        /* FIXME: Handle penetration properly */
    181         start.setOrigin (m_currentPosition + btVector3(btScalar(0.0), btScalar(0.1), btScalar(0.0)));
     182        start.setOrigin (m_currentPosition + upAxisDirection[m_upAxis] * btScalar(0.1f));
    182183        end.setOrigin (m_targetPosition);
    183184
    … …  
    344345
    345346        // phase 3: down
    346         btVector3 step_drop = btVector3(btScalar(0.0), m_currentStepOffset, btScalar(0.0));
    347         btVector3 gravity_drop = btVector3(btScalar(0.0), m_stepHeight, btScalar(0.0));
     347        btVector3 step_drop = upAxisDirection[m_upAxis] * m_currentStepOffset;
     348        btVector3 gravity_drop = upAxisDirection[m_upAxis] * m_stepHeight;
    348349        m_targetPosition -= (step_drop + gravity_drop);
    349350
    … …  
    390391
    391392
    392 void btKinematicCharacterController::preStep ( btCollisionWorld* collisionWorld)
     393void btKinematicCharacterController::preStep (  btCollisionWorld* collisionWorld)
    393394{
    394395       
    … …  
    413414}
    414415
    415 void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWorld, btScalar dt)
     416void btKinematicCharacterController::playerStep (  btCollisionWorld* collisionWorld, btScalar dt)
    416417{
    417418        btTransform xform;
    … …  
    469470        return true;
    470471}
     472
     473
     474void    btKinematicCharacterController::debugDraw(btIDebugDraw* debugDrawer)
     475{
     476}

  • code/trunk/src/bullet/BulletDynamics/Character/btKinematicCharacterController.h

    r2662 r2882  
    6363
    6464        bool    m_useGhostObjectSweepTest;
     65
     66        int m_upAxis;
    6567       
    6668        btVector3 computeReflectionDirection (const btVector3& direction, const btVector3& normal);
    … …  
    6870        btVector3 perpindicularComponent (const btVector3& direction, const btVector3& normal);
    6971
    70         bool recoverFromPenetration (btCollisionWorld* collisionWorld);
     72        bool recoverFromPenetration ( btCollisionWorld* collisionWorld);
    7173        void stepUp (btCollisionWorld* collisionWorld);
    7274        void updateTargetPositionBasedOnCollision (const btVector3& hit_normal, btScalar tangentMag = btScalar(0.0), btScalar normalMag = btScalar(1.0));
    … …  
    7476        void stepDown (btCollisionWorld* collisionWorld, btScalar dt);
    7577public:
    76         btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight);
     78        btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis = 1);
    7779        ~btKinematicCharacterController ();
    7880       
     81
     82        ///btActionInterface interface
     83        virtual void updateAction( btCollisionWorld* collisionWorld,btScalar deltaTime)
     84        {
     85                preStep ( collisionWorld);
     86                playerStep (collisionWorld, deltaTime);
     87        }
     88       
     89        ///btActionInterface interface
     90        void    debugDraw(btIDebugDraw* debugDrawer);
     91
     92        void setUpAxis (int axis)
     93        {
     94                if (axis < 0)
     95                        axis = 0;
     96                if (axis > 2)
     97                        axis = 2;
     98                m_upAxis = axis;
     99        }
     100
    79101        virtual void    setWalkDirection(const btVector3& walkDirection)
    80102        {
    … …  
    85107        void warp (const btVector3& origin);
    86108
    87         void preStep ( btCollisionWorld* collisionWorld);
    88         void playerStep (btCollisionWorld* collisionWorld, btScalar dt);
     109        void preStep (  btCollisionWorld* collisionWorld);
     110        void playerStep ( btCollisionWorld* collisionWorld, btScalar dt);
    89111
    90112        void setFallSpeed (btScalar fallSpeed);

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

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

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

    r2662 r2882  
    4343        btScalar        m_relaxationFactor;
    4444
     45        btScalar        m_damping;
     46
    4547        btScalar        m_swingSpan1;
    4648        btScalar        m_swingSpan2;
    4749        btScalar        m_twistSpan;
    4850
     51        btScalar        m_fixThresh;
     52
    4953        btVector3   m_swingAxis;
    5054        btVector3       m_twistAxis;
    … …  
    5761        btScalar        m_twistCorrection;
    5862
     63        btScalar        m_twistAngle;
     64
    5965        btScalar        m_accSwingLimitImpulse;
    6066        btScalar        m_accTwistLimitImpulse;
    … …  
    6470        bool            m_solveSwingLimit;
    6571
     72        bool    m_useSolveConstraintObsolete;
     73
     74        // not yet used...
     75        btScalar        m_swingLimitRatio;
     76        btScalar        m_twistLimitRatio;
     77        btVector3   m_twistAxisA;
     78
     79        // motor
     80        bool             m_bMotorEnabled;
     81        bool             m_bNormalizedMotorStrength;
     82        btQuaternion m_qTarget;
     83        btScalar         m_maxMotorImpulse;
     84        btVector3        m_accMotorImpulse;
    6685       
    6786public:
    … …  
    7594        virtual void    buildJacobian();
    7695
    77         virtual void    solveConstraint(btScalar        timeStep);
     96        virtual void getInfo1 (btConstraintInfo1* info);
     97       
     98        virtual void getInfo2 (btConstraintInfo2* info);
     99       
     100
     101        virtual void    solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar        timeStep);
    78102
    79103        void    updateRHS(btScalar      timeStep);
    … …  
    93117        }
    94118
    95         void    setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan,  btScalar _softness = 0.8f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
     119        void    setLimit(int limitIndex,btScalar limitValue)
     120        {
     121                switch (limitIndex)
     122                {
     123                case 3:
     124                        {
     125                                m_twistSpan = limitValue;
     126                                break;
     127                        }
     128                case 4:
     129                        {
     130                                m_swingSpan2 = limitValue;
     131                                break;
     132                        }
     133                case 5:
     134                        {
     135                                m_swingSpan1 = limitValue;
     136                                break;
     137                        }
     138                default:
     139                        {
     140                        }
     141                };
     142        }
     143
     144        void    setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan,  btScalar _softness = 1.f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
    96145        {
    97146                m_swingSpan1 = _swingSpan1;
    … …  
    122171        }
    123172
     173        void calcAngleInfo();
     174        void calcAngleInfo2();
     175
     176        inline btScalar getSwingSpan1()
     177        {
     178                return m_swingSpan1;
     179        }
     180        inline btScalar getSwingSpan2()
     181        {
     182                return m_swingSpan2;
     183        }
     184        inline btScalar getTwistSpan()
     185        {
     186                return m_twistSpan;
     187        }
     188        inline btScalar getTwistAngle()
     189        {
     190                return m_twistAngle;
     191        }
     192        bool isPastSwingLimit() { return m_solveSwingLimit; }
     193
     194
     195        void setDamping(btScalar damping) { m_damping = damping; }
     196
     197        void enableMotor(bool b) { m_bMotorEnabled = b; }
     198        void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = false; }
     199        void setMaxMotorImpulseNormalized(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = true; }
     200
     201        btScalar getFixThresh() { return m_fixThresh; }
     202        void setFixThresh(btScalar fixThresh) { m_fixThresh = fixThresh; }
     203
     204        // setMotorTarget:
     205        // q: the desired rotation of bodyA wrt bodyB.
     206        // note: if q violates the joint limits, the internal target is clamped to avoid conflicting impulses (very bad for stability)
     207        // note: don't forget to enableMotor()
     208        void setMotorTarget(const btQuaternion &q);
     209
     210        // same as above, but q is the desired rotation of frameA wrt frameB in constraint space
     211        void setMotorTargetInConstraintSpace(const btQuaternion &q);
     212
     213        btVector3 GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const;
     214
     215
     216
     217protected:
     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;
    124227};
    125228

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

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

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

    r2662 r2882  
    2323        SOLVER_USE_WARMSTARTING = 4,
    2424        SOLVER_USE_FRICTION_WARMSTARTING = 8,
    25         SOLVER_CACHE_FRIENDLY = 16
     25        SOLVER_USE_2_FRICTION_DIRECTIONS = 16,
     26        SOLVER_ENABLE_FRICTION_DIRECTION_CACHING = 32,
     27        SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION = 64,
     28        SOLVER_CACHE_FRIENDLY = 128,
     29        SOLVER_SIMD = 256,      //enabled for Windows, the solver innerloop is branchless SIMD, 40% faster than FPU/scalar version
     30        SOLVER_CUDA = 512       //will be open sourced during Game Developers Conference 2009. Much faster.
    2631};
    2732
    … …  
    4045        btScalar        m_erp;//used as Baumgarte factor
    4146        btScalar        m_erp2;//used in Split Impulse
     47        btScalar        m_globalCfm;//constraint force mixing
    4248        int                     m_splitImpulse;
    4349        btScalar        m_splitImpulsePenetrationThreshold;
    … …  
    6672                m_erp = btScalar(0.2);
    6773                m_erp2 = btScalar(0.1);
    68                 m_sor = btScalar(1.3);
     74                m_globalCfm = btScalar(0.);
     75                m_sor = btScalar(1.);
    6976                m_splitImpulse = false;
    7077                m_splitImpulsePenetrationThreshold = -0.02f;
    7178                m_linearSlop = btScalar(0.0);
    7279                m_warmstartingFactor=btScalar(0.85);
    73                 m_solverMode = SOLVER_CACHE_FRIENDLY |  SOLVER_RANDMIZE_ORDER |  SOLVER_USE_WARMSTARTING;
     80                m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD ;//SOLVER_RANDMIZE_ORDER
    7481                m_restingContactRestitutionThreshold = 2;//resting contact lifetime threshold to disable restitution
    7582        }

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

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

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

    r2662 r2882  
    2929
    3030class btRigidBody;
     31
     32
    3133
    3234
    … …  
    9395    bool isLimited()
    9496    {
    95         if(m_loLimit>=m_hiLimit) return false;
     97        if(m_loLimit > m_hiLimit) return false;
    9698        return true;
    9799    }
    … …  
    111113
    112114        //! apply the correction impulses for two bodies
    113     btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btRigidBody * body1);
    114 
     115    btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btSolverBody& bodyA,btRigidBody * body1,btSolverBody& bodyB);
    115116
    116117};
    … …  
    130131    btScalar    m_restitution;//! Bounce parameter for linear limit
    131132    //!@}
     133        bool            m_enableMotor[3];
     134    btVector3   m_targetVelocity;//!< target motor velocity
     135    btVector3   m_maxMotorForce;//!< max force on motor
     136    btVector3   m_currentLimitError;//!  How much is violated this limit
     137    int                 m_currentLimit[3];//!< 0=free, 1=at lower limit, 2=at upper limit
    132138
    133139    btTranslationalLimitMotor()
    … …  
    140146        m_damping = btScalar(1.0f);
    141147        m_restitution = btScalar(0.5f);
     148                for(int i=0; i < 3; i++)
     149                {
     150                        m_enableMotor[i] = false;
     151                        m_targetVelocity[i] = btScalar(0.f);
     152                        m_maxMotorForce[i] = btScalar(0.f);
     153                }
    142154    }
    143155
    … …  
    151163        m_damping = other.m_damping;
    152164        m_restitution = other.m_restitution;
     165                for(int i=0; i < 3; i++)
     166                {
     167                        m_enableMotor[i] = other.m_enableMotor[i];
     168                        m_targetVelocity[i] = other.m_targetVelocity[i];
     169                        m_maxMotorForce[i] = other.m_maxMotorForce[i];
     170                }
    153171    }
    154172
    … …  
    164182       return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
    165183    }
     184    inline bool needApplyForce(int limitIndex)
     185    {
     186        if(m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false;
     187        return true;
     188    }
     189        int testLimitValue(int limitIndex, btScalar test_value);
    166190
    167191
    … …  
    169193        btScalar timeStep,
    170194        btScalar jacDiagABInv,
    171         btRigidBody& body1,const btVector3 &pointInA,
    172         btRigidBody& body2,const btVector3 &pointInB,
     195        btRigidBody& body1,btSolverBody& bodyA,const btVector3 &pointInA,
     196        btRigidBody& body2,btSolverBody& bodyB,const btVector3 &pointInB,
    173197        int limit_index,
    174198        const btVector3 & axis_normal_on_a,
    … …  
    248272    btVector3 m_calculatedAxisAngleDiff;
    249273    btVector3 m_calculatedAxis[3];
     274    btVector3 m_calculatedLinearDiff;
    250275   
    251276        btVector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes
    … …  
    263288
    264289
     290        int setAngularLimits(btConstraintInfo2 *info, int row_offset);
     291
     292        int setLinearLimits(btConstraintInfo2 *info);
    265293
    266294    void buildLinearJacobian(
    … …  
    270298    void buildAngularJacobian(btJacobianEntry & jacAngular,const btVector3 & jointAxisW);
    271299
     300        // tests linear limits
     301        void calculateLinearInfo();
    272302
    273303        //! calcs the euler angles between the two bodies.
    … …  
    277307
    278308public:
     309
     310        ///for backwards compatibility during the transition to 'getInfo/getInfo2'
     311        bool            m_useSolveConstraintObsolete;
     312
    279313    btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
    280314
    … …  
    331365    virtual void        buildJacobian();
    332366
    333     virtual     void    solveConstraint(btScalar        timeStep);
     367        virtual void getInfo1 (btConstraintInfo1* info);
     368
     369        virtual void getInfo2 (btConstraintInfo2* info);
     370
     371    virtual     void    solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar        timeStep);
    334372
    335373    void        updateRHS(btScalar      timeStep);
    … …  
    433471        virtual void calcAnchorPos(void); // overridable
    434472
     473        int get_limit_motor_info2(      btRotationalLimitMotor * limot,
     474                                                                btRigidBody * body0, btRigidBody * body1,
     475                                                                btConstraintInfo2 *info, int row, btVector3& ax1, int rotational);
     476
     477
    435478};
    436479

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

    r2662 r2882  
    2020#include "LinearMath/btMinMax.h"
    2121#include <new>
     22#include "btSolverBody.h"
     23
     24//-----------------------------------------------------------------------------
     25
     26#define HINGE_USE_OBSOLETE_SOLVER false
     27
     28//-----------------------------------------------------------------------------
    2229
    2330
    2431btHingeConstraint::btHingeConstraint()
    2532: btTypedConstraint (HINGE_CONSTRAINT_TYPE),
    26 m_enableAngularMotor(false)
    27 {
    28 }
     33m_enableAngularMotor(false),
     34m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
     35m_useReferenceFrameA(false)
     36{
     37        m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
     38}
     39
     40//-----------------------------------------------------------------------------
    2941
    3042btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,
    31                                                                          btVector3& axisInA,btVector3& axisInB)
     43                                                                         btVector3& axisInA,btVector3& axisInB, bool useReferenceFrameA)
    3244                                                                         :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),
    3345                                                                         m_angularOnly(false),
    34                                                                          m_enableAngularMotor(false)
     46                                                                         m_enableAngularMotor(false),
     47                                                                         m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
     48                                                                         m_useReferenceFrameA(useReferenceFrameA)
    3549{
    3650        m_rbAFrame.getOrigin() = pivotInA;
    … …  
    6175       
    6276        m_rbBFrame.getOrigin() = pivotInB;
    63         m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),-axisInB.getX(),
    64                                                                         rbAxisB1.getY(),rbAxisB2.getY(),-axisInB.getY(),
    65                                                                         rbAxisB1.getZ(),rbAxisB2.getZ(),-axisInB.getZ() );
     77        m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
     78                                                                        rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
     79                                                                        rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
    6680       
    6781        //start with free
    … …  
    7286        m_limitSoftness = 0.9f;
    7387        m_solveLimit = false;
    74 
    75 }
    76 
    77 
    78 
    79 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA)
    80 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false)
     88        m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
     89}
     90
     91//-----------------------------------------------------------------------------
     92
     93btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA, bool useReferenceFrameA)
     94:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false),
     95m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
     96m_useReferenceFrameA(useReferenceFrameA)
    8197{
    8298
    … …  
    91107                                                                        rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
    92108
    93         btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * -axisInA;
     109        btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * axisInA;
    94110
    95111        btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
    … …  
    110126        m_limitSoftness = 0.9f;
    111127        m_solveLimit = false;
    112 }
     128        m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
     129}
     130
     131//-----------------------------------------------------------------------------
    113132
    114133btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB,
    115                                                                      const btTransform& rbAFrame, const btTransform& rbBFrame)
     134                                                                     const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA)
    116135:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
    117136m_angularOnly(false),
    118 m_enableAngularMotor(false)
    119 {
    120         // flip axis
    121         m_rbBFrame.getBasis()[0][2] *= btScalar(-1.);
    122         m_rbBFrame.getBasis()[1][2] *= btScalar(-1.);
    123         m_rbBFrame.getBasis()[2][2] *= btScalar(-1.);
    124 
     137m_enableAngularMotor(false),
     138m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
     139m_useReferenceFrameA(useReferenceFrameA)
     140{
    125141        //start with free
    126142        m_lowerLimit = btScalar(1e30);
    … …  
    130146        m_limitSoftness = 0.9f;
    131147        m_solveLimit = false;
     148        m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
    132149}                       
    133150
    134 
    135 
    136 btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame)
     151//-----------------------------------------------------------------------------
     152
     153btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame, bool useReferenceFrameA)
    137154:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame),
    138155m_angularOnly(false),
    139 m_enableAngularMotor(false)
     156m_enableAngularMotor(false),
     157m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
     158m_useReferenceFrameA(useReferenceFrameA)
    140159{
    141160        ///not providing rigidbody B means implicitly using worldspace for body B
    142 
    143         // flip axis
    144         m_rbBFrame.getBasis()[0][2] *= btScalar(-1.);
    145         m_rbBFrame.getBasis()[1][2] *= btScalar(-1.);
    146         m_rbBFrame.getBasis()[2][2] *= btScalar(-1.);
    147161
    148162        m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(m_rbAFrame.getOrigin());
    … …  
    155169        m_limitSoftness = 0.9f;
    156170        m_solveLimit = false;
    157 }
     171        m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
     172}
     173
     174//-----------------------------------------------------------------------------
    158175
    159176void    btHingeConstraint::buildJacobian()
    160177{
    161         m_appliedImpulse = btScalar(0.);
    162 
    163         if (!m_angularOnly)
    164         {
    165                 btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
    166                 btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
    167                 btVector3 relPos = pivotBInW - pivotAInW;
    168 
    169                 btVector3 normal[3];
    170                 if (relPos.length2() > SIMD_EPSILON)
    171                 {
    172                         normal[0] = relPos.normalized();
    173                 }
    174                 else
    175                 {
    176                         normal[0].setValue(btScalar(1.0),0,0);
    177                 }
    178 
    179                 btPlaneSpace1(normal[0], normal[1], normal[2]);
    180 
    181                 for (int i=0;i<3;i++)
    182                 {
    183                         new (&m_jac[i]) btJacobianEntry(
     178        if (m_useSolveConstraintObsolete)
     179        {
     180                m_appliedImpulse = btScalar(0.);
     181
     182                if (!m_angularOnly)
     183                {
     184                        btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
     185                        btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
     186                        btVector3 relPos = pivotBInW - pivotAInW;
     187
     188                        btVector3 normal[3];
     189                        if (relPos.length2() > SIMD_EPSILON)
     190                        {
     191                                normal[0] = relPos.normalized();
     192                        }
     193                        else
     194                        {
     195                                normal[0].setValue(btScalar(1.0),0,0);
     196                        }
     197
     198                        btPlaneSpace1(normal[0], normal[1], normal[2]);
     199
     200                        for (int i=0;i<3;i++)
     201                        {
     202                                new (&m_jac[i]) btJacobianEntry(
    184203                                m_rbA.getCenterOfMassTransform().getBasis().transpose(),
    185204                                m_rbB.getCenterOfMassTransform().getBasis().transpose(),
    … …  
    191210                                m_rbB.getInvInertiaDiagLocal(),
    192211                                m_rbB.getInvMass());
     212                        }
    193213                }
    194         }
    195 
    196         //calculate two perpendicular jointAxis, orthogonal to hingeAxis
    197         //these two jointAxis require equal angular velocities for both bodies
    198 
    199         //this is unused for now, it's a todo
    200         btVector3 jointAxis0local;
    201         btVector3 jointAxis1local;
    202        
    203         btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local);
    204 
    205         getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
    206         btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local;
    207         btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local;
    208         btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
     214
     215                //calculate two perpendicular jointAxis, orthogonal to hingeAxis
     216                //these two jointAxis require equal angular velocities for both bodies
     217
     218                //this is unused for now, it's a todo
     219                btVector3 jointAxis0local;
     220                btVector3 jointAxis1local;
    209221               
    210         new (&m_jacAng[0])      btJacobianEntry(jointAxis0,
    211                 m_rbA.getCenterOfMassTransform().getBasis().transpose(),
    212                 m_rbB.getCenterOfMassTransform().getBasis().transpose(),
    213                 m_rbA.getInvInertiaDiagLocal(),
    214                 m_rbB.getInvInertiaDiagLocal());
    215 
    216         new (&m_jacAng[1])      btJacobianEntry(jointAxis1,
    217                 m_rbA.getCenterOfMassTransform().getBasis().transpose(),
    218                 m_rbB.getCenterOfMassTransform().getBasis().transpose(),
    219                 m_rbA.getInvInertiaDiagLocal(),
    220                 m_rbB.getInvInertiaDiagLocal());
    221 
    222         new (&m_jacAng[2])      btJacobianEntry(hingeAxisWorld,
    223                 m_rbA.getCenterOfMassTransform().getBasis().transpose(),
    224                 m_rbB.getCenterOfMassTransform().getBasis().transpose(),
    225                 m_rbA.getInvInertiaDiagLocal(),
    226                 m_rbB.getInvInertiaDiagLocal());
    227 
    228 
     222                btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local);
     223
     224                getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
     225                btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local;
     226                btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local;
     227                btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
     228                       
     229                new (&m_jacAng[0])      btJacobianEntry(jointAxis0,
     230                        m_rbA.getCenterOfMassTransform().getBasis().transpose(),
     231                        m_rbB.getCenterOfMassTransform().getBasis().transpose(),
     232                        m_rbA.getInvInertiaDiagLocal(),
     233                        m_rbB.getInvInertiaDiagLocal());
     234
     235                new (&m_jacAng[1])      btJacobianEntry(jointAxis1,
     236                        m_rbA.getCenterOfMassTransform().getBasis().transpose(),
     237                        m_rbB.getCenterOfMassTransform().getBasis().transpose(),
     238                        m_rbA.getInvInertiaDiagLocal(),
     239                        m_rbB.getInvInertiaDiagLocal());
     240
     241                new (&m_jacAng[2])      btJacobianEntry(hingeAxisWorld,
     242                        m_rbA.getCenterOfMassTransform().getBasis().transpose(),
     243                        m_rbB.getCenterOfMassTransform().getBasis().transpose(),
     244                        m_rbA.getInvInertiaDiagLocal(),
     245                        m_rbB.getInvInertiaDiagLocal());
     246
     247                        // clear accumulator
     248                        m_accLimitImpulse = btScalar(0.);
     249
     250                        // test angular limit
     251                        testLimit();
     252
     253                //Compute K = J*W*J' for hinge axis
     254                btVector3 axisA =  getRigidBodyA().getCenterOfMassTransform().getBasis() *  m_rbAFrame.getBasis().getColumn(2);
     255                m_kHinge =   1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) +
     256                                                         getRigidBodyB().computeAngularImpulseDenominator(axisA));
     257
     258        }
     259}
     260
     261//-----------------------------------------------------------------------------
     262
     263void btHingeConstraint::getInfo1(btConstraintInfo1* info)
     264{
     265        if (m_useSolveConstraintObsolete)
     266        {
     267                info->m_numConstraintRows = 0;
     268                info->nub = 0;
     269        }
     270        else
     271        {
     272                info->m_numConstraintRows = 5; // Fixed 3 linear + 2 angular
     273                info->nub = 1;
     274                //prepare constraint
     275                testLimit();
     276                if(getSolveLimit() || getEnableAngularMotor())
     277                {
     278                        info->m_numConstraintRows++; // limit 3rd anguar as well
     279                        info->nub--;
     280                }
     281        }
     282} // btHingeConstraint::getInfo1 ()
     283
     284//-----------------------------------------------------------------------------
     285
     286void btHingeConstraint::getInfo2 (btConstraintInfo2* info)
     287{
     288        btAssert(!m_useSolveConstraintObsolete);
     289        int i, s = info->rowskip;
     290        // transforms in world space
     291        btTransform trA = m_rbA.getCenterOfMassTransform()*m_rbAFrame;
     292        btTransform trB = m_rbB.getCenterOfMassTransform()*m_rbBFrame;
     293        // pivot point
     294        btVector3 pivotAInW = trA.getOrigin();
     295        btVector3 pivotBInW = trB.getOrigin();
     296        // linear (all fixed)
     297    info->m_J1linearAxis[0] = 1;
     298    info->m_J1linearAxis[s + 1] = 1;
     299    info->m_J1linearAxis[2 * s + 2] = 1;
     300        btVector3 a1 = pivotAInW - m_rbA.getCenterOfMassTransform().getOrigin();
     301        {
     302                btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
     303                btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + s);
     304                btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * s);
     305                btVector3 a1neg = -a1;
     306                a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
     307        }
     308        btVector3 a2 = pivotBInW - m_rbB.getCenterOfMassTransform().getOrigin();
     309        {
     310                btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
     311                btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + s);
     312                btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * s);
     313                a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
     314        }
     315        // linear RHS
     316    btScalar k = info->fps * info->erp;
     317        for(i = 0; i < 3; i++)
     318    {
     319        info->m_constraintError[i * s] = k * (pivotBInW[i] - pivotAInW[i]);
     320    }
     321        // make rotations around X and Y equal
     322        // the hinge axis should be the only unconstrained
     323        // rotational axis, the angular velocity of the two bodies perpendicular to
     324        // the hinge axis should be equal. thus the constraint equations are
     325        //    p*w1 - p*w2 = 0
     326        //    q*w1 - q*w2 = 0
     327        // where p and q are unit vectors normal to the hinge axis, and w1 and w2
     328        // are the angular velocity vectors of the two bodies.
     329        // get hinge axis (Z)
     330        btVector3 ax1 = trA.getBasis().getColumn(2);
     331        // get 2 orthos to hinge axis (X, Y)
     332        btVector3 p = trA.getBasis().getColumn(0);
     333        btVector3 q = trA.getBasis().getColumn(1);
     334        // set the two hinge angular rows
     335    int s3 = 3 * info->rowskip;
     336    int s4 = 4 * info->rowskip;
     337
     338        info->m_J1angularAxis[s3 + 0] = p[0];
     339        info->m_J1angularAxis[s3 + 1] = p[1];
     340        info->m_J1angularAxis[s3 + 2] = p[2];
     341        info->m_J1angularAxis[s4 + 0] = q[0];
     342        info->m_J1angularAxis[s4 + 1] = q[1];
     343        info->m_J1angularAxis[s4 + 2] = q[2];
     344
     345        info->m_J2angularAxis[s3 + 0] = -p[0];
     346        info->m_J2angularAxis[s3 + 1] = -p[1];
     347        info->m_J2angularAxis[s3 + 2] = -p[2];
     348        info->m_J2angularAxis[s4 + 0] = -q[0];
     349        info->m_J2angularAxis[s4 + 1] = -q[1];
     350        info->m_J2angularAxis[s4 + 2] = -q[2];
     351    // compute the right hand side of the constraint equation. set relative
     352    // body velocities along p and q to bring the hinge back into alignment.
     353    // if ax1,ax2 are the unit length hinge axes as computed from body1 and
     354    // body2, we need to rotate both bodies along the axis u = (ax1 x ax2).
     355    // if `theta' is the angle between ax1 and ax2, we need an angular velocity
     356    // along u to cover angle erp*theta in one step :
     357    //   |angular_velocity| = angle/time = erp*theta / stepsize
     358    //                      = (erp*fps) * theta
     359    //    angular_velocity  = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
     360    //                      = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
     361    // ...as ax1 and ax2 are unit length. if theta is smallish,
     362    // theta ~= sin(theta), so
     363    //    angular_velocity  = (erp*fps) * (ax1 x ax2)
     364    // ax1 x ax2 is in the plane space of ax1, so we project the angular
     365    // velocity to p and q to find the right hand side.
     366    btVector3 ax2 = trB.getBasis().getColumn(2);
     367        btVector3 u = ax1.cross(ax2);
     368        info->m_constraintError[s3] = k * u.dot(p);
     369        info->m_constraintError[s4] = k * u.dot(q);
     370        // check angular limits
     371        int nrow = 4; // last filled row
     372        int srow;
     373        btScalar limit_err = btScalar(0.0);
     374        int limit = 0;
     375        if(getSolveLimit())
     376        {
     377                limit_err = m_correction * m_referenceSign;
     378                limit = (limit_err > btScalar(0.0)) ? 1 : 2;
     379        }
     380        // if the hinge has joint limits or motor, add in the extra row
     381        int powered = 0;
     382        if(getEnableAngularMotor())
     383        {
     384                powered = 1;
     385        }
     386        if(limit || powered)
     387        {
     388                nrow++;
     389                srow = nrow * info->rowskip;
     390                info->m_J1angularAxis[srow+0] = ax1[0];
     391                info->m_J1angularAxis[srow+1] = ax1[1];
     392                info->m_J1angularAxis[srow+2] = ax1[2];
     393
     394                info->m_J2angularAxis[srow+0] = -ax1[0];
     395                info->m_J2angularAxis[srow+1] = -ax1[1];
     396                info->m_J2angularAxis[srow+2] = -ax1[2];
     397
     398                btScalar lostop = getLowerLimit();
     399                btScalar histop = getUpperLimit();
     400                if(limit && (lostop == histop))
     401                {  // the joint motor is ineffective
     402                        powered = 0;
     403                }
     404                info->m_constraintError[srow] = btScalar(0.0f);
     405                if(powered)
     406                {
     407            info->cfm[srow] = btScalar(0.0);
     408                        btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * info->erp);
     409                        info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
     410                        info->m_lowerLimit[srow] = - m_maxMotorImpulse;
     411                        info->m_upperLimit[srow] =   m_maxMotorImpulse;
     412                }
     413                if(limit)
     414                {
     415                        k = info->fps * info->erp;
     416                        info->m_constraintError[srow] += k * limit_err;
     417                        info->cfm[srow] = btScalar(0.0);
     418                        if(lostop == histop)
     419                        {
     420                                // limited low and high simultaneously
     421                                info->m_lowerLimit[srow] = -SIMD_INFINITY;
     422                                info->m_upperLimit[srow] = SIMD_INFINITY;
     423                        }
     424                        else if(limit == 1)
     425                        { // low limit
     426                                info->m_lowerLimit[srow] = 0;
     427                                info->m_upperLimit[srow] = SIMD_INFINITY;
     428                        }
     429                        else
     430                        { // high limit
     431                                info->m_lowerLimit[srow] = -SIMD_INFINITY;
     432                                info->m_upperLimit[srow] = 0;
     433                        }
     434                        // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
     435                        btScalar bounce = m_relaxationFactor;
     436                        if(bounce > btScalar(0.0))
     437                        {
     438                                btScalar vel = m_rbA.getAngularVelocity().dot(ax1);
     439                                vel -= m_rbB.getAngularVelocity().dot(ax1);
     440                                // only apply bounce if the velocity is incoming, and if the
     441                                // resulting c[] exceeds what we already have.
     442                                if(limit == 1)
     443                                {       // low limit
     444                                        if(vel < 0)
     445                                        {
     446                                                btScalar newc = -bounce * vel;
     447                                                if(newc > info->m_constraintError[srow])
     448                                                {
     449                                                        info->m_constraintError[srow] = newc;
     450                                                }
     451                                        }
     452                                }
     453                                else
     454                                {       // high limit - all those computations are reversed
     455                                        if(vel > 0)
     456                                        {
     457                                                btScalar newc = -bounce * vel;
     458                                                if(newc < info->m_constraintError[srow])
     459                                                {
     460                                                        info->m_constraintError[srow] = newc;
     461                                                }
     462                                        }
     463                                }
     464                        }
     465                        info->m_constraintError[srow] *= m_biasFactor;
     466                } // if(limit)
     467        } // if angular limit or powered
     468}
     469
     470//-----------------------------------------------------------------------------
     471
     472void    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//-----------------------------------------------------------------------------
     619
     620void    btHingeConstraint::updateRHS(btScalar   timeStep)
     621{
     622        (void)timeStep;
     623
     624}
     625
     626//-----------------------------------------------------------------------------
     627
     628btScalar btHingeConstraint::getHingeAngle()
     629{
     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));
     634        return m_referenceSign * angle;
     635}
     636
     637//-----------------------------------------------------------------------------
     638
     639void btHingeConstraint::testLimit()
     640{
    229641        // Compute limit information
    230         btScalar hingeAngle = getHingeAngle(); 
    231 
    232         //set bias, sign, clear accumulator
     642        m_hingeAngle = getHingeAngle(); 
    233643        m_correction = btScalar(0.);
    234644        m_limitSign = btScalar(0.);
    235645        m_solveLimit = false;
    236         m_accLimitImpulse = btScalar(0.);
    237 
    238 //      if (m_lowerLimit < m_upperLimit)
    239646        if (m_lowerLimit <= m_upperLimit)
    240647        {
    241 //              if (hingeAngle <= m_lowerLimit*m_limitSoftness)
    242                 if (hingeAngle <= m_lowerLimit)
    243                 {
    244                         m_correction = (m_lowerLimit - hingeAngle);
     648                if (m_hingeAngle <= m_lowerLimit)
     649                {
     650                        m_correction = (m_lowerLimit - m_hingeAngle);
    245651                        m_limitSign = 1.0f;
    246652                        m_solveLimit = true;
    247653                }
    248 //              else if (hingeAngle >= m_upperLimit*m_limitSoftness)
    249                 else if (hingeAngle >= m_upperLimit)
    250                 {
    251                         m_correction = m_upperLimit - hingeAngle;
     654                else if (m_hingeAngle >= m_upperLimit)
     655                {
     656                        m_correction = m_upperLimit - m_hingeAngle;
    252657                        m_limitSign = -1.0f;
    253658                        m_solveLimit = true;
    254659                }
    255660        }
    256 
    257         //Compute K = J*W*J' for hinge axis
    258         btVector3 axisA =  getRigidBodyA().getCenterOfMassTransform().getBasis() *  m_rbAFrame.getBasis().getColumn(2);
    259         m_kHinge =   1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) +
    260                                      getRigidBodyB().computeAngularImpulseDenominator(axisA));
    261 
    262 }
    263 
    264 void    btHingeConstraint::solveConstraint(btScalar     timeStep)
    265 {
    266 
    267         btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
    268         btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
    269 
    270         btScalar tau = btScalar(0.3);
    271 
    272         //linear part
    273         if (!m_angularOnly)
    274         {
    275                 btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
    276                 btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
    277 
    278                 btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
    279                 btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
    280                 btVector3 vel = vel1 - vel2;
    281 
    282                 for (int i=0;i<3;i++)
    283                 {               
    284                         const btVector3& normal = m_jac[i].m_linearJointAxis;
    285                         btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
    286 
    287                         btScalar rel_vel;
    288                         rel_vel = normal.dot(vel);
    289                         //positional error (zeroth order error)
    290                         btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
    291                         btScalar impulse = depth*tau/timeStep  * jacDiagABInv -  rel_vel * jacDiagABInv;
    292                         m_appliedImpulse += impulse;
    293                         btVector3 impulse_vector = normal * impulse;
    294                         m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
    295                         m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
    296                 }
    297         }
    298 
    299        
    300         {
    301                 ///solve angular part
    302 
    303                 // get axes in world space
    304                 btVector3 axisA =  getRigidBodyA().getCenterOfMassTransform().getBasis() *  m_rbAFrame.getBasis().getColumn(2);
    305                 btVector3 axisB =  getRigidBodyB().getCenterOfMassTransform().getBasis() *  m_rbBFrame.getBasis().getColumn(2);
    306 
    307                 const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
    308                 const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
    309 
    310                 btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA);
    311                 btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB);
    312 
    313                 btVector3 angAorthog = angVelA - angVelAroundHingeAxisA;
    314                 btVector3 angBorthog = angVelB - angVelAroundHingeAxisB;
    315                 btVector3 velrelOrthog = angAorthog-angBorthog;
    316                 {
    317                         //solve orthogonal angular velocity correction
    318                         btScalar relaxation = btScalar(1.);
    319                         btScalar len = velrelOrthog.length();
    320                         if (len > btScalar(0.00001))
    321                         {
    322                                 btVector3 normal = velrelOrthog.normalized();
    323                                 btScalar denom = getRigidBodyA().computeAngularImpulseDenominator(normal) +
    324                                         getRigidBodyB().computeAngularImpulseDenominator(normal);
    325                                 // scale for mass and relaxation
    326                                 velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor;
    327                         }
    328 
    329                         //solve angular positional correction
    330                         btVector3 angularError = -axisA.cross(axisB) *(btScalar(1.)/timeStep);
    331                         btScalar len2 = angularError.length();
    332                         if (len2>btScalar(0.00001))
    333                         {
    334                                 btVector3 normal2 = angularError.normalized();
    335                                 btScalar denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) +
    336                                                 getRigidBodyB().computeAngularImpulseDenominator(normal2);
    337                                 angularError *= (btScalar(1.)/denom2) * relaxation;
    338                         }
    339 
    340                         m_rbA.applyTorqueImpulse(-velrelOrthog+angularError);
    341                         m_rbB.applyTorqueImpulse(velrelOrthog-angularError);
    342 
    343                         // solve limit
    344                         if (m_solveLimit)
    345                         {
    346                                 btScalar amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (btScalar(1.)/timeStep)*m_biasFactor  ) * m_limitSign;
    347 
    348                                 btScalar impulseMag = amplitude * m_kHinge;
    349 
    350                                 // Clamp the accumulated impulse
    351                                 btScalar temp = m_accLimitImpulse;
    352                                 m_accLimitImpulse = btMax(m_accLimitImpulse + impulseMag, btScalar(0) );
    353                                 impulseMag = m_accLimitImpulse - temp;
    354 
    355 
    356                                 btVector3 impulse = axisA * impulseMag * m_limitSign;
    357                                 m_rbA.applyTorqueImpulse(impulse);
    358                                 m_rbB.applyTorqueImpulse(-impulse);
    359                         }
    360                 }
    361 
    362                 //apply motor
    363                 if (m_enableAngularMotor)
    364                 {
    365                         //todo: add limits too
    366                         btVector3 angularLimit(0,0,0);
    367 
    368                         btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB;
    369                         btScalar projRelVel = velrel.dot(axisA);
    370 
    371                         btScalar desiredMotorVel = m_motorTargetVelocity;
    372                         btScalar motor_relvel = desiredMotorVel - projRelVel;
    373 
    374                         btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;;
    375                         //todo: should clip against accumulated impulse
    376                         btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse;
    377                         clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse;
    378                         btVector3 motorImp = clippedMotorImpulse * axisA;
    379 
    380                         m_rbA.applyTorqueImpulse(motorImp+angularLimit);
    381                         m_rbB.applyTorqueImpulse(-motorImp-angularLimit);
    382                        
    383                 }
    384         }
    385 
    386 }
    387 
    388 void    btHingeConstraint::updateRHS(btScalar   timeStep)
    389 {
    390         (void)timeStep;
    391 
    392 }
    393 
    394 btScalar btHingeConstraint::getHingeAngle()
    395 {
    396         const btVector3 refAxis0  = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0);
    397         const btVector3 refAxis1  = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1);
    398         const btVector3 swingAxis = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(1);
    399 
    400         return btAtan2Fast( swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)  );
    401 }
    402 
     661        return;
     662} // btHingeConstraint::testLimit()
     663
     664//-----------------------------------------------------------------------------
     665//-----------------------------------------------------------------------------
     666//-----------------------------------------------------------------------------

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

    r2662 r2882  
    5454
    5555        btScalar        m_accLimitImpulse;
     56        btScalar        m_hingeAngle;
     57        btScalar    m_referenceSign;
    5658
    5759        bool            m_angularOnly;
    5860        bool            m_enableAngularMotor;
    5961        bool            m_solveLimit;
     62        bool            m_useSolveConstraintObsolete;
     63        bool            m_useReferenceFrameA;
    6064
    6165       
    6266public:
    6367
    64         btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, btVector3& axisInA,btVector3& axisInB);
     68        btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, btVector3& axisInA,btVector3& axisInB, bool useReferenceFrameA = false);
    6569
    66         btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA);
     70        btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA, bool useReferenceFrameA = false);
    6771       
    68         btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame);
     72        btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false);
    6973
    70         btHingeConstraint(btRigidBody& rbA,const btTransform& rbAFrame);
     74        btHingeConstraint(btRigidBody& rbA,const btTransform& rbAFrame, bool useReferenceFrameA = false);
    7175
    7276        btHingeConstraint();
    … …  
    7478        virtual void    buildJacobian();
    7579
    76         virtual void    solveConstraint(btScalar        timeStep);
     80        virtual void getInfo1 (btConstraintInfo1* info);
     81
     82        virtual void getInfo2 (btConstraintInfo2* info);
     83       
     84        virtual void    solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar        timeStep);
    7785
    7886        void    updateRHS(btScalar      timeStep);
    … …  
    8795        }
    8896
     97        btRigidBody& getRigidBodyA()   
     98        {               
     99                return m_rbA;   
     100        }       
     101
     102        btRigidBody& getRigidBodyB()   
     103        {               
     104                return m_rbB;   
     105        }       
     106       
    89107        void    setAngularOnly(bool angularOnly)
    90108        {
    … …  
    123141        btScalar getHingeAngle();
    124142
     143        void testLimit();
     144
    125145
    126146        const btTransform& getAFrame() { return m_rbAFrame; }; 

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

    r2662 r2882  
    2222
    2323btPoint2PointConstraint::btPoint2PointConstraint()
    24 :btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE)
     24:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE),
     25m_useSolveConstraintObsolete(false)
    2526{
    2627}
    2728
    2829btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB)
    29 :btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB)
     30:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB),
     31m_useSolveConstraintObsolete(false)
    3032{
    3133
    … …  
    3436
    3537btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA)
    36 :btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA))
     38:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)),
     39m_useSolveConstraintObsolete(false)
    3740{
    3841       
    … …  
    4144void    btPoint2PointConstraint::buildJacobian()
    4245{
    43         m_appliedImpulse = btScalar(0.);
    44 
    45         btVector3       normal(0,0,0);
    46 
    47         for (int i=0;i<3;i++)
    48         {
    49                 normal[i] = 1;
    50                 new (&m_jac[i]) btJacobianEntry(
     46        ///we need it for both methods
     47        {
     48                m_appliedImpulse = btScalar(0.);
     49
     50                btVector3       normal(0,0,0);
     51
     52                for (int i=0;i<3;i++)
     53                {
     54                        normal[i] = 1;
     55                        new (&m_jac[i]) btJacobianEntry(
    5156                        m_rbA.getCenterOfMassTransform().getBasis().transpose(),
    5257                        m_rbB.getCenterOfMassTransform().getBasis().transpose(),
    … …  
    5964                        m_rbB.getInvMass());
    6065                normal[i] = 0;
    61         }
    62 
    63 }
    64 
    65 void    btPoint2PointConstraint::solveConstraint(btScalar       timeStep)
    66 {
    67         btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
    68         btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
    69 
    70 
    71         btVector3 normal(0,0,0);
    72        
    73 
    74 //      btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
    75 //      btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
    76 
    77         for (int i=0;i<3;i++)
    78         {               
    79                 normal[i] = 1;
    80                 btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
    81 
    82                 btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
    83                 btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
    84                 //this jacobian entry could be re-used for all iterations
    85                
    86                 btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
    87                 btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
    88                 btVector3 vel = vel1 - vel2;
    89                
    90                 btScalar rel_vel;
    91                 rel_vel = normal.dot(vel);
    92 
    93         /*
    94                 //velocity error (first order error)
    95                 btScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
    96                                                                                                                 m_rbB.getLinearVelocity(),angvelB);
     66                }
     67        }
     68
     69}
     70
     71
     72void btPoint2PointConstraint::getInfo1 (btConstraintInfo1* info)
     73{
     74        if (m_useSolveConstraintObsolete)
     75        {
     76                info->m_numConstraintRows = 0;
     77                info->nub = 0;
     78        } else
     79        {
     80                info->m_numConstraintRows = 3;
     81                info->nub = 3;
     82        }
     83}
     84
     85void btPoint2PointConstraint::getInfo2 (btConstraintInfo2* info)
     86{
     87        btAssert(!m_useSolveConstraintObsolete);
     88
     89         //retrieve matrices
     90        btTransform body0_trans;
     91        body0_trans = m_rbA.getCenterOfMassTransform();
     92    btTransform body1_trans;
     93        body1_trans = m_rbB.getCenterOfMassTransform();
     94
     95        // anchor points in global coordinates with respect to body PORs.
     96   
     97    // set jacobian
     98    info->m_J1linearAxis[0] = 1;
     99    info->m_J1linearAxis[info->rowskip+1] = 1;
     100    info->m_J1linearAxis[2*info->rowskip+2] = 1;
     101
     102        btVector3 a1 = body0_trans.getBasis()*getPivotInA();
     103        {
     104                btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
     105                btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
     106                btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip);
     107                btVector3 a1neg = -a1;
     108                a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
     109        }
     110   
     111        /*info->m_J2linearAxis[0] = -1;
     112    info->m_J2linearAxis[s+1] = -1;
     113    info->m_J2linearAxis[2*s+2] = -1;
    97114        */
    98115       
    99                 //positional error (zeroth order error)
    100                 btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
     116        btVector3 a2 = body1_trans.getBasis()*getPivotInB();
     117   
     118        {
     119                btVector3 a2n = -a2;
     120                btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
     121                btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
     122                btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip);
     123                a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
     124        }
     125   
     126
     127
     128    // set right hand side
     129    btScalar k = info->fps * info->erp;
     130    int j;
     131
     132        for (j=0; j<3; j++)
     133    {
     134        info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] -                     a1[j] - body0_trans.getOrigin()[j]);
     135                //printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
     136    }
     137
     138        btScalar impulseClamp = m_setting.m_impulseClamp;//
     139        for (j=0; j<3; j++)
     140    {
     141                if (m_setting.m_impulseClamp > 0)
     142                {
     143                        info->m_lowerLimit[j*info->rowskip] = -impulseClamp;
     144                        info->m_upperLimit[j*info->rowskip] = impulseClamp;
     145                }
     146        }
     147       
     148}
     149
     150
     151void    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);
    101160               
    102                 btScalar impulse = depth*m_setting.m_tau/timeStep  * jacDiagABInv -  m_setting.m_damping * rel_vel * jacDiagABInv;
    103 
    104                 btScalar impulseClamp = m_setting.m_impulseClamp;
    105                 if (impulseClamp > 0)
    106                 {
    107                         if (impulse < -impulseClamp)
    108                                 impulse = -impulseClamp;
    109                         if (impulse > impulseClamp)
    110                                 impulse = impulseClamp;
     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;
    111221                }
    112 
    113                 m_appliedImpulse+=impulse;
    114                 btVector3 impulse_vector = normal * impulse;
    115                 m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
    116                 m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
    117                
    118                 normal[i] = 0;
    119222        }
    120223}

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

    r2662 r2882  
    5151public:
    5252
     53        ///for backwards compatibility during the transition to 'getInfo/getInfo2'
     54        bool            m_useSolveConstraintObsolete;
     55
    5356        btConstraintSetting     m_setting;
    5457
    … …  
    6164        virtual void    buildJacobian();
    6265
     66        virtual void getInfo1 (btConstraintInfo1* info);
    6367
    64         virtual void    solveConstraint(btScalar        timeStep);
     68        virtual void getInfo2 (btConstraintInfo2* info);
     69
     70
     71        virtual void    solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar        timeStep);
    6572
    6673        void    updateRHS(btScalar      timeStep);

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

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

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

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

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

    r2662 r2882  
    6969btSliderConstraint::btSliderConstraint()
    7070        :btTypedConstraint(SLIDER_CONSTRAINT_TYPE),
    71                 m_useLinearReferenceFrameA(true)
     71                m_useLinearReferenceFrameA(true),
     72                m_useSolveConstraintObsolete(false)
     73//              m_useSolveConstraintObsolete(true)
    7274{
    7375        initParams();
    … …  
    8082        , m_frameInA(frameInA)
    8183        , m_frameInB(frameInB),
    82                 m_useLinearReferenceFrameA(useLinearReferenceFrameA)
     84                m_useLinearReferenceFrameA(useLinearReferenceFrameA),
     85                m_useSolveConstraintObsolete(false)
     86//              m_useSolveConstraintObsolete(true)
    8387{
    8488        initParams();
    … …  
    8993void btSliderConstraint::buildJacobian()
    9094{
     95        if (!m_useSolveConstraintObsolete)
     96        {
     97                return;
     98        }
    9199        if(m_useLinearReferenceFrameA)
    92100        {
    … …  
    156164//-----------------------------------------------------------------------------
    157165
    158 void btSliderConstraint::solveConstraint(btScalar timeStep)
    159 {
    160     m_timeStep = timeStep;
    161         if(m_useLinearReferenceFrameA)
    162         {
    163                 solveConstraintInt(m_rbA, m_rbB);
     166void btSliderConstraint::getInfo1(btConstraintInfo1* info)
     167{
     168        if (m_useSolveConstraintObsolete)
     169        {
     170                info->m_numConstraintRows = 0;
     171                info->nub = 0;
    164172        }
    165173        else
    166174        {
    167                 solveConstraintInt(m_rbB, m_rbA);
     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
     196void btSliderConstraint::getInfo2(btConstraintInfo2* info)
     197{
     198        btAssert(!m_useSolveConstraintObsolete);
     199        int i, s = info->rowskip;
     200        const btTransform& trA = getCalculatedTransformA();
     201        const btTransform& trB = getCalculatedTransformB();
     202        btScalar signFact = m_useLinearReferenceFrameA ? btScalar(1.0f) : btScalar(-1.0f);
     203        // make rotations around Y and Z equal
     204        // the slider axis should be the only unconstrained
     205        // rotational axis, the angular velocity of the two bodies perpendicular to
     206        // the slider axis should be equal. thus the constraint equations are
     207        //    p*w1 - p*w2 = 0
     208        //    q*w1 - q*w2 = 0
     209        // where p and q are unit vectors normal to the slider axis, and w1 and w2
     210        // are the angular velocity vectors of the two bodies.
     211        // get slider axis (X)
     212        btVector3 ax1 = trA.getBasis().getColumn(0);
     213        // get 2 orthos to slider axis (Y, Z)
     214        btVector3 p = trA.getBasis().getColumn(1);
     215        btVector3 q = trA.getBasis().getColumn(2);
     216        // set the two slider rows
     217        info->m_J1angularAxis[0] = p[0];
     218        info->m_J1angularAxis[1] = p[1];
     219        info->m_J1angularAxis[2] = p[2];
     220        info->m_J1angularAxis[s+0] = q[0];
     221        info->m_J1angularAxis[s+1] = q[1];
     222        info->m_J1angularAxis[s+2] = q[2];
     223
     224        info->m_J2angularAxis[0] = -p[0];
     225        info->m_J2angularAxis[1] = -p[1];
     226        info->m_J2angularAxis[2] = -p[2];
     227        info->m_J2angularAxis[s+0] = -q[0];
     228        info->m_J2angularAxis[s+1] = -q[1];
     229        info->m_J2angularAxis[s+2] = -q[2];
     230        // compute the right hand side of the constraint equation. set relative
     231        // body velocities along p and q to bring the slider back into alignment.
     232        // if ax1,ax2 are the unit length slider axes as computed from body1 and
     233        // body2, we need to rotate both bodies along the axis u = (ax1 x ax2).
     234        // if "theta" is the angle between ax1 and ax2, we need an angular velocity
     235        // along u to cover angle erp*theta in one step :
     236        //   |angular_velocity| = angle/time = erp*theta / stepsize
     237        //                      = (erp*fps) * theta
     238        //    angular_velocity  = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
     239        //                      = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
     240        // ...as ax1 and ax2 are unit length. if theta is smallish,
     241        // theta ~= sin(theta), so
     242        //    angular_velocity  = (erp*fps) * (ax1 x ax2)
     243        // ax1 x ax2 is in the plane space of ax1, so we project the angular
     244        // velocity to p and q to find the right hand side.
     245        btScalar k = info->fps * info->erp * getSoftnessOrthoAng();
     246    btVector3 ax2 = trB.getBasis().getColumn(0);
     247        btVector3 u = ax1.cross(ax2);
     248        info->m_constraintError[0] = k * u.dot(p);
     249        info->m_constraintError[s] = k * u.dot(q);
     250        // pull out pos and R for both bodies. also get the connection
     251        // vector c = pos2-pos1.
     252        // next two rows. we want: vel2 = vel1 + w1 x c ... but this would
     253        // result in three equations, so we project along the planespace vectors
     254        // so that sliding along the slider axis is disregarded. for symmetry we
     255        // also consider rotation around center of mass of two bodies (factA and factB).
     256        btTransform bodyA_trans = m_rbA.getCenterOfMassTransform();
     257        btTransform bodyB_trans = m_rbB.getCenterOfMassTransform();
     258        int s2 = 2 * s, s3 = 3 * s;
     259        btVector3 c;
     260        btScalar miA = m_rbA.getInvMass();
     261        btScalar miB = m_rbB.getInvMass();
     262        btScalar miS = miA + miB;
     263        btScalar factA, factB;
     264        if(miS > btScalar(0.f))
     265        {
     266                factA = miB / miS;
     267        }
     268        else
     269        {
     270                factA = btScalar(0.5f);
     271        }
     272        if(factA > 0.99f) factA = 0.99f;
     273        if(factA < 0.01f) factA = 0.01f;
     274        factB = btScalar(1.0f) - factA;
     275        c = bodyB_trans.getOrigin() - bodyA_trans.getOrigin();
     276        btVector3 tmp = c.cross(p);
     277        for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = factA*tmp[i];
     278        for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = factB*tmp[i];
     279        tmp = c.cross(q);
     280        for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = factA*tmp[i];
     281        for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = factB*tmp[i];
     282
     283        for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i];
     284        for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i];
     285        // compute two elements of right hand side. we want to align the offset
     286        // point (in body 2's frame) with the center of body 1.
     287        btVector3 ofs; // offset point in global coordinates
     288        ofs = trB.getOrigin() - trA.getOrigin();
     289        k = info->fps * info->erp * getSoftnessOrthoLin();
     290        info->m_constraintError[s2] = k * p.dot(ofs);
     291        info->m_constraintError[s3] = k * q.dot(ofs);
     292        int nrow = 3; // last filled row
     293        int srow;
     294        // check linear limits linear
     295        btScalar limit_err = btScalar(0.0);
     296        int limit = 0;
     297        if(getSolveLinLimit())
     298        {
     299                limit_err = getLinDepth() *  signFact;
     300                limit = (limit_err > btScalar(0.0)) ? 2 : 1;
     301        }
     302        int powered = 0;
     303        if(getPoweredLinMotor())
     304        {
     305                powered = 1;
     306        }
     307        // if the slider has joint limits or motor, add in the extra row
     308        if (limit || powered)
     309        {
     310                nrow++;
     311                srow = nrow * info->rowskip;
     312                info->m_J1linearAxis[srow+0] = ax1[0];
     313                info->m_J1linearAxis[srow+1] = ax1[1];
     314                info->m_J1linearAxis[srow+2] = ax1[2];
     315                // linear torque decoupling step:
     316                //
     317                // we have to be careful that the linear constraint forces (+/- ax1) applied to the two bodies
     318                // do not create a torque couple. in other words, the points that the
     319                // constraint force is applied at must lie along the same ax1 axis.
     320                // a torque couple will result in limited slider-jointed free
     321                // bodies from gaining angular momentum.
     322                // the solution used here is to apply the constraint forces at the center of mass of the two bodies
     323                btVector3 ltd;  // Linear Torque Decoupling vector (a torque)
     324//              c = btScalar(0.5) * c;
     325                ltd = c.cross(ax1);
     326                info->m_J1angularAxis[srow+0] = factA*ltd[0];
     327                info->m_J1angularAxis[srow+1] = factA*ltd[1];
     328                info->m_J1angularAxis[srow+2] = factA*ltd[2];
     329                info->m_J2angularAxis[srow+0] = factB*ltd[0];
     330                info->m_J2angularAxis[srow+1] = factB*ltd[1];
     331                info->m_J2angularAxis[srow+2] = factB*ltd[2];
     332                // right-hand part
     333                btScalar lostop = getLowerLinLimit();
     334                btScalar histop = getUpperLinLimit();
     335                if(limit && (lostop == histop))
     336                {  // the joint motor is ineffective
     337                        powered = 0;
     338                }
     339                info->m_constraintError[srow] = 0.;
     340                info->m_lowerLimit[srow] = 0.;
     341                info->m_upperLimit[srow] = 0.;
     342                if(powered)
     343                {
     344            info->cfm[nrow] = btScalar(0.0);
     345                        btScalar tag_vel = getTargetLinMotorVelocity();
     346                        btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info->fps * info->erp);
     347//                      info->m_constraintError[srow] += mot_fact * getTargetLinMotorVelocity();
     348                        info->m_constraintError[srow] -= signFact * mot_fact * getTargetLinMotorVelocity();
     349                        info->m_lowerLimit[srow] += -getMaxLinMotorForce() * info->fps;
     350                        info->m_upperLimit[srow] += getMaxLinMotorForce() * info->fps;
     351                }
     352                if(limit)
     353                {
     354                        k = info->fps * info->erp;
     355                        info->m_constraintError[srow] += k * limit_err;
     356                        info->cfm[srow] = btScalar(0.0); // stop_cfm;
     357                        if(lostop == histop)
     358                        {       // limited low and high simultaneously
     359                                info->m_lowerLimit[srow] = -SIMD_INFINITY;
     360                                info->m_upperLimit[srow] = SIMD_INFINITY;
     361                        }
     362                        else if(limit == 1)
     363                        { // low limit
     364                                info->m_lowerLimit[srow] = -SIMD_INFINITY;
     365                                info->m_upperLimit[srow] = 0;
     366                        }
     367                        else
     368                        { // high limit
     369                                info->m_lowerLimit[srow] = 0;
     370                                info->m_upperLimit[srow] = SIMD_INFINITY;
     371                        }
     372                        // bounce (we'll use slider parameter abs(1.0 - m_dampingLimLin) for that)
     373                        btScalar bounce = btFabs(btScalar(1.0) - getDampingLimLin());
     374                        if(bounce > btScalar(0.0))
     375                        {
     376                                btScalar vel = m_rbA.getLinearVelocity().dot(ax1);
     377                                vel -= m_rbB.getLinearVelocity().dot(ax1);
     378                                vel *= signFact;
     379                                // only apply bounce if the velocity is incoming, and if the
     380                                // resulting c[] exceeds what we already have.
     381                                if(limit == 1)
     382                                {       // low limit
     383                                        if(vel < 0)
     384                                        {
     385                                                btScalar newc = -bounce * vel;
     386                                                if (newc > info->m_constraintError[srow])
     387                                                {
     388                                                        info->m_constraintError[srow] = newc;
     389                                                }
     390                                        }
     391                                }
     392                                else
     393                                { // high limit - all those computations are reversed
     394                                        if(vel > 0)
     395                                        {
     396                                                btScalar newc = -bounce * vel;
     397                                                if(newc < info->m_constraintError[srow])
     398                                                {
     399                                                        info->m_constraintError[srow] = newc;
     400                                                }
     401                                        }
     402                                }
     403                        }
     404                        info->m_constraintError[srow] *= getSoftnessLimLin();
     405                } // if(limit)
     406        } // if linear limit
     407        // check angular limits
     408        limit_err = btScalar(0.0);
     409        limit = 0;
     410        if(getSolveAngLimit())
     411        {
     412                limit_err = getAngDepth();
     413                limit = (limit_err > btScalar(0.0)) ? 1 : 2;
     414        }
     415        // if the slider has joint limits, add in the extra row
     416        powered = 0;
     417        if(getPoweredAngMotor())
     418        {
     419                powered = 1;
     420        }
     421        if(limit || powered)
     422        {
     423                nrow++;
     424                srow = nrow * info->rowskip;
     425                info->m_J1angularAxis[srow+0] = ax1[0];
     426                info->m_J1angularAxis[srow+1] = ax1[1];
     427                info->m_J1angularAxis[srow+2] = ax1[2];
     428
     429                info->m_J2angularAxis[srow+0] = -ax1[0];
     430                info->m_J2angularAxis[srow+1] = -ax1[1];
     431                info->m_J2angularAxis[srow+2] = -ax1[2];
     432
     433                btScalar lostop = getLowerAngLimit();
     434                btScalar histop = getUpperAngLimit();
     435                if(limit && (lostop == histop))
     436                {  // the joint motor is ineffective
     437                        powered = 0;
     438                }
     439                if(powered)
     440                {
     441            info->cfm[srow] = btScalar(0.0);
     442                        btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * info->erp);
     443                        info->m_constraintError[srow] = mot_fact * getTargetAngMotorVelocity();
     444                        info->m_lowerLimit[srow] = -getMaxAngMotorForce() * info->fps;
     445                        info->m_upperLimit[srow] = getMaxAngMotorForce() * info->fps;
     446                }
     447                if(limit)
     448                {
     449                        k = info->fps * info->erp;
     450                        info->m_constraintError[srow] += k * limit_err;
     451                        info->cfm[srow] = btScalar(0.0); // stop_cfm;
     452                        if(lostop == histop)
     453                        {
     454                                // limited low and high simultaneously
     455                                info->m_lowerLimit[srow] = -SIMD_INFINITY;
     456                                info->m_upperLimit[srow] = SIMD_INFINITY;
     457                        }
     458                        else if(limit == 1)
     459                        { // low limit
     460                                info->m_lowerLimit[srow] = 0;
     461                                info->m_upperLimit[srow] = SIMD_INFINITY;
     462                        }
     463                        else
     464                        { // high limit
     465                                info->m_lowerLimit[srow] = -SIMD_INFINITY;
     466                                info->m_upperLimit[srow] = 0;
     467                        }
     468                        // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
     469                        btScalar bounce = btFabs(btScalar(1.0) - getDampingLimAng());
     470                        if(bounce > btScalar(0.0))
     471                        {
     472                                btScalar vel = m_rbA.getAngularVelocity().dot(ax1);
     473                                vel -= m_rbB.getAngularVelocity().dot(ax1);
     474                                // only apply bounce if the velocity is incoming, and if the
     475                                // resulting c[] exceeds what we already have.
     476                                if(limit == 1)
     477                                {       // low limit
     478                                        if(vel < 0)
     479                                        {
     480                                                btScalar newc = -bounce * vel;
     481                                                if(newc > info->m_constraintError[srow])
     482                                                {
     483                                                        info->m_constraintError[srow] = newc;
     484                                                }
     485                                        }
     486                                }
     487                                else
     488                                {       // high limit - all those computations are reversed
     489                                        if(vel > 0)
     490                                        {
     491                                                btScalar newc = -bounce * vel;
     492                                                if(newc < info->m_constraintError[srow])
     493                                                {
     494                                                        info->m_constraintError[srow] = newc;
     495                                                }
     496                                        }
     497                                }
     498                        }
     499                        info->m_constraintError[srow] *= getSoftnessLimAng();
     500                } // if(limit)
     501        } // if angular limit or powered
     502} // btSliderConstraint::getInfo2()
     503
     504//-----------------------------------------------------------------------------
     505
     506void btSliderConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
     507{
     508        if (m_useSolveConstraintObsolete)
     509        {
     510                m_timeStep = timeStep;
     511                if(m_useLinearReferenceFrameA)
     512                {
     513                        solveConstraintInt(m_rbA,bodyA, m_rbB,bodyB);
     514                }
     515                else
     516                {
     517                        solveConstraintInt(m_rbB,bodyB, m_rbA,bodyA);
     518                }
    168519        }
    169520} // btSliderConstraint::solveConstraint()
    … …  
    171522//-----------------------------------------------------------------------------
    172523
    173 void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB)
     524void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB)
    174525{
    175526    int i;
    176527    // linear
    177     btVector3 velA = rbA.getVelocityInLocalPoint(m_relPosA);
    178     btVector3 velB = rbB.getVelocityInLocalPoint(m_relPosB);
     528    btVector3 velA;
     529        bodyA.getVelocityInLocalPointObsolete(m_relPosA,velA);
     530    btVector3 velB;
     531        bodyB.getVelocityInLocalPointObsolete(m_relPosB,velB);
    179532    btVector3 vel = velA - velB;
    180533        for(i = 0; i < 3; i++)
    … …  
    191544                btScalar normalImpulse = softness * (restitution * depth / m_timeStep - damping * rel_vel) * m_jacLinDiagABInv[i];
    192545                btVector3 impulse_vector = normal * normalImpulse;
    193                 rbA.applyImpulse( impulse_vector, m_relPosA);
    194                 rbB.applyImpulse(-impulse_vector, m_relPosB);
     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
    195558                if(m_poweredLinMotor && (!i))
    196559                { // apply linear motor
    … …  
    218581                                // apply clamped impulse
    219582                                impulse_vector = normal * normalImpulse;
    220                                 rbA.applyImpulse( impulse_vector, m_relPosA);
    221                                 rbB.applyImpulse(-impulse_vector, m_relPosB);
     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
    222595                        }
    223596                }
    … …  
    228601        btVector3 axisB =  m_calculatedTransformB.getBasis().getColumn(0);
    229602
    230         const btVector3& angVelA = rbA.getAngularVelocity();
    231         const btVector3& angVelB = rbB.getAngularVelocity();
     603        btVector3 angVelA;
     604        bodyA.getAngularVelocity(angVelA);
     605        btVector3 angVelB;
     606        bodyB.getAngularVelocity(angVelB);
    232607
    233608        btVector3 angVelAroundAxisA = axisA * axisA.dot(angVelA);
    … …  
    239614        //solve orthogonal angular velocity correction
    240615        btScalar len = velrelOrthog.length();
     616        btScalar orthorImpulseMag = 0.f;
     617
    241618        if (len > btScalar(0.00001))
    242619        {
    243620                btVector3 normal = velrelOrthog.normalized();
    244621                btScalar denom = rbA.computeAngularImpulseDenominator(normal) + rbB.computeAngularImpulseDenominator(normal);
    245                 velrelOrthog *= (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng;
     622                //velrelOrthog *= (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng;
     623                orthorImpulseMag = (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng;
    246624        }
    247625        //solve angular positional correction
    248626        btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/m_timeStep);
     627        btVector3 angularAxis = angularError;
     628        btScalar angularImpulseMag = 0;
     629
    249630        btScalar len2 = angularError.length();
    250631        if (len2>btScalar(0.00001))
    … …  
    252633                btVector3 normal2 = angularError.normalized();
    253634                btScalar denom2 = rbA.computeAngularImpulseDenominator(normal2) + rbB.computeAngularImpulseDenominator(normal2);
    254                 angularError *= (btScalar(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng;
     635                angularImpulseMag = (btScalar(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng;
     636                angularError *= angularImpulseMag;
    255637        }
    256638        // apply impulse
    257         rbA.applyTorqueImpulse(-velrelOrthog+angularError);
    258         rbB.applyTorqueImpulse(velrelOrthog-angularError);
     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
    259648        btScalar impulseMag;
    260649        //solve angular limits
    … …  
    270659        }
    271660        btVector3 impulse = axisA * impulseMag;
    272         rbA.applyTorqueImpulse(impulse);
    273         rbB.applyTorqueImpulse(-impulse);
     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
    274669        //apply angular motor
    275670        if(m_poweredAngMotor)
    … …  
    302697                        // apply clamped impulse
    303698                        btVector3 motorImp = angImpulse * axisA;
    304                         m_rbA.applyTorqueImpulse(motorImp);
    305                         m_rbB.applyTorqueImpulse(-motorImp);
     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);
    306704                }
    307705        }
    … …  
    313711
    314712void btSliderConstraint::calculateTransforms(void){
    315         if(m_useLinearReferenceFrameA)
     713        if(m_useLinearReferenceFrameA || (!m_useSolveConstraintObsolete))
    316714        {
    317715                m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA;
    … …  
    326724        m_realPivotBInW = m_calculatedTransformB.getOrigin();
    327725        m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X
    328         m_delta = m_realPivotBInW - m_realPivotAInW;
     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        }
    329734        m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis;
    330735    btVector3 normalWorld;
    … …  
    368773
    369774//-----------------------------------------------------------------------------
    370  
    371775
    372776void btSliderConstraint::testAngLimits(void)
    … …  
    380784                const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1);
    381785                btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0)); 
     786                m_angPos = rot;
    382787                if(rot < m_lowerAngLimit)
    383788                {
    … …  
    392797        }
    393798} // btSliderConstraint::testAngLimits()
    394 
    395799       
    396800//-----------------------------------------------------------------------------
    397 
    398 
    399801
    400802btVector3 btSliderConstraint::getAncorInA(void)

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

    r2662 r2882  
    4747{
    4848protected:
     49        ///for backwards compatibility during the transition to 'getInfo/getInfo2'
     50        bool            m_useSolveConstraintObsolete;
    4951        btTransform     m_frameInA;
    5052    btTransform m_frameInB;
    … …  
    105107
    106108        btScalar m_linPos;
     109        btScalar m_angPos;
    107110
    108111        btScalar m_angDepth;
    … …  
    127130        // overrides
    128131    virtual void        buildJacobian();
    129     virtual     void    solveConstraint(btScalar        timeStep);
     132    virtual void getInfo1 (btConstraintInfo1* info);
     133       
     134        virtual void getInfo2 (btConstraintInfo2* info);
     135
     136    virtual     void    solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar        timeStep);
     137       
     138
    130139        // access
    131140    const btRigidBody& getRigidBodyA() const { return m_rbA; }
    … …  
    195204        btScalar getMaxAngMotorForce() { return m_maxAngMotorForce; }
    196205        btScalar getLinearPos() { return m_linPos; }
     206       
    197207
    198208        // access for ODE solver
    … …  
    203213        // internal
    204214    void        buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB);
    205     void        solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB);
     215    void        solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB);
    206216        // shared code used by ODE solver
    207217        void    calculateTransforms(void);
    208218        void    testLinLimits(void);
     219        void    testLinLimits2(btConstraintInfo2* info);
    209220        void    testAngLimits(void);
    210221        // access for PE Solver

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

    r2662 r2882  
    2424#include "LinearMath/btTransformUtil.h"
    2525
     26///Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later, and not double precision
     27#ifdef BT_USE_SSE
     28#define USE_SIMD 1
     29#endif //
     30
     31
     32#ifdef USE_SIMD
     33
     34struct  btSimdScalar
     35{
     36        SIMD_FORCE_INLINE       btSimdScalar()
     37        {
     38
     39        }
     40
     41        SIMD_FORCE_INLINE       btSimdScalar(float      fl)
     42        :m_vec128 (_mm_set1_ps(fl))
     43        {
     44        }
     45
     46        SIMD_FORCE_INLINE       btSimdScalar(__m128 v128)
     47                :m_vec128(v128)
     48        {
     49        }
     50        union
     51        {
     52                __m128          m_vec128;
     53                float           m_floats[4];
     54                int                     m_ints[4];
     55                btScalar        m_unusedPadding;
     56        };
     57        SIMD_FORCE_INLINE       __m128  get128()
     58        {
     59                return m_vec128;
     60        }
     61
     62        SIMD_FORCE_INLINE       const __m128    get128() const
     63        {
     64                return m_vec128;
     65        }
     66
     67        SIMD_FORCE_INLINE       void    set128(__m128 v128)
     68        {
     69                m_vec128 = v128;
     70        }
     71
     72        SIMD_FORCE_INLINE       operator       __m128()       
     73        {
     74                return m_vec128;
     75        }
     76        SIMD_FORCE_INLINE       operator const __m128() const
     77        {
     78                return m_vec128;
     79        }
     80       
     81        SIMD_FORCE_INLINE       operator float() const
     82        {
     83                return m_floats[0];
     84        }
     85
     86};
     87
     88///@brief Return the elementwise product of two btSimdScalar
     89SIMD_FORCE_INLINE btSimdScalar
     90operator*(const btSimdScalar& v1, const btSimdScalar& v2)
     91{
     92        return btSimdScalar(_mm_mul_ps(v1.get128(),v2.get128()));
     93}
     94
     95///@brief Return the elementwise product of two btSimdScalar
     96SIMD_FORCE_INLINE btSimdScalar
     97operator+(const btSimdScalar& v1, const btSimdScalar& v2)
     98{
     99        return btSimdScalar(_mm_add_ps(v1.get128(),v2.get128()));
     100}
     101
     102
     103#else
     104#define btSimdScalar btScalar
     105#endif
    26106
    27107///The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
    … …  
    29109{
    30110        BT_DECLARE_ALIGNED_ALLOCATOR();
     111        btVector3               m_deltaLinearVelocity;
     112        btVector3               m_deltaAngularVelocity;
     113        btScalar                m_angularFactor;
     114        btScalar                m_invMass;
     115        btScalar                m_friction;
     116        btRigidBody*    m_originalBody;
     117        btVector3               m_pushVelocity;
     118        //btVector3             m_turnVelocity;
     119
    31120       
    32         btMatrix3x3             m_worldInvInertiaTensor;
    33 
    34         btVector3               m_angularVelocity;
    35         btVector3               m_linearVelocity;
    36         btVector3               m_centerOfMassPosition;
    37         btVector3               m_pushVelocity;
    38         btVector3               m_turnVelocity;
    39 
    40         float                   m_angularFactor;
    41         float                   m_invMass;
    42         float                   m_friction;
    43         btRigidBody*    m_originalBody;
    44        
    45        
    46         SIMD_FORCE_INLINE void  getVelocityInLocalPoint(const btVector3& rel_pos, btVector3& velocity ) const
     121        SIMD_FORCE_INLINE void  getVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const
    47122        {
    48                 velocity = m_linearVelocity + m_angularVelocity.cross(rel_pos);
     123                if (m_originalBody)
     124                        velocity = m_originalBody->getLinearVelocity()+m_deltaLinearVelocity + (m_originalBody->getAngularVelocity()+m_deltaAngularVelocity).cross(rel_pos);
     125                else
     126                        velocity.setValue(0,0,0);
    49127        }
    50128
     129        SIMD_FORCE_INLINE void  getAngularVelocity(btVector3& angVel) const
     130        {
     131                if (m_originalBody)
     132                        angVel = m_originalBody->getAngularVelocity()+m_deltaAngularVelocity;
     133                else
     134                        angVel.setValue(0,0,0);
     135        }
     136
     137
    51138        //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
    52         SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
     139        SIMD_FORCE_INLINE void applyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude)
    53140        {
    54                 if (m_invMass)
     141                //if (m_invMass)
    55142                {
    56                         m_linearVelocity += linearComponent*impulseMagnitude;
    57                         m_angularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
    58                 }
    59         }
    60        
    61         SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
    62         {
    63                 if (m_invMass)
    64                 {
    65                         m_pushVelocity += linearComponent*impulseMagnitude;
    66                         m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
     143                        m_deltaLinearVelocity += linearComponent*impulseMagnitude;
     144                        m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
    67145                }
    68146        }
    69147
     148       
     149/*
    70150       
    71151        void    writebackVelocity()
    … …  
    73153                if (m_invMass)
    74154                {
    75                         m_originalBody->setLinearVelocity(m_linearVelocity);
    76                         m_originalBody->setAngularVelocity(m_angularVelocity);
     155                        m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+ m_deltaLinearVelocity);
     156                        m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity);
    77157                       
    78158                        //m_originalBody->setCompanionId(-1);
    79159                }
    80160        }
    81        
     161        */
    82162
    83         void    writebackVelocity(btScalar timeStep)
     163        void    writebackVelocity(btScalar timeStep=0)
    84164        {
    85165                if (m_invMass)
    86166                {
    87                         m_originalBody->setLinearVelocity(m_linearVelocity);
    88                         m_originalBody->setAngularVelocity(m_angularVelocity);
    89                        
    90                         //correct the position/orientation based on push/turn recovery
    91                         btTransform newTransform;
    92                         btTransformUtil::integrateTransform(m_originalBody->getWorldTransform(),m_pushVelocity,m_turnVelocity,timeStep,newTransform);
    93                         m_originalBody->setWorldTransform(newTransform);
    94                        
     167                        m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+m_deltaLinearVelocity);
     168                        m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity);
    95169                        //m_originalBody->setCompanionId(-1);
    96170                }
    97171        }
    98 
    99         void    readVelocity()
    100         {
    101                 if (m_invMass)
    102                 {
    103                         m_linearVelocity = m_originalBody->getLinearVelocity();
    104                         m_angularVelocity = m_originalBody->getAngularVelocity();
    105                 }
    106         }
    107 
    108172       
    109173

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

    r2662 r2882  
    2020#include "LinearMath/btVector3.h"
    2121#include "LinearMath/btMatrix3x3.h"
     22#include "btJacobianEntry.h"
    2223
    2324//#define NO_FRICTION_TANGENTIALS 1
     25#include "btSolverBody.h"
     26
    2427
    2528///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
    … …  
    2831        BT_DECLARE_ALIGNED_ALLOCATOR();
    2932
    30         btVector3       m_relpos1CrossNormal;
    31         btVector3       m_contactNormal;
     33        btVector3               m_relpos1CrossNormal;
     34        btVector3               m_contactNormal;
    3235
    33         btVector3       m_relpos2CrossNormal;
    34         btVector3       m_angularComponentA;
     36        btVector3               m_relpos2CrossNormal;
     37        //btVector3             m_contactNormal2;//usually m_contactNormal2 == -m_contactNormal
    3538
    36         btVector3       m_angularComponentB;
    37 
    38         mutable btScalar        m_appliedPushImpulse;
     39        btVector3               m_angularComponentA;
     40        btVector3               m_angularComponentB;
    3941       
    40         mutable btScalar        m_appliedImpulse;
    41         int                     m_solverBodyIdA;
    42         int                     m_solverBodyIdB;
     42        mutable btSimdScalar    m_appliedPushImpulse;
     43        mutable btSimdScalar    m_appliedImpulse;
     44       
    4345       
    4446        btScalar        m_friction;
    45         btScalar        m_restitution;
    4647        btScalar        m_jacDiagABInv;
    47         btScalar        m_penetration;
     48        union
     49        {
     50                int     m_numConsecutiveRowsPerKernel;
     51                btScalar        m_unusedPadding0;
     52        };
     53
     54        union
     55        {
     56                int                     m_frictionIndex;
     57                btScalar        m_unusedPadding1;
     58        };
     59        union
     60        {
     61                int                     m_solverBodyIdA;
     62                btScalar        m_unusedPadding2;
     63        };
     64        union
     65        {
     66                int                     m_solverBodyIdB;
     67                btScalar        m_unusedPadding3;
     68        };
    4869       
     70        union
     71        {
     72                void*           m_originalContactPoint;
     73                btScalar        m_unusedPadding4;
     74        };
    4975
    50        
    51         int                     m_constraintType;
    52         int                     m_frictionIndex;
    53         void*           m_originalContactPoint;
    54         int                     m_unusedPadding[1];
    55 
     76        btScalar                m_rhs;
     77        btScalar                m_cfm;
     78        btScalar                m_lowerLimit;
     79        btScalar                m_upperLimit;
    5680
    5781        enum            btSolverConstraintType
    … …  
    6286};
    6387
    64 
    65 
    66 
     88typedef btAlignedObjectArray<btSolverConstraint>        btConstraintArray;
    6789
    6890

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

    r2662 r2882  
    2020static btRigidBody s_fixed(0, 0,0);
    2121
     22#define DEFAULT_DEBUGDRAW_SIZE btScalar(0.3f)
     23
    2224btTypedConstraint::btTypedConstraint(btTypedConstraintType type)
    2325:m_userConstraintType(-1),
    … …  
    2628m_rbA(s_fixed),
    2729m_rbB(s_fixed),
    28 m_appliedImpulse(btScalar(0.))
     30m_appliedImpulse(btScalar(0.)),
     31m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE)
    2932{
    3033        s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
    … …  
    3639m_rbA(rbA),
    3740m_rbB(s_fixed),
    38 m_appliedImpulse(btScalar(0.))
     41m_appliedImpulse(btScalar(0.)),
     42m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE)
    3943{
    4044                s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
    … …  
    4953m_rbA(rbA),
    5054m_rbB(rbB),
    51 m_appliedImpulse(btScalar(0.))
     55m_appliedImpulse(btScalar(0.)),
     56m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE)
    5257{
    5358                s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
    … …  
    5560}
    5661
     62
     63//-----------------------------------------------------------------------------
     64
     65btScalar btTypedConstraint::getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact)
     66{
     67        if(lowLim > uppLim)
     68        {
     69                return btScalar(1.0f);
     70        }
     71        else if(lowLim == uppLim)
     72        {
     73                return btScalar(0.0f);
     74        }
     75        btScalar lim_fact = btScalar(1.0f);
     76        btScalar delta_max = vel / timeFact;
     77        if(delta_max < btScalar(0.0f))
     78        {
     79                if((pos >= lowLim) && (pos < (lowLim - delta_max)))
     80                {
     81                        lim_fact = (lowLim - pos) / delta_max;
     82                }
     83                else if(pos  < lowLim)
     84                {
     85                        lim_fact = btScalar(0.0f);
     86                }
     87                else
     88                {
     89                        lim_fact = btScalar(1.0f);
     90                }
     91        }
     92        else if(delta_max > btScalar(0.0f))
     93        {
     94                if((pos <= uppLim) && (pos > (uppLim - delta_max)))
     95                {
     96                        lim_fact = (uppLim - pos) / delta_max;
     97                }
     98                else if(pos  > uppLim)
     99                {
     100                        lim_fact = btScalar(0.0f);
     101                }
     102                else
     103                {
     104                        lim_fact = btScalar(1.0f);
     105                }
     106        }
     107        else
     108        {
     109                        lim_fact = btScalar(0.0f);
     110        }
     111        return lim_fact;
     112} // btTypedConstraint::getMotorFactor()
     113
     114

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

    r2662 r2882  
    1919class btRigidBody;
    2020#include "LinearMath/btScalar.h"
     21#include "btSolverConstraint.h"
     22struct  btSolverBody;
     23
     24
     25
    2126
    2227enum btTypedConstraintType
    … …  
    2631        CONETWIST_CONSTRAINT_TYPE,
    2732        D6_CONSTRAINT_TYPE,
    28         VEHICLE_CONSTRAINT_TYPE,
    2933        SLIDER_CONSTRAINT_TYPE
    3034};
    … …  
    4953        btRigidBody&    m_rbB;
    5054        btScalar        m_appliedImpulse;
     55        btScalar        m_dbgDrawSize;
    5156
    5257
    … …  
    5661        virtual ~btTypedConstraint() {};
    5762        btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA);
     63        btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB);
    5864
    59         btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB);
     65        struct btConstraintInfo1 {
     66                int m_numConstraintRows,nub;
     67        };
     68
     69        struct btConstraintInfo2 {
     70                // integrator parameters: frames per second (1/stepsize), default error
     71                // reduction parameter (0..1).
     72                btScalar fps,erp;
     73
     74                // for the first and second body, pointers to two (linear and angular)
     75                // n*3 jacobian sub matrices, stored by rows. these matrices will have
     76                // been initialized to 0 on entry. if the second body is zero then the
     77                // J2xx pointers may be 0.
     78                btScalar *m_J1linearAxis,*m_J1angularAxis,*m_J2linearAxis,*m_J2angularAxis;
     79
     80                // elements to jump from one row to the next in J's
     81                int rowskip;
     82
     83                // right hand sides of the equation J*v = c + cfm * lambda. cfm is the
     84                // "constraint force mixing" vector. c is set to zero on entry, cfm is
     85                // set to a constant value (typically very small or zero) value on entry.
     86                btScalar *m_constraintError,*cfm;
     87
     88                // lo and hi limits for variables (set to -/+ infinity on entry).
     89                btScalar *m_lowerLimit,*m_upperLimit;
     90
     91                // findex vector for variables. see the LCP solver interface for a
     92                // description of what this does. this is set to -1 on entry.
     93                // note that the returned indexes are relative to the first index of
     94                // the constraint.
     95                int *findex;
     96        };
     97
    6098
    6199        virtual void    buildJacobian() = 0;
    62100
    63         virtual void    solveConstraint(btScalar        timeStep) = 0;
     101        virtual void    setupSolverConstraint(btConstraintArray& ca, int solverBodyA,int solverBodyB, btScalar timeStep)
     102        {
     103        }
     104        virtual void getInfo1 (btConstraintInfo1* info)=0;
    64105
     106        virtual void getInfo2 (btConstraintInfo2* info)=0;
     107
     108        virtual void    solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar        timeStep) = 0;
     109
     110        btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact);
     111       
    65112        const btRigidBody& getRigidBodyA() const
    66113        {
    … …  
    95142                m_userConstraintId = uid;
    96143        }
    97        
     144
    98145        int getUserConstraintId() const
    99146        {
    … …  
    115162                return m_constraintType;
    116163        }
    117 
     164       
     165        void setDbgDrawSize(btScalar dbgDrawSize)
     166        {
     167                m_dbgDrawSize = dbgDrawSize;
     168        }
     169        btScalar getDbgDrawSize()
     170        {
     171                return m_dbgDrawSize;
     172        }
     173       
    118174};
    119175

  • code/trunk/src/bullet/BulletDynamics/Dynamics/Bullet-C-API.cpp

    r2662 r2882  
    111111{
    112112        btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world);
    113         assert(dynamicsWorld);
     113        btAssert(dynamicsWorld);
    114114        dynamicsWorld->stepSimulation(timeStep);
    115115}
    … …  
    118118{
    119119        btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world);
    120         assert(dynamicsWorld);
    121         btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
    122         assert(body);
     120        btAssert(dynamicsWorld);
     121        btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
     122        btAssert(body);
    123123
    124124        dynamicsWorld->addRigidBody(body);
    … …  
    128128{
    129129        btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world);
    130         assert(dynamicsWorld);
    131         btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
    132         assert(body);
     130        btAssert(dynamicsWorld);
     131        btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
     132        btAssert(body);
    133133
    134134        dynamicsWorld->removeRigidBody(body);
    … …  
    143143        btVector3 localInertia(0,0,0);
    144144        btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape);
    145         assert(shape);
     145        btAssert(shape);
    146146        if (mass)
    147147        {
    … …  
    159159{
    160160        btRigidBody* body = reinterpret_cast< btRigidBody* >(cbody);
    161         assert(body);
     161        btAssert(body);
    162162        btAlignedFree( body);
    163163}
    … …  
    263263{
    264264        btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape);
    265         assert(shape);
     265        btAssert(shape);
    266266        btAlignedFree(shape);
    267267}
    … …  
    269269{
    270270        btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape);
    271         assert(shape);
     271        btAssert(shape);
    272272        btVector3 scaling(cscaling[0],cscaling[1],cscaling[2]);
    273273        shape->setLocalScaling(scaling);       

  • code/trunk/src/bullet/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp

    r2662 r2882  
    9191
    9292        ///update vehicle simulation
    93         updateVehicles(timeStep);
    94 
     93        updateActions(timeStep);
    9594
    9695        updateActivationState( timeStep );

  • code/trunk/src/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp

    r2662 r2882  
    3030#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
    3131#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
     32#include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h"
     33#include "BulletDynamics/ConstraintSolver/btHingeConstraint.h"
     34#include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.h"
     35#include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h"
     36#include "BulletDynamics/ConstraintSolver/btSliderConstraint.h"
    3237
    3338//for debug rendering
    … …  
    4752
    4853
    49 
    50 //vehicle
    51 #include "BulletDynamics/Vehicle/btRaycastVehicle.h"
    52 #include "BulletDynamics/Vehicle/btVehicleRaycaster.h"
    53 #include "BulletDynamics/Vehicle/btWheelInfo.h"
    54 //character
    55 #include "BulletDynamics/Character/btCharacterControllerInterface.h"
    56 
    57 #include "LinearMath/btIDebugDraw.h"
     54#include "BulletDynamics/Dynamics/btActionInterface.h"
    5855#include "LinearMath/btQuickprof.h"
    5956#include "LinearMath/btMotionState.h"
    … …  
    114111                if (body)
    115112                {
    116                                 btTransform predictedTrans;
    117113                                if (body->getActivationState() != ISLAND_SLEEPING)
    118114                                {
    … …  
    149145                }
    150146        }
     147        bool drawConstraints = false;
     148        if (getDebugDrawer())
     149        {
     150                int mode = getDebugDrawer()->getDebugMode();
     151                if(mode  & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits))
     152                {
     153                        drawConstraints = true;
     154                }
     155        }
     156        if(drawConstraints)
     157        {
     158                for(int i = getNumConstraints()-1; i>=0 ;i--)
     159                {
     160                        btTypedConstraint* constraint = getConstraint(i);
     161                        debugDrawConstraint(constraint);
     162                }
     163        }
     164
    151165
    152166
    … …  
    191205                }
    192206       
    193                 for (  i=0;i<this->m_vehicles.size();i++)
    194                 {
    195                         for (int v=0;v<m_vehicles[i]->getNumWheels();v++)
    196                         {
    197                                 btVector3 wheelColor(0,255,255);
    198                                 if (m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_isInContact)
    199                                 {
    200                                         wheelColor.setValue(0,0,255);
    201                                 } else
    202                                 {
    203                                         wheelColor.setValue(255,0,255);
    204                                 }
    205                
    206                                 btVector3 wheelPosWS = m_vehicles[i]->getWheelInfo(v).m_worldTransform.getOrigin();
    207 
    208                                 btVector3 axle = btVector3(     
    209                                         m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[0][m_vehicles[i]->getRightAxis()],
    210                                         m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[1][m_vehicles[i]->getRightAxis()],
    211                                         m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[2][m_vehicles[i]->getRightAxis()]);
    212 
    213 
    214                                 //m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_wheelAxleWS
    215                                 //debug wheels (cylinders)
    216                                 m_debugDrawer->drawLine(wheelPosWS,wheelPosWS+axle,wheelColor);
    217                                 m_debugDrawer->drawLine(wheelPosWS,m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_contactPointWS,wheelColor);
    218 
     207                if (getDebugDrawer() && getDebugDrawer()->getDebugMode())
     208                {
     209                        for (i=0;i<m_actions.size();i++)
     210                        {
     211                                m_actions[i]->debugDraw(m_debugDrawer);
    219212                        }
    220213                }
    … …  
    288281                }
    289282        }
    290 
     283/*
    291284        if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
    292285        {
    … …  
    300293                }
    301294        }
     295        */
     296
    302297
    303298}
    … …  
    340335        if (getDebugDrawer())
    341336        {
    342                 gDisableDeactivation = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0;
     337                btIDebugDraw* debugDrawer = getDebugDrawer ();
     338                gDisableDeactivation = (debugDrawer->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0;
    343339        }
    344340        if (numSimulationSubSteps)
    … …  
    404400
    405401        ///update vehicle simulation
    406         updateVehicles(timeStep);
    407        
    408         updateCharacters(timeStep);
    409 
     402        updateActions(timeStep);
     403       
    410404        updateActivationState( timeStep );
    411405
    … …  
    471465
    472466
    473 void    btDiscreteDynamicsWorld::updateVehicles(btScalar timeStep)
    474 {
    475         BT_PROFILE("updateVehicles");
    476        
    477         for ( int i=0;i<m_vehicles.size();i++)
    478         {
    479                 btRaycastVehicle* vehicle = m_vehicles[i];
    480                 vehicle->updateVehicle( timeStep);
    481         }
    482 }
    483 
    484 void    btDiscreteDynamicsWorld::updateCharacters(btScalar timeStep)
    485 {
    486         BT_PROFILE("updateCharacters");
    487        
    488         for ( int i=0;i<m_characters.size();i++)
    489         {
    490                 btCharacterControllerInterface* character = m_characters[i];
    491                 character->preStep (this);
    492                 character->playerStep (this,timeStep);
    493         }
    494 }
    495 
     467void    btDiscreteDynamicsWorld::updateActions(btScalar timeStep)
     468{
     469        BT_PROFILE("updateActions");
     470       
     471        for ( int i=0;i<m_actions.size();i++)
     472        {
     473                m_actions[i]->updateAction( this, timeStep);
     474        }
     475}
    496476       
    497477       
    … …  
    550530}
    551531
    552 void    btDiscreteDynamicsWorld::addVehicle(btRaycastVehicle* vehicle)
    553 {
    554         m_vehicles.push_back(vehicle);
    555 }
    556 
    557 void    btDiscreteDynamicsWorld::removeVehicle(btRaycastVehicle* vehicle)
    558 {
    559         m_vehicles.remove(vehicle);
    560 }
    561 
    562 void    btDiscreteDynamicsWorld::addCharacter(btCharacterControllerInterface* character)
    563 {
    564         m_characters.push_back(character);
    565 }
    566 
    567 void    btDiscreteDynamicsWorld::removeCharacter(btCharacterControllerInterface* character)
    568 {
    569         m_characters.remove(character);
     532void    btDiscreteDynamicsWorld::addAction(btActionInterface* action)
     533{
     534        m_actions.push_back(action);
     535}
     536
     537void    btDiscreteDynamicsWorld::removeAction(btActionInterface* action)
     538{
     539        m_actions.remove(action);
     540}
     541
     542
     543void    btDiscreteDynamicsWorld::addVehicle(btActionInterface* vehicle)
     544{
     545        addAction(vehicle);
     546}
     547
     548void    btDiscreteDynamicsWorld::removeVehicle(btActionInterface* vehicle)
     549{
     550        removeAction(vehicle);
     551}
     552
     553void    btDiscreteDynamicsWorld::addCharacter(btActionInterface* character)
     554{
     555        addAction(character);
     556}
     557
     558void    btDiscreteDynamicsWorld::removeCharacter(btActionInterface* character)
     559{
     560        removeAction(character);
    570561}
    571562
    … …  
    643634                        if (islandId<0)
    644635                        {
    645                                 ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
    646                                 m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
     636                                if (numManifolds + m_numConstraints)
     637                                {
     638                                        ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
     639                                        m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
     640                                }
    647641                        } else
    648642                        {
    … …  
    690684        }
    691685
    692 //      assert(0);
     686//      btAssert(0);
    693687               
    694688       
    … …  
    754748        btScalar m_allowedPenetration;
    755749        btOverlappingPairCache* m_pairCache;
     750        btDispatcher* m_dispatcher;
    756751
    757752
    758753public:
    759         btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache) :
     754        btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) :
    760755          btCollisionWorld::ClosestConvexResultCallback(fromA,toA),
    761756                m_allowedPenetration(0.0f),
    762757                m_me(me),
    763                 m_pairCache(pairCache)
     758                m_pairCache(pairCache),
     759                m_dispatcher(dispatcher)
    764760        {
    765761        }
    … …  
    796792                        return false;
    797793
    798                 ///don't do CCD when there are already contact points (touching contact/penetration)
    799                 btAlignedObjectArray<btPersistentManifold*> manifoldArray;
    800                 btBroadphasePair* collisionPair = m_pairCache->findPair(m_me->getBroadphaseHandle(),proxy0);
    801                 if (collisionPair)
    802                 {
    803                         if (collisionPair->m_algorithm)
    804                         {
    805                                 manifoldArray.resize(0);
    806                                 collisionPair->m_algorithm->getAllContactManifolds(manifoldArray);
    807                                 for (int j=0;j<manifoldArray.size();j++)
    808                                 {
    809                                         btPersistentManifold* manifold = manifoldArray[j];
    810                                         if (manifold->getNumContacts()>0)
    811                                                 return false;
     794                btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject;
     795
     796                //call needsResponse, see http://code.google.com/p/bullet/issues/detail?id=179
     797                if (m_dispatcher->needsResponse(m_me,otherObj))
     798                {
     799                        ///don't do CCD when there are already contact points (touching contact/penetration)
     800                        btAlignedObjectArray<btPersistentManifold*> manifoldArray;
     801                        btBroadphasePair* collisionPair = m_pairCache->findPair(m_me->getBroadphaseHandle(),proxy0);
     802                        if (collisionPair)
     803                        {
     804                                if (collisionPair->m_algorithm)
     805                                {
     806                                        manifoldArray.resize(0);
     807                                        collisionPair->m_algorithm->getAllContactManifolds(manifoldArray);
     808                                        for (int j=0;j<manifoldArray.size();j++)
     809                                        {
     810                                                btPersistentManifold* manifold = manifoldArray[j];
     811                                                if (manifold->getNumContacts()>0)
     812                                                        return false;
     813                                        }
    812814                                }
    813815                        }
    … …  
    847849                                                gNumClampedCcdMotions++;
    848850                                               
    849                                                 btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache());
     851                                                btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
    850852                                                btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
    851853                                                btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
    … …  
    11881190
    11891191
     1192void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
     1193{
     1194        bool drawFrames = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraints) != 0;
     1195        bool drawLimits = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraintLimits) != 0;
     1196        btScalar dbgDrawSize = constraint->getDbgDrawSize();
     1197        if(dbgDrawSize <= btScalar(0.f))
     1198        {
     1199                return;
     1200        }
     1201
     1202        switch(constraint->getConstraintType())
     1203        {
     1204                case POINT2POINT_CONSTRAINT_TYPE:
     1205                        {
     1206                                btPoint2PointConstraint* p2pC = (btPoint2PointConstraint*)constraint;
     1207                                btTransform tr;
     1208                                tr.setIdentity();
     1209                                btVector3 pivot = p2pC->getPivotInA();
     1210                                pivot = p2pC->getRigidBodyA().getCenterOfMassTransform() * pivot;
     1211                                tr.setOrigin(pivot);
     1212                                getDebugDrawer()->drawTransform(tr, dbgDrawSize);
     1213                                // that ideally should draw the same frame     
     1214                                pivot = p2pC->getPivotInB();
     1215                                pivot = p2pC->getRigidBodyB().getCenterOfMassTransform() * pivot;
     1216                                tr.setOrigin(pivot);
     1217                                if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
     1218                        }
     1219                        break;
     1220                case HINGE_CONSTRAINT_TYPE:
     1221                        {
     1222                                btHingeConstraint* pHinge = (btHingeConstraint*)constraint;
     1223                                btTransform tr = pHinge->getRigidBodyA().getCenterOfMassTransform() * pHinge->getAFrame();
     1224                                if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
     1225                                tr = pHinge->getRigidBodyB().getCenterOfMassTransform() * pHinge->getBFrame();
     1226                                if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
     1227                                btScalar minAng = pHinge->getLowerLimit();
     1228                                btScalar maxAng = pHinge->getUpperLimit();
     1229                                if(minAng == maxAng)
     1230                                {
     1231                                        break;
     1232                                }
     1233                                bool drawSect = true;
     1234                                if(minAng > maxAng)
     1235                                {
     1236                                        minAng = btScalar(0.f);
     1237                                        maxAng = SIMD_2_PI;
     1238                                        drawSect = false;
     1239                                }
     1240                                if(drawLimits)
     1241                                {
     1242                                        btVector3& center = tr.getOrigin();
     1243                                        btVector3 normal = tr.getBasis().getColumn(2);
     1244                                        btVector3 axis = tr.getBasis().getColumn(0);
     1245                                        getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, minAng, maxAng, btVector3(0,0,0), drawSect);
     1246                                }
     1247                        }
     1248                        break;
     1249                case CONETWIST_CONSTRAINT_TYPE:
     1250                        {
     1251                                btConeTwistConstraint* pCT = (btConeTwistConstraint*)constraint;
     1252                                btTransform tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame();
     1253                                if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
     1254                                tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame();
     1255                                if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
     1256                                if(drawLimits)
     1257                                {
     1258                                        //const btScalar length = btScalar(5);
     1259                                        const btScalar length = dbgDrawSize;
     1260                                        static int nSegments = 8*4;
     1261                                        btScalar fAngleInRadians = btScalar(2.*3.1415926) * (btScalar)(nSegments-1)/btScalar(nSegments);
     1262                                        btVector3 pPrev = pCT->GetPointForAngle(fAngleInRadians, length);
     1263                                        pPrev = tr * pPrev;
     1264                                        for (int i=0; i<nSegments; i++)
     1265                                        {
     1266                                                fAngleInRadians = btScalar(2.*3.1415926) * (btScalar)i/btScalar(nSegments);
     1267                                                btVector3 pCur = pCT->GetPointForAngle(fAngleInRadians, length);
     1268                                                pCur = tr * pCur;
     1269                                                getDebugDrawer()->drawLine(pPrev, pCur, btVector3(0,0,0));
     1270
     1271                                                if (i%(nSegments/8) == 0)
     1272                                                        getDebugDrawer()->drawLine(tr.getOrigin(), pCur, btVector3(0,0,0));
     1273
     1274                                                pPrev = pCur;
     1275                                        }                                               
     1276                                        btScalar tws = pCT->getTwistSpan();
     1277                                        btScalar twa = pCT->getTwistAngle();
     1278                                        bool useFrameB = (pCT->getRigidBodyB().getInvMass() > btScalar(0.f));
     1279                                        if(useFrameB)
     1280                                        {
     1281                                                tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame();
     1282                                        }
     1283                                        else
     1284                                        {
     1285                                                tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame();
     1286                                        }
     1287                                        btVector3 pivot = tr.getOrigin();
     1288                                        btVector3 normal = tr.getBasis().getColumn(0);
     1289                                        btVector3 axis1 = tr.getBasis().getColumn(1);
     1290                                        getDebugDrawer()->drawArc(pivot, normal, axis1, dbgDrawSize, dbgDrawSize, -twa-tws, -twa+tws, btVector3(0,0,0), true);
     1291
     1292                                }
     1293                        }
     1294                        break;
     1295                case D6_CONSTRAINT_TYPE:
     1296                        {
     1297                                btGeneric6DofConstraint* p6DOF = (btGeneric6DofConstraint*)constraint;
     1298                                btTransform tr = p6DOF->getCalculatedTransformA();
     1299                                if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
     1300                                tr = p6DOF->getCalculatedTransformB();
     1301                                if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
     1302                                if(drawLimits)
     1303                                {
     1304                                        tr = p6DOF->getCalculatedTransformA();
     1305                                        const btVector3& center = p6DOF->getCalculatedTransformB().getOrigin();
     1306                                        btVector3 up = tr.getBasis().getColumn(2);
     1307                                        btVector3 axis = tr.getBasis().getColumn(0);
     1308                                        btScalar minTh = p6DOF->getRotationalLimitMotor(1)->m_loLimit;
     1309                                        btScalar maxTh = p6DOF->getRotationalLimitMotor(1)->m_hiLimit;
     1310                                        btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit;
     1311                                        btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit;
     1312                                        getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0,0,0));
     1313                                        axis = tr.getBasis().getColumn(1);
     1314                                        btScalar ay = p6DOF->getAngle(1);
     1315                                        btScalar az = p6DOF->getAngle(2);
     1316                                        btScalar cy = btCos(ay);
     1317                                        btScalar sy = btSin(ay);
     1318                                        btScalar cz = btCos(az);
     1319                                        btScalar sz = btSin(az);
     1320                                        btVector3 ref;
     1321                                        ref[0] = cy*cz*axis[0] + cy*sz*axis[1] - sy*axis[2];
     1322                                        ref[1] = -sz*axis[0] + cz*axis[1];
     1323                                        ref[2] = cz*sy*axis[0] + sz*sy*axis[1] + cy*axis[2];
     1324                                        tr = p6DOF->getCalculatedTransformB();
     1325                                        btVector3 normal = -tr.getBasis().getColumn(0);
     1326                                        btScalar minFi = p6DOF->getRotationalLimitMotor(0)->m_loLimit;
     1327                                        btScalar maxFi = p6DOF->getRotationalLimitMotor(0)->m_hiLimit;
     1328                                        if(minFi > maxFi)
     1329                                        {
     1330                                                getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, -SIMD_PI, SIMD_PI, btVector3(0,0,0), false);
     1331                                        }
     1332                                        else if(minFi < maxFi)
     1333                                        {
     1334                                                getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, minFi, maxFi, btVector3(0,0,0), true);
     1335                                        }
     1336                                        tr = p6DOF->getCalculatedTransformA();
     1337                                        btVector3 bbMin = p6DOF->getTranslationalLimitMotor()->m_lowerLimit;
     1338                                        btVector3 bbMax = p6DOF->getTranslationalLimitMotor()->m_upperLimit;
     1339                                        getDebugDrawer()->drawBox(bbMin, bbMax, tr, btVector3(0,0,0));
     1340                                }
     1341                        }
     1342                        break;
     1343                case SLIDER_CONSTRAINT_TYPE:
     1344                        {
     1345                                btSliderConstraint* pSlider = (btSliderConstraint*)constraint;
     1346                                btTransform tr = pSlider->getCalculatedTransformA();
     1347                                if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
     1348                                tr = pSlider->getCalculatedTransformB();
     1349                                if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
     1350                                if(drawLimits)
     1351                                {
     1352                                        btTransform tr = pSlider->getCalculatedTransformA();
     1353                                        btVector3 li_min = tr * btVector3(pSlider->getLowerLinLimit(), 0.f, 0.f);
     1354                                        btVector3 li_max = tr * btVector3(pSlider->getUpperLinLimit(), 0.f, 0.f);
     1355                                        getDebugDrawer()->drawLine(li_min, li_max, btVector3(0, 0, 0));
     1356                                        btVector3 normal = tr.getBasis().getColumn(0);
     1357                                        btVector3 axis = tr.getBasis().getColumn(1);
     1358                                        btScalar a_min = pSlider->getLowerAngLimit();
     1359                                        btScalar a_max = pSlider->getUpperAngLimit();
     1360                                        const btVector3& center = pSlider->getCalculatedTransformB().getOrigin();
     1361                                        getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, a_min, a_max, btVector3(0,0,0), true);
     1362                                }
     1363                        }
     1364                        break;
     1365                default :
     1366                        break;
     1367        }
     1368        return;
     1369} // btDiscreteDynamicsWorld::debugDrawConstraint()
     1370
     1371
     1372
     1373
     1374
    11901375void    btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
    11911376{
    … …  
    12161401        return m_constraints[index];
    12171402}
     1403
     1404

  • code/trunk/src/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h

    r2662 r2882  
    2424class btSimulationIslandManager;
    2525class btTypedConstraint;
     26class btActionInterface;
    2627
    27 
    28 class btRaycastVehicle;
    29 class btCharacterControllerInterface;
    3028class btIDebugDraw;
    3129#include "LinearMath/btAlignedObjectArray.h"
    … …  
    5351        bool    m_ownsConstraintSolver;
    5452
     53        btAlignedObjectArray<btActionInterface*>        m_actions;
    5554       
    56         btAlignedObjectArray<btRaycastVehicle*> m_vehicles;
    57        
    58         btAlignedObjectArray<btCharacterControllerInterface*> m_characters;
    59        
    60 
    6155        int     m_profileTimings;
    6256
    … …  
    7165        void    updateActivationState(btScalar timeStep);
    7266
    73         void    updateVehicles(btScalar timeStep);
    74 
    75         void    updateCharacters(btScalar timeStep);
     67        void    updateActions(btScalar timeStep);
    7668
    7769        void    startProfiling(btScalar timeStep);
    … …  
    10698        virtual void    removeConstraint(btTypedConstraint* constraint);
    10799
    108         virtual void    addVehicle(btRaycastVehicle* vehicle);
     100        virtual void    addAction(btActionInterface*);
    109101
    110         virtual void    removeVehicle(btRaycastVehicle* vehicle);
     102        virtual void    removeAction(btActionInterface*);
    111103       
    112         virtual void    addCharacter(btCharacterControllerInterface* character);
    113 
    114         virtual void    removeCharacter(btCharacterControllerInterface* character);
    115                
    116 
    117104        btSimulationIslandManager*      getSimulationIslandManager()
    118105        {
    … …  
    131118
    132119        virtual void    setGravity(const btVector3& gravity);
     120
    133121        virtual btVector3 getGravity () const;
    134122
    … …  
    140128
    141129        void    debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color);
     130
     131        void    debugDrawConstraint(btTypedConstraint* constraint);
    142132
    143133        virtual void    debugDrawWorld();
    … …  
    170160        }
    171161
     162        ///obsolete, use updateActions instead
     163        virtual void updateVehicles(btScalar timeStep)
     164        {
     165                updateActions(timeStep);
     166        }
     167
     168        ///obsolete, use addAction instead
     169        virtual void    addVehicle(btActionInterface* vehicle);
     170        ///obsolete, use removeAction instead
     171        virtual void    removeVehicle(btActionInterface* vehicle);
     172        ///obsolete, use addAction instead
     173        virtual void    addCharacter(btActionInterface* character);
     174        ///obsolete, use removeAction instead
     175        virtual void    removeCharacter(btActionInterface* character);
     176
    172177};
    173178

  • code/trunk/src/bullet/BulletDynamics/Dynamics/btDynamicsWorld.h

    r2662 r2882  
    2121
    2222class btTypedConstraint;
    23 class btRaycastVehicle;
     23class btActionInterface;
    2424class btConstraintSolver;
    2525class btDynamicsWorld;
    26 class btCharacterControllerInterface;
     26
    2727
    2828/// Type for the callback for each tick
    … …  
    7373                virtual void    removeConstraint(btTypedConstraint* constraint) {(void)constraint;}
    7474
    75                 virtual void    addVehicle(btRaycastVehicle* vehicle) {(void)vehicle;}
     75                virtual void    addAction(btActionInterface* action) = 0;
    7676
    77                 virtual void    removeVehicle(btRaycastVehicle* vehicle) {(void)vehicle;}
    78 
    79                 virtual void    addCharacter(btCharacterControllerInterface* character) {(void)character;}
    80 
    81                 virtual void    removeCharacter(btCharacterControllerInterface* character) {(void)character;}
    82 
     77                virtual void    removeAction(btActionInterface* action) = 0;
    8378
    8479                //once a rigidbody is added to the dynamics world, it will get this gravity assigned
    … …  
    130125
    131126
     127                ///obsolete, use addAction instead.
     128                virtual void    addVehicle(btActionInterface* vehicle) {(void)vehicle;}
     129                ///obsolete, use removeAction instead
     130                virtual void    removeVehicle(btActionInterface* vehicle) {(void)vehicle;}
     131                ///obsolete, use addAction instead.
     132                virtual void    addCharacter(btActionInterface* character) {(void)character;}
     133                ///obsolete, use removeAction instead
     134                virtual void    removeCharacter(btActionInterface* character) {(void)character;}
     135
     136
    132137};
    133138

  • code/trunk/src/bullet/BulletDynamics/Dynamics/btRigidBody.cpp

    r2662 r2882  
    4747        m_angularFactor = btScalar(1.);
    4848        m_gravity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
     49        m_gravity_acceleration.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
    4950        m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
    5051        m_totalTorque.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)),
    … …  
    125126                m_gravity = acceleration * (btScalar(1.0) / m_inverseMass);
    126127        }
     128        m_gravity_acceleration = acceleration;
    127129}
    128130

  • code/trunk/src/bullet/BulletDynamics/Dynamics/btRigidBody.h

    r2662 r2882  
    4949
    5050        btVector3               m_gravity;     
     51        btVector3               m_gravity_acceleration;
    5152        btVector3               m_invInertiaLocal;
    5253        btVector3               m_totalForce;
    … …  
    182183        const btVector3&        getGravity() const
    183184        {
    184                 return m_gravity;
     185                return m_gravity_acceleration;
    185186        }
    186187
    … …  
    232233                m_totalForce += force;
    233234        }
     235
     236        const btVector3& getTotalForce()
     237        {
     238                return m_totalForce;
     239        };
     240
     241        const btVector3& getTotalTorque()
     242        {
     243                return m_totalTorque;
     244        };
    234245   
    235         const btVector3& getInvInertiaDiagLocal()
     246        const btVector3& getInvInertiaDiagLocal() const
    236247        {
    237248                return m_invInertiaLocal;

  • code/trunk/src/bullet/BulletDynamics/Vehicle/btRaycastVehicle.cpp

    r2662 r2882  
    2020#include "btWheelInfo.h"
    2121#include "LinearMath/btMinMax.h"
    22 
    23 
     22#include "LinearMath/btIDebugDraw.h"
    2423#include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
    2524
    … …  
    2726
    2827btRaycastVehicle::btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis,  btVehicleRaycaster* raycaster )
    29 : btTypedConstraint(VEHICLE_CONSTRAINT_TYPE),
    30 m_vehicleRaycaster(raycaster),
     28:m_vehicleRaycaster(raycaster),
    3129m_pitchControl(btScalar(0.))
    3230{
    … …  
    8886const btTransform&      btRaycastVehicle::getWheelTransformWS( int wheelIndex ) const
    8987{
    90         assert(wheelIndex < getNumWheels());
     88        btAssert(wheelIndex < getNumWheels());
    9189        const btWheelInfo& wheel = m_wheelInfo[wheelIndex];
    9290        return wheel.m_worldTransform;
    … …  
    176174        btVehicleRaycaster::btVehicleRaycasterResult    rayResults;
    177175
    178         assert(m_vehicleRaycaster);
     176        btAssert(m_vehicleRaycaster);
    179177
    180178        void* object = m_vehicleRaycaster->castRay(source,target,rayResults);
    … …  
    360358void    btRaycastVehicle::setSteeringValue(btScalar steering,int wheel)
    361359{
    362         assert(wheel>=0 && wheel < getNumWheels());
     360        btAssert(wheel>=0 && wheel < getNumWheels());
    363361
    364362        btWheelInfo& wheelInfo = getWheelInfo(wheel);
    … …  
    376374void    btRaycastVehicle::applyEngineForce(btScalar force, int wheel)
    377375{
    378         assert(wheel>=0 && wheel < getNumWheels());
     376        btAssert(wheel>=0 && wheel < getNumWheels());
    379377        btWheelInfo& wheelInfo = getWheelInfo(wheel);
    380378        wheelInfo.m_engineForce = force;
    … …  
    705703
    706704
     705
     706void    btRaycastVehicle::debugDraw(btIDebugDraw* debugDrawer)
     707{
     708
     709        for (int v=0;v<this->getNumWheels();v++)
     710        {
     711                btVector3 wheelColor(0,255,255);
     712                if (getWheelInfo(v).m_raycastInfo.m_isInContact)
     713                {
     714                        wheelColor.setValue(0,0,255);
     715                } else
     716                {
     717                        wheelColor.setValue(255,0,255);
     718                }
     719
     720                btVector3 wheelPosWS = getWheelInfo(v).m_worldTransform.getOrigin();
     721
     722                btVector3 axle = btVector3(     
     723                        getWheelInfo(v).m_worldTransform.getBasis()[0][getRightAxis()],
     724                        getWheelInfo(v).m_worldTransform.getBasis()[1][getRightAxis()],
     725                        getWheelInfo(v).m_worldTransform.getBasis()[2][getRightAxis()]);
     726
     727                //debug wheels (cylinders)
     728                debugDrawer->drawLine(wheelPosWS,wheelPosWS+axle,wheelColor);
     729                debugDrawer->drawLine(wheelPosWS,getWheelInfo(v).m_raycastInfo.m_contactPointWS,wheelColor);
     730
     731        }
     732}
     733
     734
    707735void* btDefaultVehicleRaycaster::castRay(const btVector3& from,const btVector3& to, btVehicleRaycasterResult& result)
    708736{
    … …  
    728756        return 0;
    729757}
     758

  • code/trunk/src/bullet/BulletDynamics/Vehicle/btRaycastVehicle.h

    r2662 r2882  
    1818#include "LinearMath/btAlignedObjectArray.h"
    1919#include "btWheelInfo.h"
     20#include "BulletDynamics/Dynamics/btActionInterface.h"
    2021
    2122class btVehicleTuning;
    2223
    2324///rayCast vehicle, very special constraint that turn a rigidbody into a vehicle.
    24 class btRaycastVehicle : public btTypedConstraint
     25class btRaycastVehicle : public btActionInterface
    2526{
    2627
    … …  
    7475        virtual ~btRaycastVehicle() ;
    7576
    76                
     77
     78        ///btActionInterface interface
     79        virtual void updateAction( btCollisionWorld* collisionWorld, btScalar step)
     80        {
     81                updateVehicle(step);
     82        }
     83       
     84
     85        ///btActionInterface interface
     86        void    debugDraw(btIDebugDraw* debugDrawer);
     87                       
    7788        const btTransform& getChassisWorldTransform() const;
    7889       
    … …  
    8091
    8192        virtual void updateVehicle(btScalar step);
    82 
     93       
     94       
    8395        void resetSuspension();
    8496
    … …  
    176188        }
    177189
    178         virtual void    buildJacobian()
    179         {
    180                 //not yet
    181         }
    182 
    183         virtual void    solveConstraint(btScalar        timeStep)
    184         {
    185                 (void)timeStep;
    186                 //not yet
    187         }
    188190
    189191
Note: See TracChangeset for help on using the changeset viewer.