diff options
Diffstat (limited to 'extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp')
| -rw-r--r-- | extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp | 749 |
1 files changed, 444 insertions, 305 deletions
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp index 747d10d1f8b..077b326d13a 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp @@ -4,14 +4,20 @@ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions: 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ +/* +2007-09-09 +Refactored by Francisco Le?n +email: projectileman@yahoo.com +http://gimpact.sf.net +*/ #include "btGeneric6DofConstraint.h" @@ -19,371 +25,504 @@ subject to the following restrictions: #include "LinearMath/btTransformUtil.h" #include <new> + static const btScalar kSign[] = { btScalar(1.0), btScalar(-1.0), btScalar(1.0) }; static const int kAxisA[] = { 1, 0, 0 }; static const int kAxisB[] = { 2, 2, 1 }; #define GENERIC_D6_DISABLE_WARMSTARTING 1 -btGeneric6DofConstraint::btGeneric6DofConstraint() +btScalar btGetMatrixElem(const btMatrix3x3& mat, int index); +btScalar btGetMatrixElem(const btMatrix3x3& mat, int index) { + int i = index%3; + int j = index/3; + return mat[i][j]; } -btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB) -: btTypedConstraint(rbA, rbB) -, m_frameInA(frameInA) -, m_frameInB(frameInB) +///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) { - //free means upper < lower, - //locked means upper == lower - //limited means upper > lower - //so start all locked - for (int i=0; i<6;++i) +// // 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 +// + + if (btGetMatrixElem(mat,2) < btScalar(1.0)) + { + if (btGetMatrixElem(mat,2) > btScalar(-1.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[1] = -SIMD_HALF_PI; + xyz[2] = btScalar(0.0); + return false; + } + } + else + { + // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11) + xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4)); + xyz[1] = SIMD_HALF_PI; + xyz[2] = 0.0; + + } + + + return false; +} + + + +//////////////////////////// btRotationalLimitMotor //////////////////////////////////// + + +int btRotationalLimitMotor::testLimitValue(btScalar test_value) +{ + if(m_loLimit>m_hiLimit) + { + m_currentLimit = 0;//Free from violation + return 0; + } + + if (test_value < m_loLimit) { - m_lowerLimit[i] = btScalar(0.0); - m_upperLimit[i] = btScalar(0.0); - m_accumulatedImpulse[i] = btScalar(0.0); + m_currentLimit = 1;//low limit violation + m_currentLimitError = test_value - m_loLimit; + return 1; } + else if (test_value> m_hiLimit) + { + m_currentLimit = 2;//High limit violation + m_currentLimitError = test_value - m_hiLimit; + return 2; + }; + m_currentLimit = 0;//Free from violation + return 0; + } -void btGeneric6DofConstraint::buildJacobian() +btScalar btRotationalLimitMotor::solveAngularLimits( + btScalar timeStep,btVector3& axis,btScalar jacDiagABInv, + btRigidBody * body0, btRigidBody * body1) { - btVector3 localNormalInA(0,0,0); + if (needApplyTorques()==false) return 0.0f; - const btVector3& pivotInA = m_frameInA.getOrigin(); - const btVector3& pivotInB = m_frameInB.getOrigin(); + btScalar target_velocity = m_targetVelocity; + btScalar maxMotorForce = m_maxMotorForce; - btVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_frameInA.getOrigin(); - btVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_frameInB.getOrigin(); + //current error correction + if (m_currentLimit!=0) + { + target_velocity = -m_ERP*m_currentLimitError/(timeStep); + maxMotorForce = m_maxLimitForce; + } - btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); - btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); + maxMotorForce *= timeStep; - int i; - //linear part - for (i=0;i<3;i++) - { - if (isLimited(i)) - { - localNormalInA[i] = 1; - btVector3 normalWorld = m_rbA.getCenterOfMassTransform().getBasis() * localNormalInA; - - - // Create linear atom - new (&m_jacLinear[i]) btJacobianEntry( - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - m_rbA.getCenterOfMassTransform()*pivotInA - m_rbA.getCenterOfMassPosition(), - m_rbB.getCenterOfMassTransform()*pivotInB - m_rbB.getCenterOfMassPosition(), - normalWorld, - m_rbA.getInvInertiaDiagLocal(), - m_rbA.getInvMass(), - m_rbB.getInvInertiaDiagLocal(), - m_rbB.getInvMass()); - - //optionally disable warmstarting -#ifdef GENERIC_D6_DISABLE_WARMSTARTING - m_accumulatedImpulse[i] = btScalar(0.); -#endif //GENERIC_D6_DISABLE_WARMSTARTING - - // Apply accumulated impulse - btVector3 impulse_vector = m_accumulatedImpulse[i] * normalWorld; - - m_rbA.applyImpulse( impulse_vector, rel_pos1); - m_rbB.applyImpulse(-impulse_vector, rel_pos2); - - localNormalInA[i] = 0; - } - } + // current velocity difference + btVector3 vel_diff = body0->getAngularVelocity(); + if (body1) + { + vel_diff -= body1->getAngularVelocity(); + } - // angular part - for (i=0;i<3;i++) - { - if (isLimited(i+3)) - { - btVector3 axisA = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn( kAxisA[i] ); - btVector3 axisB = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn( kAxisB[i] ); - // Dirk: This is IMO mathematically the correct way, but we should consider axisA and axisB being near parallel maybe - btVector3 axis = kSign[i] * axisA.cross(axisB); - // Create angular atom - new (&m_jacAng[i]) btJacobianEntry(axis, - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - m_rbA.getInvInertiaDiagLocal(), - m_rbB.getInvInertiaDiagLocal()); + btScalar rel_vel = axis.dot(vel_diff); -#ifdef GENERIC_D6_DISABLE_WARMSTARTING - m_accumulatedImpulse[i + 3] = btScalar(0.); -#endif //GENERIC_D6_DISABLE_WARMSTARTING + // correction velocity + btScalar motor_relvel = m_limitSoftness*(target_velocity - m_damping*rel_vel); - // Apply accumulated impulse - btVector3 impulse_vector = m_accumulatedImpulse[i + 3] * axis; - m_rbA.applyTorqueImpulse( impulse_vector); - m_rbB.applyTorqueImpulse(-impulse_vector); - } - } -} + if ( motor_relvel < SIMD_EPSILON && motor_relvel > -SIMD_EPSILON ) + { + return 0.0f;//no need for applying force + } + + + // correction impulse + btScalar unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv; + + // clip correction impulse + btScalar clippedMotorImpulse; + + //todo: should clip against accumulated impulse + if (unclippedMotorImpulse>0.0f) + { + clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse; + } + else + { + clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse; + } + + + // sort with accumulated impulses + btScalar lo = btScalar(-1e30); + btScalar hi = btScalar(1e30); + + btScalar oldaccumImpulse = m_accumulatedImpulse; + btScalar sum = oldaccumImpulse + clippedMotorImpulse; + m_accumulatedImpulse = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum; + + clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse; + + + + btVector3 motorImp = clippedMotorImpulse * axis; + + + body0->applyTorqueImpulse(motorImp); + if (body1) body1->applyTorqueImpulse(-motorImp); + + return clippedMotorImpulse; + -btScalar getMatrixElem(const btMatrix3x3& mat,int index) -{ - int row = index%3; - int col = index / 3; - return mat[row][col]; } -///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html -bool MatrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz) +//////////////////////////// End btRotationalLimitMotor //////////////////////////////////// + +//////////////////////////// btTranslationalLimitMotor //////////////////////////////////// +btScalar btTranslationalLimitMotor::solveLinearAxis( + btScalar timeStep, + btScalar jacDiagABInv, + btRigidBody& body1,const btVector3 &pointInA, + btRigidBody& body2,const btVector3 &pointInB, + int limit_index, + const btVector3 & axis_normal_on_a, + const btVector3 & anchorPos) { - // 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 -/// 0..8 +///find relative velocity +// btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition(); +// btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition(); + btVector3 rel_pos1 = anchorPos - body1.getCenterOfMassPosition(); + btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition(); + + btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1); + btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2); + btVector3 vel = vel1 - vel2; + + btScalar rel_vel = axis_normal_on_a.dot(vel); + + + +/// apply displacement correction - if (getMatrixElem(mat,2) < btScalar(1.0)) +//positional error (zeroth order error) + btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a); + btScalar lo = btScalar(-1e30); + btScalar hi = btScalar(1e30); + + btScalar minLimit = m_lowerLimit[limit_index]; + btScalar maxLimit = m_upperLimit[limit_index]; + + //handle the limits + if (minLimit < maxLimit) { - if (getMatrixElem(mat,2) > btScalar(-1.0)) { - xyz[0] = btAtan2(-getMatrixElem(mat,5),getMatrixElem(mat,8)); - xyz[1] = btAsin(getMatrixElem(mat,2)); - xyz[2] = btAtan2(-getMatrixElem(mat,1),getMatrixElem(mat,0)); - return true; + if (depth > maxLimit) + { + depth -= maxLimit; + lo = btScalar(0.); + + } + else + { + if (depth < minLimit) + { + depth -= minLimit; + hi = btScalar(0.); + } + else + { + return 0.0f; + } + } } - else - { - // WARNING. Not unique. XA - ZA = -atan2(r10,r11) - xyz[0] = -btAtan2(getMatrixElem(mat,3),getMatrixElem(mat,4)); - xyz[1] = -SIMD_HALF_PI; - xyz[2] = btScalar(0.0); - return false; - } - } - else - { - // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11) - xyz[0] = btAtan2(getMatrixElem(mat,3),getMatrixElem(mat,4)); - xyz[1] = SIMD_HALF_PI; - xyz[2] = 0.0; - } - - return false; + + btScalar normalImpulse= m_limitSoftness*(m_restitution*depth/timeStep - m_damping*rel_vel) * jacDiagABInv; + + + + + btScalar oldNormalImpulse = m_accumulatedImpulse[limit_index]; + btScalar sum = oldNormalImpulse + normalImpulse; + m_accumulatedImpulse[limit_index] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum; + normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse; + + btVector3 impulse_vector = axis_normal_on_a * normalImpulse; + body1.applyImpulse( impulse_vector, rel_pos1); + body2.applyImpulse(-impulse_vector, rel_pos2); + return normalImpulse; +} + +//////////////////////////// btTranslationalLimitMotor //////////////////////////////////// + + +btGeneric6DofConstraint::btGeneric6DofConstraint() + :btTypedConstraint(D6_CONSTRAINT_TYPE), + m_useLinearReferenceFrameA(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) +{ + } -void btGeneric6DofConstraint::solveConstraint(btScalar timeStep) + + + +void btGeneric6DofConstraint::calculateAngleInfo() { - btScalar tau = btScalar(0.1); - btScalar damping = btScalar(1.0); + 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]) : + // + // to get constrain w2-w1 along ...not + // ------ --------------------- ------ + // d(angle[0])/dt = 0 ax[1] x ax[2] ax[0] + // d(angle[1])/dt = 0 ax[1] + // d(angle[2])/dt = 0 ax[0] x ax[1] ax[2] + // + // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0. + // to prove the result for angle[0], write the expression for angle[0] from + // GetInfo1 then take the derivative. to prove this for angle[2] it is + // easier to take the euler rate expression for d(angle[2])/dt with respect + // to the components of w and set that to 0. + + btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0); + btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2); + + m_calculatedAxis[1] = axis2.cross(axis0); + m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2); + m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]); + + +// if(m_debugDrawer) +// { +// +// char buff[300]; +// sprintf(buff,"\n X: %.2f ; Y: %.2f ; Z: %.2f ", +// m_calculatedAxisAngleDiff[0], +// m_calculatedAxisAngleDiff[1], +// m_calculatedAxisAngleDiff[2]); +// m_debugDrawer->reportErrorWarning(buff); +// } - btVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_frameInA.getOrigin(); - btVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_frameInB.getOrigin(); +} - btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); - btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); - - btVector3 localNormalInA(0,0,0); - int i; +void btGeneric6DofConstraint::calculateTransforms() +{ + m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA; + m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB; - // linear - for (i=0;i<3;i++) - { - if (isLimited(i)) - { - btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); - btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); - - localNormalInA.setValue(0,0,0); - localNormalInA[i] = 1; - btVector3 normalWorld = m_rbA.getCenterOfMassTransform().getBasis() * localNormalInA; - - btScalar jacDiagABInv = btScalar(1.) / m_jacLinear[i].getDiagonal(); - - //velocity error (first order error) - btScalar rel_vel = m_jacLinear[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, - m_rbB.getLinearVelocity(),angvelB); - - //positional error (zeroth order error) - btScalar depth = -(pivotAInW - pivotBInW).dot(normalWorld); - btScalar lo = btScalar(-1e30); - btScalar hi = btScalar(1e30); - - //handle the limits - if (m_lowerLimit[i] < m_upperLimit[i]) - { - { - if (depth > m_upperLimit[i]) - { - depth -= m_upperLimit[i]; - lo = btScalar(0.); - - } else - { - if (depth < m_lowerLimit[i]) - { - depth -= m_lowerLimit[i]; - hi = btScalar(0.); - } else - { - continue; - } - } - } - } + calculateAngleInfo(); +} - btScalar normalImpulse= (tau*depth/timeStep - damping*rel_vel) * jacDiagABInv; - btScalar oldNormalImpulse = m_accumulatedImpulse[i]; - btScalar sum = oldNormalImpulse + normalImpulse; - m_accumulatedImpulse[i] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum; - normalImpulse = m_accumulatedImpulse[i] - oldNormalImpulse; - - btVector3 impulse_vector = normalWorld * normalImpulse; - m_rbA.applyImpulse( impulse_vector, rel_pos1); - m_rbB.applyImpulse(-impulse_vector, rel_pos2); - - localNormalInA[i] = 0; - } - } - btVector3 axis; - btScalar angle; - btTransform frameAWorld = m_rbA.getCenterOfMassTransform() * m_frameInA; - btTransform frameBWorld = m_rbB.getCenterOfMassTransform() * m_frameInB; +void btGeneric6DofConstraint::buildLinearJacobian( + 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()); - btTransformUtil::calculateDiffAxisAngle(frameAWorld,frameBWorld,axis,angle); - btQuaternion diff(axis,angle); - btMatrix3x3 diffMat (diff); - btVector3 xyz; - ///this is not perfect, we can first check which axis are limited, and choose a more appropriate order - MatrixToEulerXYZ(diffMat,xyz); +} + +void btGeneric6DofConstraint::buildAngularJacobian( + 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()); - // angular - for (i=0;i<3;i++) - { - if (isLimited(i+3)) - { - btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); - btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); - - btScalar jacDiagABInv = btScalar(1.) / m_jacAng[i].getDiagonal(); - - //velocity error (first order error) - btScalar rel_vel = m_jacAng[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, - m_rbB.getLinearVelocity(),angvelB); - - //positional error (zeroth order error) - btVector3 axisA = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn( kAxisA[i] ); - btVector3 axisB = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn( kAxisB[i] ); - - btScalar rel_pos = kSign[i] * axisA.dot(axisB); - - btScalar lo = btScalar(-1e30); - btScalar hi = btScalar(1e30); - - //handle the twist limit - if (m_lowerLimit[i+3] < m_upperLimit[i+3]) - { - //clamp the values - btScalar loLimit = m_lowerLimit[i+3] > -3.1415 ? m_lowerLimit[i+3] : btScalar(-1e30); - btScalar hiLimit = m_upperLimit[i+3] < 3.1415 ? m_upperLimit[i+3] : btScalar(1e30); - - btScalar projAngle = btScalar(-1.)*xyz[i]; - - if (projAngle < loLimit) - { - hi = btScalar(0.); - rel_pos = (loLimit - projAngle); - } else - { - if (projAngle > hiLimit) - { - lo = btScalar(0.); - rel_pos = (hiLimit - projAngle); - } else - { - continue; - } - } - } - - //impulse - - btScalar normalImpulse= -(tau*rel_pos/timeStep + damping*rel_vel) * jacDiagABInv; - btScalar oldNormalImpulse = m_accumulatedImpulse[i+3]; - btScalar sum = oldNormalImpulse + normalImpulse; - m_accumulatedImpulse[i+3] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum; - normalImpulse = m_accumulatedImpulse[i+3] - oldNormalImpulse; - - // Dirk: Not needed - we could actually project onto Jacobian entry here (same as above) - btVector3 axis = kSign[i] * axisA.cross(axisB); - btVector3 impulse_vector = axis * normalImpulse; - - m_rbA.applyTorqueImpulse( impulse_vector); - m_rbB.applyTorqueImpulse(-impulse_vector); - } - } } -void btGeneric6DofConstraint::updateRHS(btScalar timeStep) +bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index) { - (void)timeStep; + btScalar angle = m_calculatedAxisAngleDiff[axis_index]; + //test limits + m_angularLimits[axis_index].testLimitValue(angle); + return m_angularLimits[axis_index].needApplyTorques(); } -btScalar btGeneric6DofConstraint::computeAngle(int axis) const - { - btScalar angle = btScalar(0.f); +void btGeneric6DofConstraint::buildJacobian() +{ - switch (axis) - { - case 0: - { - btVector3 v1 = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn(1); - btVector3 v2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(1); - btVector3 w2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(2); + // 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++) + { + m_angularLimits[i].m_accumulatedImpulse = btScalar(0.); + } + //calculates transform + calculateTransforms(); + +// const btVector3& pivotAInW = m_calculatedTransformA.getOrigin(); +// const btVector3& pivotBInW = m_calculatedTransformB.getOrigin(); + calcAnchorPos(); + btVector3 pivotAInW = m_AnchorPos; + btVector3 pivotBInW = m_AnchorPos; + +// not used here +// btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); +// btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); + + btVector3 normalWorld; + //linear part + for (i=0;i<3;i++) + { + if (m_linearLimits.isLimited(i)) + { + if (m_useLinearReferenceFrameA) + normalWorld = m_calculatedTransformA.getBasis().getColumn(i); + else + normalWorld = m_calculatedTransformB.getBasis().getColumn(i); - btScalar s = v1.dot(w2); - btScalar c = v1.dot(v2); + buildLinearJacobian( + m_jacLinear[i],normalWorld , + pivotAInW,pivotBInW); - angle = btAtan2( s, c ); - } - break; + } + } - case 1: - { - btVector3 w1 = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn(2); - btVector3 w2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(2); - btVector3 u2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(0); + // angular part + for (i=0;i<3;i++) + { + //calculates error angle + if (testAngularLimitMotor(i)) + { + normalWorld = this->getAxis(i); + // Create angular atom + buildAngularJacobian(m_jacAng[i],normalWorld); + } + } - btScalar s = w1.dot(u2); - btScalar c = w1.dot(w2); - angle = btAtan2( s, c ); - } - break; +} - case 2: - { - btVector3 u1 = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn(0); - btVector3 u2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(0); - btVector3 v2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(1); - btScalar s = u1.dot(v2); - btScalar c = u1.dot(u2); +void btGeneric6DofConstraint::solveConstraint(btScalar timeStep) +{ + m_timeStep = timeStep; - angle = btAtan2( s, c ); - } - break; - default: - btAssert ( 0 ) ; - - break ; - } + //calculateTransforms(); + + int i; + + // linear + + btVector3 pointInA = m_calculatedTransformA.getOrigin(); + btVector3 pointInB = m_calculatedTransformB.getOrigin(); + + btScalar jacDiagABInv; + btVector3 linear_axis; + for (i=0;i<3;i++) + { + if (m_linearLimits.isLimited(i)) + { + jacDiagABInv = btScalar(1.) / m_jacLinear[i].getDiagonal(); + + if (m_useLinearReferenceFrameA) + linear_axis = m_calculatedTransformA.getBasis().getColumn(i); + else + linear_axis = m_calculatedTransformB.getBasis().getColumn(i); + + m_linearLimits.solveLinearAxis( + m_timeStep, + jacDiagABInv, + m_rbA,pointInA, + m_rbB,pointInB, + i,linear_axis, m_AnchorPos); + + } + } + + // angular + btVector3 angular_axis; + btScalar angularJacDiagABInv; + for (i=0;i<3;i++) + { + if (m_angularLimits[i].needApplyTorques()) + { + + // get axis + angular_axis = getAxis(i); + + angularJacDiagABInv = btScalar(1.) / m_jacAng[i].getDiagonal(); + + m_angularLimits[i].solveAngularLimits(m_timeStep,angular_axis,angularJacDiagABInv, &m_rbA,&m_rbB); + } + } +} + +void btGeneric6DofConstraint::updateRHS(btScalar timeStep) +{ + (void)timeStep; + +} + +btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const +{ + return m_calculatedAxis[axis_index]; +} - return angle; +btScalar btGeneric6DofConstraint::getAngle(int axis_index) const +{ + return m_calculatedAxisAngleDiff[axis_index]; +} + +void btGeneric6DofConstraint::calcAnchorPos(void) +{ + btScalar imA = m_rbA.getInvMass(); + btScalar imB = m_rbB.getInvMass(); + btScalar weight; + if(imB == btScalar(0.0)) + { + weight = btScalar(1.0); + } + else + { + weight = imA / (imA + imB); } + const btVector3& pA = m_calculatedTransformA.getOrigin(); + const btVector3& pB = m_calculatedTransformB.getOrigin(); + m_AnchorPos = pA * weight + pB * (btScalar(1.0) - weight); + return; +} // btGeneric6DofConstraint::calcAnchorPos() |