diff options
Diffstat (limited to 'extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp')
| -rw-r--r-- | extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp | 611 |
1 files changed, 292 insertions, 319 deletions
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp index 09b7388b63e..ac046aa6eaf 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp @@ -15,49 +15,38 @@ subject to the following restrictions: Written by: Marcus Hennix */ - #include "btConeTwistConstraint.h" #include "BulletDynamics/Dynamics/btRigidBody.h" #include "LinearMath/btTransformUtil.h" #include "LinearMath/btMinMax.h" +#include <cmath> #include <new> - - //#define CONETWIST_USE_OBSOLETE_SOLVER true #define CONETWIST_USE_OBSOLETE_SOLVER false #define CONETWIST_DEF_FIX_THRESH btScalar(.05f) - SIMD_FORCE_INLINE btScalar computeAngularImpulseDenominator(const btVector3& axis, const btMatrix3x3& invInertiaWorld) { btVector3 vec = axis * invInertiaWorld; return axis.dot(vec); } - - - -btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB, - const btTransform& rbAFrame,const btTransform& rbBFrame) - :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame), - m_angularOnly(false), - m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER) +btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA, btRigidBody& rbB, + const btTransform& rbAFrame, const btTransform& rbBFrame) + : btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA, rbB), m_rbAFrame(rbAFrame), m_rbBFrame(rbBFrame), m_angularOnly(false), m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER) { init(); } -btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame) - :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE,rbA),m_rbAFrame(rbAFrame), - m_angularOnly(false), - m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER) +btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA, const btTransform& rbAFrame) + : btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA), m_rbAFrame(rbAFrame), m_angularOnly(false), m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER) { m_rbBFrame = m_rbAFrame; m_rbBFrame.setOrigin(btVector3(0., 0., 0.)); - init(); + init(); } - void btConeTwistConstraint::init() { m_angularOnly = false; @@ -75,30 +64,29 @@ void btConeTwistConstraint::init() m_angCFM = btScalar(0.f); } - -void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info) +void btConeTwistConstraint::getInfo1(btConstraintInfo1* info) { if (m_useSolveConstraintObsolete) { info->m_numConstraintRows = 0; info->nub = 0; - } + } else { info->m_numConstraintRows = 3; info->nub = 3; - calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld()); - if(m_solveSwingLimit) + calcAngleInfo2(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform(), m_rbA.getInvInertiaTensorWorld(), m_rbB.getInvInertiaTensorWorld()); + if (m_solveSwingLimit) { info->m_numConstraintRows++; info->nub--; - if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh)) + if ((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh)) { info->m_numConstraintRows++; info->nub--; } } - if(m_solveTwistLimit) + if (m_solveTwistLimit) { info->m_numConstraintRows++; info->nub--; @@ -106,90 +94,88 @@ void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info) } } -void btConeTwistConstraint::getInfo1NonVirtual (btConstraintInfo1* info) +void btConeTwistConstraint::getInfo1NonVirtual(btConstraintInfo1* info) { //always reserve 6 rows: object transform is not available on SPU info->m_numConstraintRows = 6; info->nub = 0; - } - -void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info) +void btConeTwistConstraint::getInfo2(btConstraintInfo2* info) { - getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld()); + getInfo2NonVirtual(info, m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform(), m_rbA.getInvInertiaTensorWorld(), m_rbB.getInvInertiaTensorWorld()); } -void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB) +void btConeTwistConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA, const btMatrix3x3& invInertiaWorldB) { - calcAngleInfo2(transA,transB,invInertiaWorldA,invInertiaWorldB); - + calcAngleInfo2(transA, transB, invInertiaWorldA, invInertiaWorldB); + btAssert(!m_useSolveConstraintObsolete); - // set jacobian - info->m_J1linearAxis[0] = 1; - info->m_J1linearAxis[info->rowskip+1] = 1; - info->m_J1linearAxis[2*info->rowskip+2] = 1; + // set jacobian + info->m_J1linearAxis[0] = 1; + info->m_J1linearAxis[info->rowskip + 1] = 1; + info->m_J1linearAxis[2 * info->rowskip + 2] = 1; btVector3 a1 = transA.getBasis() * m_rbAFrame.getOrigin(); { btVector3* angular0 = (btVector3*)(info->m_J1angularAxis); - btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip); - btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip); + btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + info->rowskip); + btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * info->rowskip); btVector3 a1neg = -a1; - a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); + a1neg.getSkewSymmetricMatrix(angular0, angular1, angular2); } - info->m_J2linearAxis[0] = -1; - info->m_J2linearAxis[info->rowskip+1] = -1; - info->m_J2linearAxis[2*info->rowskip+2] = -1; + info->m_J2linearAxis[0] = -1; + info->m_J2linearAxis[info->rowskip + 1] = -1; + info->m_J2linearAxis[2 * info->rowskip + 2] = -1; btVector3 a2 = transB.getBasis() * m_rbBFrame.getOrigin(); { btVector3* angular0 = (btVector3*)(info->m_J2angularAxis); - btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip); - btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip); - a2.getSkewSymmetricMatrix(angular0,angular1,angular2); + btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + info->rowskip); + btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * info->rowskip); + a2.getSkewSymmetricMatrix(angular0, angular1, angular2); } - // set right hand side + // set right hand side btScalar linERP = (m_flags & BT_CONETWIST_FLAGS_LIN_ERP) ? m_linERP : info->erp; - btScalar k = info->fps * linERP; - int j; - for (j=0; j<3; j++) - { - info->m_constraintError[j*info->rowskip] = k * (a2[j] + transB.getOrigin()[j] - a1[j] - transA.getOrigin()[j]); - info->m_lowerLimit[j*info->rowskip] = -SIMD_INFINITY; - info->m_upperLimit[j*info->rowskip] = SIMD_INFINITY; - if(m_flags & BT_CONETWIST_FLAGS_LIN_CFM) + btScalar k = info->fps * linERP; + int j; + for (j = 0; j < 3; j++) + { + info->m_constraintError[j * info->rowskip] = k * (a2[j] + transB.getOrigin()[j] - a1[j] - transA.getOrigin()[j]); + info->m_lowerLimit[j * info->rowskip] = -SIMD_INFINITY; + info->m_upperLimit[j * info->rowskip] = SIMD_INFINITY; + if (m_flags & BT_CONETWIST_FLAGS_LIN_CFM) { - info->cfm[j*info->rowskip] = m_linCFM; + info->cfm[j * info->rowskip] = m_linCFM; } - } + } int row = 3; - int srow = row * info->rowskip; + int srow = row * info->rowskip; btVector3 ax1; // angular limits - if(m_solveSwingLimit) + if (m_solveSwingLimit) { - btScalar *J1 = info->m_J1angularAxis; - btScalar *J2 = info->m_J2angularAxis; - if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh)) + btScalar* J1 = info->m_J1angularAxis; + btScalar* J2 = info->m_J2angularAxis; + if ((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh)) { - btTransform trA = transA*m_rbAFrame; + btTransform trA = transA * m_rbAFrame; btVector3 p = trA.getBasis().getColumn(1); btVector3 q = trA.getBasis().getColumn(2); int srow1 = srow + info->rowskip; - J1[srow+0] = p[0]; - J1[srow+1] = p[1]; - J1[srow+2] = p[2]; - J1[srow1+0] = q[0]; - J1[srow1+1] = q[1]; - J1[srow1+2] = q[2]; - J2[srow+0] = -p[0]; - J2[srow+1] = -p[1]; - J2[srow+2] = -p[2]; - J2[srow1+0] = -q[0]; - J2[srow1+1] = -q[1]; - J2[srow1+2] = -q[2]; + J1[srow + 0] = p[0]; + J1[srow + 1] = p[1]; + J1[srow + 2] = p[2]; + J1[srow1 + 0] = q[0]; + J1[srow1 + 1] = q[1]; + J1[srow1 + 2] = q[2]; + J2[srow + 0] = -p[0]; + J2[srow + 1] = -p[1]; + J2[srow + 2] = -p[2]; + J2[srow1 + 0] = -q[0]; + J2[srow1 + 1] = -q[1]; + J2[srow1 + 2] = -q[2]; btScalar fact = info->fps * m_relaxationFactor; - info->m_constraintError[srow] = fact * m_swingAxis.dot(p); - info->m_constraintError[srow1] = fact * m_swingAxis.dot(q); + info->m_constraintError[srow] = fact * m_swingAxis.dot(p); + info->m_constraintError[srow1] = fact * m_swingAxis.dot(q); info->m_lowerLimit[srow] = -SIMD_INFINITY; info->m_upperLimit[srow] = SIMD_INFINITY; info->m_lowerLimit[srow1] = -SIMD_INFINITY; @@ -199,16 +185,16 @@ void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const bt else { ax1 = m_swingAxis * m_relaxationFactor * m_relaxationFactor; - J1[srow+0] = ax1[0]; - J1[srow+1] = ax1[1]; - J1[srow+2] = ax1[2]; - J2[srow+0] = -ax1[0]; - J2[srow+1] = -ax1[1]; - J2[srow+2] = -ax1[2]; + J1[srow + 0] = ax1[0]; + J1[srow + 1] = ax1[1]; + J1[srow + 2] = ax1[2]; + J2[srow + 0] = -ax1[0]; + J2[srow + 1] = -ax1[1]; + J2[srow + 2] = -ax1[2]; btScalar k = info->fps * m_biasFactor; info->m_constraintError[srow] = k * m_swingCorrection; - if(m_flags & BT_CONETWIST_FLAGS_ANG_CFM) + if (m_flags & BT_CONETWIST_FLAGS_ANG_CFM) { info->cfm[srow] = m_angCFM; } @@ -218,36 +204,35 @@ void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const bt srow += info->rowskip; } } - if(m_solveTwistLimit) + if (m_solveTwistLimit) { ax1 = m_twistAxis * m_relaxationFactor * m_relaxationFactor; - btScalar *J1 = info->m_J1angularAxis; - btScalar *J2 = info->m_J2angularAxis; - J1[srow+0] = ax1[0]; - J1[srow+1] = ax1[1]; - J1[srow+2] = ax1[2]; - J2[srow+0] = -ax1[0]; - J2[srow+1] = -ax1[1]; - J2[srow+2] = -ax1[2]; + btScalar* J1 = info->m_J1angularAxis; + btScalar* J2 = info->m_J2angularAxis; + J1[srow + 0] = ax1[0]; + J1[srow + 1] = ax1[1]; + J1[srow + 2] = ax1[2]; + J2[srow + 0] = -ax1[0]; + J2[srow + 1] = -ax1[1]; + J2[srow + 2] = -ax1[2]; btScalar k = info->fps * m_biasFactor; info->m_constraintError[srow] = k * m_twistCorrection; - if(m_flags & BT_CONETWIST_FLAGS_ANG_CFM) + if (m_flags & BT_CONETWIST_FLAGS_ANG_CFM) { info->cfm[srow] = m_angCFM; } - if(m_twistSpan > 0.0f) + if (m_twistSpan > 0.0f) { - - if(m_twistCorrection > 0.0f) + if (m_twistCorrection > 0.0f) { info->m_lowerLimit[srow] = 0; info->m_upperLimit[srow] = SIMD_INFINITY; - } + } else { info->m_lowerLimit[srow] = -SIMD_INFINITY; info->m_upperLimit[srow] = 0; - } + } } else { @@ -257,22 +242,20 @@ void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const bt srow += info->rowskip; } } - - -void btConeTwistConstraint::buildJacobian() +void btConeTwistConstraint::buildJacobian() { if (m_useSolveConstraintObsolete) { m_appliedImpulse = btScalar(0.); m_accTwistLimitImpulse = btScalar(0.); m_accSwingLimitImpulse = btScalar(0.); - m_accMotorImpulse = btVector3(0.,0.,0.); + m_accMotorImpulse = btVector3(0., 0., 0.); 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]; @@ -282,71 +265,68 @@ void btConeTwistConstraint::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()); } } - calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld()); + calcAngleInfo2(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform(), m_rbA.getInvInertiaTensorWorld(), m_rbB.getInvInertiaTensorWorld()); } } - - -void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) +void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA, btSolverBody& bodyB, btScalar timeStep) { - #ifndef __SPU__ +#ifndef __SPU__ if (m_useSolveConstraintObsolete) { - 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(); btScalar tau = btScalar(0.3); //linear part if (!m_angularOnly) { - btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); + btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); btVector3 vel1; - bodyA.internalGetVelocityInLocalPointObsolete(rel_pos1,vel1); + bodyA.internalGetVelocityInLocalPointObsolete(rel_pos1, vel1); btVector3 vel2; - bodyB.internalGetVelocityInLocalPointObsolete(rel_pos2,vel2); + bodyB.internalGetVelocityInLocalPointObsolete(rel_pos2, vel2); btVector3 vel = vel1 - vel2; - for (int i=0;i<3;i++) - { + for (int i = 0; i < 3; i++) + { const btVector3& normal = m_jac[i].m_linearJointAxis; btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal(); btScalar rel_vel; rel_vel = normal.dot(vel); //positional error (zeroth order error) - btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal - btScalar impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv; + btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal + btScalar impulse = depth * tau / timeStep * jacDiagABInv - rel_vel * jacDiagABInv; m_appliedImpulse += impulse; - + btVector3 ftorqueAxis1 = rel_pos1.cross(normal); btVector3 ftorqueAxis2 = rel_pos2.cross(normal); - bodyA.internalApplyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse); - bodyB.internalApplyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse); - + bodyA.internalApplyImpulse(normal * m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld() * ftorqueAxis1, impulse); + bodyB.internalApplyImpulse(normal * m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld() * ftorqueAxis2, -impulse); } } @@ -356,13 +336,17 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver // compute current and predicted transforms btTransform trACur = m_rbA.getCenterOfMassTransform(); btTransform trBCur = m_rbB.getCenterOfMassTransform(); - btVector3 omegaA; bodyA.internalGetAngularVelocity(omegaA); - btVector3 omegaB; bodyB.internalGetAngularVelocity(omegaB); - btTransform trAPred; trAPred.setIdentity(); - btVector3 zerovec(0,0,0); + btVector3 omegaA; + bodyA.internalGetAngularVelocity(omegaA); + btVector3 omegaB; + bodyB.internalGetAngularVelocity(omegaB); + btTransform trAPred; + trAPred.setIdentity(); + btVector3 zerovec(0, 0, 0); btTransformUtil::integrateTransform( trACur, zerovec, omegaA, timeStep, trAPred); - btTransform trBPred; trBPred.setIdentity(); + btTransform trBPred; + trBPred.setIdentity(); btTransformUtil::integrateTransform( trBCur, zerovec, omegaB, timeStep, trBPred); @@ -374,7 +358,7 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver // compute desired omegas in world btVector3 omegaADes, omegaBDes; - + btTransformUtil::calculateVelocity(trACur, trADes, timeStep, zerovec, omegaADes); btTransformUtil::calculateVelocity(trBCur, trBDes, timeStep, zerovec, omegaBDes); @@ -415,10 +399,10 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver { btScalar fMaxImpulse = m_maxMotorImpulse; if (m_bNormalizedMotorStrength) - fMaxImpulse = fMaxImpulse/kAxisAInv; + fMaxImpulse = fMaxImpulse / kAxisAInv; btVector3 newUnclampedAccImpulse = m_accMotorImpulse + impulse; - btScalar newUnclampedMag = newUnclampedAccImpulse.length(); + btScalar newUnclampedMag = newUnclampedAccImpulse.length(); if (newUnclampedMag > fMaxImpulse) { newUnclampedAccImpulse.normalize(); @@ -428,31 +412,32 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver m_accMotorImpulse += impulse; } - btScalar impulseMag = impulse.length(); - btVector3 impulseAxis = impulse / impulseMag; - - bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag); - bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag); + btScalar impulseMag = impulse.length(); + btVector3 impulseAxis = impulse / impulseMag; + bodyA.internalApplyImpulse(btVector3(0, 0, 0), m_rbA.getInvInertiaTensorWorld() * impulseAxis, impulseMag); + bodyB.internalApplyImpulse(btVector3(0, 0, 0), m_rbB.getInvInertiaTensorWorld() * impulseAxis, -impulseMag); } } - else if (m_damping > SIMD_EPSILON) // no motor: do a little damping + else if (m_damping > SIMD_EPSILON) // no motor: do a little damping { - btVector3 angVelA; bodyA.internalGetAngularVelocity(angVelA); - btVector3 angVelB; bodyB.internalGetAngularVelocity(angVelB); + btVector3 angVelA; + bodyA.internalGetAngularVelocity(angVelA); + btVector3 angVelB; + bodyB.internalGetAngularVelocity(angVelB); btVector3 relVel = angVelB - angVelA; if (relVel.length2() > SIMD_EPSILON) { btVector3 relVelAxis = relVel.normalized(); - btScalar m_kDamping = btScalar(1.) / - (getRigidBodyA().computeAngularImpulseDenominator(relVelAxis) + - getRigidBodyB().computeAngularImpulseDenominator(relVelAxis)); + btScalar m_kDamping = btScalar(1.) / + (getRigidBodyA().computeAngularImpulseDenominator(relVelAxis) + + getRigidBodyB().computeAngularImpulseDenominator(relVelAxis)); btVector3 impulse = m_damping * m_kDamping * relVel; - btScalar impulseMag = impulse.length(); + btScalar impulseMag = impulse.length(); btVector3 impulseAxis = impulse / impulseMag; - bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag); - bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag); + bodyA.internalApplyImpulse(btVector3(0, 0, 0), m_rbA.getInvInertiaTensorWorld() * impulseAxis, impulseMag); + bodyB.internalApplyImpulse(btVector3(0, 0, 0), m_rbB.getInvInertiaTensorWorld() * impulseAxis, -impulseMag); } } @@ -467,7 +452,7 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver // solve swing limit if (m_solveSwingLimit) { - btScalar amplitude = m_swingLimitRatio * m_swingCorrection*m_biasFactor/timeStep; + btScalar amplitude = m_swingLimitRatio * m_swingCorrection * m_biasFactor / timeStep; btScalar relSwingVel = (angVelB - angVelA).dot(m_swingAxis); if (relSwingVel > 0) amplitude += m_swingLimitRatio * relSwingVel * m_relaxationFactor; @@ -475,7 +460,7 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver // Clamp the accumulated impulse btScalar temp = m_accSwingLimitImpulse; - m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) ); + m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0)); impulseMag = m_accSwingLimitImpulse - temp; btVector3 impulse = m_swingAxis * impulseMag; @@ -491,47 +476,41 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver impulseMag = impulse.length(); btVector3 noTwistSwingAxis = impulse / impulseMag; - bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*noTwistSwingAxis, impulseMag); - bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*noTwistSwingAxis, -impulseMag); + bodyA.internalApplyImpulse(btVector3(0, 0, 0), m_rbA.getInvInertiaTensorWorld() * noTwistSwingAxis, impulseMag); + bodyB.internalApplyImpulse(btVector3(0, 0, 0), m_rbB.getInvInertiaTensorWorld() * noTwistSwingAxis, -impulseMag); } - // solve twist limit if (m_solveTwistLimit) { - btScalar amplitude = m_twistLimitRatio * m_twistCorrection*m_biasFactor/timeStep; - btScalar relTwistVel = (angVelB - angVelA).dot( m_twistAxis ); - if (relTwistVel > 0) // only damp when moving towards limit (m_twistAxis flipping is important) + btScalar amplitude = m_twistLimitRatio * m_twistCorrection * m_biasFactor / timeStep; + btScalar relTwistVel = (angVelB - angVelA).dot(m_twistAxis); + if (relTwistVel > 0) // only damp when moving towards limit (m_twistAxis flipping is important) amplitude += m_twistLimitRatio * relTwistVel * m_relaxationFactor; btScalar impulseMag = amplitude * m_kTwist; // Clamp the accumulated impulse btScalar temp = m_accTwistLimitImpulse; - m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) ); + m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0)); impulseMag = m_accTwistLimitImpulse - temp; - // btVector3 impulse = m_twistAxis * impulseMag; + // btVector3 impulse = m_twistAxis * impulseMag; - bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_twistAxis,impulseMag); - bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_twistAxis,-impulseMag); - } + bodyA.internalApplyImpulse(btVector3(0, 0, 0), m_rbA.getInvInertiaTensorWorld() * m_twistAxis, impulseMag); + bodyB.internalApplyImpulse(btVector3(0, 0, 0), m_rbB.getInvInertiaTensorWorld() * m_twistAxis, -impulseMag); + } } } #else -btAssert(0); -#endif //__SPU__ + btAssert(0); +#endif //__SPU__ } - - - -void btConeTwistConstraint::updateRHS(btScalar timeStep) +void btConeTwistConstraint::updateRHS(btScalar timeStep) { (void)timeStep; - } - #ifndef __SPU__ void btConeTwistConstraint::calcAngleInfo() { @@ -540,15 +519,15 @@ void btConeTwistConstraint::calcAngleInfo() m_solveTwistLimit = false; m_solveSwingLimit = false; - btVector3 b1Axis1(0,0,0),b1Axis2(0,0,0),b1Axis3(0,0,0); - btVector3 b2Axis1(0,0,0),b2Axis2(0,0,0); + btVector3 b1Axis1(0, 0, 0), b1Axis2(0, 0, 0), b1Axis3(0, 0, 0); + btVector3 b2Axis1(0, 0, 0), b2Axis2(0, 0, 0); b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0); b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0); - btScalar swing1=btScalar(0.),swing2 = btScalar(0.); + btScalar swing1 = btScalar(0.), swing2 = btScalar(0.); - btScalar swx=btScalar(0.),swy = btScalar(0.); + btScalar swx = btScalar(0.), swy = btScalar(0.); btScalar thresh = btScalar(10.); btScalar fact; @@ -558,33 +537,33 @@ void btConeTwistConstraint::calcAngleInfo() b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1); swx = b2Axis1.dot(b1Axis1); swy = b2Axis1.dot(b1Axis2); - swing1 = btAtan2Fast(swy, swx); - fact = (swy*swy + swx*swx) * thresh * thresh; + swing1 = btAtan2Fast(swy, swx); + fact = (swy * swy + swx * swx) * thresh * thresh; fact = fact / (fact + btScalar(1.0)); - swing1 *= fact; + swing1 *= fact; } if (m_swingSpan2 >= btScalar(0.05f)) { - b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2); + b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2); swx = b2Axis1.dot(b1Axis1); swy = b2Axis1.dot(b1Axis3); - swing2 = btAtan2Fast(swy, swx); - fact = (swy*swy + swx*swx) * thresh * thresh; + swing2 = btAtan2Fast(swy, swx); + fact = (swy * swy + swx * swx) * thresh * thresh; fact = fact / (fact + btScalar(1.0)); - swing2 *= fact; + swing2 *= fact; } - btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1); - btScalar RMaxAngle2Sq = 1.0f / (m_swingSpan2*m_swingSpan2); - btScalar EllipseAngle = btFabs(swing1*swing1)* RMaxAngle1Sq + btFabs(swing2*swing2) * RMaxAngle2Sq; + btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1 * m_swingSpan1); + btScalar RMaxAngle2Sq = 1.0f / (m_swingSpan2 * m_swingSpan2); + btScalar EllipseAngle = btFabs(swing1 * swing1) * RMaxAngle1Sq + btFabs(swing2 * swing2) * RMaxAngle2Sq; if (EllipseAngle > 1.0f) { - m_swingCorrection = EllipseAngle-1.0f; + m_swingCorrection = EllipseAngle - 1.0f; m_solveSwingLimit = true; // Calculate necessary axis & factors - m_swingAxis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3)); + m_swingAxis = b2Axis1.cross(b1Axis2 * b2Axis1.dot(b1Axis2) + b1Axis3 * b2Axis1.dot(b1Axis3)); m_swingAxis.normalize(); btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f; m_swingAxis *= swingAxisSign; @@ -594,14 +573,14 @@ void btConeTwistConstraint::calcAngleInfo() if (m_twistSpan >= btScalar(0.)) { btVector3 b2Axis2 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(1); - btQuaternion rotationArc = shortestArcQuat(b2Axis1,b1Axis1); - btVector3 TwistRef = quatRotate(rotationArc,b2Axis2); - btScalar twist = btAtan2Fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) ); + btQuaternion rotationArc = shortestArcQuat(b2Axis1, b1Axis1); + btVector3 TwistRef = quatRotate(rotationArc, b2Axis2); + btScalar twist = btAtan2Fast(TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2)); m_twistAngle = twist; -// btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.); + // btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.); btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? btScalar(1.0f) : btScalar(0.); - if (twist <= -m_twistSpan*lockedFreeFactor) + if (twist <= -m_twistSpan * lockedFreeFactor) { m_twistCorrection = -(twist + m_twistSpan); m_solveTwistLimit = true; @@ -609,7 +588,7 @@ void btConeTwistConstraint::calcAngleInfo() m_twistAxis.normalize(); m_twistAxis *= -1.0f; } - else if (twist > m_twistSpan*lockedFreeFactor) + else if (twist > m_twistSpan * lockedFreeFactor) { m_twistCorrection = (twist - m_twistSpan); m_solveTwistLimit = true; @@ -618,13 +597,11 @@ void btConeTwistConstraint::calcAngleInfo() } } } -#endif //__SPU__ - -static btVector3 vTwist(1,0,0); // twist axis in constraint's space +#endif //__SPU__ +static btVector3 vTwist(1, 0, 0); // twist axis in constraint's space - -void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB) +void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA, const btMatrix3x3& invInertiaWorldB) { m_swingCorrection = btScalar(0.); m_twistLimitSign = btScalar(0.); @@ -632,7 +609,7 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr m_solveSwingLimit = false; // compute rotation of A wrt B (in constraint space) if (m_bMotorEnabled && (!m_useSolveConstraintObsolete)) - { // it is assumed that setMotorTarget() was alredy called + { // it is assumed that setMotorTarget() was alredy called // and motor target m_qTarget is within constraint limits // TODO : split rotation to pure swing and pure twist // compute desired transforms in world @@ -641,23 +618,22 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr btTransform trB = transB * m_rbBFrame; btTransform trDeltaAB = trB * trPose * trA.inverse(); btQuaternion qDeltaAB = trDeltaAB.getRotation(); - btVector3 swingAxis = btVector3(qDeltaAB.x(), qDeltaAB.y(), qDeltaAB.z()); - float swingAxisLen2 = swingAxis.length2(); - if(btFuzzyZero(swingAxisLen2)) + btVector3 swingAxis = btVector3(qDeltaAB.x(), qDeltaAB.y(), qDeltaAB.z()); + btScalar swingAxisLen2 = swingAxis.length2(); + if (btFuzzyZero(swingAxisLen2)) { - return; + return; } m_swingAxis = swingAxis; m_swingAxis.normalize(); m_swingCorrection = qDeltaAB.getAngle(); - if(!btFuzzyZero(m_swingCorrection)) + if (!btFuzzyZero(m_swingCorrection)) { m_solveSwingLimit = true; } return; } - { // compute rotation of A wrt B (in constraint space) btQuaternion qA = transA.getRotation() * m_rbAFrame.getRotation(); @@ -665,13 +641,17 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr btQuaternion qAB = qB.inverse() * qA; // split rotation into cone and twist // (all this is done from B's perspective. Maybe I should be averaging axes...) - btVector3 vConeNoTwist = quatRotate(qAB, vTwist); vConeNoTwist.normalize(); - btQuaternion qABCone = shortestArcQuat(vTwist, vConeNoTwist); qABCone.normalize(); - btQuaternion qABTwist = qABCone.inverse() * qAB; qABTwist.normalize(); + btVector3 vConeNoTwist = quatRotate(qAB, vTwist); + vConeNoTwist.normalize(); + btQuaternion qABCone = shortestArcQuat(vTwist, vConeNoTwist); + qABCone.normalize(); + btQuaternion qABTwist = qABCone.inverse() * qAB; + qABTwist.normalize(); if (m_swingSpan1 >= m_fixThresh && m_swingSpan2 >= m_fixThresh) { - btScalar swingAngle, swingLimit = 0; btVector3 swingAxis; + btScalar swingAngle, swingLimit = 0; + btVector3 swingAxis; computeConeLimitInfo(qABCone, swingAngle, swingAxis, swingLimit); if (swingAngle > swingLimit * m_limitSoftness) @@ -684,9 +664,9 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr m_swingLimitRatio = 1.f; if (swingAngle < swingLimit && m_limitSoftness < 1.f - SIMD_EPSILON) { - m_swingLimitRatio = (swingAngle - swingLimit * m_limitSoftness)/ + m_swingLimitRatio = (swingAngle - swingLimit * m_limitSoftness) / (swingLimit - swingLimit * m_limitSoftness); - } + } // swing correction tries to get back to soft limit m_swingCorrection = swingAngle - (swingLimit * m_limitSoftness); @@ -694,14 +674,14 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr // adjustment of swing axis (based on ellipse normal) adjustSwingAxisToUseEllipseNormal(swingAxis); - // Calculate necessary axis & factors + // Calculate necessary axis & factors m_swingAxis = quatRotate(qB, -swingAxis); - m_twistAxisA.setValue(0,0,0); + m_twistAxisA.setValue(0, 0, 0); - m_kSwing = btScalar(1.) / - (computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldA) + - computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldB)); + m_kSwing = btScalar(1.) / + (computeAngularImpulseDenominator(m_swingAxis, invInertiaWorldA) + + computeAngularImpulseDenominator(m_swingAxis, invInertiaWorldB)); } } else @@ -717,9 +697,9 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr btScalar x = ivB.dot(ivA); btScalar y = ivB.dot(jvA); btScalar z = ivB.dot(kvA); - if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh)) - { // fixed. We'll need to add one more row to constraint - if((!btFuzzyZero(y)) || (!(btFuzzyZero(z)))) + if ((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh)) + { // fixed. We'll need to add one more row to constraint + if ((!btFuzzyZero(y)) || (!(btFuzzyZero(z)))) { m_solveSwingLimit = true; m_swingAxis = -ivB.cross(ivA); @@ -727,47 +707,47 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr } else { - if(m_swingSpan1 < m_fixThresh) - { // hinge around Y axis -// if(!(btFuzzyZero(y))) - if((!(btFuzzyZero(x))) || (!(btFuzzyZero(z)))) + if (m_swingSpan1 < m_fixThresh) + { // hinge around Y axis + // if(!(btFuzzyZero(y))) + if ((!(btFuzzyZero(x))) || (!(btFuzzyZero(z)))) { m_solveSwingLimit = true; - if(m_swingSpan2 >= m_fixThresh) + if (m_swingSpan2 >= m_fixThresh) { y = btScalar(0.f); btScalar span2 = btAtan2(z, x); - if(span2 > m_swingSpan2) + if (span2 > m_swingSpan2) { x = btCos(m_swingSpan2); z = btSin(m_swingSpan2); } - else if(span2 < -m_swingSpan2) + else if (span2 < -m_swingSpan2) { - x = btCos(m_swingSpan2); + x = btCos(m_swingSpan2); z = -btSin(m_swingSpan2); } } } } else - { // hinge around Z axis -// if(!btFuzzyZero(z)) - if((!(btFuzzyZero(x))) || (!(btFuzzyZero(y)))) + { // hinge around Z axis + // if(!btFuzzyZero(z)) + if ((!(btFuzzyZero(x))) || (!(btFuzzyZero(y)))) { m_solveSwingLimit = true; - if(m_swingSpan1 >= m_fixThresh) + if (m_swingSpan1 >= m_fixThresh) { z = btScalar(0.f); btScalar span1 = btAtan2(y, x); - if(span1 > m_swingSpan1) + if (span1 > m_swingSpan1) { x = btCos(m_swingSpan1); y = btSin(m_swingSpan1); } - else if(span1 < -m_swingSpan1) + else if (span1 < -m_swingSpan1) { - x = btCos(m_swingSpan1); + x = btCos(m_swingSpan1); y = -btSin(m_swingSpan1); } } @@ -778,10 +758,10 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr target[2] = x * ivA[2] + y * jvA[2] + z * kvA[2]; target.normalize(); m_swingAxis = -ivB.cross(target); - m_swingCorrection = m_swingAxis.length(); + m_swingCorrection = m_swingAxis.length(); - if (!btFuzzyZero(m_swingCorrection)) - m_swingAxis.normalize(); + if (!btFuzzyZero(m_swingCorrection)) + m_swingAxis.normalize(); } } @@ -790,15 +770,15 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr btVector3 twistAxis; computeTwistLimitInfo(qABTwist, m_twistAngle, twistAxis); - if (m_twistAngle > m_twistSpan*m_limitSoftness) + if (m_twistAngle > m_twistSpan * m_limitSoftness) { m_solveTwistLimit = true; m_twistLimitRatio = 1.f; if (m_twistAngle < m_twistSpan && m_limitSoftness < 1.f - SIMD_EPSILON) { - m_twistLimitRatio = (m_twistAngle - m_twistSpan * m_limitSoftness)/ - (m_twistSpan - m_twistSpan * m_limitSoftness); + m_twistLimitRatio = (m_twistAngle - m_twistSpan * m_limitSoftness) / + (m_twistSpan - m_twistSpan * m_limitSoftness); } // twist correction tries to get back to soft limit @@ -807,8 +787,8 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr m_twistAxis = quatRotate(qB, -twistAxis); m_kTwist = btScalar(1.) / - (computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldA) + - computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldB)); + (computeAngularImpulseDenominator(m_twistAxis, invInertiaWorldA) + + computeAngularImpulseDenominator(m_twistAxis, invInertiaWorldB)); } if (m_solveSwingLimit) @@ -821,15 +801,13 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr } } - - // given a cone rotation in constraint space, (pre: twist must already be removed) // this method computes its corresponding swing angle and axis. // more interestingly, it computes the cone/swing limit (angle) for this cone "pose". void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone, - btScalar& swingAngle, // out - btVector3& vSwingAxis, // out - btScalar& swingLimit) // out + btScalar& swingAngle, // out + btVector3& vSwingAxis, // out + btScalar& swingLimit) // out { swingAngle = qCone.getAngle(); if (swingAngle > SIMD_EPSILON) @@ -840,7 +818,7 @@ void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone, // non-zero twist?! this should never happen. btAssert(fabs(vSwingAxis.x()) <= SIMD_EPSILON)); #endif - + // Compute limit for given swing. tricky: // Given a swing axis, we're looking for the intersection with the bounding cone ellipse. // (Since we're dealing with angles, this ellipse is embedded on the surface of a sphere.) @@ -848,7 +826,7 @@ void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone, // For starters, compute the direction from center to surface of ellipse. // This is just the perpendicular (ie. rotate 2D vector by PI/2) of the swing axis. // (vSwingAxis is the cone rotation (in z,y); change vars and rotate to (x,y) coords.) - btScalar xEllipse = vSwingAxis.y(); + btScalar xEllipse = vSwingAxis.y(); btScalar yEllipse = -vSwingAxis.z(); // Now, we use the slope of the vector (using x/yEllipse) and find the length @@ -858,14 +836,14 @@ void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone, // a^2 b^2 // Do the math and it should be clear. - swingLimit = m_swingSpan1; // if xEllipse == 0, we have a pure vSwingAxis.z rotation: just use swingspan1 + swingLimit = m_swingSpan1; // if xEllipse == 0, we have a pure vSwingAxis.z rotation: just use swingspan1 if (fabs(xEllipse) > SIMD_EPSILON) { - btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse); + btScalar surfaceSlope2 = (yEllipse * yEllipse) / (xEllipse * xEllipse); btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2); norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1); btScalar swingLimit2 = (1 + surfaceSlope2) / norm; - swingLimit = sqrt(swingLimit2); + swingLimit = std::sqrt(swingLimit2); } // test! @@ -887,7 +865,7 @@ void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone, #if 0 btAssert(0); #endif - } + } } btVector3 btConeTwistConstraint::GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const @@ -903,34 +881,34 @@ btVector3 btConeTwistConstraint::GetPointForAngle(btScalar fAngleInRadians, btSc // a^2 b^2 // Do the math and it should be clear. - float swingLimit = m_swingSpan1; // if xEllipse == 0, just use axis b (1) + btScalar swingLimit = m_swingSpan1; // if xEllipse == 0, just use axis b (1) if (fabs(xEllipse) > SIMD_EPSILON) { - btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse); + btScalar surfaceSlope2 = (yEllipse * yEllipse) / (xEllipse * xEllipse); btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2); norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1); btScalar swingLimit2 = (1 + surfaceSlope2) / norm; - swingLimit = sqrt(swingLimit2); + swingLimit = std::sqrt(swingLimit2); } // convert into point in constraint space: // note: twist is x-axis, swing 1 and 2 are along the z and y axes respectively btVector3 vSwingAxis(0, xEllipse, -yEllipse); btQuaternion qSwing(vSwingAxis, swingLimit); - btVector3 vPointInConstraintSpace(fLength,0,0); + btVector3 vPointInConstraintSpace(fLength, 0, 0); return quatRotate(qSwing, vPointInConstraintSpace); } // given a twist rotation in constraint space, (pre: cone must already be removed) // this method computes its corresponding angle and axis. void btConeTwistConstraint::computeTwistLimitInfo(const btQuaternion& qTwist, - btScalar& twistAngle, // out - btVector3& vTwistAxis) // out + btScalar& twistAngle, // out + btVector3& vTwistAxis) // out { btQuaternion qMinTwist = qTwist; twistAngle = qTwist.getAngle(); - if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate. + if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate. { qMinTwist = -(qTwist); twistAngle = qMinTwist.getAngle(); @@ -948,80 +926,79 @@ void btConeTwistConstraint::computeTwistLimitInfo(const btQuaternion& qTwist, vTwistAxis.normalize(); } - void btConeTwistConstraint::adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const { // the swing axis is computed as the "twist-free" cone rotation, // but the cone limit is not circular, but elliptical (if swingspan1 != swingspan2). - // so, if we're outside the limits, the closest way back inside the cone isn't + // so, if we're outside the limits, the closest way back inside the cone isn't // along the vector back to the center. better (and more stable) to use the ellipse normal. // convert swing axis to direction from center to surface of ellipse // (ie. rotate 2D vector by PI/2) btScalar y = -vSwingAxis.z(); - btScalar z = vSwingAxis.y(); + btScalar z = vSwingAxis.y(); // do the math... - if (fabs(z) > SIMD_EPSILON) // avoid division by 0. and we don't need an update if z == 0. + if (fabs(z) > SIMD_EPSILON) // avoid division by 0. and we don't need an update if z == 0. { // compute gradient/normal of ellipse surface at current "point" - btScalar grad = y/z; + btScalar grad = y / z; grad *= m_swingSpan2 / m_swingSpan1; // adjust y/z to represent normal at point (instead of vector to point) if (y > 0) - y = fabs(grad * z); + y = fabs(grad * z); else y = -fabs(grad * z); // convert ellipse direction back to swing axis vSwingAxis.setZ(-y); - vSwingAxis.setY( z); + vSwingAxis.setY(z); vSwingAxis.normalize(); } } - - -void btConeTwistConstraint::setMotorTarget(const btQuaternion &q) +void btConeTwistConstraint::setMotorTarget(const btQuaternion& q) { //btTransform trACur = m_rbA.getCenterOfMassTransform(); //btTransform trBCur = m_rbB.getCenterOfMassTransform(); -// btTransform trABCur = trBCur.inverse() * trACur; -// btQuaternion qABCur = trABCur.getRotation(); -// btTransform trConstraintCur = (trBCur * m_rbBFrame).inverse() * (trACur * m_rbAFrame); + // btTransform trABCur = trBCur.inverse() * trACur; + // btQuaternion qABCur = trABCur.getRotation(); + // btTransform trConstraintCur = (trBCur * m_rbBFrame).inverse() * (trACur * m_rbAFrame); //btQuaternion qConstraintCur = trConstraintCur.getRotation(); btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * q * m_rbAFrame.getRotation(); setMotorTargetInConstraintSpace(qConstraint); } - -void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion &q) +void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion& q) { m_qTarget = q; // clamp motor target to within limits { - btScalar softness = 1.f;//m_limitSoftness; + btScalar softness = 1.f; //m_limitSoftness; // split into twist and cone btVector3 vTwisted = quatRotate(m_qTarget, vTwist); - btQuaternion qTargetCone = shortestArcQuat(vTwist, vTwisted); qTargetCone.normalize(); - btQuaternion qTargetTwist = qTargetCone.inverse() * m_qTarget; qTargetTwist.normalize(); + btQuaternion qTargetCone = shortestArcQuat(vTwist, vTwisted); + qTargetCone.normalize(); + btQuaternion qTargetTwist = qTargetCone.inverse() * m_qTarget; + qTargetTwist.normalize(); // clamp cone if (m_swingSpan1 >= btScalar(0.05f) && m_swingSpan2 >= btScalar(0.05f)) { - btScalar swingAngle, swingLimit; btVector3 swingAxis; + btScalar swingAngle, swingLimit; + btVector3 swingAxis; computeConeLimitInfo(qTargetCone, swingAngle, swingAxis, swingLimit); if (fabs(swingAngle) > SIMD_EPSILON) { - if (swingAngle > swingLimit*softness) - swingAngle = swingLimit*softness; - else if (swingAngle < -swingLimit*softness) - swingAngle = -swingLimit*softness; + if (swingAngle > swingLimit * softness) + swingAngle = swingLimit * softness; + else if (swingAngle < -swingLimit * softness) + swingAngle = -swingLimit * softness; qTargetCone = btQuaternion(swingAxis, swingAngle); } } @@ -1029,16 +1006,17 @@ void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion & // clamp twist if (m_twistSpan >= btScalar(0.05f)) { - btScalar twistAngle; btVector3 twistAxis; + btScalar twistAngle; + btVector3 twistAxis; computeTwistLimitInfo(qTargetTwist, twistAngle, twistAxis); if (fabs(twistAngle) > SIMD_EPSILON) { // eddy todo: limitSoftness used here??? - if (twistAngle > m_twistSpan*softness) - twistAngle = m_twistSpan*softness; - else if (twistAngle < -m_twistSpan*softness) - twistAngle = -m_twistSpan*softness; + if (twistAngle > m_twistSpan * softness) + twistAngle = m_twistSpan * softness; + else if (twistAngle < -m_twistSpan * softness) + twistAngle = -m_twistSpan * softness; qTargetTwist = btQuaternion(twistAxis, twistAngle); } } @@ -1047,15 +1025,15 @@ void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion & } } -///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 btConeTwistConstraint::setParam(int num, btScalar value, int axis) { - switch(num) + switch (num) { - case BT_CONSTRAINT_ERP : - case BT_CONSTRAINT_STOP_ERP : - if((axis >= 0) && (axis < 3)) + case BT_CONSTRAINT_ERP: + case BT_CONSTRAINT_STOP_ERP: + if ((axis >= 0) && (axis < 3)) { m_linERP = value; m_flags |= BT_CONETWIST_FLAGS_LIN_ERP; @@ -1065,9 +1043,9 @@ void btConeTwistConstraint::setParam(int num, btScalar value, int axis) m_biasFactor = value; } break; - case BT_CONSTRAINT_CFM : - case BT_CONSTRAINT_STOP_CFM : - if((axis >= 0) && (axis < 3)) + case BT_CONSTRAINT_CFM: + case BT_CONSTRAINT_STOP_CFM: + if ((axis >= 0) && (axis < 3)) { m_linCFM = value; m_flags |= BT_CONETWIST_FLAGS_LIN_CFM; @@ -1085,19 +1063,19 @@ void btConeTwistConstraint::setParam(int num, btScalar value, int axis) } ///return the local value of parameter -btScalar btConeTwistConstraint::getParam(int num, int axis) const +btScalar btConeTwistConstraint::getParam(int num, int axis) const { btScalar retVal = 0; - switch(num) + switch (num) { - case BT_CONSTRAINT_ERP : - case BT_CONSTRAINT_STOP_ERP : - if((axis >= 0) && (axis < 3)) + case BT_CONSTRAINT_ERP: + case BT_CONSTRAINT_STOP_ERP: + if ((axis >= 0) && (axis < 3)) { btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_LIN_ERP); retVal = m_linERP; } - else if((axis >= 3) && (axis < 6)) + else if ((axis >= 3) && (axis < 6)) { retVal = m_biasFactor; } @@ -1106,14 +1084,14 @@ btScalar btConeTwistConstraint::getParam(int num, int axis) const btAssertConstrParams(0); } break; - case BT_CONSTRAINT_CFM : - case BT_CONSTRAINT_STOP_CFM : - if((axis >= 0) && (axis < 3)) + case BT_CONSTRAINT_CFM: + case BT_CONSTRAINT_STOP_CFM: + if ((axis >= 0) && (axis < 3)) { btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_LIN_CFM); retVal = m_linCFM; } - else if((axis >= 3) && (axis < 6)) + else if ((axis >= 3) && (axis < 6)) { btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_ANG_CFM); retVal = m_angCFM; @@ -1123,21 +1101,16 @@ btScalar btConeTwistConstraint::getParam(int num, int axis) const btAssertConstrParams(0); } break; - default : + default: btAssertConstrParams(0); } return retVal; } - -void btConeTwistConstraint::setFrames(const btTransform & frameA, const btTransform & frameB) +void btConeTwistConstraint::setFrames(const btTransform& frameA, const btTransform& frameB) { m_rbAFrame = frameA; m_rbBFrame = frameB; buildJacobian(); //calculateTransforms(); } - - - - |