btScalar btTranslationalLimitMotor::solveLinearAxis( btScalar timeStep, btScalar jacDiagABInv, btRigidBody& body1,const btVector3 &pointInA, btRigidBody& body2,const btVector3 &pointInB, int limit_index, const btVector3 & axis_normal_on_a, const btVector3 & anchorPos) { ///find relative velocity // btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition(); // btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition(); btVector3 rel_pos1 = anchorPos - body1.getCenterOfMassPosition(); btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition(); btVector3 vel1; body1.internalGetVelocityInLocalPointObsolete(rel_pos1,vel1); btVector3 vel2; body2.internalGetVelocityInLocalPointObsolete(rel_pos2,vel2); btVector3 vel = vel1 - vel2; btScalar rel_vel = axis_normal_on_a.dot(vel); /// apply displacement correction //positional error (zeroth order error) btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a); btScalar lo = btScalar(-BT_LARGE_FLOAT); btScalar hi = btScalar(BT_LARGE_FLOAT); btScalar minLimit = m_lowerLimit[limit_index]; btScalar maxLimit = m_upperLimit[limit_index]; //handle the limits if (minLimit < maxLimit) { { if (depth > maxLimit) { depth -= maxLimit; lo = btScalar(0.); } else { if (depth < minLimit) { depth -= minLimit; hi = btScalar(0.); } else { return 0.0f; } } } } btScalar normalImpulse= m_limitSoftness*(m_restitution*depth/timeStep - m_damping*rel_vel) * jacDiagABInv; btScalar oldNormalImpulse = m_accumulatedImpulse[limit_index]; btScalar sum = oldNormalImpulse + normalImpulse; m_accumulatedImpulse[limit_index] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum; normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse; btVector3 impulse_vector = axis_normal_on_a * normalImpulse; //body1.applyImpulse( impulse_vector, rel_pos1); //body2.applyImpulse(-impulse_vector, rel_pos2); btVector3 ftorqueAxis1 = rel_pos1.cross(axis_normal_on_a); btVector3 ftorqueAxis2 = rel_pos2.cross(axis_normal_on_a); body1.internalApplyImpulse(axis_normal_on_a*body1.getInvMass(), body1.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse); body2.internalApplyImpulse(axis_normal_on_a*body2.getInvMass(), body2.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse); return normalImpulse; }
void btConeTwistConstraint::solveConstraintObsolete(btRigidBody& bodyA,btRigidBody& bodyB,btScalar timeStep) { #ifndef __SPU__ if (m_useSolveConstraintObsolete) { 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_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); btVector3 vel1; bodyA.internalGetVelocityInLocalPointObsolete(rel_pos1,vel1); btVector3 vel2; bodyB.internalGetVelocityInLocalPointObsolete(rel_pos2,vel2); btVector3 vel = vel1 - vel2; 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; 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); } } // apply motor if (m_bMotorEnabled) { // 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); btTransformUtil::integrateTransform( trACur, zerovec, omegaA, timeStep, trAPred); btTransform trBPred; trBPred.setIdentity(); btTransformUtil::integrateTransform( trBCur, zerovec, omegaB, timeStep, trBPred); // compute desired transforms in world btTransform trPose(m_qTarget); btTransform trABDes = m_rbBFrame * trPose * m_rbAFrame.inverse(); btTransform trADes = trBPred * trABDes; btTransform trBDes = trAPred * trABDes.inverse(); // compute desired omegas in world btVector3 omegaADes, omegaBDes; btTransformUtil::calculateVelocity(trACur, trADes, timeStep, zerovec, omegaADes); btTransformUtil::calculateVelocity(trBCur, trBDes, timeStep, zerovec, omegaBDes); // compute delta omegas btVector3 dOmegaA = omegaADes - omegaA; btVector3 dOmegaB = omegaBDes - omegaB; // compute weighted avg axis of dOmega (weighting based on inertias) btVector3 axisA, axisB; btScalar kAxisAInv = 0, kAxisBInv = 0; if (dOmegaA.length2() > SIMD_EPSILON) { axisA = dOmegaA.normalized(); kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(axisA); } if (dOmegaB.length2() > SIMD_EPSILON) { axisB = dOmegaB.normalized(); kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(axisB); } btVector3 avgAxis = kAxisAInv * axisA + kAxisBInv * axisB; static bool bDoTorque = true; if (bDoTorque && avgAxis.length2() > SIMD_EPSILON) { avgAxis.normalize(); kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(avgAxis); kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(avgAxis); btScalar kInvCombined = kAxisAInv + kAxisBInv; btVector3 impulse = (kAxisAInv * dOmegaA - kAxisBInv * dOmegaB) / (kInvCombined * kInvCombined); if (m_maxMotorImpulse >= 0) { btScalar fMaxImpulse = m_maxMotorImpulse; if (m_bNormalizedMotorStrength) fMaxImpulse = fMaxImpulse/kAxisAInv; btVector3 newUnclampedAccImpulse = m_accMotorImpulse + impulse; btScalar newUnclampedMag = newUnclampedAccImpulse.length(); if (newUnclampedMag > fMaxImpulse) { newUnclampedAccImpulse.normalize(); newUnclampedAccImpulse *= fMaxImpulse; impulse = newUnclampedAccImpulse - m_accMotorImpulse; } 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); } } else if (m_damping > SIMD_EPSILON) // no motor: do a little damping { 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)); btVector3 impulse = m_damping * m_kDamping * relVel; 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); } } // joint limits { ///solve angular part btVector3 angVelA; bodyA.internalGetAngularVelocity(angVelA); btVector3 angVelB; bodyB.internalGetAngularVelocity(angVelB); // solve swing limit if (m_solveSwingLimit) { 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; btScalar impulseMag = amplitude * m_kSwing; // Clamp the accumulated impulse btScalar temp = m_accSwingLimitImpulse; m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) ); impulseMag = m_accSwingLimitImpulse - temp; btVector3 impulse = m_swingAxis * impulseMag; // don't let cone response affect twist // (this can happen since body A's twist doesn't match body B's AND we use an elliptical cone limit) { btVector3 impulseTwistCouple = impulse.dot(m_twistAxisA) * m_twistAxisA; btVector3 impulseNoTwistCouple = impulse - impulseTwistCouple; impulse = impulseNoTwistCouple; } 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); } // 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) 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) ); impulseMag = m_accTwistLimitImpulse - temp; 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); } } } #else btAssert(0); #endif //__SPU__ }