/// <summary>
/// UpdateController
/// </summary>
/// <param name="dt"></param>
public override void UpdateController(float dt)
{
if (body0 == null || body1 == null)
{
return;
}
//Assert(0 != mBody0);
//Assert(0 != mBody1);
if (damping > 0.0f)
{
// Some hinges can bend in wonky ways. Derive the effective hinge axis
// using the relative rotation of the bodies.
Vector3 hingeAxis = body1.AngularVelocity - body0.AngularVelocity;
JiggleMath.NormalizeSafe(ref hingeAxis);
float angRot1;//
Vector3.Dot(ref body0.transformRate.AngularVelocity, ref hingeAxis, out angRot1);
float angRot2;
Vector3.Dot(ref body1.transformRate.AngularVelocity, ref hingeAxis, out angRot2);
float avAngRot = 0.5f * (angRot1 + angRot2);
float frac = 1.0f - damping;
float newAngRot1 = avAngRot + (angRot1 - avAngRot) * frac;
float newAngRot2 = avAngRot + (angRot2 - avAngRot) * frac;
Vector3 newAngVel1;// = body0.AngVel + (newAngRot1 - angRot1) * hingeAxis;
Vector3.Multiply(ref hingeAxis, newAngRot1 - angRot1, out newAngVel1);
Vector3.Add(ref newAngVel1, ref body0.transformRate.AngularVelocity, out newAngVel1);
Vector3 newAngVel2;// = body1.AngVel + (newAngRot2 - angRot2) * hingeAxis;
Vector3.Multiply(ref hingeAxis, newAngRot2 - angRot2, out newAngVel2);
Vector3.Add(ref newAngVel2, ref body1.transformRate.AngularVelocity, out newAngVel2);
body0.AngularVelocity = newAngVel1;
body1.AngularVelocity = newAngVel2;
}
// the extra torque
if (extraTorque != 0.0f)
{
Vector3 torque1;// = extraTorque * Vector3.Transform(hingeAxis, body0.Orientation);
Vector3.TransformNormal(ref hingeAxis, ref body0.transform.Orientation, out torque1);
Vector3.Multiply(ref torque1, extraTorque, out torque1);
body0.AddWorldTorque(torque1);
body1.AddWorldTorque(-torque1);
}
}
public override void UpdateController(float dt)
{
if (body0 == null || body1 == null)
{
return;
}
if (damping > 0.0f)
{
var hingeAxis = body1.AngularVelocity - body0.AngularVelocity;
JiggleMath.NormalizeSafe(ref hingeAxis);
Vector3.Dot(ref body0.TransformRate.AngularVelocity, ref hingeAxis, out var angRot1);
Vector3.Dot(ref body1.TransformRate.AngularVelocity, ref hingeAxis, out var angRot2);
var avAngRot = 0.5f * (angRot1 + angRot2);
var frac = 1.0f - damping;
var newAngRot1 = avAngRot + (angRot1 - avAngRot) * frac;
var newAngRot2 = avAngRot + (angRot2 - avAngRot) * frac;
Vector3.Multiply(ref hingeAxis, newAngRot1 - angRot1, out var newAngVel1);
Vector3.Add(ref newAngVel1, ref body0.TransformRate.AngularVelocity, out newAngVel1);
Vector3.Multiply(ref hingeAxis, newAngRot2 - angRot2, out var newAngVel2);
Vector3.Add(ref newAngVel2, ref body1.TransformRate.AngularVelocity, out newAngVel2);
body0.AngularVelocity = newAngVel1;
body1.AngularVelocity = newAngVel2;
}
if (extraTorque != 0.0f)
{
Vector3.TransformNormal(ref hingeAxis, ref body0.Transform.Orientation, out var torque1);
Vector3.Multiply(ref torque1, extraTorque, out torque1);
body0.AddWorldTorque(torque1);
body1.AddWorldTorque(-torque1);
}
}