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/btHingeConstraint.cpp

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