public override void SolveConstraint(float timeStep)
{
Vector3 pivotAInW = MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform);
Vector3 pivotBInW = MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform);
Vector3 normal = new Vector3();
for (int i = 0; i < 3; i++)
{
MathHelper.SetElement(ref normal, i, 1);
float jacDiagABInv = 1.0f / _jacobian[i].Diagonal;
Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition;
Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition;
Vector3 vel1 = RigidBodyA.GetVelocityInLocalPoint(rel_pos1);
Vector3 vel2 = RigidBodyB.GetVelocityInLocalPoint(rel_pos2);
Vector3 vel = vel1 - vel2;
float rel_vel = Vector3.Dot(normal, vel);
float depth = -Vector3.Dot((pivotAInW - pivotBInW), normal);
float impulse = depth * _setting.Tau / timeStep * jacDiagABInv - _setting.Damping * rel_vel * jacDiagABInv;
AppliedImpulse += impulse;
Vector3 impulseVector = normal * impulse;
RigidBodyA.ApplyImpulse(impulseVector, pivotAInW - RigidBodyA.CenterOfMassPosition);
RigidBodyB.ApplyImpulse(-impulseVector, pivotBInW - RigidBodyB.CenterOfMassPosition);
MathHelper.SetElement(ref normal, i, 0);
}
}
public override void SolveConstraint(float timeStep)
{
Vector3 pivotAInW = MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform);
Vector3 pivotBInW = MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform);
Vector3 normal = new Vector3(0, 0, 0);
float tau = 0.3f;
float damping = 1f;
//linear part
if (!_angularOnly)
{
for (int i = 0; i < 3; i++)
{
if (i == 0)
{
normal = new Vector3(1, 0, 0);
}
else if (i == 1)
{
normal = new Vector3(0, 1, 0);
}
else
{
normal = new Vector3(0, 0, 1);
}
float jacDiagABInv = 1f / _jac[i].Diagonal;
Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition;
Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition;
Vector3 vel1 = RigidBodyA.GetVelocityInLocalPoint(rel_pos1);
Vector3 vel2 = RigidBodyB.GetVelocityInLocalPoint(rel_pos2);
Vector3 vel = vel1 - vel2;
float rel_vel;
rel_vel = Vector3.Dot(normal, vel);
//positional error (zeroth order error)
float depth = -Vector3.Dot(pivotAInW - pivotBInW, normal); //this is the error projected on the normal
float impulse = depth * tau / timeStep * jacDiagABInv - damping * rel_vel * jacDiagABInv * damping;
AppliedImpulse += impulse;
Vector3 impulse_vector = normal * impulse;
RigidBodyA.ApplyImpulse(impulse_vector, pivotAInW - RigidBodyA.CenterOfMassPosition);
RigidBodyB.ApplyImpulse(-impulse_vector, pivotBInW - RigidBodyB.CenterOfMassPosition);
}
}
//solve angular part
// get axes in world space
Vector3 axisA = MathHelper.TransformNormal(_axisInA, RigidBodyA.CenterOfMassTransform);
Vector3 axisB = MathHelper.TransformNormal(_axisInB, RigidBodyB.CenterOfMassTransform);
Vector3 angVelA = RigidBodyA.AngularVelocity;
Vector3 angVelB = RigidBodyB.AngularVelocity;
Vector3 angVelAroundHingeAxisA = axisA * Vector3.Dot(axisA, angVelA);
Vector3 angVelAroundHingeAxisB = axisB * Vector3.Dot(axisB, angVelB);
Vector3 angAOrthog = angVelA - angVelAroundHingeAxisA;
Vector3 angBOrthog = angVelB - angVelAroundHingeAxisB;
Vector3 velrelOrthog = angAOrthog - angBOrthog;
//solve angular velocity correction
float relaxation = 1f;
float len = velrelOrthog.Length();
if (len > 0.00001f)
{
Vector3 normal2 = Vector3.Normalize(velrelOrthog);
float denom = RigidBodyA.ComputeAngularImpulseDenominator(normal2) +
RigidBodyB.ComputeAngularImpulseDenominator(normal2);
// scale for mass and relaxation
velrelOrthog *= (1f / denom) * 0.9f;
}
//solve angular positional correction
Vector3 angularError = -Vector3.Cross(axisA, axisB) * (1f / timeStep);
float len2 = angularError.Length();
if (len2 > 0.00001f)
{
Vector3 normal2 = Vector3.Normalize(angularError);
float denom2 = RigidBodyA.ComputeAngularImpulseDenominator(normal2) +
RigidBodyB.ComputeAngularImpulseDenominator(normal2);
angularError *= (1f / denom2) * relaxation;
}
RigidBodyA.ApplyTorqueImpulse(-velrelOrthog + angularError);
RigidBodyB.ApplyTorqueImpulse(velrelOrthog - angularError);
//apply motor
if (_enableAngularMotor)
{
//todo: add limits too
Vector3 angularLimit = Vector3.Zero;
Vector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB;
float projRelVel = Vector3.Dot(velrel, axisA);
float desiredMotorVel = _motorTargetVelocity;
float motorRelvel = desiredMotorVel - projRelVel;
float denom3 = RigidBodyA.ComputeAngularImpulseDenominator(axisA) +
RigidBodyB.ComputeAngularImpulseDenominator(axisA);
float unclippedMotorImpulse = (1f / denom3) * motorRelvel;
//todo: should clip against accumulated impulse
float clippedMotorImpulse = unclippedMotorImpulse > _maxMotorImpulse ? _maxMotorImpulse : unclippedMotorImpulse;
clippedMotorImpulse = clippedMotorImpulse < -_maxMotorImpulse ? -_maxMotorImpulse : clippedMotorImpulse;
Vector3 motorImp = clippedMotorImpulse * axisA;
RigidBodyA.ApplyTorqueImpulse(motorImp + angularLimit);
RigidBodyB.ApplyTorqueImpulse(-motorImp - angularLimit);
}
}