protected void ResolveSingleConstraintRowLowerLimit(RigidBody body1, RigidBody body2, ref SolverConstraint c)
{
m_lowerLimitCount++;
//check magniture of applied impulse from SolverConstraint
float deltaImpulse = c.m_rhs - c.m_appliedImpulse * c.m_cfm;
if (deltaImpulse > 200)
{
int ibreak = 0;
}
float deltaVel1Dotn = (c.m_contactNormal.X * body1.m_deltaLinearVelocity.X) + (c.m_contactNormal.Y * body1.m_deltaLinearVelocity.Y) + (c.m_contactNormal.Z * body1.m_deltaLinearVelocity.Z) +
(c.m_relpos1CrossNormal.X * body1.m_deltaAngularVelocity.X) + (c.m_relpos1CrossNormal.Y * body1.m_deltaAngularVelocity.Y) + (c.m_relpos1CrossNormal.Z * body1.m_deltaAngularVelocity.Z);
float deltaVel2Dotn = -((c.m_contactNormal.X * body2.m_deltaLinearVelocity.X) + (c.m_contactNormal.Y * body2.m_deltaLinearVelocity.Y) + (c.m_contactNormal.Z * body2.m_deltaLinearVelocity.Z)) +
(c.m_relpos2CrossNormal.X * body2.m_deltaAngularVelocity.X) + (c.m_relpos2CrossNormal.Y * body2.m_deltaAngularVelocity.Y) + (c.m_relpos2CrossNormal.Z * body2.m_deltaAngularVelocity.Z);
deltaImpulse -= deltaVel1Dotn * c.m_jacDiagABInv;
deltaImpulse -= deltaVel2Dotn * c.m_jacDiagABInv;
float sum = c.m_appliedImpulse + deltaImpulse;
if (sum < c.m_lowerLimit)
{
deltaImpulse = c.m_lowerLimit - c.m_appliedImpulse;
c.m_appliedImpulse = c.m_lowerLimit;
}
else
{
c.m_appliedImpulse = sum;
}
IndexedVector3 temp = new IndexedVector3(c.m_contactNormal.X * body1.m_invMass.X,c.m_contactNormal.Y * body1.m_invMass.Y,c.m_contactNormal.Z * body1.m_invMass.Z);
body1.InternalApplyImpulse(ref temp, ref c.m_angularComponentA, deltaImpulse, "ResolveSingleConstraintRowLowerLimit-body1");
temp = new IndexedVector3(-c.m_contactNormal.X * body2.m_invMass.X, -c.m_contactNormal.Y * body2.m_invMass.Y, -c.m_contactNormal.Z * body2.m_invMass.Z);
body2.InternalApplyImpulse(ref temp, ref c.m_angularComponentB, deltaImpulse, "ResolveSingleConstraintRowLowerLimit-body2");
}
protected void ResolveSingleConstraintRowGeneric(RigidBody body1, RigidBody body2, ref SolverConstraint c)
{
m_genericCount++;
float deltaImpulse = c.m_rhs - c.m_appliedImpulse * c.m_cfm;
float deltaVel1Dotn = (c.m_contactNormal.X * body1.m_deltaLinearVelocity.X) + (c.m_contactNormal.Y * body1.m_deltaLinearVelocity.Y) + (c.m_contactNormal.Z * body1.m_deltaLinearVelocity.Z) +
(c.m_relpos1CrossNormal.X * body1.m_deltaAngularVelocity.X) + (c.m_relpos1CrossNormal.Y * body1.m_deltaAngularVelocity.Y) + (c.m_relpos1CrossNormal.Z * body1.m_deltaAngularVelocity.Z);
float deltaVel2Dotn = -((c.m_contactNormal.X * body2.m_deltaLinearVelocity.X) + (c.m_contactNormal.Y * body2.m_deltaLinearVelocity.Y) + (c.m_contactNormal.Z * body2.m_deltaLinearVelocity.Z)) +
(c.m_relpos2CrossNormal.X * body2.m_deltaAngularVelocity.X) + (c.m_relpos2CrossNormal.Y * body2.m_deltaAngularVelocity.Y) + (c.m_relpos2CrossNormal.Z * body2.m_deltaAngularVelocity.Z);
float originalDeltaImpulse = deltaImpulse;
deltaImpulse -= deltaVel1Dotn * c.m_jacDiagABInv;
deltaImpulse -= deltaVel2Dotn * c.m_jacDiagABInv;
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugSolver && false)
{
BulletGlobals.g_streamWriter.WriteLine("ResolveSingleConstraintRowGeneric start [{0}][{1}][{2}][{3}].", originalDeltaImpulse, deltaVel1Dotn, deltaVel2Dotn, c.m_jacDiagABInv);
}
#endif
float sum = c.m_appliedImpulse + deltaImpulse;
if (sum < c.m_lowerLimit)
{
deltaImpulse = c.m_lowerLimit - c.m_appliedImpulse;
c.m_appliedImpulse = c.m_lowerLimit;
}
else if (sum > c.m_upperLimit)
{
deltaImpulse = c.m_upperLimit - c.m_appliedImpulse;
c.m_appliedImpulse = c.m_upperLimit;
}
else
{
c.m_appliedImpulse = sum;
}
IndexedVector3 temp = new IndexedVector3(c.m_contactNormal.X * body1.m_invMass.X, c.m_contactNormal.Y * body1.m_invMass.Y, c.m_contactNormal.Z * body1.m_invMass.Z);
body1.InternalApplyImpulse(ref temp, ref c.m_angularComponentA, deltaImpulse, "ResolveSingleConstraintGeneric-body1");
temp = new IndexedVector3(-c.m_contactNormal.X * body2.m_invMass.X, -c.m_contactNormal.Y * body2.m_invMass.Y, -c.m_contactNormal.Z * body2.m_invMass.Z);
body2.InternalApplyImpulse(ref temp, ref c.m_angularComponentB, deltaImpulse, "ResolveSingleConstraintGeneric-body2");
}
protected void SetFrictionConstraintImpulse(ref SolverConstraint solverConstraint, RigidBody rb0, RigidBody rb1,
ManifoldPoint cp, ContactSolverInfo infoGlobal)
{
if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_FRICTION_WARMSTARTING))
{
{
SolverConstraint frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_WARMSTARTING))
{
frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
if (rb0 != null)
{
rb0.InternalApplyImpulse(frictionConstraint1.m_contactNormal * rb0.GetInvMass(), frictionConstraint1.m_angularComponentA, frictionConstraint1.m_appliedImpulse,"SetupFriction-rb0");
}
if (rb1 != null)
{
rb1.InternalApplyImpulse(frictionConstraint1.m_contactNormal * rb1.GetInvMass(), -frictionConstraint1.m_angularComponentB, -frictionConstraint1.m_appliedImpulse, "SetupFriction-rb1");
}
}
else
{
frictionConstraint1.m_appliedImpulse = 0f;
}
m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex] = frictionConstraint1;
}
if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS))
{
SolverConstraint frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex + 1];
if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_WARMSTARTING))
{
frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
if (rb0 != null)
{
rb0.InternalApplyImpulse(frictionConstraint2.m_contactNormal * rb0.GetInvMass(), frictionConstraint2.m_angularComponentA, frictionConstraint2.m_appliedImpulse,"SetFriction-rb0");
}
if (rb1 != null)
{
rb1.InternalApplyImpulse(frictionConstraint2.m_contactNormal * rb1.GetInvMass(), -frictionConstraint2.m_angularComponentB, -frictionConstraint2.m_appliedImpulse,"SetFriction-rb1");
}
}
else
{
frictionConstraint2.m_appliedImpulse = 0f;
}
m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex + 1] = frictionConstraint2;
}
}
else
{
SolverConstraint frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
frictionConstraint1.m_appliedImpulse = 0f;
if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS))
{
SolverConstraint frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex + 1];
frictionConstraint2.m_appliedImpulse = 0f;
m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex + 1] = frictionConstraint2;
}
m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex] = frictionConstraint1;
}
}
//! apply the correction impulses for two bodies
public float SolveAngularLimits(float timeStep, ref IndexedVector3 axis, float jacDiagABInv, RigidBody body0, RigidBody body1)
{
if (NeedApplyTorques() == false)
{
return 0.0f;
}
float target_velocity = m_targetVelocity;
float maxMotorForce = m_maxMotorForce;
//current error correction
if (m_currentLimit != 0)
{
target_velocity = -m_stopERP * m_currentLimitError / (timeStep);
maxMotorForce = m_maxLimitForce;
}
maxMotorForce *= timeStep;
// current velocity difference
IndexedVector3 angVelA = IndexedVector3.Zero;
body0.InternalGetAngularVelocity(ref angVelA);
IndexedVector3 angVelB = IndexedVector3.Zero;
body1.InternalGetAngularVelocity(ref angVelB);
IndexedVector3 vel_diff = angVelA - angVelB;
float rel_vel = IndexedVector3.Dot(axis, vel_diff);
// correction velocity
float motor_relvel = m_limitSoftness * (target_velocity - m_damping * rel_vel);
if (motor_relvel < MathUtil.SIMD_EPSILON && motor_relvel > -MathUtil.SIMD_EPSILON)
{
return 0.0f;//no need for applying force
}
// correction impulse
float unclippedMotorImpulse = (1 + m_bounce) * motor_relvel * jacDiagABInv;
// clip correction impulse
float clippedMotorImpulse;
///@todo: should clip against accumulated impulse
if (unclippedMotorImpulse > 0.0f)
{
clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce ? maxMotorForce : unclippedMotorImpulse;
}
else
{
clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce : unclippedMotorImpulse;
}
// sort with accumulated impulses
float lo = float.MinValue;
float hi = float.MaxValue;
float oldaccumImpulse = m_accumulatedImpulse;
float sum = oldaccumImpulse + clippedMotorImpulse;
m_accumulatedImpulse = sum > hi ? 0f : sum < lo ? 0f : sum;
clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
IndexedVector3 motorImp = clippedMotorImpulse * axis;
//body0.applyTorqueImpulse(motorImp);
//body1.applyTorqueImpulse(-motorImp);
body0.InternalApplyImpulse(IndexedVector3.Zero, body0.GetInvInertiaTensorWorld() * axis, clippedMotorImpulse, "Generic6DoF body0");
body1.InternalApplyImpulse(IndexedVector3.Zero, body1.GetInvInertiaTensorWorld() * axis, -clippedMotorImpulse, "Generic6DoF body1");
return clippedMotorImpulse;
}
public float SolveLinearAxis(
float timeStep,
float jacDiagABInv,
RigidBody body1, ref IndexedVector3 pointInA,
RigidBody body2, ref IndexedVector3 pointInB,
int limit_index,
ref IndexedVector3 axis_normal_on_a,
ref IndexedVector3 anchorPos)
{
///find relative velocity
// IndexedVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition();
// IndexedVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition();
IndexedVector3 rel_pos1 = anchorPos - body1.GetCenterOfMassPosition();
IndexedVector3 rel_pos2 = anchorPos - body2.GetCenterOfMassPosition();
IndexedVector3 vel1 = IndexedVector3.Zero;
body1.InternalGetVelocityInLocalPointObsolete(ref rel_pos1, ref vel1);
IndexedVector3 vel2 = IndexedVector3.Zero; ;
body2.InternalGetVelocityInLocalPointObsolete(ref rel_pos2, ref vel2);
IndexedVector3 vel = vel1 - vel2;
float rel_vel = IndexedVector3.Dot(axis_normal_on_a, vel);
/// apply displacement correction
//positional error (zeroth order error)
float depth = -IndexedVector3.Dot((pointInA - pointInB), axis_normal_on_a);
float lo = float.MinValue;
float hi = float.MaxValue;
float minLimit = m_lowerLimit[limit_index];
float maxLimit = m_upperLimit[limit_index];
//handle the limits
if (minLimit < maxLimit)
{
{
if (depth > maxLimit)
{
depth -= maxLimit;
lo = 0f;
}
else
{
if (depth < minLimit)
{
depth -= minLimit;
hi = 0f;
}
else
{
return 0.0f;
}
}
}
}
float normalImpulse = m_limitSoftness * (m_restitution * depth / timeStep - m_damping * rel_vel) * jacDiagABInv;
float oldNormalImpulse = m_accumulatedImpulse[limit_index];
float sum = oldNormalImpulse + normalImpulse;
m_accumulatedImpulse[limit_index] = (sum > hi ? 0f : sum < lo ? 0f : sum);
normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
IndexedVector3 impulse_vector = axis_normal_on_a * normalImpulse;
//body1.applyImpulse( impulse_vector, rel_pos1);
//body2.applyImpulse(-impulse_vector, rel_pos2);
IndexedVector3 ftorqueAxis1 = IndexedVector3.Cross(rel_pos1, axis_normal_on_a);
IndexedVector3 ftorqueAxis2 = IndexedVector3.Cross(rel_pos2, axis_normal_on_a);
body1.InternalApplyImpulse(axis_normal_on_a * body1.GetInvMass(), body1.GetInvInertiaTensorWorld() * ftorqueAxis1, normalImpulse, "Generic6DoF body1");
body2.InternalApplyImpulse(axis_normal_on_a * body2.GetInvMass(), body2.GetInvInertiaTensorWorld() * ftorqueAxis2, -normalImpulse, "Generic6DoF body2");
return normalImpulse;
}
public void solveConstraintObsolete(RigidBody bodyA, RigidBody bodyB, float timeStep)
{
if (m_useSolveConstraintObsolete)
{
IndexedVector3 pivotAInW = m_rbA.GetCenterOfMassTransform()*m_rbAFrame._origin;
IndexedVector3 pivotBInW = m_rbB.GetCenterOfMassTransform()*m_rbBFrame._origin;
float tau = 0.3f;
//linear part
if (!m_angularOnly)
{
IndexedVector3 rel_pos1 = pivotAInW - m_rbA.GetCenterOfMassPosition();
IndexedVector3 rel_pos2 = pivotBInW - m_rbB.GetCenterOfMassPosition();
IndexedVector3 vel1 = IndexedVector3.Zero;
bodyA.InternalGetVelocityInLocalPointObsolete(ref rel_pos1,ref vel1);
IndexedVector3 vel2 = IndexedVector3.Zero;
bodyB.InternalGetVelocityInLocalPointObsolete(ref rel_pos2,ref vel2);
IndexedVector3 vel = vel1 - vel2;
for (int i=0;i<3;i++)
{
IndexedVector3 normal = m_jac[i].m_linearJointAxis;
float jacDiagABInv = 1.0f / m_jac[i].GetDiagonal();
float rel_vel = normal.Dot(ref vel);
//positional error (zeroth order error)
float depth = -(pivotAInW - pivotBInW).Dot(ref normal); //this is the error projected on the normal
float impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv;
m_appliedImpulse += impulse;
IndexedVector3 ftorqueAxis1 = rel_pos1.Cross(ref normal);
IndexedVector3 ftorqueAxis2 = rel_pos2.Cross(ref normal);
bodyA.InternalApplyImpulse(normal*m_rbA.GetInvMass(), m_rbA.GetInvInertiaTensorWorld()*ftorqueAxis1,impulse,null);
bodyB.InternalApplyImpulse(normal*m_rbB.GetInvMass(), m_rbB.GetInvInertiaTensorWorld()*ftorqueAxis2,-impulse,null);
}
}
// apply motor
if (m_bMotorEnabled)
{
// compute current and predicted transforms
IndexedMatrix trACur = m_rbA.GetCenterOfMassTransform();
IndexedMatrix trBCur = m_rbB.GetCenterOfMassTransform();
IndexedVector3 omegaA = IndexedVector3.Zero; bodyA.InternalGetAngularVelocity(ref omegaA);
IndexedVector3 omegaB = IndexedVector3.Zero; bodyB.InternalGetAngularVelocity(ref omegaB);
IndexedMatrix trAPred;
IndexedVector3 zerovec = new IndexedVector3(0,0,0);
TransformUtil.IntegrateTransform(ref trACur, ref zerovec, ref omegaA, timeStep, out trAPred);
IndexedMatrix trBPred;
TransformUtil.IntegrateTransform(ref trBCur, ref zerovec, ref omegaB, timeStep, out trBPred);
// compute desired transforms in world
IndexedMatrix trPose = IndexedMatrix.CreateFromQuaternion(m_qTarget);
IndexedMatrix trABDes = m_rbBFrame * trPose * m_rbAFrame.Inverse();
IndexedMatrix trADes = trBPred * trABDes;
IndexedMatrix trBDes = trAPred * trABDes.Inverse();
// compute desired omegas in world
IndexedVector3 omegaADes, omegaBDes;
TransformUtil.CalculateVelocity(ref trACur, ref trADes, timeStep, out zerovec, out omegaADes);
TransformUtil.CalculateVelocity(ref trBCur, ref trBDes, timeStep, out zerovec, out omegaBDes);
// compute delta omegas
IndexedVector3 dOmegaA = omegaADes - omegaA;
IndexedVector3 dOmegaB = omegaBDes - omegaB;
// compute weighted avg axis of dOmega (weighting based on inertias)
IndexedVector3 axisA = IndexedVector3.Zero, axisB = IndexedVector3.Zero;
float kAxisAInv = 0, kAxisBInv = 0;
if (dOmegaA.LengthSquared() > MathUtil.SIMD_EPSILON)
{
axisA = dOmegaA.Normalized();
kAxisAInv = GetRigidBodyA().ComputeAngularImpulseDenominator(ref axisA);
}
if (dOmegaB.LengthSquared() > MathUtil.SIMD_EPSILON)
{
axisB = dOmegaB.Normalized();
kAxisBInv = GetRigidBodyB().ComputeAngularImpulseDenominator(ref axisB);
}
IndexedVector3 avgAxis = kAxisAInv * axisA + kAxisBInv * axisB;
if (bDoTorque && avgAxis.LengthSquared() > MathUtil.SIMD_EPSILON)
{
avgAxis.Normalize();
kAxisAInv = GetRigidBodyA().ComputeAngularImpulseDenominator(ref avgAxis);
kAxisBInv = GetRigidBodyB().ComputeAngularImpulseDenominator(ref avgAxis);
float kInvCombined = kAxisAInv + kAxisBInv;
IndexedVector3 impulse = (kAxisAInv * dOmegaA - kAxisBInv * dOmegaB) /
(kInvCombined * kInvCombined);
if (m_maxMotorImpulse >= 0)
{
float fMaxImpulse = m_maxMotorImpulse;
if (m_bNormalizedMotorStrength)
fMaxImpulse = fMaxImpulse/kAxisAInv;
IndexedVector3 newUnclampedAccImpulse = m_accMotorImpulse + impulse;
float newUnclampedMag = newUnclampedAccImpulse.Length();
if (newUnclampedMag > fMaxImpulse)
{
newUnclampedAccImpulse.Normalize();
newUnclampedAccImpulse *= fMaxImpulse;
impulse = newUnclampedAccImpulse - m_accMotorImpulse;
}
m_accMotorImpulse += impulse;
}
float impulseMag = impulse.Length();
IndexedVector3 impulseAxis = impulse / impulseMag;
bodyA.InternalApplyImpulse(new IndexedVector3(0,0,0), m_rbA.GetInvInertiaTensorWorld()*impulseAxis, impulseMag,null);
bodyB.InternalApplyImpulse(new IndexedVector3(0,0,0), m_rbB.GetInvInertiaTensorWorld()*impulseAxis, -impulseMag,null);
}
}
else if (m_damping > MathUtil.SIMD_EPSILON) // no motor: do a little damping
{
IndexedVector3 angVelA = IndexedVector3.Zero; bodyA.InternalGetAngularVelocity(ref angVelA);
IndexedVector3 angVelB= IndexedVector3.Zero; bodyB.InternalGetAngularVelocity(ref angVelB);
IndexedVector3 relVel = angVelB - angVelA;
if (relVel.LengthSquared() > MathUtil.SIMD_EPSILON)
{
IndexedVector3 relVelAxis = relVel.Normalized();
float m_kDamping = 1.0f /
(GetRigidBodyA().ComputeAngularImpulseDenominator(ref relVelAxis) +
GetRigidBodyB().ComputeAngularImpulseDenominator(ref relVelAxis));
IndexedVector3 impulse = m_damping * m_kDamping * relVel;
float impulseMag = impulse.Length();
IndexedVector3 impulseAxis = impulse / impulseMag;
bodyA.InternalApplyImpulse(new IndexedVector3(0,0,0), m_rbA.GetInvInertiaTensorWorld()*impulseAxis, impulseMag,null);
bodyB.InternalApplyImpulse(new IndexedVector3(0,0,0), m_rbB.GetInvInertiaTensorWorld()*impulseAxis, -impulseMag,null);
}
}
// joint limits
{
///solve angular part
IndexedVector3 angVelA = IndexedVector3.Zero;
bodyA.InternalGetAngularVelocity(ref angVelA);
IndexedVector3 angVelB= IndexedVector3.Zero;
bodyB.InternalGetAngularVelocity(ref angVelB);
// solve swing limit
if (m_solveSwingLimit)
{
float amplitude = m_swingLimitRatio * m_swingCorrection*m_biasFactor/timeStep;
float relSwingVel = (angVelB - angVelA).Dot(ref m_swingAxis);
if (relSwingVel > 0)
amplitude += m_swingLimitRatio * relSwingVel * m_relaxationFactor;
float impulseMag = amplitude * m_kSwing;
// Clamp the accumulated impulse
float temp = m_accSwingLimitImpulse;
m_accSwingLimitImpulse = Math.Max(m_accSwingLimitImpulse + impulseMag, 0.0f);
impulseMag = m_accSwingLimitImpulse - temp;
IndexedVector3 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)
{
IndexedVector3 impulseTwistCouple = impulse.Dot(ref m_twistAxisA) * m_twistAxisA;
IndexedVector3 impulseNoTwistCouple = impulse - impulseTwistCouple;
impulse = impulseNoTwistCouple;
}
impulseMag = impulse.Length();
IndexedVector3 noTwistSwingAxis = impulse / impulseMag;
bodyA.InternalApplyImpulse(new IndexedVector3(0,0,0), m_rbA.GetInvInertiaTensorWorld()*noTwistSwingAxis, impulseMag,null);
bodyB.InternalApplyImpulse(new IndexedVector3(0,0,0), m_rbB.GetInvInertiaTensorWorld()*noTwistSwingAxis, -impulseMag,null);
}
// solve twist limit
if (m_solveTwistLimit)
{
float amplitude = m_twistLimitRatio * m_twistCorrection*m_biasFactor/timeStep;
float relTwistVel = (angVelB - angVelA).Dot( ref m_twistAxis );
if (relTwistVel > 0) // only damp when moving towards limit (m_twistAxis flipping is important)
amplitude += m_twistLimitRatio * relTwistVel * m_relaxationFactor;
float impulseMag = amplitude * m_kTwist;
// Clamp the accumulated impulse
float temp = m_accTwistLimitImpulse;
m_accTwistLimitImpulse = Math.Max(m_accTwistLimitImpulse + impulseMag, 0.0f );
impulseMag = m_accTwistLimitImpulse - temp;
IndexedVector3 impulse = m_twistAxis * impulseMag;
bodyA.InternalApplyImpulse(new IndexedVector3(0,0,0), m_rbA.GetInvInertiaTensorWorld()*m_twistAxis,impulseMag,null);
bodyB.InternalApplyImpulse(new IndexedVector3(0,0,0), m_rbB.GetInvInertiaTensorWorld()*m_twistAxis,-impulseMag,null);
}
}
}
}
