///<summary>
/// Performs the frame's configuration step.
///</summary>
///<param name="dt">Timestep duration.</param>
public override void Update(float dt)
{
//Transform local axes into world space
Matrix3x3.Transform(ref localRestrictedAxis1, ref connectionA.orientationMatrix, out worldRestrictedAxis1);
Matrix3x3.Transform(ref localRestrictedAxis2, ref connectionA.orientationMatrix, out worldRestrictedAxis2);
Matrix3x3.Transform(ref localAxisAnchor, ref connectionA.orientationMatrix, out worldLineAnchor);
Vector3.Add(ref worldLineAnchor, ref connectionA.position, out worldLineAnchor);
Matrix3x3.Transform(ref localLineDirection, ref connectionA.orientationMatrix, out worldLineDirection);
//Transform local
Matrix3x3.Transform(ref localPoint, ref connectionB.orientationMatrix, out rB);
Vector3.Add(ref rB, ref connectionB.position, out worldPoint);
//Find the point on the line closest to the world point.
Vector3 offset;
Vector3.Subtract(ref worldPoint, ref worldLineAnchor, out offset);
float distanceAlongAxis;
Vector3.Dot(ref offset, ref worldLineDirection, out distanceAlongAxis);
Vector3 worldNearPoint;
Vector3.Multiply(ref worldLineDirection, distanceAlongAxis, out offset);
Vector3.Add(ref worldLineAnchor, ref offset, out worldNearPoint);
Vector3.Subtract(ref worldNearPoint, ref connectionA.position, out rA);
//Error
Vector3 error3D;
Vector3.Subtract(ref worldPoint, ref worldNearPoint, out error3D);
Vector3.Dot(ref error3D, ref worldRestrictedAxis1, out error.X);
Vector3.Dot(ref error3D, ref worldRestrictedAxis2, out error.Y);
float errorReduction;
springSettings.ComputeErrorReductionAndSoftness(dt, 1 / dt, out errorReduction, out softness);
float bias = -errorReduction;
biasVelocity.X = bias * error.X;
biasVelocity.Y = bias * error.Y;
//Ensure that the corrective velocity doesn't exceed the max.
float length = biasVelocity.LengthSquared();
if (length > maxCorrectiveVelocitySquared)
{
float multiplier = maxCorrectiveVelocity / (float)Math.Sqrt(length);
biasVelocity.X *= multiplier;
biasVelocity.Y *= multiplier;
}
//Set up the jacobians
Vector3.Cross(ref rA, ref worldRestrictedAxis1, out angularA1);
Vector3.Cross(ref worldRestrictedAxis1, ref rB, out angularB1);
Vector3.Cross(ref rA, ref worldRestrictedAxis2, out angularA2);
Vector3.Cross(ref worldRestrictedAxis2, ref rB, out angularB2);
float m11 = 0, m22 = 0, m1221 = 0;
float inverseMass;
Vector3 intermediate;
//Compute the effective mass matrix.
if (connectionA.isDynamic)
{
inverseMass = connectionA.inverseMass;
Matrix3x3.Transform(ref angularA1, ref connectionA.inertiaTensorInverse, out intermediate);
Vector3.Dot(ref intermediate, ref angularA1, out m11);
m11 += inverseMass;
Vector3.Dot(ref intermediate, ref angularA2, out m1221);
Matrix3x3.Transform(ref angularA2, ref connectionA.inertiaTensorInverse, out intermediate);
Vector3.Dot(ref intermediate, ref angularA2, out m22);
m22 += inverseMass;
}
#region Mass Matrix B
if (connectionB.isDynamic)
{
float extra;
inverseMass = connectionB.inverseMass;
Matrix3x3.Transform(ref angularB1, ref connectionB.inertiaTensorInverse, out intermediate);
Vector3.Dot(ref intermediate, ref angularB1, out extra);
m11 += inverseMass + extra;
Vector3.Dot(ref intermediate, ref angularB2, out extra);
m1221 += extra;
Matrix3x3.Transform(ref angularB2, ref connectionB.inertiaTensorInverse, out intermediate);
Vector3.Dot(ref intermediate, ref angularB2, out extra);
m22 += inverseMass + extra;
}
#endregion
negativeEffectiveMassMatrix.M11 = m11 + softness;
negativeEffectiveMassMatrix.M12 = m1221;
negativeEffectiveMassMatrix.M21 = m1221;
negativeEffectiveMassMatrix.M22 = m22 + softness;
Matrix2x2.Invert(ref negativeEffectiveMassMatrix, out negativeEffectiveMassMatrix);
Matrix2x2.Negate(ref negativeEffectiveMassMatrix, out negativeEffectiveMassMatrix);
}
///<summary>
/// Performs the frame's configuration step.
///</summary>
///<param name="dt">Timestep duration.</param>
public override void Update(float dt)
{
Matrix3x3.Transform(ref localAxisA, ref connectionA.orientationMatrix, out worldAxisA);
Matrix3x3.Transform(ref localAxisB, ref connectionB.orientationMatrix, out worldAxisB);
Matrix3x3.Transform(ref localConstrainedAxis1, ref connectionA.orientationMatrix, out worldConstrainedAxis1);
Matrix3x3.Transform(ref localConstrainedAxis2, ref connectionA.orientationMatrix, out worldConstrainedAxis2);
Vector3 error;
Vector3.Cross(ref worldAxisA, ref worldAxisB, out error);
Vector3.Dot(ref error, ref worldConstrainedAxis1, out this.error.X);
Vector3.Dot(ref error, ref worldConstrainedAxis2, out this.error.Y);
float errorReduction;
springSettings.ComputeErrorReductionAndSoftness(dt, 1 / dt, out errorReduction, out softness);
errorReduction = -errorReduction;
biasVelocity.X = errorReduction * this.error.X;
biasVelocity.Y = errorReduction * this.error.Y;
//Ensure that the corrective velocity doesn't exceed the max.
float length = biasVelocity.LengthSquared();
if (length > maxCorrectiveVelocitySquared)
{
float multiplier = maxCorrectiveVelocity / (float)Math.Sqrt(length);
biasVelocity.X *= multiplier;
biasVelocity.Y *= multiplier;
}
Vector3 axis1I, axis2I;
if (connectionA.isDynamic && connectionB.isDynamic)
{
Matrix3x3 inertiaTensorSum;
Matrix3x3.Add(ref connectionA.inertiaTensorInverse, ref connectionB.inertiaTensorInverse, out inertiaTensorSum);
Matrix3x3.Transform(ref worldConstrainedAxis1, ref inertiaTensorSum, out axis1I);
Matrix3x3.Transform(ref worldConstrainedAxis2, ref inertiaTensorSum, out axis2I);
}
else if (connectionA.isDynamic && !connectionB.isDynamic)
{
Matrix3x3.Transform(ref worldConstrainedAxis1, ref connectionA.inertiaTensorInverse, out axis1I);
Matrix3x3.Transform(ref worldConstrainedAxis2, ref connectionA.inertiaTensorInverse, out axis2I);
}
else if (!connectionA.isDynamic && connectionB.isDynamic)
{
Matrix3x3.Transform(ref worldConstrainedAxis1, ref connectionB.inertiaTensorInverse, out axis1I);
Matrix3x3.Transform(ref worldConstrainedAxis2, ref connectionB.inertiaTensorInverse, out axis2I);
}
else
{
throw new InvalidOperationException("Cannot constrain two kinematic bodies.");
}
Vector3.Dot(ref axis1I, ref worldConstrainedAxis1, out effectiveMassMatrix.M11);
Vector3.Dot(ref axis1I, ref worldConstrainedAxis2, out effectiveMassMatrix.M12);
Vector3.Dot(ref axis2I, ref worldConstrainedAxis1, out effectiveMassMatrix.M21);
Vector3.Dot(ref axis2I, ref worldConstrainedAxis2, out effectiveMassMatrix.M22);
effectiveMassMatrix.M11 += softness;
effectiveMassMatrix.M22 += softness;
Matrix2x2.Invert(ref effectiveMassMatrix, out effectiveMassMatrix);
Matrix2x2.Negate(ref effectiveMassMatrix, out effectiveMassMatrix);
}
/// <summary>
/// Gets the mass matrix of the constraint.
/// </summary>
/// <param name="massMatrix">Constraint's mass matrix.</param>
public void GetMassMatrix(out Matrix2x2 massMatrix)
{
Matrix2x2.Negate(ref negativeEffectiveMassMatrix, out massMatrix);
}
