Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

Ignore:
Timestamp:
Apr 21, 2011, 6:58:23 PM (13 years ago)
Author:
rgrieder
Message:

Merged revisions 7978 - 8096 from kicklib to kicklib2.

Location:
code/branches/kicklib2
Files:
2 edited

Legend:

Unmodified
Added
Removed

  • code/branches/kicklib2

  • code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp

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