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__
}
