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

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