diff options
Diffstat (limited to 'extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp')
| -rwxr-xr-x[-rw-r--r--] | extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp | 659 |
1 files changed, 309 insertions, 350 deletions
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp index f8f81bfe6fa..cac5302a73e 100644..100755 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp @@ -18,8 +18,6 @@ Added by Roman Ponomarev (rponom@gmail.com) April 04, 2008 */ - - #include "btSliderConstraint.h" #include "BulletDynamics/Dynamics/btRigidBody.h" #include "LinearMath/btTransformUtil.h" @@ -29,10 +27,10 @@ April 04, 2008 void btSliderConstraint::initParams() { - m_lowerLinLimit = btScalar(1.0); - m_upperLinLimit = btScalar(-1.0); - m_lowerAngLimit = btScalar(0.); - m_upperAngLimit = btScalar(0.); + m_lowerLinLimit = btScalar(1.0); + m_upperLinLimit = btScalar(-1.0); + m_lowerAngLimit = btScalar(0.); + m_upperAngLimit = btScalar(0.); m_softnessDirLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; m_restitutionDirLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; m_dampingDirLin = btScalar(0.); @@ -59,13 +57,13 @@ void btSliderConstraint::initParams() m_cfmLimAng = SLIDER_CONSTRAINT_DEF_CFM; m_poweredLinMotor = false; - m_targetLinMotorVelocity = btScalar(0.); - m_maxLinMotorForce = btScalar(0.); + m_targetLinMotorVelocity = btScalar(0.); + m_maxLinMotorForce = btScalar(0.); m_accumulatedLinMotorImpulse = btScalar(0.0); m_poweredAngMotor = false; - m_targetAngMotorVelocity = btScalar(0.); - m_maxAngMotorForce = btScalar(0.); + m_targetAngMotorVelocity = btScalar(0.); + m_maxAngMotorForce = btScalar(0.); m_accumulatedAngMotorImpulse = btScalar(0.0); m_flags = 0; @@ -73,43 +71,32 @@ void btSliderConstraint::initParams() m_useOffsetForConstraintFrame = USE_OFFSET_FOR_CONSTANT_FRAME; - calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); + calculateTransforms(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform()); } - - - - btSliderConstraint::btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA) - : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, rbA, rbB), - m_useSolveConstraintObsolete(false), - m_frameInA(frameInA), - m_frameInB(frameInB), - m_useLinearReferenceFrameA(useLinearReferenceFrameA) + : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, rbA, rbB), + m_useSolveConstraintObsolete(false), + m_frameInA(frameInA), + m_frameInB(frameInB), + m_useLinearReferenceFrameA(useLinearReferenceFrameA) { initParams(); } - - btSliderConstraint::btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameA) - : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, getFixedBody(), rbB), - m_useSolveConstraintObsolete(false), - m_frameInB(frameInB), - m_useLinearReferenceFrameA(useLinearReferenceFrameA) + : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, getFixedBody(), rbB), + m_useSolveConstraintObsolete(false), + m_frameInB(frameInB), + m_useLinearReferenceFrameA(useLinearReferenceFrameA) { ///not providing rigidbody A means implicitly using worldspace for body A m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB; -// m_frameInA.getOrigin() = m_rbA.getCenterOfMassTransform()(m_frameInA.getOrigin()); + // m_frameInA.getOrigin() = m_rbA.getCenterOfMassTransform()(m_frameInA.getOrigin()); initParams(); } - - - - - void btSliderConstraint::getInfo1(btConstraintInfo1* info) { if (m_useSolveConstraintObsolete) @@ -119,46 +106,39 @@ void btSliderConstraint::getInfo1(btConstraintInfo1* info) } else { - info->m_numConstraintRows = 4; // Fixed 2 linear + 2 angular - info->nub = 2; + info->m_numConstraintRows = 4; // Fixed 2 linear + 2 angular + info->nub = 2; //prepare constraint - calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); + calculateTransforms(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform()); testAngLimits(); testLinLimits(); - if(getSolveLinLimit() || getPoweredLinMotor()) + if (getSolveLinLimit() || getPoweredLinMotor()) { - info->m_numConstraintRows++; // limit 3rd linear as well - info->nub--; + info->m_numConstraintRows++; // limit 3rd linear as well + info->nub--; } - if(getSolveAngLimit() || getPoweredAngMotor()) + if (getSolveAngLimit() || getPoweredAngMotor()) { - info->m_numConstraintRows++; // limit 3rd angular as well - info->nub--; + info->m_numConstraintRows++; // limit 3rd angular as well + info->nub--; } } } void btSliderConstraint::getInfo1NonVirtual(btConstraintInfo1* info) { - - info->m_numConstraintRows = 6; // Fixed 2 linear + 2 angular + 1 limit (even if not used) - info->nub = 0; + info->m_numConstraintRows = 6; // Fixed 2 linear + 2 angular + 1 limit (even if not used) + info->nub = 0; } void btSliderConstraint::getInfo2(btConstraintInfo2* info) { - getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(), m_rbA.getLinearVelocity(),m_rbB.getLinearVelocity(), m_rbA.getInvMass(),m_rbB.getInvMass()); + getInfo2NonVirtual(info, m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform(), m_rbA.getLinearVelocity(), m_rbB.getLinearVelocity(), m_rbA.getInvMass(), m_rbB.getInvMass()); } - - - - - - -void btSliderConstraint::calculateTransforms(const btTransform& transA,const btTransform& transB) +void btSliderConstraint::calculateTransforms(const btTransform& transA, const btTransform& transB) { - if(m_useLinearReferenceFrameA || (!m_useSolveConstraintObsolete)) + if (m_useLinearReferenceFrameA || (!m_useSolveConstraintObsolete)) { m_calculatedTransformA = transA * m_frameInA; m_calculatedTransformB = transB * m_frameInB; @@ -170,8 +150,8 @@ void btSliderConstraint::calculateTransforms(const btTransform& transA,const btT } m_realPivotAInW = m_calculatedTransformA.getOrigin(); m_realPivotBInW = m_calculatedTransformB.getOrigin(); - m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X - if(m_useLinearReferenceFrameA || m_useSolveConstraintObsolete) + m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X + if (m_useLinearReferenceFrameA || m_useSolveConstraintObsolete) { m_delta = m_realPivotBInW - m_realPivotAInW; } @@ -180,30 +160,28 @@ void btSliderConstraint::calculateTransforms(const btTransform& transA,const btT m_delta = m_realPivotAInW - m_realPivotBInW; } m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis; - btVector3 normalWorld; - int i; - //linear part - for(i = 0; i < 3; i++) - { + btVector3 normalWorld; + int i; + //linear part + for (i = 0; i < 3; i++) + { normalWorld = m_calculatedTransformA.getBasis().getColumn(i); m_depth[i] = m_delta.dot(normalWorld); - } + } } - - void btSliderConstraint::testLinLimits(void) { m_solveLinLim = false; m_linPos = m_depth[0]; - if(m_lowerLinLimit <= m_upperLinLimit) + if (m_lowerLinLimit <= m_upperLinLimit) { - if(m_depth[0] > m_upperLinLimit) + if (m_depth[0] > m_upperLinLimit) { m_depth[0] -= m_upperLinLimit; m_solveLinLim = true; } - else if(m_depth[0] < m_lowerLinLimit) + else if (m_depth[0] < m_lowerLinLimit) { m_depth[0] -= m_lowerLinLimit; m_solveLinLim = true; @@ -219,27 +197,25 @@ void btSliderConstraint::testLinLimits(void) } } - - void btSliderConstraint::testAngLimits(void) { m_angDepth = btScalar(0.); m_solveAngLim = false; - if(m_lowerAngLimit <= m_upperAngLimit) + if (m_lowerAngLimit <= m_upperAngLimit) { const btVector3 axisA0 = m_calculatedTransformA.getBasis().getColumn(1); const btVector3 axisA1 = m_calculatedTransformA.getBasis().getColumn(2); const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1); -// btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0)); - btScalar rot = btAtan2(axisB0.dot(axisA1), axisB0.dot(axisA0)); + // btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0)); + btScalar rot = btAtan2(axisB0.dot(axisA1), axisB0.dot(axisA0)); rot = btAdjustAngleToLimits(rot, m_lowerAngLimit, m_upperAngLimit); m_angPos = rot; - if(rot < m_lowerAngLimit) + if (rot < m_lowerAngLimit) { m_angDepth = rot - m_lowerAngLimit; m_solveAngLim = true; - } - else if(rot > m_upperAngLimit) + } + else if (rot > m_upperAngLimit) { m_angDepth = rot - m_upperAngLimit; m_solveAngLim = true; @@ -255,8 +231,6 @@ btVector3 btSliderConstraint::getAncorInA(void) return ancorInA; } - - btVector3 btSliderConstraint::getAncorInB(void) { btVector3 ancorInB; @@ -264,17 +238,16 @@ btVector3 btSliderConstraint::getAncorInB(void) return ancorInB; } - -void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB, const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass ) +void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, btScalar rbAinvMass, btScalar rbBinvMass) { const btTransform& trA = getCalculatedTransformA(); const btTransform& trB = getCalculatedTransformB(); - + btAssert(!m_useSolveConstraintObsolete); int i, s = info->rowskip; - + btScalar signFact = m_useLinearReferenceFrameA ? btScalar(1.0f) : btScalar(-1.0f); - + // difference between frames in WCS btVector3 ofs = trB.getOrigin() - trA.getOrigin(); // now get weight factors depending on masses @@ -283,11 +256,11 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra 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); } @@ -295,17 +268,17 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra btVector3 ax1, p, q; btVector3 ax1A = trA.getBasis().getColumn(0); btVector3 ax1B = trB.getBasis().getColumn(0); - if(m_useOffsetForConstraintFrame) + if (m_useOffsetForConstraintFrame) { // get the desired direction of slider axis // as weighted sum of X-orthos of frameA and frameB in WCS ax1 = ax1A * factA + ax1B * factB; ax1.normalize(); // construct two orthos to slider axis - btPlaneSpace1 (ax1, p, q); + btPlaneSpace1(ax1, p, q); } else - { // old way - use frameA + { // old way - use frameA ax1 = trA.getBasis().getColumn(0); // get 2 orthos to slider axis (Y, Z) p = trA.getBasis().getColumn(1); @@ -322,16 +295,16 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra info->m_J1angularAxis[0] = p[0]; info->m_J1angularAxis[1] = p[1]; info->m_J1angularAxis[2] = p[2]; - info->m_J1angularAxis[s+0] = q[0]; - info->m_J1angularAxis[s+1] = q[1]; - info->m_J1angularAxis[s+2] = q[2]; + info->m_J1angularAxis[s + 0] = q[0]; + info->m_J1angularAxis[s + 1] = q[1]; + info->m_J1angularAxis[s + 2] = q[2]; info->m_J2angularAxis[0] = -p[0]; info->m_J2angularAxis[1] = -p[1]; info->m_J2angularAxis[2] = -p[2]; - info->m_J2angularAxis[s+0] = -q[0]; - info->m_J2angularAxis[s+1] = -q[1]; - info->m_J2angularAxis[s+2] = -q[2]; + info->m_J2angularAxis[s + 0] = -q[0]; + info->m_J2angularAxis[s + 1] = -q[1]; + info->m_J2angularAxis[s + 2] = -q[2]; // compute the right hand side of the constraint equation. set relative // body velocities along p and q to bring the slider back into alignment. // if ax1A,ax1B are the unit length slider axes as computed from bodyA and @@ -347,26 +320,25 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra // 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. -// btScalar k = info->fps * info->erp * getSoftnessOrthoAng(); + // btScalar k = info->fps * info->erp * getSoftnessOrthoAng(); btScalar currERP = (m_flags & BT_SLIDER_FLAGS_ERP_ORTANG) ? m_softnessOrthoAng : m_softnessOrthoAng * info->erp; btScalar k = info->fps * currERP; btVector3 u = ax1A.cross(ax1B); info->m_constraintError[0] = k * u.dot(p); info->m_constraintError[s] = k * u.dot(q); - if(m_flags & BT_SLIDER_FLAGS_CFM_ORTANG) + if (m_flags & BT_SLIDER_FLAGS_CFM_ORTANG) { info->cfm[0] = m_cfmOrthoAng; info->cfm[s] = m_cfmOrthoAng; } - int nrow = 1; // last filled row + int nrow = 1; // last filled row int srow; btScalar limit_err; int limit; - int powered; - // next two rows. + // next two rows. // we want: velA + wA x relA == velB + wB x relB ... but this would // result in three equations, so we project along two orthos to the slider axis @@ -376,8 +348,8 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra int s2 = nrow * s; nrow++; int s3 = nrow * s; - btVector3 tmpA(0,0,0), tmpB(0,0,0), relA(0,0,0), relB(0,0,0), c(0,0,0); - if(m_useOffsetForConstraintFrame) + btVector3 tmpA(0, 0, 0), tmpB(0, 0, 0), relA(0, 0, 0), relB(0, 0, 0), c(0, 0, 0); + if (m_useOffsetForConstraintFrame) { // get vector from bodyB to frameB in WCS relB = trB.getOrigin() - bodyB_trans.getOrigin(); @@ -399,7 +371,7 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra // now choose average ortho to slider axis p = orthoB * factA + orthoA * factB; btScalar len2 = p.length2(); - if(len2 > SIMD_EPSILON) + if (len2 > SIMD_EPSILON) { p /= btSqrt(len2); } @@ -412,38 +384,38 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra // fill two rows tmpA = relA.cross(p); tmpB = relB.cross(p); - 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]; tmpA = relA.cross(q); tmpB = relB.cross(q); - if(hasStaticBody && getSolveAngLimit()) - { // to make constraint between static and dynamic objects more rigid + if (hasStaticBody && getSolveAngLimit()) + { // 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[s3+i] = tmpA[i]; - for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = -tmpB[i]; - for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i]; - for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i]; - for (i=0; i<3; i++) info->m_J2linearAxis[s2+i] = -p[i]; - for (i=0; i<3; i++) info->m_J2linearAxis[s3+i] = -q[i]; + for (i = 0; i < 3; i++) info->m_J1angularAxis[s3 + i] = tmpA[i]; + for (i = 0; i < 3; i++) info->m_J2angularAxis[s3 + i] = -tmpB[i]; + for (i = 0; i < 3; i++) info->m_J1linearAxis[s2 + i] = p[i]; + for (i = 0; i < 3; i++) info->m_J1linearAxis[s3 + i] = q[i]; + for (i = 0; i < 3; i++) info->m_J2linearAxis[s2 + i] = -p[i]; + for (i = 0; i < 3; i++) info->m_J2linearAxis[s3 + i] = -q[i]; } else - { // old way - maybe incorrect if bodies are not on the slider axis + { // old way - maybe incorrect if bodies are not on the slider axis // see discussion "Bug in slider constraint" http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=9&t=4024&start=0 c = bodyB_trans.getOrigin() - bodyA_trans.getOrigin(); btVector3 tmp = c.cross(p); - for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = factA*tmp[i]; - for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = factB*tmp[i]; + for (i = 0; i < 3; i++) info->m_J1angularAxis[s2 + i] = factA * tmp[i]; + for (i = 0; i < 3; i++) info->m_J2angularAxis[s2 + i] = factB * tmp[i]; tmp = c.cross(q); - for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = factA*tmp[i]; - for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = factB*tmp[i]; + for (i = 0; i < 3; i++) info->m_J1angularAxis[s3 + i] = factA * tmp[i]; + for (i = 0; i < 3; i++) info->m_J2angularAxis[s3 + i] = factB * tmp[i]; - for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i]; - for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i]; - for (i=0; i<3; i++) info->m_J2linearAxis[s2+i] = -p[i]; - for (i=0; i<3; i++) info->m_J2linearAxis[s3+i] = -q[i]; + for (i = 0; i < 3; i++) info->m_J1linearAxis[s2 + i] = p[i]; + for (i = 0; i < 3; i++) info->m_J1linearAxis[s3 + i] = q[i]; + for (i = 0; i < 3; i++) info->m_J2linearAxis[s2 + i] = -p[i]; + for (i = 0; i < 3; i++) info->m_J2linearAxis[s3 + i] = -q[i]; } // compute two elements of right hand side @@ -455,37 +427,32 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra info->m_constraintError[s2] = rhs; rhs = k * q.dot(ofs); info->m_constraintError[s3] = rhs; - if(m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN) + if (m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN) { info->cfm[s2] = m_cfmOrthoLin; info->cfm[s3] = m_cfmOrthoLin; } - // check linear limits limit_err = btScalar(0.0); limit = 0; - if(getSolveLinLimit()) + if (getSolveLinLimit()) { - limit_err = getLinDepth() * signFact; + limit_err = getLinDepth() * signFact; limit = (limit_err > btScalar(0.0)) ? 2 : 1; } - powered = 0; - if(getPoweredLinMotor()) - { - powered = 1; - } + bool powered = getPoweredLinMotor(); // if the slider has joint limits or motor, add in the extra row - if (limit || powered) + if (limit || powered) { nrow++; srow = nrow * info->rowskip; - info->m_J1linearAxis[srow+0] = ax1[0]; - info->m_J1linearAxis[srow+1] = ax1[1]; - info->m_J1linearAxis[srow+2] = ax1[2]; - info->m_J2linearAxis[srow+0] = -ax1[0]; - info->m_J2linearAxis[srow+1] = -ax1[1]; - info->m_J2linearAxis[srow+2] = -ax1[2]; + info->m_J1linearAxis[srow + 0] = ax1[0]; + info->m_J1linearAxis[srow + 1] = ax1[1]; + info->m_J1linearAxis[srow + 2] = ax1[2]; + info->m_J2linearAxis[srow + 0] = -ax1[0]; + info->m_J2linearAxis[srow + 1] = -ax1[1]; + info->m_J2linearAxis[srow + 2] = -ax1[2]; // linear torque decoupling step: // // we have to be careful that the linear constraint forces (+/- ax1) applied to the two bodies @@ -493,46 +460,46 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra // constraint force is applied at must lie along the same ax1 axis. // a torque couple will result in limited slider-jointed free // bodies from gaining angular momentum. - if(m_useOffsetForConstraintFrame) + if (m_useOffsetForConstraintFrame) { // this is needed only when bodyA and bodyB are both dynamic. - if(!hasStaticBody) + if (!hasStaticBody) { tmpA = relA.cross(ax1); tmpB = relB.cross(ax1); - info->m_J1angularAxis[srow+0] = tmpA[0]; - info->m_J1angularAxis[srow+1] = tmpA[1]; - info->m_J1angularAxis[srow+2] = tmpA[2]; - info->m_J2angularAxis[srow+0] = -tmpB[0]; - info->m_J2angularAxis[srow+1] = -tmpB[1]; - info->m_J2angularAxis[srow+2] = -tmpB[2]; + info->m_J1angularAxis[srow + 0] = tmpA[0]; + info->m_J1angularAxis[srow + 1] = tmpA[1]; + info->m_J1angularAxis[srow + 2] = tmpA[2]; + info->m_J2angularAxis[srow + 0] = -tmpB[0]; + info->m_J2angularAxis[srow + 1] = -tmpB[1]; + info->m_J2angularAxis[srow + 2] = -tmpB[2]; } } else - { // The old way. May be incorrect if bodies are not on the slider axis - btVector3 ltd; // Linear Torque Decoupling vector (a torque) + { // The old way. May be incorrect if bodies are not on the slider axis + btVector3 ltd; // Linear Torque Decoupling vector (a torque) ltd = c.cross(ax1); - info->m_J1angularAxis[srow+0] = factA*ltd[0]; - info->m_J1angularAxis[srow+1] = factA*ltd[1]; - info->m_J1angularAxis[srow+2] = factA*ltd[2]; - info->m_J2angularAxis[srow+0] = factB*ltd[0]; - info->m_J2angularAxis[srow+1] = factB*ltd[1]; - info->m_J2angularAxis[srow+2] = factB*ltd[2]; + info->m_J1angularAxis[srow + 0] = factA * ltd[0]; + info->m_J1angularAxis[srow + 1] = factA * ltd[1]; + info->m_J1angularAxis[srow + 2] = factA * ltd[2]; + info->m_J2angularAxis[srow + 0] = factB * ltd[0]; + info->m_J2angularAxis[srow + 1] = factB * ltd[1]; + info->m_J2angularAxis[srow + 2] = factB * ltd[2]; } // right-hand part btScalar lostop = getLowerLinLimit(); btScalar histop = getUpperLinLimit(); - if(limit && (lostop == histop)) + if (limit && (lostop == histop)) { // the joint motor is ineffective - powered = 0; + powered = false; } info->m_constraintError[srow] = 0.; info->m_lowerLimit[srow] = 0.; info->m_upperLimit[srow] = 0.; currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMLIN) ? m_softnessLimLin : info->erp; - if(powered) + if (powered) { - if(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN) + if (m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN) { info->cfm[srow] = m_cfmDirLin; } @@ -542,41 +509,41 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra info->m_lowerLimit[srow] += -getMaxLinMotorForce() / info->fps; info->m_upperLimit[srow] += getMaxLinMotorForce() / info->fps; } - if(limit) + if (limit) { k = info->fps * currERP; info->m_constraintError[srow] += k * limit_err; - if(m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN) + if (m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN) { info->cfm[srow] = m_cfmLimLin; } - if(lostop == histop) - { // limited low and high simultaneously + 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] = -SIMD_INFINITY; info->m_upperLimit[srow] = 0; } - else - { // high limit + else + { // high limit info->m_lowerLimit[srow] = 0; info->m_upperLimit[srow] = SIMD_INFINITY; } // bounce (we'll use slider parameter abs(1.0 - m_dampingLimLin) for that) btScalar bounce = btFabs(btScalar(1.0) - getDampingLimLin()); - if(bounce > btScalar(0.0)) + if (bounce > btScalar(0.0)) { btScalar vel = linVelA.dot(ax1); vel -= linVelB.dot(ax1); vel *= signFact; // 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]) @@ -586,11 +553,11 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra } } 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; } @@ -598,44 +565,40 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra } } info->m_constraintError[srow] *= getSoftnessLimLin(); - } // if(limit) - } // if linear limit + } // if(limit) + } // if linear limit // check angular limits limit_err = btScalar(0.0); limit = 0; - if(getSolveAngLimit()) + if (getSolveAngLimit()) { limit_err = getAngDepth(); limit = (limit_err > btScalar(0.0)) ? 1 : 2; } // if the slider has joint limits, add in the extra row - powered = 0; - if(getPoweredAngMotor()) - { - powered = 1; - } - if(limit || powered) + powered = getPoweredAngMotor(); + 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 = getLowerAngLimit(); btScalar histop = getUpperAngLimit(); - if(limit && (lostop == histop)) + if (limit && (lostop == histop)) { // the joint motor is ineffective - powered = 0; + powered = false; } currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMANG) ? m_softnessLimAng : info->erp; - if(powered) + if (powered) { - if(m_flags & BT_SLIDER_FLAGS_CFM_DIRANG) + if (m_flags & BT_SLIDER_FLAGS_CFM_DIRANG) { info->cfm[srow] = m_cfmDirAng; } @@ -644,55 +607,55 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra info->m_lowerLimit[srow] = -getMaxAngMotorForce() / info->fps; info->m_upperLimit[srow] = getMaxAngMotorForce() / info->fps; } - if(limit) + if (limit) { k = info->fps * currERP; info->m_constraintError[srow] += k * limit_err; - if(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG) + if (m_flags & BT_SLIDER_FLAGS_CFM_LIMANG) { info->cfm[srow] = m_cfmLimAng; } - 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) btScalar bounce = btFabs(btScalar(1.0) - getDampingLimAng()); - if(bounce > btScalar(0.0)) + if (bounce > btScalar(0.0)) { btScalar vel = m_rbA.getAngularVelocity().dot(ax1); vel -= m_rbB.getAngularVelocity().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; } @@ -700,165 +663,161 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra } } info->m_constraintError[srow] *= getSoftnessLimAng(); - } // 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 btSliderConstraint::setParam(int num, btScalar value, int axis) { - switch(num) + switch (num) { - case BT_CONSTRAINT_STOP_ERP : - if(axis < 1) - { - m_softnessLimLin = value; - m_flags |= BT_SLIDER_FLAGS_ERP_LIMLIN; - } - else if(axis < 3) - { - m_softnessOrthoLin = value; - m_flags |= BT_SLIDER_FLAGS_ERP_ORTLIN; - } - else if(axis == 3) - { - m_softnessLimAng = value; - m_flags |= BT_SLIDER_FLAGS_ERP_LIMANG; - } - else if(axis < 6) - { - m_softnessOrthoAng = value; - m_flags |= BT_SLIDER_FLAGS_ERP_ORTANG; - } - else - { - btAssertConstrParams(0); - } - break; - case BT_CONSTRAINT_CFM : - if(axis < 1) - { - m_cfmDirLin = value; - m_flags |= BT_SLIDER_FLAGS_CFM_DIRLIN; - } - else if(axis == 3) - { - m_cfmDirAng = value; - m_flags |= BT_SLIDER_FLAGS_CFM_DIRANG; - } - else - { - btAssertConstrParams(0); - } - break; - case BT_CONSTRAINT_STOP_CFM : - if(axis < 1) - { - m_cfmLimLin = value; - m_flags |= BT_SLIDER_FLAGS_CFM_LIMLIN; - } - else if(axis < 3) - { - m_cfmOrthoLin = value; - m_flags |= BT_SLIDER_FLAGS_CFM_ORTLIN; - } - else if(axis == 3) - { - m_cfmLimAng = value; - m_flags |= BT_SLIDER_FLAGS_CFM_LIMANG; - } - else if(axis < 6) - { - m_cfmOrthoAng = value; - m_flags |= BT_SLIDER_FLAGS_CFM_ORTANG; - } - else - { - btAssertConstrParams(0); - } - break; + case BT_CONSTRAINT_STOP_ERP: + if (axis < 1) + { + m_softnessLimLin = value; + m_flags |= BT_SLIDER_FLAGS_ERP_LIMLIN; + } + else if (axis < 3) + { + m_softnessOrthoLin = value; + m_flags |= BT_SLIDER_FLAGS_ERP_ORTLIN; + } + else if (axis == 3) + { + m_softnessLimAng = value; + m_flags |= BT_SLIDER_FLAGS_ERP_LIMANG; + } + else if (axis < 6) + { + m_softnessOrthoAng = value; + m_flags |= BT_SLIDER_FLAGS_ERP_ORTANG; + } + else + { + btAssertConstrParams(0); + } + break; + case BT_CONSTRAINT_CFM: + if (axis < 1) + { + m_cfmDirLin = value; + m_flags |= BT_SLIDER_FLAGS_CFM_DIRLIN; + } + else if (axis == 3) + { + m_cfmDirAng = value; + m_flags |= BT_SLIDER_FLAGS_CFM_DIRANG; + } + else + { + btAssertConstrParams(0); + } + break; + case BT_CONSTRAINT_STOP_CFM: + if (axis < 1) + { + m_cfmLimLin = value; + m_flags |= BT_SLIDER_FLAGS_CFM_LIMLIN; + } + else if (axis < 3) + { + m_cfmOrthoLin = value; + m_flags |= BT_SLIDER_FLAGS_CFM_ORTLIN; + } + else if (axis == 3) + { + m_cfmLimAng = value; + m_flags |= BT_SLIDER_FLAGS_CFM_LIMANG; + } + else if (axis < 6) + { + m_cfmOrthoAng = value; + m_flags |= BT_SLIDER_FLAGS_CFM_ORTANG; + } + else + { + btAssertConstrParams(0); + } + break; } } ///return the local value of parameter -btScalar btSliderConstraint::getParam(int num, int axis) const +btScalar btSliderConstraint::getParam(int num, int axis) const { btScalar retVal(SIMD_INFINITY); - switch(num) + switch (num) { - case BT_CONSTRAINT_STOP_ERP : - if(axis < 1) - { - btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_LIMLIN); - retVal = m_softnessLimLin; - } - else if(axis < 3) - { - btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_ORTLIN); - retVal = m_softnessOrthoLin; - } - else if(axis == 3) - { - btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_LIMANG); - retVal = m_softnessLimAng; - } - else if(axis < 6) - { - btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_ORTANG); - retVal = m_softnessOrthoAng; - } - else - { - btAssertConstrParams(0); - } - break; - case BT_CONSTRAINT_CFM : - if(axis < 1) - { - btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN); - retVal = m_cfmDirLin; - } - else if(axis == 3) - { - btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_DIRANG); - retVal = m_cfmDirAng; - } - else - { - btAssertConstrParams(0); - } - break; - case BT_CONSTRAINT_STOP_CFM : - if(axis < 1) - { - btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN); - retVal = m_cfmLimLin; - } - else if(axis < 3) - { - btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN); - retVal = m_cfmOrthoLin; - } - else if(axis == 3) - { - btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG); - retVal = m_cfmLimAng; - } - else if(axis < 6) - { - btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_ORTANG); - retVal = m_cfmOrthoAng; - } - else - { - btAssertConstrParams(0); - } - break; + case BT_CONSTRAINT_STOP_ERP: + if (axis < 1) + { + btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_LIMLIN); + retVal = m_softnessLimLin; + } + else if (axis < 3) + { + btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_ORTLIN); + retVal = m_softnessOrthoLin; + } + else if (axis == 3) + { + btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_LIMANG); + retVal = m_softnessLimAng; + } + else if (axis < 6) + { + btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_ORTANG); + retVal = m_softnessOrthoAng; + } + else + { + btAssertConstrParams(0); + } + break; + case BT_CONSTRAINT_CFM: + if (axis < 1) + { + btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN); + retVal = m_cfmDirLin; + } + else if (axis == 3) + { + btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_DIRANG); + retVal = m_cfmDirAng; + } + else + { + btAssertConstrParams(0); + } + break; + case BT_CONSTRAINT_STOP_CFM: + if (axis < 1) + { + btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN); + retVal = m_cfmLimLin; + } + else if (axis < 3) + { + btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN); + retVal = m_cfmOrthoLin; + } + else if (axis == 3) + { + btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG); + retVal = m_cfmLimAng; + } + else if (axis < 6) + { + btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_ORTANG); + retVal = m_cfmOrthoAng; + } + else + { + btAssertConstrParams(0); + } + break; } return retVal; } - - - |