public override void Update(float dt)
{
Matrix3x3.Transform(ref localPlaneNormal, ref connectionA.orientationMatrix, out worldPlaneNormal);
Matrix3x3.Transform(ref localPlaneAnchor, ref connectionA.orientationMatrix, out worldPlaneAnchor);
Vector3.Add(ref worldPlaneAnchor, ref connectionA.position, out worldPlaneAnchor);
Matrix3x3.Transform(ref localPointAnchor, ref connectionB.orientationMatrix, out rB);
Vector3.Add(ref rB, ref connectionB.position, out worldPointAnchor);
//Find rA and rB.
//So find the closest point on the plane to worldPointAnchor.
float pointDistance, planeDistance;
Vector3.Dot(ref worldPointAnchor, ref worldPlaneNormal, out pointDistance);
Vector3.Dot(ref worldPlaneAnchor, ref worldPlaneNormal, out planeDistance);
float distanceChange = planeDistance - pointDistance;
Vector3 closestPointOnPlane;
Vector3.Multiply(ref worldPlaneNormal, distanceChange, out closestPointOnPlane);
Vector3.Add(ref closestPointOnPlane, ref worldPointAnchor, out closestPointOnPlane);
Vector3.Subtract(ref closestPointOnPlane, ref connectionA.position, out rA);
Vector3.Cross(ref rA, ref worldPlaneNormal, out rAcrossN);
Vector3.Cross(ref rB, ref worldPlaneNormal, out rBcrossN);
Vector3.Negate(ref rBcrossN, out rBcrossN);
Vector3 offset;
Vector3.Subtract(ref worldPointAnchor, ref closestPointOnPlane, out offset);
Vector3.Dot(ref offset, ref worldPlaneNormal, out error);
float errorReduction;
springSettings.ComputeErrorReductionAndSoftness(dt, out errorReduction, out softness);
biasVelocity = MathHelper.Clamp(-errorReduction * error, -maxCorrectiveVelocity, maxCorrectiveVelocity);
if (connectionA.IsDynamic && connectionB.IsDynamic)
{
Vector3 IrACrossN, IrBCrossN;
Matrix3x3.Transform(ref rAcrossN, ref connectionA.inertiaTensorInverse, out IrACrossN);
Matrix3x3.Transform(ref rBcrossN, ref connectionB.inertiaTensorInverse, out IrBCrossN);
float angularA, angularB;
Vector3.Dot(ref rAcrossN, ref IrACrossN, out angularA);
Vector3.Dot(ref rBcrossN, ref IrBCrossN, out angularB);
negativeEffectiveMass = connectionA.inverseMass + connectionB.inverseMass + angularA + angularB;
negativeEffectiveMass = -1 / (negativeEffectiveMass + softness);
}
else if (connectionA.IsDynamic && !connectionB.IsDynamic)
{
Vector3 IrACrossN;
Matrix3x3.Transform(ref rAcrossN, ref connectionA.inertiaTensorInverse, out IrACrossN);
float angularA;
Vector3.Dot(ref rAcrossN, ref IrACrossN, out angularA);
negativeEffectiveMass = connectionA.inverseMass + angularA;
negativeEffectiveMass = -1 / (negativeEffectiveMass + softness);
}
else if (!connectionA.IsDynamic && connectionB.IsDynamic)
{
Vector3 IrBCrossN;
Matrix3x3.Transform(ref rBcrossN, ref connectionB.inertiaTensorInverse, out IrBCrossN);
float angularB;
Vector3.Dot(ref rBcrossN, ref IrBCrossN, out angularB);
negativeEffectiveMass = connectionB.inverseMass + angularB;
negativeEffectiveMass = -1 / (negativeEffectiveMass + softness);
}
else
negativeEffectiveMass = 0;
}
