public override void Update(float dt)
{
//Transform the anchors and offsets into world space.
Matrix3x3.Transform(ref localAnchorA, ref connectionA.orientationMatrix, out offsetA);
Matrix3x3.Transform(ref localAnchorB, ref connectionB.orientationMatrix, out offsetB);
Vector3.Add(ref connectionA.position, ref offsetA, out anchorA);
Vector3.Add(ref connectionB.position, ref offsetB, out anchorB);
//Compute the distance.
Vector3 separation;
Vector3.Subtract(ref anchorB, ref anchorA, out separation);
float distance = separation.Length();
if (distance < maximumLength && distance > minimumLength)
{
isActiveInSolver = false;
accumulatedImpulse = 0;
error = 0;
isLimitActive = false;
return;
}
isLimitActive = true;
//Compute jacobians
if (distance > maximumLength)
{
//If it's beyond the max, all of the jacobians are reversed compared to what they are when it's below the min.
if (distance > Toolbox.Epsilon)
{
jLinearA.X = separation.X / distance;
jLinearA.Y = separation.Y / distance;
jLinearA.Z = separation.Z / distance;
}
else
jLinearB = Toolbox.ZeroVector;
jLinearB.X = -jLinearA.X;
jLinearB.Y = -jLinearA.Y;
jLinearB.Z = -jLinearA.Z;
Vector3.Cross(ref jLinearA, ref offsetA, out jAngularA);
//Still need to negate angular A. It's done after the effective mass matrix.
Vector3.Cross(ref jLinearA, ref offsetB, out jAngularB);
}
else
{
if (distance > Toolbox.Epsilon)
{
jLinearB.X = separation.X / distance;
jLinearB.Y = separation.Y / distance;
jLinearB.Z = separation.Z / distance;
}
else
jLinearB = Toolbox.ZeroVector;
jLinearA.X = -jLinearB.X;
jLinearA.Y = -jLinearB.Y;
jLinearA.Z = -jLinearB.Z;
Vector3.Cross(ref offsetA, ref jLinearB, out jAngularA);
//Still need to negate angular A. It's done after the effective mass matrix.
Vector3.Cross(ref offsetB, ref jLinearB, out jAngularB);
}
//Debug.WriteLine("BiasVelocity: " + biasVelocity);
//Compute effective mass matrix
if (connectionA.isDynamic && connectionB.isDynamic)
{
Vector3 aAngular;
Matrix3x3.Transform(ref jAngularA, ref connectionA.localInertiaTensorInverse, out aAngular);
Vector3.Cross(ref aAngular, ref offsetA, out aAngular);
Vector3 bAngular;
Matrix3x3.Transform(ref jAngularB, ref connectionB.localInertiaTensorInverse, out bAngular);
Vector3.Cross(ref bAngular, ref offsetB, out bAngular);
Vector3.Add(ref aAngular, ref bAngular, out aAngular);
Vector3.Dot(ref aAngular, ref jLinearB, out velocityToImpulse);
velocityToImpulse += connectionA.inverseMass + connectionB.inverseMass;
}
else if (connectionA.isDynamic)
{
Vector3 aAngular;
Matrix3x3.Transform(ref jAngularA, ref connectionA.localInertiaTensorInverse, out aAngular);
Vector3.Cross(ref aAngular, ref offsetA, out aAngular);
Vector3.Dot(ref aAngular, ref jLinearB, out velocityToImpulse);
velocityToImpulse += connectionA.inverseMass;
}
else if (connectionB.isDynamic)
{
Vector3 bAngular;
Matrix3x3.Transform(ref jAngularB, ref connectionB.localInertiaTensorInverse, out bAngular);
Vector3.Cross(ref bAngular, ref offsetB, out bAngular);
Vector3.Dot(ref bAngular, ref jLinearB, out velocityToImpulse);
velocityToImpulse += connectionB.inverseMass;
}
else
{
//No point in trying to solve with two kinematics.
isActiveInSolver = false;
accumulatedImpulse = 0;
return;
}
float errorReduction;
springSettings.ComputeErrorReductionAndSoftness(dt, out errorReduction, out softness);
velocityToImpulse = 1 / (softness + velocityToImpulse);
//Finish computing jacobian; it's down here as an optimization (since it didn't need to be negated in mass matrix)
jAngularA.X = -jAngularA.X;
jAngularA.Y = -jAngularA.Y;
jAngularA.Z = -jAngularA.Z;
//Compute bias velocity
if (distance > maximumLength)
error = Math.Max(0, distance - maximumLength - Margin);
else
error = Math.Max(0, minimumLength - Margin - distance);
biasVelocity = Math.Min(errorReduction * error, maxCorrectiveVelocity);
if (bounciness > 0)
{
//Compute currently relative velocity for bounciness.
float relativeVelocity, dot;
Vector3.Dot(ref jLinearA, ref connectionA.linearVelocity, out relativeVelocity);
Vector3.Dot(ref jAngularA, ref connectionA.angularVelocity, out dot);
relativeVelocity += dot;
Vector3.Dot(ref jLinearB, ref connectionB.linearVelocity, out dot);
relativeVelocity += dot;
Vector3.Dot(ref jAngularB, ref connectionB.angularVelocity, out dot);
relativeVelocity += dot;
if (-relativeVelocity > bounceVelocityThreshold)
biasVelocity = Math.Max(biasVelocity, -relativeVelocity * bounciness);
}
}
