diff options
Diffstat (limited to 'thirdparty/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp')
| -rw-r--r-- | thirdparty/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp | 683 |
1 files changed, 323 insertions, 360 deletions
diff --git a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp index 7e5e6f9e54..aa6f69000d 100644 --- a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp +++ b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btHingeConstraint.h" #include "BulletDynamics/Dynamics/btRigidBody.h" #include "LinearMath/btTransformUtil.h" @@ -21,8 +20,6 @@ subject to the following restrictions: #include <new> #include "btSolverBody.h" - - //#define HINGE_USE_OBSOLETE_SOLVER false #define HINGE_USE_OBSOLETE_SOLVER false @@ -30,59 +27,60 @@ subject to the following restrictions: #ifndef __SPU__ - - - - -btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, - const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA) - :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB), +btHingeConstraint::btHingeConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& pivotInA, const btVector3& pivotInB, + const btVector3& axisInA, const btVector3& axisInB, bool useReferenceFrameA) + : btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA, rbB), #ifdef _BT_USE_CENTER_LIMIT_ - m_limit(), + m_limit(), #endif - m_angularOnly(false), - m_enableAngularMotor(false), - m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), - m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), - m_useReferenceFrameA(useReferenceFrameA), - m_flags(0), - m_normalCFM(0), - m_normalERP(0), - m_stopCFM(0), - m_stopERP(0) + m_angularOnly(false), + m_enableAngularMotor(false), + m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), + m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), + m_useReferenceFrameA(useReferenceFrameA), + m_flags(0), + m_normalCFM(0), + m_normalERP(0), + m_stopCFM(0), + m_stopERP(0) { m_rbAFrame.getOrigin() = pivotInA; - + // since no frame is given, assume this to be zero angle and just pick rb transform axis btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0); btVector3 rbAxisA2; btScalar projection = axisInA.dot(rbAxisA1); - if (projection >= 1.0f - SIMD_EPSILON) { + if (projection >= 1.0f - SIMD_EPSILON) + { rbAxisA1 = -rbA.getCenterOfMassTransform().getBasis().getColumn(2); rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1); - } else if (projection <= -1.0f + SIMD_EPSILON) { + } + else if (projection <= -1.0f + SIMD_EPSILON) + { rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(2); - rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1); - } else { + rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1); + } + else + { rbAxisA2 = axisInA.cross(rbAxisA1); rbAxisA1 = rbAxisA2.cross(axisInA); } - m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(), - rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(), - rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() ); + m_rbAFrame.getBasis().setValue(rbAxisA1.getX(), rbAxisA2.getX(), axisInA.getX(), + rbAxisA1.getY(), rbAxisA2.getY(), axisInA.getY(), + rbAxisA1.getZ(), rbAxisA2.getZ(), axisInA.getZ()); + + btQuaternion rotationArc = shortestArcQuat(axisInA, axisInB); + btVector3 rbAxisB1 = quatRotate(rotationArc, rbAxisA1); + btVector3 rbAxisB2 = axisInB.cross(rbAxisB1); - btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB); - btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1); - btVector3 rbAxisB2 = axisInB.cross(rbAxisB1); - m_rbBFrame.getOrigin() = pivotInB; - m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(), - rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(), - rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() ); - -#ifndef _BT_USE_CENTER_LIMIT_ + m_rbBFrame.getBasis().setValue(rbAxisB1.getX(), rbAxisB2.getX(), axisInB.getX(), + rbAxisB1.getY(), rbAxisB2.getY(), axisInB.getY(), + rbAxisB1.getZ(), rbAxisB2.getZ(), axisInB.getZ()); + +#ifndef _BT_USE_CENTER_LIMIT_ //start with free m_lowerLimit = btScalar(1.0f); m_upperLimit = btScalar(-1.0f); @@ -94,47 +92,44 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); } - - -btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA) -:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), +btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btVector3& pivotInA, const btVector3& axisInA, bool useReferenceFrameA) + : btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), #ifdef _BT_USE_CENTER_LIMIT_ -m_limit(), + m_limit(), #endif -m_angularOnly(false), m_enableAngularMotor(false), -m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), -m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), -m_useReferenceFrameA(useReferenceFrameA), -m_flags(0), -m_normalCFM(0), -m_normalERP(0), -m_stopCFM(0), -m_stopERP(0) + m_angularOnly(false), + m_enableAngularMotor(false), + m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), + m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), + m_useReferenceFrameA(useReferenceFrameA), + m_flags(0), + m_normalCFM(0), + m_normalERP(0), + m_stopCFM(0), + m_stopERP(0) { - // since no frame is given, assume this to be zero angle and just pick rb transform axis // fixed axis in worldspace btVector3 rbAxisA1, rbAxisA2; btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2); m_rbAFrame.getOrigin() = pivotInA; - m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(), - rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(), - rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() ); + m_rbAFrame.getBasis().setValue(rbAxisA1.getX(), rbAxisA2.getX(), axisInA.getX(), + rbAxisA1.getY(), rbAxisA2.getY(), axisInA.getY(), + rbAxisA1.getZ(), rbAxisA2.getZ(), axisInA.getZ()); btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * axisInA; - btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB); - btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1); + btQuaternion rotationArc = shortestArcQuat(axisInA, axisInB); + btVector3 rbAxisB1 = quatRotate(rotationArc, rbAxisA1); btVector3 rbAxisB2 = axisInB.cross(rbAxisB1); - m_rbBFrame.getOrigin() = rbA.getCenterOfMassTransform()(pivotInA); - m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(), - rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(), - rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() ); - -#ifndef _BT_USE_CENTER_LIMIT_ + m_rbBFrame.getBasis().setValue(rbAxisB1.getX(), rbAxisB2.getX(), axisInB.getX(), + rbAxisB1.getY(), rbAxisB2.getY(), axisInB.getY(), + rbAxisB1.getZ(), rbAxisB2.getZ(), axisInB.getZ()); + +#ifndef _BT_USE_CENTER_LIMIT_ //start with free m_lowerLimit = btScalar(1.0f); m_upperLimit = btScalar(-1.0f); @@ -146,26 +141,24 @@ m_stopERP(0) m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); } - - -btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, - const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA) -:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame), +btHingeConstraint::btHingeConstraint(btRigidBody& rbA, btRigidBody& rbB, + const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA) + : btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA, rbB), m_rbAFrame(rbAFrame), m_rbBFrame(rbBFrame), #ifdef _BT_USE_CENTER_LIMIT_ -m_limit(), + m_limit(), #endif -m_angularOnly(false), -m_enableAngularMotor(false), -m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), -m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), -m_useReferenceFrameA(useReferenceFrameA), -m_flags(0), -m_normalCFM(0), -m_normalERP(0), -m_stopCFM(0), -m_stopERP(0) + m_angularOnly(false), + m_enableAngularMotor(false), + m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), + m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), + m_useReferenceFrameA(useReferenceFrameA), + m_flags(0), + m_normalCFM(0), + m_normalERP(0), + m_stopCFM(0), + m_stopERP(0) { -#ifndef _BT_USE_CENTER_LIMIT_ +#ifndef _BT_USE_CENTER_LIMIT_ //start with free m_lowerLimit = btScalar(1.0f); m_upperLimit = btScalar(-1.0f); @@ -175,30 +168,28 @@ m_stopERP(0) m_solveLimit = false; #endif m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); -} - - +} btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame, bool useReferenceFrameA) -:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame), + : btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_rbAFrame(rbAFrame), m_rbBFrame(rbAFrame), #ifdef _BT_USE_CENTER_LIMIT_ -m_limit(), + m_limit(), #endif -m_angularOnly(false), -m_enableAngularMotor(false), -m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), -m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), -m_useReferenceFrameA(useReferenceFrameA), -m_flags(0), -m_normalCFM(0), -m_normalERP(0), -m_stopCFM(0), -m_stopERP(0) + m_angularOnly(false), + m_enableAngularMotor(false), + m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), + m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), + m_useReferenceFrameA(useReferenceFrameA), + m_flags(0), + m_normalCFM(0), + m_normalERP(0), + m_stopCFM(0), + m_stopERP(0) { ///not providing rigidbody B means implicitly using worldspace for body B m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(m_rbAFrame.getOrigin()); -#ifndef _BT_USE_CENTER_LIMIT_ +#ifndef _BT_USE_CENTER_LIMIT_ //start with free m_lowerLimit = btScalar(1.0f); m_upperLimit = btScalar(-1.0f); @@ -210,9 +201,7 @@ m_stopERP(0) m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); } - - -void btHingeConstraint::buildJacobian() +void btHingeConstraint::buildJacobian() { if (m_useSolveConstraintObsolete) { @@ -221,8 +210,8 @@ void btHingeConstraint::buildJacobian() if (!m_angularOnly) { - btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); - btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); + btVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_rbAFrame.getOrigin(); + btVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_rbBFrame.getOrigin(); btVector3 relPos = pivotBInW - pivotAInW; btVector3 normal[3]; @@ -232,23 +221,23 @@ void btHingeConstraint::buildJacobian() } else { - normal[0].setValue(btScalar(1.0),0,0); + normal[0].setValue(btScalar(1.0), 0, 0); } btPlaneSpace1(normal[0], normal[1], normal[2]); - for (int i=0;i<3;i++) + for (int i = 0; i < 3; i++) { new (&m_jac[i]) btJacobianEntry( - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - pivotAInW - m_rbA.getCenterOfMassPosition(), - pivotBInW - m_rbB.getCenterOfMassPosition(), - normal[i], - 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(), + normal[i], + m_rbA.getInvInertiaDiagLocal(), + m_rbA.getInvMass(), + m_rbB.getInvInertiaDiagLocal(), + m_rbB.getInvMass()); } } @@ -258,60 +247,55 @@ void btHingeConstraint::buildJacobian() //this is unused for now, it's a todo btVector3 jointAxis0local; btVector3 jointAxis1local; - - btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local); + + btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2), jointAxis0local, jointAxis1local); btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local; btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local; btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); - - new (&m_jacAng[0]) btJacobianEntry(jointAxis0, - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - m_rbA.getInvInertiaDiagLocal(), - m_rbB.getInvInertiaDiagLocal()); - - new (&m_jacAng[1]) btJacobianEntry(jointAxis1, - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - m_rbA.getInvInertiaDiagLocal(), - m_rbB.getInvInertiaDiagLocal()); - - new (&m_jacAng[2]) btJacobianEntry(hingeAxisWorld, - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - m_rbA.getInvInertiaDiagLocal(), - m_rbB.getInvInertiaDiagLocal()); - - // clear accumulator - m_accLimitImpulse = btScalar(0.); - - // test angular limit - testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); - //Compute K = J*W*J' for hinge axis - btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); - m_kHinge = 1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) + - getRigidBodyB().computeAngularImpulseDenominator(axisA)); + new (&m_jacAng[0]) btJacobianEntry(jointAxis0, + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + m_rbA.getInvInertiaDiagLocal(), + m_rbB.getInvInertiaDiagLocal()); - } -} + new (&m_jacAng[1]) btJacobianEntry(jointAxis1, + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + m_rbA.getInvInertiaDiagLocal(), + m_rbB.getInvInertiaDiagLocal()); + new (&m_jacAng[2]) btJacobianEntry(hingeAxisWorld, + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + m_rbA.getInvInertiaDiagLocal(), + m_rbB.getInvInertiaDiagLocal()); -#endif //__SPU__ + // clear accumulator + m_accLimitImpulse = btScalar(0.); + // test angular limit + testLimit(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform()); -static inline btScalar btNormalizeAnglePositive(btScalar angle) -{ - return btFmod(btFmod(angle, btScalar(2.0*SIMD_PI)) + btScalar(2.0*SIMD_PI), btScalar(2.0*SIMD_PI)); + //Compute K = J*W*J' for hinge axis + btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); + m_kHinge = 1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) + + getRigidBodyB().computeAngularImpulseDenominator(axisA)); + } } +#endif //__SPU__ +static inline btScalar btNormalizeAnglePositive(btScalar angle) +{ + return btFmod(btFmod(angle, btScalar(2.0 * SIMD_PI)) + btScalar(2.0 * SIMD_PI), btScalar(2.0 * SIMD_PI)); +} static btScalar btShortestAngularDistance(btScalar accAngle, btScalar curAngle) { btScalar result = btNormalizeAngle(btNormalizeAnglePositive(btNormalizeAnglePositive(curAngle) - - btNormalizeAnglePositive(accAngle))); + btNormalizeAnglePositive(accAngle))); return result; } @@ -320,41 +304,36 @@ static btScalar btShortestAngleUpdate(btScalar accAngle, btScalar curAngle) btScalar tol(0.3); btScalar result = btShortestAngularDistance(accAngle, curAngle); - if (btFabs(result) > tol) + if (btFabs(result) > tol) return curAngle; - else + else return accAngle + result; return curAngle; } - btScalar btHingeAccumulatedAngleConstraint::getAccumulatedHingeAngle() { btScalar hingeAngle = getHingeAngle(); - m_accumulatedAngle = btShortestAngleUpdate(m_accumulatedAngle,hingeAngle); + m_accumulatedAngle = btShortestAngleUpdate(m_accumulatedAngle, hingeAngle); return m_accumulatedAngle; } -void btHingeAccumulatedAngleConstraint::setAccumulatedHingeAngle(btScalar accAngle) +void btHingeAccumulatedAngleConstraint::setAccumulatedHingeAngle(btScalar accAngle) { - m_accumulatedAngle = accAngle; + m_accumulatedAngle = accAngle; } void btHingeAccumulatedAngleConstraint::getInfo1(btConstraintInfo1* info) { //update m_accumulatedAngle btScalar curHingeAngle = getHingeAngle(); - m_accumulatedAngle = btShortestAngleUpdate(m_accumulatedAngle,curHingeAngle); + m_accumulatedAngle = btShortestAngleUpdate(m_accumulatedAngle, curHingeAngle); btHingeConstraint::getInfo1(info); - } - void btHingeConstraint::getInfo1(btConstraintInfo1* info) { - - if (m_useSolveConstraintObsolete) { info->m_numConstraintRows = 0; @@ -362,17 +341,16 @@ void btHingeConstraint::getInfo1(btConstraintInfo1* info) } else { - info->m_numConstraintRows = 5; // Fixed 3 linear + 2 angular - info->nub = 1; + info->m_numConstraintRows = 5; // Fixed 3 linear + 2 angular + info->nub = 1; //always add the row, to avoid computation (data is not available yet) //prepare constraint - testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); - if(getSolveLimit() || getEnableAngularMotor()) + testLimit(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform()); + if (getSolveLimit() || getEnableAngularMotor()) { - info->m_numConstraintRows++; // limit 3rd anguar as well - info->nub--; + info->m_numConstraintRows++; // limit 3rd anguar as well + info->nub--; } - } } @@ -386,41 +364,38 @@ void btHingeConstraint::getInfo1NonVirtual(btConstraintInfo1* info) else { //always add the 'limit' row, to avoid computation (data is not available yet) - info->m_numConstraintRows = 6; // Fixed 3 linear + 2 angular - info->nub = 0; + info->m_numConstraintRows = 6; // Fixed 3 linear + 2 angular + info->nub = 0; } } -void btHingeConstraint::getInfo2 (btConstraintInfo2* info) +void btHingeConstraint::getInfo2(btConstraintInfo2* info) { - if(m_useOffsetForConstraintFrame) + if (m_useOffsetForConstraintFrame) { - getInfo2InternalUsingFrameOffset(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getAngularVelocity(),m_rbB.getAngularVelocity()); + getInfo2InternalUsingFrameOffset(info, m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform(), m_rbA.getAngularVelocity(), m_rbB.getAngularVelocity()); } else { - getInfo2Internal(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getAngularVelocity(),m_rbB.getAngularVelocity()); + getInfo2Internal(info, m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform(), m_rbA.getAngularVelocity(), m_rbB.getAngularVelocity()); } } - -void btHingeConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB) +void btHingeConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB, const btVector3& angVelA, const btVector3& angVelB) { ///the regular (virtual) implementation getInfo2 already performs 'testLimit' during getInfo1, so we need to do it now - testLimit(transA,transB); + testLimit(transA, transB); - getInfo2Internal(info,transA,transB,angVelA,angVelB); + getInfo2Internal(info, transA, transB, angVelA, angVelB); } - -void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB) +void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB, const btVector3& angVelA, const btVector3& angVelB) { - btAssert(!m_useSolveConstraintObsolete); int i, skip = info->rowskip; // transforms in world space - btTransform trA = transA*m_rbAFrame; - btTransform trB = transB*m_rbBFrame; + btTransform trA = transA * m_rbAFrame; + btTransform trB = transB * m_rbBFrame; // pivot point btVector3 pivotAInW = trA.getOrigin(); btVector3 pivotBInW = trB.getOrigin(); @@ -448,7 +423,7 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf info->m_constraintError[i*skip]=0.f; } } -#endif //#if 0 +#endif //#if 0 // linear (all fixed) if (!m_angularOnly) @@ -460,10 +435,7 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf info->m_J2linearAxis[0] = -1; info->m_J2linearAxis[skip + 1] = -1; info->m_J2linearAxis[2 * skip + 2] = -1; - } - - - + } btVector3 a1 = pivotAInW - transA.getOrigin(); { @@ -471,22 +443,22 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + skip); btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * skip); btVector3 a1neg = -a1; - a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); + a1neg.getSkewSymmetricMatrix(angular0, angular1, angular2); } btVector3 a2 = pivotBInW - transB.getOrigin(); { btVector3* angular0 = (btVector3*)(info->m_J2angularAxis); btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + skip); btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * skip); - a2.getSkewSymmetricMatrix(angular0,angular1,angular2); + a2.getSkewSymmetricMatrix(angular0, angular1, angular2); } // linear RHS btScalar normalErp = (m_flags & BT_HINGE_FLAGS_ERP_NORM) ? m_normalERP : info->erp; - btScalar k = info->fps * normalErp; + btScalar k = info->fps * normalErp; if (!m_angularOnly) { - for(i = 0; i < 3; i++) + for (i = 0; i < 3; i++) { info->m_constraintError[i * skip] = k * (pivotBInW[i] - pivotAInW[i]); } @@ -504,9 +476,9 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf // get 2 orthos to hinge axis (X, Y) btVector3 p = trA.getBasis().getColumn(0); btVector3 q = trA.getBasis().getColumn(1); - // set the two hinge angular rows - int s3 = 3 * info->rowskip; - int s4 = 4 * info->rowskip; + // set the two hinge angular rows + int s3 = 3 * info->rowskip; + int s4 = 4 * info->rowskip; info->m_J1angularAxis[s3 + 0] = p[0]; info->m_J1angularAxis[s3 + 1] = p[1]; @@ -521,181 +493,172 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf info->m_J2angularAxis[s4 + 0] = -q[0]; info->m_J2angularAxis[s4 + 1] = -q[1]; info->m_J2angularAxis[s4 + 2] = -q[2]; - // compute the right hand side of the constraint equation. set relative - // body velocities along p and q to bring the hinge back into alignment. - // if ax1,ax2 are the unit length hinge axes as computed from body1 and - // body2, we need to rotate both bodies along the axis u = (ax1 x ax2). - // if `theta' is the angle between ax1 and ax2, we need an angular velocity - // along u to cover angle erp*theta in one step : - // |angular_velocity| = angle/time = erp*theta / stepsize - // = (erp*fps) * theta - // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| - // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) - // ...as ax1 and ax2 are unit length. if theta is smallish, - // theta ~= sin(theta), so - // angular_velocity = (erp*fps) * (ax1 x ax2) - // ax1 x ax2 is in the plane space of ax1, so we project the angular - // velocity to p and q to find the right hand side. - btVector3 ax2 = trB.getBasis().getColumn(2); + // compute the right hand side of the constraint equation. set relative + // body velocities along p and q to bring the hinge back into alignment. + // if ax1,ax2 are the unit length hinge axes as computed from body1 and + // body2, we need to rotate both bodies along the axis u = (ax1 x ax2). + // if `theta' is the angle between ax1 and ax2, we need an angular velocity + // along u to cover angle erp*theta in one step : + // |angular_velocity| = angle/time = erp*theta / stepsize + // = (erp*fps) * theta + // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| + // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) + // ...as ax1 and ax2 are unit length. if theta is smallish, + // theta ~= sin(theta), so + // angular_velocity = (erp*fps) * (ax1 x ax2) + // ax1 x ax2 is in the plane space of ax1, so we project the angular + // velocity to p and q to find the right hand side. + btVector3 ax2 = trB.getBasis().getColumn(2); btVector3 u = ax1.cross(ax2); info->m_constraintError[s3] = k * u.dot(p); info->m_constraintError[s4] = k * u.dot(q); // check angular limits - int nrow = 4; // last filled row + int nrow = 4; // last filled row int srow; btScalar limit_err = btScalar(0.0); int limit = 0; - if(getSolveLimit()) + if (getSolveLimit()) { -#ifdef _BT_USE_CENTER_LIMIT_ - limit_err = m_limit.getCorrection() * m_referenceSign; +#ifdef _BT_USE_CENTER_LIMIT_ + limit_err = m_limit.getCorrection() * m_referenceSign; #else - limit_err = m_correction * m_referenceSign; + limit_err = m_correction * m_referenceSign; #endif - limit = (limit_err > btScalar(0.0)) ? 1 : 2; - + limit = (limit_err > btScalar(0.0)) ? 1 : 2; } // if the hinge has joint limits or motor, add in the extra row bool powered = getEnableAngularMotor(); - if(limit || powered) + if (limit || powered) { nrow++; srow = nrow * info->rowskip; - info->m_J1angularAxis[srow+0] = ax1[0]; - info->m_J1angularAxis[srow+1] = ax1[1]; - info->m_J1angularAxis[srow+2] = ax1[2]; + info->m_J1angularAxis[srow + 0] = ax1[0]; + info->m_J1angularAxis[srow + 1] = ax1[1]; + info->m_J1angularAxis[srow + 2] = ax1[2]; - info->m_J2angularAxis[srow+0] = -ax1[0]; - info->m_J2angularAxis[srow+1] = -ax1[1]; - info->m_J2angularAxis[srow+2] = -ax1[2]; + info->m_J2angularAxis[srow + 0] = -ax1[0]; + info->m_J2angularAxis[srow + 1] = -ax1[1]; + info->m_J2angularAxis[srow + 2] = -ax1[2]; btScalar lostop = getLowerLimit(); btScalar histop = getUpperLimit(); - if(limit && (lostop == histop)) + if (limit && (lostop == histop)) { // the joint motor is ineffective powered = false; } info->m_constraintError[srow] = btScalar(0.0f); btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : normalErp; - if(powered) + if (powered) { - if(m_flags & BT_HINGE_FLAGS_CFM_NORM) + if (m_flags & BT_HINGE_FLAGS_CFM_NORM) { info->cfm[srow] = m_normalCFM; } btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP); info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign; - info->m_lowerLimit[srow] = - m_maxMotorImpulse; - info->m_upperLimit[srow] = m_maxMotorImpulse; + info->m_lowerLimit[srow] = -m_maxMotorImpulse; + info->m_upperLimit[srow] = m_maxMotorImpulse; } - if(limit) + if (limit) { k = info->fps * currERP; info->m_constraintError[srow] += k * limit_err; - if(m_flags & BT_HINGE_FLAGS_CFM_STOP) + if (m_flags & BT_HINGE_FLAGS_CFM_STOP) { info->cfm[srow] = m_stopCFM; } - if(lostop == histop) + if (lostop == histop) { // limited low and high simultaneously info->m_lowerLimit[srow] = -SIMD_INFINITY; info->m_upperLimit[srow] = SIMD_INFINITY; } - else if(limit == 1) - { // low limit + else if (limit == 1) + { // low limit info->m_lowerLimit[srow] = 0; info->m_upperLimit[srow] = SIMD_INFINITY; } - else - { // high limit + else + { // high limit info->m_lowerLimit[srow] = -SIMD_INFINITY; info->m_upperLimit[srow] = 0; } // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that) -#ifdef _BT_USE_CENTER_LIMIT_ +#ifdef _BT_USE_CENTER_LIMIT_ btScalar bounce = m_limit.getRelaxationFactor(); #else btScalar bounce = m_relaxationFactor; #endif - if(bounce > btScalar(0.0)) + if (bounce > btScalar(0.0)) { btScalar vel = angVelA.dot(ax1); vel -= angVelB.dot(ax1); // only apply bounce if the velocity is incoming, and if the // resulting c[] exceeds what we already have. - if(limit == 1) - { // low limit - if(vel < 0) + if (limit == 1) + { // low limit + if (vel < 0) { btScalar newc = -bounce * vel; - if(newc > info->m_constraintError[srow]) + if (newc > info->m_constraintError[srow]) { info->m_constraintError[srow] = newc; } } } else - { // high limit - all those computations are reversed - if(vel > 0) + { // high limit - all those computations are reversed + if (vel > 0) { btScalar newc = -bounce * vel; - if(newc < info->m_constraintError[srow]) + if (newc < info->m_constraintError[srow]) { info->m_constraintError[srow] = newc; } } } } -#ifdef _BT_USE_CENTER_LIMIT_ +#ifdef _BT_USE_CENTER_LIMIT_ info->m_constraintError[srow] *= m_limit.getBiasFactor(); #else info->m_constraintError[srow] *= m_biasFactor; #endif - } // if(limit) - } // if angular limit or powered + } // if(limit) + } // if angular limit or powered } - -void btHingeConstraint::setFrames(const btTransform & frameA, const btTransform & frameB) +void btHingeConstraint::setFrames(const btTransform& frameA, const btTransform& frameB) { m_rbAFrame = frameA; m_rbBFrame = frameB; buildJacobian(); } - -void btHingeConstraint::updateRHS(btScalar timeStep) +void btHingeConstraint::updateRHS(btScalar timeStep) { (void)timeStep; - } - - - btScalar btHingeConstraint::getHingeAngle() { - return getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); + return getHingeAngle(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform()); } -btScalar btHingeConstraint::getHingeAngle(const btTransform& transA,const btTransform& transB) +btScalar btHingeConstraint::getHingeAngle(const btTransform& transA, const btTransform& transB) { - const btVector3 refAxis0 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0); - const btVector3 refAxis1 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1); + const btVector3 refAxis0 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0); + const btVector3 refAxis1 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1); const btVector3 swingAxis = transB.getBasis() * m_rbBFrame.getBasis().getColumn(1); -// btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)); + // btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)); btScalar angle = btAtan2(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)); return m_referenceSign * angle; } - - -void btHingeConstraint::testLimit(const btTransform& transA,const btTransform& transB) +void btHingeConstraint::testLimit(const btTransform& transA, const btTransform& transB) { // Compute limit information - m_hingeAngle = getHingeAngle(transA,transB); -#ifdef _BT_USE_CENTER_LIMIT_ + m_hingeAngle = getHingeAngle(transA, transB); +#ifdef _BT_USE_CENTER_LIMIT_ m_limit.test(m_hingeAngle); #else m_correction = btScalar(0.); @@ -709,7 +672,7 @@ void btHingeConstraint::testLimit(const btTransform& transA,const btTransform& t m_correction = (m_lowerLimit - m_hingeAngle); m_limitSign = 1.0f; m_solveLimit = true; - } + } else if (m_hingeAngle >= m_upperLimit) { m_correction = m_upperLimit - m_hingeAngle; @@ -721,7 +684,6 @@ void btHingeConstraint::testLimit(const btTransform& transA,const btTransform& t return; } - static btVector3 vHinge(0, 0, btScalar(1)); void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt) @@ -731,14 +693,15 @@ void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt) qConstraint.normalize(); // extract "pure" hinge component - btVector3 vNoHinge = quatRotate(qConstraint, vHinge); vNoHinge.normalize(); + btVector3 vNoHinge = quatRotate(qConstraint, vHinge); + vNoHinge.normalize(); btQuaternion qNoHinge = shortestArcQuat(vHinge, vNoHinge); btQuaternion qHinge = qNoHinge.inverse() * qConstraint; qHinge.normalize(); // compute angular target, clamped to limits btScalar targetAngle = qHinge.getAngle(); - if (targetAngle > SIMD_PI) // long way around. flip quat and recalculate. + if (targetAngle > SIMD_PI) // long way around. flip quat and recalculate. { qHinge = -(qHinge); targetAngle = qHinge.getAngle(); @@ -751,7 +714,7 @@ void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt) void btHingeConstraint::setMotorTarget(btScalar targetAngle, btScalar dt) { -#ifdef _BT_USE_CENTER_LIMIT_ +#ifdef _BT_USE_CENTER_LIMIT_ m_limit.fit(targetAngle); #else if (m_lowerLimit < m_upperLimit) @@ -763,20 +726,18 @@ void btHingeConstraint::setMotorTarget(btScalar targetAngle, btScalar dt) } #endif // compute angular velocity - btScalar curAngle = getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); + btScalar curAngle = getHingeAngle(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform()); btScalar dAngle = targetAngle - curAngle; m_motorTargetVelocity = dAngle / dt; } - - -void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB) +void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB, const btVector3& angVelA, const btVector3& angVelB) { btAssert(!m_useSolveConstraintObsolete); int i, s = info->rowskip; // transforms in world space - btTransform trA = transA*m_rbAFrame; - btTransform trB = transB*m_rbBFrame; + btTransform trA = transA * m_rbAFrame; + btTransform trB = transB * m_rbBFrame; // pivot point // btVector3 pivotAInW = trA.getOrigin(); // btVector3 pivotBInW = trB.getOrigin(); @@ -789,11 +750,11 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON); btScalar miS = miA + miB; btScalar factA, factB; - if(miS > btScalar(0.f)) + if (miS > btScalar(0.f)) { factA = miB / miS; } - else + else { factA = btScalar(0.5f); } @@ -803,15 +764,21 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info btVector3 ax1A = trA.getBasis().getColumn(2); btVector3 ax1B = trB.getBasis().getColumn(2); btVector3 ax1 = ax1A * factA + ax1B * factB; + if (ax1.length2()<SIMD_EPSILON) + { + factA=0.f; + factB=1.f; + ax1 = ax1A * factA + ax1B * factB; + } ax1.normalize(); - // fill first 3 rows + // fill first 3 rows // we want: velA + wA x relA == velB + wB x relB btTransform bodyA_trans = transA; btTransform bodyB_trans = transB; int s0 = 0; int s1 = s; int s2 = s * 2; - int nrow = 2; // last filled row + int nrow = 2; // last filled row btVector3 tmpA, tmpB, relA, relB, p, q; // get vector from bodyB to frameB in WCS relB = trB.getOrigin() - bodyB_trans.getOrigin(); @@ -830,7 +797,7 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info // now choose average ortho to hinge axis p = orthoB * factA + orthoA * factB; btScalar len2 = p.length2(); - if(len2 > SIMD_EPSILON) + if (len2 > SIMD_EPSILON) { p /= btSqrt(len2); } @@ -843,44 +810,44 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info // fill three rows tmpA = relA.cross(p); tmpB = relB.cross(p); - for (i=0; i<3; i++) info->m_J1angularAxis[s0+i] = tmpA[i]; - for (i=0; i<3; i++) info->m_J2angularAxis[s0+i] = -tmpB[i]; + for (i = 0; i < 3; i++) info->m_J1angularAxis[s0 + i] = tmpA[i]; + for (i = 0; i < 3; i++) info->m_J2angularAxis[s0 + i] = -tmpB[i]; tmpA = relA.cross(q); tmpB = relB.cross(q); - if(hasStaticBody && getSolveLimit()) - { // to make constraint between static and dynamic objects more rigid + if (hasStaticBody && getSolveLimit()) + { // to make constraint between static and dynamic objects more rigid // remove wA (or wB) from equation if angular limit is hit tmpB *= factB; tmpA *= factA; } - for (i=0; i<3; i++) info->m_J1angularAxis[s1+i] = tmpA[i]; - for (i=0; i<3; i++) info->m_J2angularAxis[s1+i] = -tmpB[i]; + for (i = 0; i < 3; i++) info->m_J1angularAxis[s1 + i] = tmpA[i]; + for (i = 0; i < 3; i++) info->m_J2angularAxis[s1 + i] = -tmpB[i]; tmpA = relA.cross(ax1); tmpB = relB.cross(ax1); - if(hasStaticBody) - { // to make constraint between static and dynamic objects more rigid + if (hasStaticBody) + { // to make constraint between static and dynamic objects more rigid // remove wA (or wB) from equation tmpB *= factB; tmpA *= factA; } - for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i]; - for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i]; + for (i = 0; i < 3; i++) info->m_J1angularAxis[s2 + i] = tmpA[i]; + for (i = 0; i < 3; i++) info->m_J2angularAxis[s2 + i] = -tmpB[i]; - btScalar normalErp = (m_flags & BT_HINGE_FLAGS_ERP_NORM)? m_normalERP : info->erp; + btScalar normalErp = (m_flags & BT_HINGE_FLAGS_ERP_NORM) ? m_normalERP : info->erp; btScalar k = info->fps * normalErp; if (!m_angularOnly) { - for (i=0; i<3; i++) info->m_J1linearAxis[s0+i] = p[i]; - for (i=0; i<3; i++) info->m_J1linearAxis[s1+i] = q[i]; - for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = ax1[i]; + for (i = 0; i < 3; i++) info->m_J1linearAxis[s0 + i] = p[i]; + for (i = 0; i < 3; i++) info->m_J1linearAxis[s1 + i] = q[i]; + for (i = 0; i < 3; i++) info->m_J1linearAxis[s2 + i] = ax1[i]; + + for (i = 0; i < 3; i++) info->m_J2linearAxis[s0 + i] = -p[i]; + for (i = 0; i < 3; i++) info->m_J2linearAxis[s1 + i] = -q[i]; + for (i = 0; i < 3; i++) info->m_J2linearAxis[s2 + i] = -ax1[i]; - for (i=0; i<3; i++) info->m_J2linearAxis[s0+i] = -p[i]; - for (i=0; i<3; i++) info->m_J2linearAxis[s1+i] = -q[i]; - for (i=0; i<3; i++) info->m_J2linearAxis[s2+i] = -ax1[i]; + // compute three elements of right hand side - // compute three elements of right hand side - btScalar rhs = k * p.dot(ofs); info->m_constraintError[s0] = rhs; rhs = k * q.dot(ofs); @@ -925,146 +892,144 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info // angular_velocity = (erp*fps) * (ax1 x ax2) // ax1 x ax2 is in the plane space of ax1, so we project the angular // velocity to p and q to find the right hand side. - k = info->fps * normalErp;//?? + k = info->fps * normalErp; //?? btVector3 u = ax1A.cross(ax1B); info->m_constraintError[s3] = k * u.dot(p); info->m_constraintError[s4] = k * u.dot(q); #endif // check angular limits - nrow = 4; // last filled row + nrow = 4; // last filled row int srow; btScalar limit_err = btScalar(0.0); int limit = 0; - if(getSolveLimit()) + if (getSolveLimit()) { -#ifdef _BT_USE_CENTER_LIMIT_ - limit_err = m_limit.getCorrection() * m_referenceSign; +#ifdef _BT_USE_CENTER_LIMIT_ + limit_err = m_limit.getCorrection() * m_referenceSign; #else - limit_err = m_correction * m_referenceSign; + limit_err = m_correction * m_referenceSign; #endif - limit = (limit_err > btScalar(0.0)) ? 1 : 2; - + limit = (limit_err > btScalar(0.0)) ? 1 : 2; } // if the hinge has joint limits or motor, add in the extra row bool powered = getEnableAngularMotor(); - if(limit || powered) + if (limit || powered) { nrow++; srow = nrow * info->rowskip; - info->m_J1angularAxis[srow+0] = ax1[0]; - info->m_J1angularAxis[srow+1] = ax1[1]; - info->m_J1angularAxis[srow+2] = ax1[2]; + info->m_J1angularAxis[srow + 0] = ax1[0]; + info->m_J1angularAxis[srow + 1] = ax1[1]; + info->m_J1angularAxis[srow + 2] = ax1[2]; - info->m_J2angularAxis[srow+0] = -ax1[0]; - info->m_J2angularAxis[srow+1] = -ax1[1]; - info->m_J2angularAxis[srow+2] = -ax1[2]; + info->m_J2angularAxis[srow + 0] = -ax1[0]; + info->m_J2angularAxis[srow + 1] = -ax1[1]; + info->m_J2angularAxis[srow + 2] = -ax1[2]; btScalar lostop = getLowerLimit(); btScalar histop = getUpperLimit(); - if(limit && (lostop == histop)) + if (limit && (lostop == histop)) { // the joint motor is ineffective powered = false; } info->m_constraintError[srow] = btScalar(0.0f); btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : normalErp; - if(powered) + if (powered) { - if(m_flags & BT_HINGE_FLAGS_CFM_NORM) + if (m_flags & BT_HINGE_FLAGS_CFM_NORM) { info->cfm[srow] = m_normalCFM; } btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP); info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign; - info->m_lowerLimit[srow] = - m_maxMotorImpulse; - info->m_upperLimit[srow] = m_maxMotorImpulse; + info->m_lowerLimit[srow] = -m_maxMotorImpulse; + info->m_upperLimit[srow] = m_maxMotorImpulse; } - if(limit) + if (limit) { k = info->fps * currERP; info->m_constraintError[srow] += k * limit_err; - if(m_flags & BT_HINGE_FLAGS_CFM_STOP) + if (m_flags & BT_HINGE_FLAGS_CFM_STOP) { info->cfm[srow] = m_stopCFM; } - if(lostop == histop) + if (lostop == histop) { // limited low and high simultaneously info->m_lowerLimit[srow] = -SIMD_INFINITY; info->m_upperLimit[srow] = SIMD_INFINITY; } - else if(limit == 1) - { // low limit + else if (limit == 1) + { // low limit info->m_lowerLimit[srow] = 0; info->m_upperLimit[srow] = SIMD_INFINITY; } - else - { // high limit + else + { // high limit info->m_lowerLimit[srow] = -SIMD_INFINITY; info->m_upperLimit[srow] = 0; } // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that) -#ifdef _BT_USE_CENTER_LIMIT_ +#ifdef _BT_USE_CENTER_LIMIT_ btScalar bounce = m_limit.getRelaxationFactor(); #else btScalar bounce = m_relaxationFactor; #endif - if(bounce > btScalar(0.0)) + if (bounce > btScalar(0.0)) { btScalar vel = angVelA.dot(ax1); vel -= angVelB.dot(ax1); // only apply bounce if the velocity is incoming, and if the // resulting c[] exceeds what we already have. - if(limit == 1) - { // low limit - if(vel < 0) + if (limit == 1) + { // low limit + if (vel < 0) { btScalar newc = -bounce * vel; - if(newc > info->m_constraintError[srow]) + if (newc > info->m_constraintError[srow]) { info->m_constraintError[srow] = newc; } } } else - { // high limit - all those computations are reversed - if(vel > 0) + { // high limit - all those computations are reversed + if (vel > 0) { btScalar newc = -bounce * vel; - if(newc < info->m_constraintError[srow]) + if (newc < info->m_constraintError[srow]) { info->m_constraintError[srow] = newc; } } } } -#ifdef _BT_USE_CENTER_LIMIT_ +#ifdef _BT_USE_CENTER_LIMIT_ info->m_constraintError[srow] *= m_limit.getBiasFactor(); #else info->m_constraintError[srow] *= m_biasFactor; #endif - } // if(limit) - } // if angular limit or powered + } // if(limit) + } // if angular limit or powered } - -///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). +///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 btHingeConstraint::setParam(int num, btScalar value, int axis) { - if((axis == -1) || (axis == 5)) + if ((axis == -1) || (axis == 5)) { - switch(num) - { - case BT_CONSTRAINT_STOP_ERP : + switch (num) + { + case BT_CONSTRAINT_STOP_ERP: m_stopERP = value; m_flags |= BT_HINGE_FLAGS_ERP_STOP; break; - case BT_CONSTRAINT_STOP_CFM : + case BT_CONSTRAINT_STOP_CFM: m_stopCFM = value; m_flags |= BT_HINGE_FLAGS_CFM_STOP; break; - case BT_CONSTRAINT_CFM : + case BT_CONSTRAINT_CFM: m_normalCFM = value; m_flags |= BT_HINGE_FLAGS_CFM_NORM; break; @@ -1072,7 +1037,7 @@ void btHingeConstraint::setParam(int num, btScalar value, int axis) m_normalERP = value; m_flags |= BT_HINGE_FLAGS_ERP_NORM; break; - default : + default: btAssertConstrParams(0); } } @@ -1083,22 +1048,22 @@ void btHingeConstraint::setParam(int num, btScalar value, int axis) } ///return the local value of parameter -btScalar btHingeConstraint::getParam(int num, int axis) const +btScalar btHingeConstraint::getParam(int num, int axis) const { btScalar retVal = 0; - if((axis == -1) || (axis == 5)) + if ((axis == -1) || (axis == 5)) { - switch(num) - { - case BT_CONSTRAINT_STOP_ERP : + switch (num) + { + case BT_CONSTRAINT_STOP_ERP: btAssertConstrParams(m_flags & BT_HINGE_FLAGS_ERP_STOP); retVal = m_stopERP; break; - case BT_CONSTRAINT_STOP_CFM : + case BT_CONSTRAINT_STOP_CFM: btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_STOP); retVal = m_stopCFM; break; - case BT_CONSTRAINT_CFM : + case BT_CONSTRAINT_CFM: btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_NORM); retVal = m_normalCFM; break; @@ -1106,7 +1071,7 @@ btScalar btHingeConstraint::getParam(int num, int axis) const btAssertConstrParams(m_flags & BT_HINGE_FLAGS_ERP_NORM); retVal = m_normalERP; break; - default : + default: btAssertConstrParams(0); } } @@ -1116,5 +1081,3 @@ btScalar btHingeConstraint::getParam(int num, int axis) const } return retVal; } - - |