protected internal override void UpdateJacobiansAndVelocityBias()
{
linearJacobianA = Matrix3x3.Identity;
//The jacobian entries are is [ La, Aa, -Lb, -Ab ] because the relative velocity is computed using A-B. So, negate B's jacobians!
linearJacobianB = new Matrix3x3 {
M11 = -1, M22 = -1, M33 = -1
};
System.Numerics.Vector3 rA;
QuaternionEx.Transform(ref LocalOffsetA, ref ConnectionA.Orientation, out rA);
Matrix3x3.CreateCrossProduct(ref rA, out angularJacobianA);
//Transposing a skew-symmetric matrix is equivalent to negating it.
Matrix3x3.Transpose(ref angularJacobianA, out angularJacobianA);
System.Numerics.Vector3 worldPositionA;
Vector3Ex.Add(ref ConnectionA.Position, ref rA, out worldPositionA);
System.Numerics.Vector3 rB;
QuaternionEx.Transform(ref LocalOffsetB, ref ConnectionB.Orientation, out rB);
Matrix3x3.CreateCrossProduct(ref rB, out angularJacobianB);
System.Numerics.Vector3 worldPositionB;
Vector3Ex.Add(ref ConnectionB.Position, ref rB, out worldPositionB);
System.Numerics.Vector3 linearError;
Vector3Ex.Subtract(ref worldPositionB, ref worldPositionA, out linearError);
Vector3Ex.Multiply(ref linearError, errorCorrectionFactor, out velocityBias);
}
protected internal override void UpdateJacobiansAndVelocityBias()
{
linearJacobian = Matrix3x3.Identity;
System.Numerics.Vector3 r;
QuaternionEx.Transform(ref LocalOffset, ref TargetBone.Orientation, out r);
Matrix3x3.CreateCrossProduct(ref r, out angularJacobian);
//Transposing a skew symmetric matrix is equivalent to negating it.
Matrix3x3.Transpose(ref angularJacobian, out angularJacobian);
System.Numerics.Vector3 worldPosition;
Vector3Ex.Add(ref TargetBone.Position, ref r, out worldPosition);
//Error is in world space.
System.Numerics.Vector3 linearError;
Vector3Ex.Subtract(ref TargetPosition, ref worldPosition, out linearError);
//This is equivalent to projecting the error onto the linear jacobian. The linear jacobian just happens to be the identity matrix!
Vector3Ex.Multiply(ref linearError, errorCorrectionFactor, out velocityBias);
}
public void Do()
{
Matrix3x3.CreateCrossProduct(v, out var skew);
Matrix3x3.MultiplyTransposed(skew, symmetric, out var intermediate);
Matrix3x3.Multiply(intermediate, skew, out result);
}
/// <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 System.Numerics.Matrix4x4
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);
Vector3Ex.Add(ref connectionB.position, ref worldOffsetB, out error);
Vector3Ex.Subtract(ref error, ref connectionA.position, out error);
Vector3Ex.Subtract(ref error, ref worldOffsetA, out error);
Vector3Ex.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);
}
