void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB)
{
int i;
// linear
btVector3 velA;
bodyA.getVelocityInLocalPointObsolete(m_relPosA,velA);
btVector3 velB;
bodyB.getVelocityInLocalPointObsolete(m_relPosB,velB);
btVector3 vel = velA - velB;
for(i = 0; i < 3; i++)
{
const btVector3& normal = m_jacLin[i].m_linearJointAxis;
btScalar rel_vel = normal.dot(vel);
// calculate positional error
btScalar depth = m_depth[i];
// get parameters
btScalar softness = (i) ? m_softnessOrthoLin : (m_solveLinLim ? m_softnessLimLin : m_softnessDirLin);
btScalar restitution = (i) ? m_restitutionOrthoLin : (m_solveLinLim ? m_restitutionLimLin : m_restitutionDirLin);
btScalar damping = (i) ? m_dampingOrthoLin : (m_solveLinLim ? m_dampingLimLin : m_dampingDirLin);
// calcutate and apply impulse
btScalar normalImpulse = softness * (restitution * depth / m_timeStep - damping * rel_vel) * m_jacLinDiagABInv[i];
btVector3 impulse_vector = normal * normalImpulse;
//rbA.applyImpulse( impulse_vector, m_relPosA);
//rbB.applyImpulse(-impulse_vector, m_relPosB);
{
btVector3 ftorqueAxis1 = m_relPosA.cross(normal);
btVector3 ftorqueAxis2 = m_relPosB.cross(normal);
bodyA.applyImpulse(normal*rbA.getInvMass(), rbA.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse);
bodyB.applyImpulse(normal*rbB.getInvMass(), rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse);
}
if(m_poweredLinMotor && (!i))
{ // apply linear motor
if(m_accumulatedLinMotorImpulse < m_maxLinMotorForce)
{
btScalar desiredMotorVel = m_targetLinMotorVelocity;
btScalar motor_relvel = desiredMotorVel + rel_vel;
normalImpulse = -motor_relvel * m_jacLinDiagABInv[i];
// clamp accumulated impulse
btScalar new_acc = m_accumulatedLinMotorImpulse + btFabs(normalImpulse);
if(new_acc > m_maxLinMotorForce)
{
new_acc = m_maxLinMotorForce;
}
btScalar del = new_acc - m_accumulatedLinMotorImpulse;
if(normalImpulse < btScalar(0.0))
{
normalImpulse = -del;
}
else
{
normalImpulse = del;
}
m_accumulatedLinMotorImpulse = new_acc;
// apply clamped impulse
impulse_vector = normal * normalImpulse;
//rbA.applyImpulse( impulse_vector, m_relPosA);
//rbB.applyImpulse(-impulse_vector, m_relPosB);
{
btVector3 ftorqueAxis1 = m_relPosA.cross(normal);
btVector3 ftorqueAxis2 = m_relPosB.cross(normal);
bodyA.applyImpulse(normal*rbA.getInvMass(), rbA.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse);
bodyB.applyImpulse(normal*rbB.getInvMass(), rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse);
}
}
}
}
// angular
// get axes in world space
btVector3 axisA = m_calculatedTransformA.getBasis().getColumn(0);
btVector3 axisB = m_calculatedTransformB.getBasis().getColumn(0);
btVector3 angVelA;
bodyA.getAngularVelocity(angVelA);
btVector3 angVelB;
bodyB.getAngularVelocity(angVelB);
btVector3 angVelAroundAxisA = axisA * axisA.dot(angVelA);
btVector3 angVelAroundAxisB = axisB * axisB.dot(angVelB);
btVector3 angAorthog = angVelA - angVelAroundAxisA;
btVector3 angBorthog = angVelB - angVelAroundAxisB;
btVector3 velrelOrthog = angAorthog-angBorthog;
//solve orthogonal angular velocity correction
btScalar len = velrelOrthog.length();
btScalar orthorImpulseMag = 0.f;
if (len > btScalar(0.00001))
{
btVector3 normal = velrelOrthog.normalized();
btScalar denom = rbA.computeAngularImpulseDenominator(normal) + rbB.computeAngularImpulseDenominator(normal);
//velrelOrthog *= (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng;
orthorImpulseMag = (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng;
}
//solve angular positional correction
btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/m_timeStep);
btVector3 angularAxis = angularError;
btScalar angularImpulseMag = 0;
btScalar len2 = angularError.length();
if (len2>btScalar(0.00001))
{
btVector3 normal2 = angularError.normalized();
btScalar denom2 = rbA.computeAngularImpulseDenominator(normal2) + rbB.computeAngularImpulseDenominator(normal2);
angularImpulseMag = (btScalar(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng;
angularError *= angularImpulseMag;
}
// apply impulse
//rbA.applyTorqueImpulse(-velrelOrthog+angularError);
//rbB.applyTorqueImpulse(velrelOrthog-angularError);
bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*velrelOrthog,-orthorImpulseMag);
bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*velrelOrthog,orthorImpulseMag);
bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*angularAxis,angularImpulseMag);
bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*angularAxis,-angularImpulseMag);
btScalar impulseMag;
//solve angular limits
if(m_solveAngLim)
{
impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingLimAng + m_angDepth * m_restitutionLimAng / m_timeStep;
impulseMag *= m_kAngle * m_softnessLimAng;
}
else
{
impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingDirAng + m_angDepth * m_restitutionDirAng / m_timeStep;
impulseMag *= m_kAngle * m_softnessDirAng;
}
btVector3 impulse = axisA * impulseMag;
//rbA.applyTorqueImpulse(impulse);
//rbB.applyTorqueImpulse(-impulse);
bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*axisA,impulseMag);
bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*axisA,-impulseMag);
//apply angular motor
if(m_poweredAngMotor)
{
if(m_accumulatedAngMotorImpulse < m_maxAngMotorForce)
{
btVector3 velrel = angVelAroundAxisA - angVelAroundAxisB;
btScalar projRelVel = velrel.dot(axisA);
btScalar desiredMotorVel = m_targetAngMotorVelocity;
btScalar motor_relvel = desiredMotorVel - projRelVel;
btScalar angImpulse = m_kAngle * motor_relvel;
// clamp accumulated impulse
btScalar new_acc = m_accumulatedAngMotorImpulse + btFabs(angImpulse);
if(new_acc > m_maxAngMotorForce)
{
new_acc = m_maxAngMotorForce;
}
btScalar del = new_acc - m_accumulatedAngMotorImpulse;
if(angImpulse < btScalar(0.0))
{
angImpulse = -del;
}
else
{
angImpulse = del;
}
m_accumulatedAngMotorImpulse = new_acc;
// apply clamped impulse
btVector3 motorImp = angImpulse * axisA;
//rbA.applyTorqueImpulse(motorImp);
//rbB.applyTorqueImpulse(-motorImp);
bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*axisA,angImpulse);
bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*axisA,-angImpulse);
}
}
}
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;
}
