/// <summary>
/// Modifies a contribution using a transform, position, and weight.
/// </summary>
/// <param name="transform">Transform to use to modify the contribution.</param>
/// <param name="center">Center to use to modify the contribution.</param>
/// <param name="baseContribution">Original unmodified contribution.</param>
/// <param name="weight">Weight of the contribution.</param>
/// <param name="contribution">Transformed contribution.</param>
public static void TransformContribution(ref RigidTransform transform, ref Vector3 center, ref Matrix3x3 baseContribution, float weight, out Matrix3x3 contribution)
{
Matrix3x3 rotation;
Matrix3x3.CreateFromQuaternion(ref transform.Orientation, out rotation);
Matrix3x3 temp;
//Do angular transformed contribution first...
Matrix3x3.MultiplyTransposed(ref rotation, ref baseContribution, out temp);
Matrix3x3.Multiply(ref temp, ref rotation, out temp);
contribution = temp;
//Now add in the offset from the origin.
Vector3 offset;
Vector3.Subtract(ref transform.Position, ref center, out offset);
Matrix3x3 innerProduct;
Matrix3x3.CreateScale(offset.LengthSquared(), out innerProduct);
Matrix3x3 outerProduct;
Matrix3x3.CreateOuterProduct(ref offset, ref offset, out outerProduct);
Matrix3x3.Subtract(ref innerProduct, ref outerProduct, out temp);
Matrix3x3.Add(ref contribution, ref temp, out contribution);
Matrix3x3.Multiply(ref contribution, weight, out contribution);
}
/// <summary>
/// Calculates necessary information for velocity solving.
/// Called by preStep(float dt)
/// </summary>
/// <param name="dt">Time in seconds since the last update.</param>
public override void Update(float dt)
{
Matrix3x3.Transform(ref localAnchorA, ref connectionA.orientationMatrix, out worldOffsetA);
Matrix3x3.Transform(ref localAnchorB, ref connectionB.orientationMatrix, out worldOffsetB);
float errorReductionParameter;
springSettings.ComputeErrorReductionAndSoftness(dt, 1 / dt, out errorReductionParameter, out softness);
//Mass Matrix
Matrix3x3 k;
Matrix3x3 linearComponent;
Matrix3x3.CreateCrossProduct(ref worldOffsetA, out rACrossProduct);
Matrix3x3.CreateCrossProduct(ref worldOffsetB, out rBCrossProduct);
if (connectionA.isDynamic && connectionB.isDynamic)
{
Matrix3x3.CreateScale(connectionA.inverseMass + connectionB.inverseMass, out linearComponent);
Matrix3x3 angularComponentA, angularComponentB;
Matrix3x3.Multiply(ref rACrossProduct, ref connectionA.inertiaTensorInverse, out angularComponentA);
Matrix3x3.Multiply(ref rBCrossProduct, ref connectionB.inertiaTensorInverse, out angularComponentB);
Matrix3x3.Multiply(ref angularComponentA, ref rACrossProduct, out angularComponentA);
Matrix3x3.Multiply(ref angularComponentB, ref rBCrossProduct, out angularComponentB);
Matrix3x3.Subtract(ref linearComponent, ref angularComponentA, out k);
Matrix3x3.Subtract(ref k, ref angularComponentB, out k);
}
else if (connectionA.isDynamic && !connectionB.isDynamic)
{
Matrix3x3.CreateScale(connectionA.inverseMass, out linearComponent);
Matrix3x3 angularComponentA;
Matrix3x3.Multiply(ref rACrossProduct, ref connectionA.inertiaTensorInverse, out angularComponentA);
Matrix3x3.Multiply(ref angularComponentA, ref rACrossProduct, out angularComponentA);
Matrix3x3.Subtract(ref linearComponent, ref angularComponentA, out k);
}
else if (!connectionA.isDynamic && connectionB.isDynamic)
{
Matrix3x3.CreateScale(connectionB.inverseMass, out linearComponent);
Matrix3x3 angularComponentB;
Matrix3x3.Multiply(ref rBCrossProduct, ref connectionB.inertiaTensorInverse, out angularComponentB);
Matrix3x3.Multiply(ref angularComponentB, ref rBCrossProduct, out angularComponentB);
Matrix3x3.Subtract(ref linearComponent, ref angularComponentB, out k);
}
else
{
throw new InvalidOperationException("Cannot constrain two kinematic bodies.");
}
k.M11 += softness;
k.M22 += softness;
k.M33 += softness;
Matrix3x3.Invert(ref k, out massMatrix);
Vector3.Add(ref connectionB.position, ref worldOffsetB, out error);
Vector3.Subtract(ref error, ref connectionA.position, out error);
Vector3.Subtract(ref error, ref worldOffsetA, out error);
Vector3.Multiply(ref error, -errorReductionParameter, out biasVelocity);
//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;
biasVelocity.Z *= multiplier;
}
}
///<summary>
/// Performs the frame's configuration step.
///</summary>
///<param name="dt">Timestep duration.</param>
public override void Update(float dt)
{
//Transform point into world space.
Matrix3x3.Transform(ref localPoint, ref entity.orientationMatrix, out r);
Vector3.Add(ref r, ref entity.position, out worldPoint);
float updateRate = 1 / dt;
if (settings.mode == MotorMode.Servomechanism)
{
Vector3.Subtract(ref settings.servo.goal, ref worldPoint, out error);
float separationDistance = error.Length();
if (separationDistance > Toolbox.BigEpsilon)
{
float errorReduction;
settings.servo.springSettings.ComputeErrorReductionAndSoftness(dt, updateRate, out errorReduction, out usedSoftness);
//The rate of correction can be based on a constant correction velocity as well as a 'spring like' correction velocity.
//The constant correction velocity could overshoot the destination, so clamp it.
float correctionSpeed = MathHelper.Min(settings.servo.baseCorrectiveSpeed, separationDistance * updateRate) +
separationDistance * errorReduction;
Vector3.Multiply(ref error, correctionSpeed / separationDistance, out biasVelocity);
//Ensure that the corrective velocity doesn't exceed the max.
float length = biasVelocity.LengthSquared();
if (length > settings.servo.maxCorrectiveVelocitySquared)
{
float multiplier = settings.servo.maxCorrectiveVelocity / (float)Math.Sqrt(length);
biasVelocity.X *= multiplier;
biasVelocity.Y *= multiplier;
biasVelocity.Z *= multiplier;
}
}
else
{
//Wouldn't want to use a bias from an earlier frame.
biasVelocity = new Vector3();
}
}
else
{
usedSoftness = settings.velocityMotor.softness * updateRate;
biasVelocity = settings.velocityMotor.goalVelocity;
error = Vector3.Zero;
}
//Compute the maximum force that can be applied this frame.
ComputeMaxForces(settings.maximumForce, dt);
//COMPUTE EFFECTIVE MASS MATRIX
//Transforms a change in velocity to a change in momentum when multiplied.
Matrix3x3 linearComponent;
Matrix3x3.CreateScale(entity.inverseMass, out linearComponent);
Matrix3x3 rACrossProduct;
Matrix3x3.CreateCrossProduct(ref r, out rACrossProduct);
Matrix3x3 angularComponentA;
Matrix3x3.Multiply(ref rACrossProduct, ref entity.inertiaTensorInverse, out angularComponentA);
Matrix3x3.Multiply(ref angularComponentA, ref rACrossProduct, out angularComponentA);
Matrix3x3.Subtract(ref linearComponent, ref angularComponentA, out effectiveMassMatrix);
effectiveMassMatrix.M11 += usedSoftness;
effectiveMassMatrix.M22 += usedSoftness;
effectiveMassMatrix.M33 += usedSoftness;
Matrix3x3.Invert(ref effectiveMassMatrix, out effectiveMassMatrix);
}
