Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

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

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

Notes for updating

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

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

Key new features:

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

There are also some regressions:

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

Legend:

Unmodified
Added
Removed

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

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