diff options
Diffstat (limited to 'thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp')
| -rw-r--r-- | thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp | 708 |
1 files changed, 310 insertions, 398 deletions
diff --git a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp index c38b8353f0..1f54203532 100644 --- a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp +++ b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp @@ -25,83 +25,61 @@ http://gimpact.sf.net #include "LinearMath/btTransformUtil.h" #include <new> - - #define D6_USE_OBSOLETE_METHOD false #define D6_USE_FRAME_OFFSET true - - - - - btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA) -: btTypedConstraint(D6_CONSTRAINT_TYPE, rbA, rbB) -, m_frameInA(frameInA) -, m_frameInB(frameInB), -m_useLinearReferenceFrameA(useLinearReferenceFrameA), -m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET), -m_flags(0), -m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD) + : btTypedConstraint(D6_CONSTRAINT_TYPE, rbA, rbB), m_frameInA(frameInA), m_frameInB(frameInB), m_useLinearReferenceFrameA(useLinearReferenceFrameA), m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET), m_flags(0), m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD) { calculateTransforms(); } - - btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB) - : btTypedConstraint(D6_CONSTRAINT_TYPE, getFixedBody(), rbB), - m_frameInB(frameInB), - m_useLinearReferenceFrameA(useLinearReferenceFrameB), - m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET), - m_flags(0), - m_useSolveConstraintObsolete(false) + : btTypedConstraint(D6_CONSTRAINT_TYPE, getFixedBody(), rbB), + m_frameInB(frameInB), + m_useLinearReferenceFrameA(useLinearReferenceFrameB), + m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET), + m_flags(0), + m_useSolveConstraintObsolete(false) { ///not providing rigidbody A means implicitly using worldspace for body A m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB; calculateTransforms(); } - - - #define GENERIC_D6_DISABLE_WARMSTARTING 1 - - btScalar btGetMatrixElem(const btMatrix3x3& mat, int index); btScalar btGetMatrixElem(const btMatrix3x3& mat, int index) { - int i = index%3; - int j = index/3; + int i = index % 3; + int j = index / 3; return mat[i][j]; } - - ///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html -bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz); -bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz) +bool matrixToEulerXYZ(const btMatrix3x3& mat, btVector3& xyz); +bool matrixToEulerXYZ(const btMatrix3x3& mat, btVector3& xyz) { // // rot = cy*cz -cy*sz sy // // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx // // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy // - btScalar fi = btGetMatrixElem(mat,2); + btScalar fi = btGetMatrixElem(mat, 2); if (fi < btScalar(1.0f)) { if (fi > btScalar(-1.0f)) { - xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8)); - xyz[1] = btAsin(btGetMatrixElem(mat,2)); - xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0)); + xyz[0] = btAtan2(-btGetMatrixElem(mat, 5), btGetMatrixElem(mat, 8)); + xyz[1] = btAsin(btGetMatrixElem(mat, 2)); + xyz[2] = btAtan2(-btGetMatrixElem(mat, 1), btGetMatrixElem(mat, 0)); return true; } else { // WARNING. Not unique. XA - ZA = -atan2(r10,r11) - xyz[0] = -btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4)); + xyz[0] = -btAtan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4)); xyz[1] = -SIMD_HALF_PI; xyz[2] = btScalar(0.0); return false; @@ -110,7 +88,7 @@ bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz) else { // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11) - xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4)); + xyz[0] = btAtan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4)); xyz[1] = SIMD_HALF_PI; xyz[2] = 0.0; } @@ -121,52 +99,49 @@ bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz) int btRotationalLimitMotor::testLimitValue(btScalar test_value) { - if(m_loLimit>m_hiLimit) + if (m_loLimit > m_hiLimit) { - m_currentLimit = 0;//Free from violation + m_currentLimit = 0; //Free from violation return 0; } if (test_value < m_loLimit) { - m_currentLimit = 1;//low limit violation - m_currentLimitError = test_value - m_loLimit; - if(m_currentLimitError>SIMD_PI) - m_currentLimitError-=SIMD_2_PI; - else if(m_currentLimitError<-SIMD_PI) - m_currentLimitError+=SIMD_2_PI; + m_currentLimit = 1; //low limit violation + m_currentLimitError = test_value - m_loLimit; + if (m_currentLimitError > SIMD_PI) + m_currentLimitError -= SIMD_2_PI; + else if (m_currentLimitError < -SIMD_PI) + m_currentLimitError += SIMD_2_PI; return 1; } - else if (test_value> m_hiLimit) + else if (test_value > m_hiLimit) { - m_currentLimit = 2;//High limit violation + m_currentLimit = 2; //High limit violation m_currentLimitError = test_value - m_hiLimit; - if(m_currentLimitError>SIMD_PI) - m_currentLimitError-=SIMD_2_PI; - else if(m_currentLimitError<-SIMD_PI) - m_currentLimitError+=SIMD_2_PI; + if (m_currentLimitError > SIMD_PI) + m_currentLimitError -= SIMD_2_PI; + else if (m_currentLimitError < -SIMD_PI) + m_currentLimitError += SIMD_2_PI; return 2; }; - m_currentLimit = 0;//Free from violation + m_currentLimit = 0; //Free from violation return 0; - } - - btScalar btRotationalLimitMotor::solveAngularLimits( - btScalar timeStep,btVector3& axis,btScalar jacDiagABInv, - btRigidBody * body0, btRigidBody * body1 ) + btScalar timeStep, btVector3& axis, btScalar jacDiagABInv, + btRigidBody* body0, btRigidBody* body1) { - if (needApplyTorques()==false) return 0.0f; + if (needApplyTorques() == false) return 0.0f; btScalar target_velocity = m_targetVelocity; btScalar maxMotorForce = m_maxMotorForce; //current error correction - if (m_currentLimit!=0) + if (m_currentLimit != 0) { - target_velocity = -m_stopERP*m_currentLimitError/(timeStep); + target_velocity = -m_stopERP * m_currentLimitError / (timeStep); maxMotorForce = m_maxLimitForce; } @@ -178,42 +153,37 @@ btScalar btRotationalLimitMotor::solveAngularLimits( btVector3 angVelB = body1->getAngularVelocity(); btVector3 vel_diff; - vel_diff = angVelA-angVelB; - - + vel_diff = angVelA - angVelB; btScalar rel_vel = axis.dot(vel_diff); // correction velocity - btScalar motor_relvel = m_limitSoftness*(target_velocity - m_damping*rel_vel); + btScalar motor_relvel = m_limitSoftness * (target_velocity - m_damping * rel_vel); - - if ( motor_relvel < SIMD_EPSILON && motor_relvel > -SIMD_EPSILON ) + if (motor_relvel < SIMD_EPSILON && motor_relvel > -SIMD_EPSILON) { - return 0.0f;//no need for applying force + return 0.0f; //no need for applying force } - // correction impulse - btScalar unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv; + btScalar unclippedMotorImpulse = (1 + m_bounce) * motor_relvel * jacDiagABInv; // clip correction impulse btScalar clippedMotorImpulse; ///@todo: should clip against accumulated impulse - if (unclippedMotorImpulse>0.0f) + if (unclippedMotorImpulse > 0.0f) { - clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse; + clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce ? maxMotorForce : unclippedMotorImpulse; } else { - clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse; + clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce : unclippedMotorImpulse; } - // sort with accumulated impulses - btScalar lo = btScalar(-BT_LARGE_FLOAT); - btScalar hi = btScalar(BT_LARGE_FLOAT); + btScalar lo = btScalar(-BT_LARGE_FLOAT); + btScalar hi = btScalar(BT_LARGE_FLOAT); btScalar oldaccumImpulse = m_accumulatedImpulse; btScalar sum = oldaccumImpulse + clippedMotorImpulse; @@ -227,59 +197,50 @@ btScalar btRotationalLimitMotor::solveAngularLimits( body1->applyTorqueImpulse(-motorImp); return clippedMotorImpulse; - - } //////////////////////////// End btRotationalLimitMotor //////////////////////////////////// - - - //////////////////////////// btTranslationalLimitMotor //////////////////////////////////// - int btTranslationalLimitMotor::testLimitValue(int limitIndex, btScalar test_value) { btScalar loLimit = m_lowerLimit[limitIndex]; btScalar hiLimit = m_upperLimit[limitIndex]; - if(loLimit > hiLimit) + if (loLimit > hiLimit) { - m_currentLimit[limitIndex] = 0;//Free from violation + m_currentLimit[limitIndex] = 0; //Free from violation m_currentLimitError[limitIndex] = btScalar(0.f); return 0; } if (test_value < loLimit) { - m_currentLimit[limitIndex] = 2;//low limit violation - m_currentLimitError[limitIndex] = test_value - loLimit; + m_currentLimit[limitIndex] = 2; //low limit violation + m_currentLimitError[limitIndex] = test_value - loLimit; return 2; } - else if (test_value> hiLimit) + else if (test_value > hiLimit) { - m_currentLimit[limitIndex] = 1;//High limit violation + m_currentLimit[limitIndex] = 1; //High limit violation m_currentLimitError[limitIndex] = test_value - hiLimit; return 1; }; - m_currentLimit[limitIndex] = 0;//Free from violation + m_currentLimit[limitIndex] = 0; //Free from violation m_currentLimitError[limitIndex] = btScalar(0.f); return 0; } - - btScalar btTranslationalLimitMotor::solveLinearAxis( btScalar timeStep, btScalar jacDiagABInv, - btRigidBody& body1,const btVector3 &pointInA, - btRigidBody& body2,const btVector3 &pointInB, + btRigidBody& body1, const btVector3& pointInA, + btRigidBody& body2, const btVector3& pointInB, int limit_index, - const btVector3 & axis_normal_on_a, - const btVector3 & anchorPos) + const btVector3& axis_normal_on_a, + const btVector3& anchorPos) { - ///find relative velocity // btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition(); // btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition(); @@ -292,14 +253,12 @@ btScalar btTranslationalLimitMotor::solveLinearAxis( btScalar rel_vel = axis_normal_on_a.dot(vel); - - /// apply displacement correction //positional error (zeroth order error) btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a); - btScalar lo = btScalar(-BT_LARGE_FLOAT); - btScalar hi = btScalar(BT_LARGE_FLOAT); + btScalar lo = btScalar(-BT_LARGE_FLOAT); + btScalar hi = btScalar(BT_LARGE_FLOAT); btScalar minLimit = m_lowerLimit[limit_index]; btScalar maxLimit = m_upperLimit[limit_index]; @@ -312,7 +271,6 @@ btScalar btTranslationalLimitMotor::solveLinearAxis( { depth -= maxLimit; lo = btScalar(0.); - } else { @@ -329,10 +287,7 @@ btScalar btTranslationalLimitMotor::solveLinearAxis( } } - btScalar normalImpulse= m_limitSoftness*(m_restitution*depth/timeStep - m_damping*rel_vel) * jacDiagABInv; - - - + btScalar normalImpulse = m_limitSoftness * (m_restitution * depth / timeStep - m_damping * rel_vel) * jacDiagABInv; btScalar oldNormalImpulse = m_accumulatedImpulse[limit_index]; btScalar sum = oldNormalImpulse + normalImpulse; @@ -340,11 +295,9 @@ btScalar btTranslationalLimitMotor::solveLinearAxis( normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse; btVector3 impulse_vector = axis_normal_on_a * normalImpulse; - body1.applyImpulse( impulse_vector, rel_pos1); + body1.applyImpulse(impulse_vector, rel_pos1); body2.applyImpulse(-impulse_vector, rel_pos2); - - return normalImpulse; } @@ -352,8 +305,8 @@ btScalar btTranslationalLimitMotor::solveLinearAxis( void btGeneric6DofConstraint::calculateAngleInfo() { - btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis(); - matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff); + btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse() * m_calculatedTransformB.getBasis(); + matrixToEulerXYZ(relative_frame, m_calculatedAxisAngleDiff); // in euler angle mode we do not actually constrain the angular velocity // along the axes axis[0] and axis[2] (although we do use axis[1]) : // @@ -378,31 +331,30 @@ void btGeneric6DofConstraint::calculateAngleInfo() m_calculatedAxis[0].normalize(); m_calculatedAxis[1].normalize(); m_calculatedAxis[2].normalize(); - } void btGeneric6DofConstraint::calculateTransforms() { - calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); + calculateTransforms(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform()); } -void btGeneric6DofConstraint::calculateTransforms(const btTransform& transA,const btTransform& transB) +void btGeneric6DofConstraint::calculateTransforms(const btTransform& transA, const btTransform& transB) { m_calculatedTransformA = transA * m_frameInA; m_calculatedTransformB = transB * m_frameInB; calculateLinearInfo(); calculateAngleInfo(); - if(m_useOffsetForConstraintFrame) - { // get weight factors depending on masses + if (m_useOffsetForConstraintFrame) + { // get weight factors depending on masses btScalar miA = getRigidBodyA().getInvMass(); btScalar miB = getRigidBodyB().getInvMass(); m_hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON); btScalar miS = miA + miB; - if(miS > btScalar(0.f)) + if (miS > btScalar(0.f)) { m_factA = miB / miS; } - else + else { m_factA = btScalar(0.5f); } @@ -410,39 +362,32 @@ void btGeneric6DofConstraint::calculateTransforms(const btTransform& transA,cons } } - - void btGeneric6DofConstraint::buildLinearJacobian( - btJacobianEntry & jacLinear,const btVector3 & normalWorld, - const btVector3 & pivotAInW,const btVector3 & pivotBInW) + btJacobianEntry& jacLinear, const btVector3& normalWorld, + const btVector3& pivotAInW, const btVector3& pivotBInW) { new (&jacLinear) btJacobianEntry( - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - pivotAInW - m_rbA.getCenterOfMassPosition(), - pivotBInW - m_rbB.getCenterOfMassPosition(), - normalWorld, - m_rbA.getInvInertiaDiagLocal(), - m_rbA.getInvMass(), - m_rbB.getInvInertiaDiagLocal(), - m_rbB.getInvMass()); + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + pivotAInW - m_rbA.getCenterOfMassPosition(), + pivotBInW - m_rbB.getCenterOfMassPosition(), + normalWorld, + m_rbA.getInvInertiaDiagLocal(), + m_rbA.getInvMass(), + m_rbB.getInvInertiaDiagLocal(), + m_rbB.getInvMass()); } - - void btGeneric6DofConstraint::buildAngularJacobian( - btJacobianEntry & jacAngular,const btVector3 & jointAxisW) + btJacobianEntry& jacAngular, const btVector3& jointAxisW) { - new (&jacAngular) btJacobianEntry(jointAxisW, - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - m_rbA.getInvInertiaDiagLocal(), - m_rbB.getInvInertiaDiagLocal()); - + new (&jacAngular) btJacobianEntry(jointAxisW, + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + m_rbA.getInvInertiaDiagLocal(), + m_rbB.getInvInertiaDiagLocal()); } - - bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index) { btScalar angle = m_calculatedAxisAngleDiff[axis_index]; @@ -453,23 +398,20 @@ bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index) return m_angularLimits[axis_index].needApplyTorques(); } - - void btGeneric6DofConstraint::buildJacobian() { #ifndef __SPU__ if (m_useSolveConstraintObsolete) { - // Clear accumulated impulses for the next simulation step m_linearLimits.m_accumulatedImpulse.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); int i; - for(i = 0; i < 3; i++) + for (i = 0; i < 3; i++) { m_angularLimits[i].m_accumulatedImpulse = btScalar(0.); } //calculates transform - calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); + calculateTransforms(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform()); // const btVector3& pivotAInW = m_calculatedTransformA.getOrigin(); // const btVector3& pivotBInW = m_calculatedTransformB.getOrigin(); @@ -483,7 +425,7 @@ void btGeneric6DofConstraint::buildJacobian() btVector3 normalWorld; //linear part - for (i=0;i<3;i++) + for (i = 0; i < 3; i++) { if (m_linearLimits.isLimited(i)) { @@ -493,56 +435,53 @@ void btGeneric6DofConstraint::buildJacobian() normalWorld = m_calculatedTransformB.getBasis().getColumn(i); buildLinearJacobian( - m_jacLinear[i],normalWorld , - pivotAInW,pivotBInW); - + m_jacLinear[i], normalWorld, + pivotAInW, pivotBInW); } } // angular part - for (i=0;i<3;i++) + for (i = 0; i < 3; i++) { //calculates error angle if (testAngularLimitMotor(i)) { normalWorld = this->getAxis(i); // Create angular atom - buildAngularJacobian(m_jacAng[i],normalWorld); + buildAngularJacobian(m_jacAng[i], normalWorld); } } - } -#endif //__SPU__ - +#endif //__SPU__ } - -void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info) +void btGeneric6DofConstraint::getInfo1(btConstraintInfo1* info) { if (m_useSolveConstraintObsolete) { info->m_numConstraintRows = 0; info->nub = 0; - } else + } + else { //prepare constraint - calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); + calculateTransforms(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform()); info->m_numConstraintRows = 0; info->nub = 6; int i; //test linear limits - for(i = 0; i < 3; i++) + for (i = 0; i < 3; i++) { - if(m_linearLimits.needApplyForce(i)) + if (m_linearLimits.needApplyForce(i)) { info->m_numConstraintRows++; info->nub--; } } //test angular limits - for (i=0;i<3 ;i++ ) + for (i = 0; i < 3; i++) { - if(testAngularLimitMotor(i)) + if (testAngularLimitMotor(i)) { info->m_numConstraintRows++; info->nub--; @@ -551,13 +490,14 @@ void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info) } } -void btGeneric6DofConstraint::getInfo1NonVirtual (btConstraintInfo1* info) +void btGeneric6DofConstraint::getInfo1NonVirtual(btConstraintInfo1* info) { if (m_useSolveConstraintObsolete) { info->m_numConstraintRows = 0; info->nub = 0; - } else + } + else { //pre-allocate all 6 info->m_numConstraintRows = 6; @@ -565,8 +505,7 @@ void btGeneric6DofConstraint::getInfo1NonVirtual (btConstraintInfo1* info) } } - -void btGeneric6DofConstraint::getInfo2 (btConstraintInfo2* info) +void btGeneric6DofConstraint::getInfo2(btConstraintInfo2* info) { btAssert(!m_useSolveConstraintObsolete); @@ -577,136 +516,124 @@ void btGeneric6DofConstraint::getInfo2 (btConstraintInfo2* info) const btVector3& angVelA = m_rbA.getAngularVelocity(); const btVector3& angVelB = m_rbB.getAngularVelocity(); - if(m_useOffsetForConstraintFrame) - { // for stability better to solve angular limits first - int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB); - setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB); + if (m_useOffsetForConstraintFrame) + { // for stability better to solve angular limits first + int row = setAngularLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB); + setLinearLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB); } else - { // leave old version for compatibility - int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB); - setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB); + { // leave old version for compatibility + int row = setLinearLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB); + setAngularLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB); } - } - -void btGeneric6DofConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB) +void btGeneric6DofConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB) { - btAssert(!m_useSolveConstraintObsolete); //prepare constraint - calculateTransforms(transA,transB); + calculateTransforms(transA, transB); int i; - for (i=0;i<3 ;i++ ) + for (i = 0; i < 3; i++) { testAngularLimitMotor(i); } - if(m_useOffsetForConstraintFrame) - { // for stability better to solve angular limits first - int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB); - setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB); + if (m_useOffsetForConstraintFrame) + { // for stability better to solve angular limits first + int row = setAngularLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB); + setLinearLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB); } else - { // leave old version for compatibility - int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB); - setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB); + { // leave old version for compatibility + int row = setLinearLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB); + setAngularLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB); } } - - -int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB) +int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info, int row, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB) { -// int row = 0; + // int row = 0; //solve linear limits btRotationalLimitMotor limot; - for (int i=0;i<3 ;i++ ) + for (int i = 0; i < 3; i++) { - if(m_linearLimits.needApplyForce(i)) - { // re-use rotational motor code + if (m_linearLimits.needApplyForce(i)) + { // re-use rotational motor code limot.m_bounce = btScalar(0.f); limot.m_currentLimit = m_linearLimits.m_currentLimit[i]; limot.m_currentPosition = m_linearLimits.m_currentLinearDiff[i]; - limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i]; - limot.m_damping = m_linearLimits.m_damping; - limot.m_enableMotor = m_linearLimits.m_enableMotor[i]; - limot.m_hiLimit = m_linearLimits.m_upperLimit[i]; - limot.m_limitSoftness = m_linearLimits.m_limitSoftness; - limot.m_loLimit = m_linearLimits.m_lowerLimit[i]; - limot.m_maxLimitForce = btScalar(0.f); - limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i]; - limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i]; + limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i]; + limot.m_damping = m_linearLimits.m_damping; + limot.m_enableMotor = m_linearLimits.m_enableMotor[i]; + limot.m_hiLimit = m_linearLimits.m_upperLimit[i]; + limot.m_limitSoftness = m_linearLimits.m_limitSoftness; + limot.m_loLimit = m_linearLimits.m_lowerLimit[i]; + limot.m_maxLimitForce = btScalar(0.f); + limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i]; + limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i]; btVector3 axis = m_calculatedTransformA.getBasis().getColumn(i); int flags = m_flags >> (i * BT_6DOF_FLAGS_AXIS_SHIFT); - limot.m_normalCFM = (flags & BT_6DOF_FLAGS_CFM_NORM) ? m_linearLimits.m_normalCFM[i] : info->cfm[0]; - limot.m_stopCFM = (flags & BT_6DOF_FLAGS_CFM_STOP) ? m_linearLimits.m_stopCFM[i] : info->cfm[0]; - limot.m_stopERP = (flags & BT_6DOF_FLAGS_ERP_STOP) ? m_linearLimits.m_stopERP[i] : info->erp; - if(m_useOffsetForConstraintFrame) + limot.m_normalCFM = (flags & BT_6DOF_FLAGS_CFM_NORM) ? m_linearLimits.m_normalCFM[i] : info->cfm[0]; + limot.m_stopCFM = (flags & BT_6DOF_FLAGS_CFM_STOP) ? m_linearLimits.m_stopCFM[i] : info->cfm[0]; + limot.m_stopERP = (flags & BT_6DOF_FLAGS_ERP_STOP) ? m_linearLimits.m_stopERP[i] : info->erp; + if (m_useOffsetForConstraintFrame) { int indx1 = (i + 1) % 3; int indx2 = (i + 2) % 3; - int rotAllowed = 1; // rotations around orthos to current axis - if(m_angularLimits[indx1].m_currentLimit && m_angularLimits[indx2].m_currentLimit) + int rotAllowed = 1; // rotations around orthos to current axis + if (m_angularLimits[indx1].m_currentLimit && m_angularLimits[indx2].m_currentLimit) { rotAllowed = 0; } - row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0, rotAllowed); + row += get_limit_motor_info2(&limot, transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 0, rotAllowed); } else { - row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0); + row += get_limit_motor_info2(&limot, transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 0); } } } return row; } - - -int 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) +int 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) { - btGeneric6DofConstraint * d6constraint = this; + btGeneric6DofConstraint* d6constraint = this; int row = row_offset; //solve angular limits - for (int i=0;i<3 ;i++ ) + for (int i = 0; i < 3; i++) { - if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques()) + if (d6constraint->getRotationalLimitMotor(i)->needApplyTorques()) { btVector3 axis = d6constraint->getAxis(i); int flags = m_flags >> ((i + 3) * BT_6DOF_FLAGS_AXIS_SHIFT); - if(!(flags & BT_6DOF_FLAGS_CFM_NORM)) + if (!(flags & BT_6DOF_FLAGS_CFM_NORM)) { m_angularLimits[i].m_normalCFM = info->cfm[0]; } - if(!(flags & BT_6DOF_FLAGS_CFM_STOP)) + if (!(flags & BT_6DOF_FLAGS_CFM_STOP)) { m_angularLimits[i].m_stopCFM = info->cfm[0]; } - if(!(flags & BT_6DOF_FLAGS_ERP_STOP)) + if (!(flags & BT_6DOF_FLAGS_ERP_STOP)) { m_angularLimits[i].m_stopERP = info->erp; } row += get_limit_motor_info2(d6constraint->getRotationalLimitMotor(i), - transA,transB,linVelA,linVelB,angVelA,angVelB, info,row,axis,1); + transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 1); } } return row; } - - - -void btGeneric6DofConstraint::updateRHS(btScalar timeStep) +void btGeneric6DofConstraint::updateRHS(btScalar timeStep) { (void)timeStep; - } - void btGeneric6DofConstraint::setFrames(const btTransform& frameA, const btTransform& frameB) { m_frameInA = frameA; @@ -715,33 +642,27 @@ void btGeneric6DofConstraint::setFrames(const btTransform& frameA, const btTrans calculateTransforms(); } - - btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const { return m_calculatedAxis[axis_index]; } - -btScalar btGeneric6DofConstraint::getRelativePivotPosition(int axisIndex) const +btScalar btGeneric6DofConstraint::getRelativePivotPosition(int axisIndex) const { return m_calculatedLinearDiff[axisIndex]; } - btScalar btGeneric6DofConstraint::getAngle(int axisIndex) const { return m_calculatedAxisAngleDiff[axisIndex]; } - - void btGeneric6DofConstraint::calcAnchorPos(void) { btScalar imA = m_rbA.getInvMass(); btScalar imB = m_rbB.getInvMass(); btScalar weight; - if(imB == btScalar(0.0)) + if (imB == btScalar(0.0)) { weight = btScalar(1.0); } @@ -755,43 +676,39 @@ void btGeneric6DofConstraint::calcAnchorPos(void) return; } - - void btGeneric6DofConstraint::calculateLinearInfo() { m_calculatedLinearDiff = m_calculatedTransformB.getOrigin() - m_calculatedTransformA.getOrigin(); m_calculatedLinearDiff = m_calculatedTransformA.getBasis().inverse() * m_calculatedLinearDiff; - for(int i = 0; i < 3; i++) + for (int i = 0; i < 3; i++) { m_linearLimits.m_currentLinearDiff[i] = m_calculatedLinearDiff[i]; m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]); } } - - int btGeneric6DofConstraint::get_limit_motor_info2( - btRotationalLimitMotor * limot, - const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB, - btConstraintInfo2 *info, int row, btVector3& ax1, int rotational,int rotAllowed) -{ - int srow = row * info->rowskip; - bool powered = limot->m_enableMotor; - int limit = limot->m_currentLimit; - if (powered || limit) - { // if the joint is powered, or has joint limits, add in the extra row - btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis; - btScalar *J2 = rotational ? info->m_J2angularAxis : info->m_J2linearAxis; - J1[srow+0] = ax1[0]; - J1[srow+1] = ax1[1]; - J1[srow+2] = ax1[2]; - - J2[srow+0] = -ax1[0]; - J2[srow+1] = -ax1[1]; - J2[srow+2] = -ax1[2]; - - if((!rotational)) - { + btRotationalLimitMotor* limot, + const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB, + btConstraintInfo2* info, int row, btVector3& ax1, int rotational, int rotAllowed) +{ + int srow = row * info->rowskip; + bool powered = limot->m_enableMotor; + int limit = limot->m_currentLimit; + if (powered || limit) + { // if the joint is powered, or has joint limits, add in the extra row + btScalar* J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis; + btScalar* J2 = rotational ? info->m_J2angularAxis : info->m_J2linearAxis; + J1[srow + 0] = ax1[0]; + J1[srow + 1] = ax1[1]; + J1[srow + 2] = ax1[2]; + + J2[srow + 0] = -ax1[0]; + J2[srow + 1] = -ax1[1]; + J2[srow + 2] = -ax1[2]; + + if ((!rotational)) + { if (m_useOffsetForConstraintFrame) { btVector3 tmpA, tmpB, relA, relB; @@ -814,55 +731,56 @@ int btGeneric6DofConstraint::get_limit_motor_info2( relB = orthoB - totalDist * m_factB; tmpA = relA.cross(ax1); tmpB = relB.cross(ax1); - if(m_hasStaticBody && (!rotAllowed)) + if (m_hasStaticBody && (!rotAllowed)) { tmpA *= m_factA; tmpB *= m_factB; } int i; - for (i=0; i<3; i++) info->m_J1angularAxis[srow+i] = tmpA[i]; - for (i=0; i<3; i++) info->m_J2angularAxis[srow+i] = -tmpB[i]; - } else + for (i = 0; i < 3; i++) info->m_J1angularAxis[srow + i] = tmpA[i]; + for (i = 0; i < 3; i++) info->m_J2angularAxis[srow + i] = -tmpB[i]; + } + else { - btVector3 ltd; // Linear Torque Decoupling vector + btVector3 ltd; // Linear Torque Decoupling vector btVector3 c = m_calculatedTransformB.getOrigin() - transA.getOrigin(); ltd = c.cross(ax1); - info->m_J1angularAxis[srow+0] = ltd[0]; - info->m_J1angularAxis[srow+1] = ltd[1]; - info->m_J1angularAxis[srow+2] = ltd[2]; + info->m_J1angularAxis[srow + 0] = ltd[0]; + info->m_J1angularAxis[srow + 1] = ltd[1]; + info->m_J1angularAxis[srow + 2] = ltd[2]; c = m_calculatedTransformB.getOrigin() - transB.getOrigin(); ltd = -c.cross(ax1); - info->m_J2angularAxis[srow+0] = ltd[0]; - info->m_J2angularAxis[srow+1] = ltd[1]; - info->m_J2angularAxis[srow+2] = ltd[2]; + info->m_J2angularAxis[srow + 0] = ltd[0]; + info->m_J2angularAxis[srow + 1] = ltd[1]; + info->m_J2angularAxis[srow + 2] = ltd[2]; } - } - // if we're limited low and high simultaneously, the joint motor is - // ineffective - if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = false; - info->m_constraintError[srow] = btScalar(0.f); - if (powered) - { + } + // if we're limited low and high simultaneously, the joint motor is + // ineffective + if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = false; + info->m_constraintError[srow] = btScalar(0.f); + if (powered) + { info->cfm[srow] = limot->m_normalCFM; - if(!limit) - { + if (!limit) + { btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity; - btScalar mot_fact = getMotorFactor( limot->m_currentPosition, - limot->m_loLimit, - limot->m_hiLimit, - tag_vel, - info->fps * limot->m_stopERP); + btScalar mot_fact = getMotorFactor(limot->m_currentPosition, + limot->m_loLimit, + limot->m_hiLimit, + tag_vel, + info->fps * limot->m_stopERP); info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity; - info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps; - info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps; - } - } - if(limit) - { - btScalar k = info->fps * limot->m_stopERP; - if(!rotational) + info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps; + info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps; + } + } + if (limit) + { + btScalar k = info->fps * limot->m_stopERP; + if (!rotational) { info->m_constraintError[srow] += k * limot->m_currentLimitError; } @@ -871,116 +789,112 @@ int btGeneric6DofConstraint::get_limit_motor_info2( info->m_constraintError[srow] += -k * limot->m_currentLimitError; } info->cfm[srow] = limot->m_stopCFM; - if (limot->m_loLimit == limot->m_hiLimit) - { // limited low and high simultaneously - info->m_lowerLimit[srow] = -SIMD_INFINITY; - info->m_upperLimit[srow] = SIMD_INFINITY; - } - else - { - if (limit == 1) - { - info->m_lowerLimit[srow] = 0; - info->m_upperLimit[srow] = SIMD_INFINITY; - } - else - { - info->m_lowerLimit[srow] = -SIMD_INFINITY; - info->m_upperLimit[srow] = 0; - } - // deal with bounce - if (limot->m_bounce > 0) - { - // calculate joint velocity - btScalar vel; - if (rotational) - { - vel = angVelA.dot(ax1); -//make sure that if no body -> angVelB == zero vec -// if (body1) - vel -= angVelB.dot(ax1); - } - else - { - vel = linVelA.dot(ax1); -//make sure that if no body -> angVelB == zero vec -// if (body1) - vel -= linVelB.dot(ax1); - } - // only apply bounce if the velocity is incoming, and if the - // resulting c[] exceeds what we already have. - if (limit == 1) - { - if (vel < 0) - { - btScalar newc = -limot->m_bounce* vel; - if (newc > info->m_constraintError[srow]) + if (limot->m_loLimit == limot->m_hiLimit) + { // limited low and high simultaneously + info->m_lowerLimit[srow] = -SIMD_INFINITY; + info->m_upperLimit[srow] = SIMD_INFINITY; + } + else + { + if (limit == 1) + { + info->m_lowerLimit[srow] = 0; + info->m_upperLimit[srow] = SIMD_INFINITY; + } + else + { + info->m_lowerLimit[srow] = -SIMD_INFINITY; + info->m_upperLimit[srow] = 0; + } + // deal with bounce + if (limot->m_bounce > 0) + { + // calculate joint velocity + btScalar vel; + if (rotational) + { + vel = angVelA.dot(ax1); + //make sure that if no body -> angVelB == zero vec + // if (body1) + vel -= angVelB.dot(ax1); + } + else + { + vel = linVelA.dot(ax1); + //make sure that if no body -> angVelB == zero vec + // if (body1) + vel -= linVelB.dot(ax1); + } + // only apply bounce if the velocity is incoming, and if the + // resulting c[] exceeds what we already have. + if (limit == 1) + { + if (vel < 0) + { + btScalar newc = -limot->m_bounce * vel; + if (newc > info->m_constraintError[srow]) info->m_constraintError[srow] = newc; - } - } - else - { - if (vel > 0) - { - btScalar newc = -limot->m_bounce * vel; - if (newc < info->m_constraintError[srow]) + } + } + else + { + if (vel > 0) + { + btScalar newc = -limot->m_bounce * vel; + if (newc < info->m_constraintError[srow]) info->m_constraintError[srow] = newc; - } - } - } - } - } - return 1; - } - else return 0; + } + } + } + } + } + return 1; + } + else + return 0; } - - - - - - ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). - ///If no axis is provided, it uses the default axis for this constraint. +///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). +///If no axis is provided, it uses the default axis for this constraint. void btGeneric6DofConstraint::setParam(int num, btScalar value, int axis) { - if((axis >= 0) && (axis < 3)) + if ((axis >= 0) && (axis < 3)) { - switch(num) + switch (num) { - case BT_CONSTRAINT_STOP_ERP : + case BT_CONSTRAINT_STOP_ERP: m_linearLimits.m_stopERP[axis] = value; m_flags |= BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT); break; - case BT_CONSTRAINT_STOP_CFM : + case BT_CONSTRAINT_STOP_CFM: m_linearLimits.m_stopCFM[axis] = value; m_flags |= BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT); break; - case BT_CONSTRAINT_CFM : + case BT_CONSTRAINT_CFM: m_linearLimits.m_normalCFM[axis] = value; m_flags |= BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT); break; - default : + default: btAssertConstrParams(0); } } - else if((axis >=3) && (axis < 6)) + else if ((axis >= 3) && (axis < 6)) { - switch(num) + switch (num) { - case BT_CONSTRAINT_STOP_ERP : + case BT_CONSTRAINT_STOP_ERP: m_angularLimits[axis - 3].m_stopERP = value; m_flags |= BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT); break; - case BT_CONSTRAINT_STOP_CFM : + case BT_CONSTRAINT_STOP_CFM: m_angularLimits[axis - 3].m_stopCFM = value; m_flags |= BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT); break; - case BT_CONSTRAINT_CFM : + case BT_CONSTRAINT_CFM: m_angularLimits[axis - 3].m_normalCFM = value; m_flags |= BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT); break; - default : + default: btAssertConstrParams(0); } } @@ -990,47 +904,47 @@ void btGeneric6DofConstraint::setParam(int num, btScalar value, int axis) } } - ///return the local value of parameter -btScalar btGeneric6DofConstraint::getParam(int num, int axis) const +///return the local value of parameter +btScalar btGeneric6DofConstraint::getParam(int num, int axis) const { btScalar retVal = 0; - if((axis >= 0) && (axis < 3)) + if ((axis >= 0) && (axis < 3)) { - switch(num) + switch (num) { - case BT_CONSTRAINT_STOP_ERP : + case BT_CONSTRAINT_STOP_ERP: btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT))); retVal = m_linearLimits.m_stopERP[axis]; break; - case BT_CONSTRAINT_STOP_CFM : + case BT_CONSTRAINT_STOP_CFM: btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT))); retVal = m_linearLimits.m_stopCFM[axis]; break; - case BT_CONSTRAINT_CFM : + case BT_CONSTRAINT_CFM: btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT))); retVal = m_linearLimits.m_normalCFM[axis]; break; - default : + default: btAssertConstrParams(0); } } - else if((axis >=3) && (axis < 6)) + else if ((axis >= 3) && (axis < 6)) { - switch(num) + switch (num) { - case BT_CONSTRAINT_STOP_ERP : + case BT_CONSTRAINT_STOP_ERP: btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT))); retVal = m_angularLimits[axis - 3].m_stopERP; break; - case BT_CONSTRAINT_STOP_CFM : + case BT_CONSTRAINT_STOP_CFM: btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT))); retVal = m_angularLimits[axis - 3].m_stopCFM; break; - case BT_CONSTRAINT_CFM : + case BT_CONSTRAINT_CFM: btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT))); retVal = m_angularLimits[axis - 3].m_normalCFM; break; - default : + default: btAssertConstrParams(0); } } @@ -1041,23 +955,21 @@ btScalar btGeneric6DofConstraint::getParam(int num, int axis) const return retVal; } - - -void btGeneric6DofConstraint::setAxis(const btVector3& axis1,const btVector3& axis2) +void btGeneric6DofConstraint::setAxis(const btVector3& axis1, const btVector3& axis2) { btVector3 zAxis = axis1.normalized(); btVector3 yAxis = axis2.normalized(); - btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system - + btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system + btTransform frameInW; frameInW.setIdentity(); - frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0], - xAxis[1], yAxis[1], zAxis[1], - xAxis[2], yAxis[2], zAxis[2]); - + frameInW.getBasis().setValue(xAxis[0], yAxis[0], zAxis[0], + xAxis[1], yAxis[1], zAxis[1], + xAxis[2], yAxis[2], zAxis[2]); + // now get constraint frame in local coordinate systems m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW; m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW; - + calculateTransforms(); } |