///<summary>
/// Constructs a new transformable shape.
///</summary>
///<param name="shape">Base shape to transform.</param>
///<param name="transform">Transform to use.</param>
public TransformableShape(ConvexShape shape, Matrix3x3 transform)
{
this.shape = shape;
this.transform = transform;
UpdateConvexShapeInfo();
}
///<summary>
/// Constructs a new transformable shape.
///</summary>
/// <param name="shape">Base shape to transform.</param>
/// <param name="transform">Transform to use.</param>
/// <param name="description">Cached information about the shape. Assumed to be correct; no extra processing or validation is performed.</param>
public TransformableShape(ConvexShape shape, Matrix3x3 transform, ConvexShapeDescription description)
{
this.shape = shape;
this.transform = transform;
UpdateConvexShapeInfo(description);
}
protected internal override void UpdateJacobiansAndVelocityBias()
{
linearJacobian = new Matrix3x3();
Vector3 boneAxis;
Quaternion.Transform(ref BoneLocalAxis, ref TargetBone.Orientation, out boneAxis);
Vector3 jacobian;
Vector3.Cross(ref boneAxis, ref PlaneNormal, out jacobian);
angularJacobian = new Matrix3x3
{
M11 = jacobian.X,
M12 = jacobian.Y,
M13 = jacobian.Z,
};
Vector3.Dot(ref boneAxis, ref PlaneNormal, out velocityBias.X);
velocityBias.X = -errorCorrectionFactor * velocityBias.X;
}
protected internal override void UpdateJacobiansAndVelocityBias()
{
linearJacobianA = linearJacobianB = new Matrix3x3();
angularJacobianA = new Matrix3x3 { M11 = 1, M22 = 1, M33 = 1 };
angularJacobianB = new Matrix3x3 { M11 = -1, M22 = -1, M33 = -1 };
//The error is computed using this equation:
//GoalRelativeOrientation * ConnectionA.Orientation * Error = ConnectionB.Orientation
//GoalRelativeOrientation is the original rotation from A to B in A's local space.
//Multiplying by A's orientation gives us where B *should* be.
//Of course, B won't be exactly where it should be after initialization.
//The Error component holds the difference between what is and what should be.
//Error = (GoalRelativeOrientation * ConnectionA.Orientation)^-1 * ConnectionB.Orientation
Quaternion bTarget;
Quaternion.Concatenate(ref GoalRelativeOrientation, ref ConnectionA.Orientation, out bTarget);
Quaternion bTargetConjugate;
Quaternion.Conjugate(ref bTarget, out bTargetConjugate);
Quaternion error;
Quaternion.Concatenate(ref bTargetConjugate, ref ConnectionB.Orientation, out error);
//Convert the error into an axis-angle vector usable for bias velocity.
float angle;
Vector3 axis;
Quaternion.GetAxisAngleFromQuaternion(ref error, out axis, out angle);
velocityBias.X = errorCorrectionFactor * axis.X * angle;
velocityBias.Y = errorCorrectionFactor * axis.Y * angle;
velocityBias.Z = errorCorrectionFactor * axis.Z * angle;
}
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 = new Matrix3x3();
Vector3 boneAxis;
Quaternion.Transform(ref BoneLocalFreeAxis, ref TargetBone.Orientation, out boneAxis);
angularJacobian = new Matrix3x3
{
M11 = constrainedAxis1.X,
M12 = constrainedAxis1.Y,
M13 = constrainedAxis1.Z,
M21 = constrainedAxis2.X,
M22 = constrainedAxis2.Y,
M23 = constrainedAxis2.Z
};
Vector3 error;
Vector3.Cross(ref boneAxis, ref freeAxis, out error);
Vector2 constraintSpaceError;
Vector3.Dot(ref error, ref constrainedAxis1, out constraintSpaceError.X);
Vector3.Dot(ref error, ref constrainedAxis2, out constraintSpaceError.Y);
velocityBias.X = errorCorrectionFactor * constraintSpaceError.X;
velocityBias.Y = errorCorrectionFactor * constraintSpaceError.Y;
}
protected internal override void UpdateJacobiansAndVelocityBias()
{
linearJacobian = new Matrix3x3();
angularJacobian = Matrix3x3.Identity;
//Error is in world space. It gets projected onto the jacobians later.
System.Numerics.Quaternion errorQuaternion;
QuaternionEx.Conjugate(ref TargetBone.Orientation, out errorQuaternion);
QuaternionEx.Multiply(ref TargetOrientation, ref errorQuaternion, out errorQuaternion);
float angle;
System.Numerics.Vector3 angularError;
QuaternionEx.GetAxisAngleFromQuaternion(ref errorQuaternion, out angularError, out angle);
Vector3Ex.Multiply(ref angularError, angle, out angularError);
//This is equivalent to projecting the error onto the angular jacobian. The angular jacobian just happens to be the identity matrix!
Vector3Ex.Multiply(ref angularError, errorCorrectionFactor, out velocityBias);
}
public MobileChunkShape(Vector3i csize, BlockInternal[] blocks, out Vector3 center)
{
Matrix3x3 boxMat = new BoxShape(csize.X, csize.Y, csize.Z).VolumeDistribution;
ChunkSize = csize;
Blocks = blocks;
double weightInv = 1f / blocks.Length;
center = new Vector3(csize.X / 2f, csize.Y / 2f, csize.Z / 2f);
// TODO: More accurately get center of weight based on which blocks are solid or not!?
Matrix3x3 volumeDistribution = new Matrix3x3();
RigidTransform transform = new RigidTransform(center);
Matrix3x3 contribution;
CompoundShape.TransformContribution(ref transform, ref center, ref boxMat, blocks.Length, out contribution);
Matrix3x3.Add(ref volumeDistribution, ref contribution, out volumeDistribution);
Matrix3x3.Multiply(ref volumeDistribution, weightInv, out volumeDistribution);
UpdateEntityShapeVolume(new EntityShapeVolumeDescription() { Volume = csize.X * csize.Y * csize.Z, VolumeDistribution = volumeDistribution });
Center = center;
}
protected internal override void UpdateJacobiansAndVelocityBias()
{
linearJacobianA = linearJacobianB = new Matrix3x3();
//There are two free axes and one restricted axis.
//The constraint attempts to keep the hinge axis attached to connection A and the twist axis attached to connection B perpendicular to each other.
//The restricted axis is the cross product between the twist and hinge axes.
Vector3 worldTwistAxis, worldHingeAxis;
Quaternion.Transform(ref LocalHingeAxis, ref ConnectionA.Orientation, out worldHingeAxis);
Quaternion.Transform(ref LocalTwistAxis, ref ConnectionB.Orientation, out worldTwistAxis);
Vector3 restrictedAxis;
Vector3.Cross(ref worldHingeAxis, ref worldTwistAxis, out restrictedAxis);
//Attempt to normalize the restricted axis.
float lengthSquared = restrictedAxis.LengthSquared();
if (lengthSquared > Toolbox.Epsilon)
{
Vector3.Divide(ref restrictedAxis, (float)Math.Sqrt(lengthSquared), out restrictedAxis);
}
else
{
restrictedAxis = new Vector3();
}
angularJacobianA = new Matrix3x3
{
M11 = restrictedAxis.X,
M12 = restrictedAxis.Y,
M13 = restrictedAxis.Z,
};
Matrix3x3.Negate(ref angularJacobianA, out angularJacobianB);
float error;
Vector3.Dot(ref worldHingeAxis, ref worldTwistAxis, out error);
error = (float)Math.Acos(MathHelper.Clamp(error, -1, 1)) - MathHelper.PiOver2;
velocityBias = new Vector3(errorCorrectionFactor * error, 0, 0);
}
protected internal override void UpdateJacobiansAndVelocityBias()
{
//This constraint doesn't consider linear motion.
linearJacobianA = linearJacobianB = new Matrix3x3();
//Compute the world axes.
Vector3 axisA, axisB;
Quaternion.Transform(ref LocalAxisA, ref ConnectionA.Orientation, out axisA);
Quaternion.Transform(ref LocalAxisB, ref ConnectionB.Orientation, out axisB);
float dot;
Vector3.Dot(ref axisA, ref axisB, out dot);
//Yes, we could avoid this acos here. Performance is not the highest goal of this system; the less tricks used, the easier it is to understand.
float angle = (float)Math.Acos(MathHelper.Clamp(dot, -1, 1));
//One angular DOF is constrained by this limit.
Vector3 hingeAxis;
Vector3.Cross(ref axisA, ref axisB, out hingeAxis);
angularJacobianA.M1 = hingeAxis;
hingeAxis.Invert( out angularJacobianB.M1 );
//Note how we've computed the jacobians despite the limit being potentially inactive.
//This is to enable 'speculative' limits.
if (angle >= maximumAngle)
{
velocityBias = new Vector3(errorCorrectionFactor * (angle - maximumAngle), 0, 0);
}
else
{
//The constraint is not yet violated. But, it may be- allow only as much motion as could occur without violating the constraint.
//Limits can't 'pull,' so this will not result in erroneous sticking.
velocityBias = new Vector3(angle - maximumAngle, 0, 0);
}
}
/// <summary>
/// Creates a 4x4 matrix from a 3x3 matrix.
/// </summary>
/// <param name="a">3x3 matrix.</param>
/// <returns>Created 4x4 matrix.</returns>
public static Matrix ToMatrix4X4(Matrix3x3 a)
{
#if !WINDOWS
Matrix b = new Matrix();
#else
Matrix b;
#endif
b.M11 = a.M11;
b.M12 = a.M12;
b.M13 = a.M13;
b.M21 = a.M21;
b.M22 = a.M22;
b.M23 = a.M23;
b.M31 = a.M31;
b.M32 = a.M32;
b.M33 = a.M33;
b.M44 = 1;
b.M14 = 0;
b.M24 = 0;
b.M34 = 0;
b.M41 = 0;
b.M42 = 0;
b.M43 = 0;
return b;
}
/// <summary>
/// Creates a 4x4 matrix from a 3x3 matrix.
/// </summary>
/// <param name="a">3x3 matrix.</param>
/// <param name="b">Created 4x4 matrix.</param>
public static void ToMatrix4X4(ref Matrix3x3 a, out Matrix b)
{
#if !WINDOWS
b = new Matrix();
#endif
b.M11 = a.M11;
b.M12 = a.M12;
b.M13 = a.M13;
b.M21 = a.M21;
b.M22 = a.M22;
b.M23 = a.M23;
b.M31 = a.M31;
b.M32 = a.M32;
b.M33 = a.M33;
b.M44 = 1;
b.M14 = 0;
b.M24 = 0;
b.M34 = 0;
b.M41 = 0;
b.M42 = 0;
b.M43 = 0;
}
/// <summary>
/// Multiplies the two matrices.
/// </summary>
/// <param name="a">First matrix to multiply.</param>
/// <param name="b">Second matrix to multiply.</param>
/// <param name="result">Product of the multiplication.</param>
public static void Multiply(ref Matrix3x3 a, ref Matrix3x2 b, out Matrix3x2 result)
{
float resultM11 = a.M11 * b.M11 + a.M12 * b.M21 + a.M13 * b.M31;
float resultM12 = a.M11 * b.M12 + a.M12 * b.M22 + a.M13 * b.M32;
float resultM21 = a.M21 * b.M11 + a.M22 * b.M21 + a.M23 * b.M31;
float resultM22 = a.M21 * b.M12 + a.M22 * b.M22 + a.M23 * b.M32;
float resultM31 = a.M31 * b.M11 + a.M32 * b.M21 + a.M33 * b.M31;
float resultM32 = a.M31 * b.M12 + a.M32 * b.M22 + a.M33 * b.M32;
result.M11 = resultM11;
result.M12 = resultM12;
result.M21 = resultM21;
result.M22 = resultM22;
result.M31 = resultM31;
result.M32 = resultM32;
}
///<summary>
/// Computes the volume contribution of a point.
///</summary>
///<param name="pointWeight">Weight of the point.</param>
///<param name="center">Location to use as the center for the purposes of computing the contribution.</param>
///<param name="p">Point to compute the contribution of.</param>
///<param name="contribution">Contribution of the point.</param>
public static void GetPointContribution(float pointWeight, ref Vector3 center, Vector3 p, out Matrix3x3 contribution)
{
Vector3.Subtract(ref p, ref center, out p);
float xx = pointWeight * p.X * p.X;
float yy = pointWeight * p.Y * p.Y;
float zz = pointWeight * p.Z * p.Z;
contribution.M11 = yy + zz;
contribution.M22 = xx + zz;
contribution.M33 = xx + yy;
contribution.M12 = -pointWeight * p.X * p.Y;
contribution.M13 = -pointWeight * p.X * p.Z;
contribution.M23 = -pointWeight * p.Y * p.Z;
contribution.M21 = contribution.M12;
contribution.M31 = contribution.M13;
contribution.M32 = contribution.M23;
}
/// <summary>
/// Updates the quaternion using RK4 integration.
/// </summary>
/// <param name="q">Quaternion to update.</param>
/// <param name="localInertiaTensorInverse">Local-space inertia tensor of the object being updated.</param>
/// <param name="angularMomentum">Angular momentum of the object.</param>
/// <param name="dt">Time since last frame, in seconds.</param>
/// <param name="newOrientation">New orientation quaternion.</param>
public static void UpdateOrientationRK4(ref Quaternion q, ref Matrix3x3 localInertiaTensorInverse, ref Vector3 angularMomentum, float dt, out Quaternion newOrientation)
{
//TODO: This is a little goofy
//Quaternion diff = differentiateQuaternion(ref q, ref localInertiaTensorInverse, ref angularMomentum);
Quaternion d1;
DifferentiateQuaternion(ref q, ref localInertiaTensorInverse, ref angularMomentum, out d1);
Quaternion s2;
Quaternion.Multiply(ref d1, dt * .5f, out s2);
Quaternion.Add(ref q, ref s2, out s2);
Quaternion d2;
DifferentiateQuaternion(ref s2, ref localInertiaTensorInverse, ref angularMomentum, out d2);
Quaternion s3;
Quaternion.Multiply(ref d2, dt * .5f, out s3);
Quaternion.Add(ref q, ref s3, out s3);
Quaternion d3;
DifferentiateQuaternion(ref s3, ref localInertiaTensorInverse, ref angularMomentum, out d3);
Quaternion s4;
Quaternion.Multiply(ref d3, dt, out s4);
Quaternion.Add(ref q, ref s4, out s4);
Quaternion d4;
DifferentiateQuaternion(ref s4, ref localInertiaTensorInverse, ref angularMomentum, out d4);
Quaternion.Multiply(ref d1, dt / 6, out d1);
Quaternion.Multiply(ref d2, dt / 3, out d2);
Quaternion.Multiply(ref d3, dt / 3, out d3);
Quaternion.Multiply(ref d4, dt / 6, out d4);
Quaternion added;
Quaternion.Add(ref q, ref d1, out added);
Quaternion.Add(ref added, ref d2, out added);
Quaternion.Add(ref added, ref d3, out added);
Quaternion.Add(ref added, ref d4, out added);
Quaternion.Normalize(ref added, out newOrientation);
}
/// <summary>
/// Constructs a quaternion from a rotation matrix.
/// </summary>
/// <param name="r">Rotation matrix to create the quaternion from.</param>
/// <param name="q">System.Numerics.Quaternion based on the rotation matrix.</param>
public static void CreateFromRotationMatrix(ref Matrix3x3 r, out System.Numerics.Quaternion q)
{
float trace = r.M11 + r.M22 + r.M33;
#if !WINDOWS
q = new System.Numerics.Quaternion();
#endif
if (trace >= 0)
{
var S = (float)Math.Sqrt(trace + 1.0) * 2; // S=4*qw
var inverseS = 1 / S;
q.W = 0.25f * S;
q.X = (r.M23 - r.M32) * inverseS;
q.Y = (r.M31 - r.M13) * inverseS;
q.Z = (r.M12 - r.M21) * inverseS;
}
else if ((r.M11 > r.M22) & (r.M11 > r.M33))
{
var S = (float)Math.Sqrt(1.0 + r.M11 - r.M22 - r.M33) * 2; // S=4*qx
var inverseS = 1 / S;
q.W = (r.M23 - r.M32) * inverseS;
q.X = 0.25f * S;
q.Y = (r.M21 + r.M12) * inverseS;
q.Z = (r.M31 + r.M13) * inverseS;
}
else if (r.M22 > r.M33)
{
var S = (float)Math.Sqrt(1.0 + r.M22 - r.M11 - r.M33) * 2; // S=4*qy
var inverseS = 1 / S;
q.W = (r.M31 - r.M13) * inverseS;
q.X = (r.M21 + r.M12) * inverseS;
q.Y = 0.25f * S;
q.Z = (r.M32 + r.M23) * inverseS;
}
else
{
var S = (float)Math.Sqrt(1.0 + r.M33 - r.M11 - r.M22) * 2; // S=4*qz
var inverseS = 1 / S;
q.W = (r.M12 - r.M21) * inverseS;
q.X = (r.M31 + r.M13) * inverseS;
q.Y = (r.M32 + r.M23) * inverseS;
q.Z = 0.25f * S;
}
}
protected internal override void ComputeEffectiveMass()
{
//For all constraints, the effective mass matrix is 1 / (J * M^-1 * JT).
//For two bone constraints, J has 4 3x3 matrices. M^-1 (W below) is a 12x12 matrix with 4 3x3 block diagonal matrices.
//To compute the whole denominator,
Matrix3x3 linearW;
Matrix3x3 linearA, angularA, linearB, angularB;
if (!ConnectionA.Pinned)
{
Matrix3x3.CreateScale(ConnectionA.inverseMass, out linearW);
Matrix3x3.Multiply(ref linearJacobianA, ref linearW, out linearA); //Compute J * M^-1 for linear component
Matrix3x3.MultiplyByTransposed(ref linearA, ref linearJacobianA, out linearA); //Compute (J * M^-1) * JT for linear component
Matrix3x3.Multiply(ref angularJacobianA, ref ConnectionA.inertiaTensorInverse, out angularA); //Compute J * M^-1 for angular component
Matrix3x3.MultiplyByTransposed(ref angularA, ref angularJacobianA, out angularA); //Compute (J * M^-1) * JT for angular component
}
else
{
//Treat pinned bones as if they have infinite inertia.
linearA = new Matrix3x3();
angularA = new Matrix3x3();
}
if (!ConnectionB.Pinned)
{
Matrix3x3.CreateScale(ConnectionB.inverseMass, out linearW);
Matrix3x3.Multiply(ref linearJacobianB, ref linearW, out linearB); //Compute J * M^-1 for linear component
Matrix3x3.MultiplyByTransposed(ref linearB, ref linearJacobianB, out linearB); //Compute (J * M^-1) * JT for linear component
Matrix3x3.Multiply(ref angularJacobianB, ref ConnectionB.inertiaTensorInverse, out angularB); //Compute J * M^-1 for angular component
Matrix3x3.MultiplyByTransposed(ref angularB, ref angularJacobianB, out angularB); //Compute (J * M^-1) * JT for angular component
}
else
{
//Treat pinned bones as if they have infinite inertia.
linearB = new Matrix3x3();
angularB = new Matrix3x3();
}
//A nice side effect of the block diagonal nature of M^-1 is that the above separated components are now combined into the complete denominator matrix by addition!
Matrix3x3.Add(ref linearA, ref angularA, out effectiveMass);
Matrix3x3.Add(ref effectiveMass, ref linearB, out effectiveMass);
Matrix3x3.Add(ref effectiveMass, ref angularB, out effectiveMass);
//Incorporate the constraint softness into the effective mass denominator. This pushes the matrix away from singularity.
//Softness will also be incorporated into the velocity solve iterations to complete the implementation.
if (effectiveMass.M1.X != 0)
effectiveMass.M1.X += softness;
if (effectiveMass.M2.Y != 0)
effectiveMass.M2.Y += softness;
if (effectiveMass.M3.Z != 0)
effectiveMass.M3.Z += softness;
//Invert! Takes us from J * M^-1 * JT to 1 / (J * M^-1 * JT).
Matrix3x3.AdaptiveInvert(ref effectiveMass, out effectiveMass);
}
///<summary>
/// Computes a volume distribution based on a bunch of point contributions.
///</summary>
///<param name="pointContributions">Point contributions to the volume distribution.</param>
///<param name="center">Location to use as the center for purposes of computing point contributions.</param>
///<returns>Volume distribution of the point contributions.</returns>
public static Matrix3x3 ComputeVolumeDistribution(RawList<Vector3> pointContributions, ref Vector3 center)
{
var volumeDistribution = new Matrix3x3();
float pointWeight = 1f / pointContributions.Count;
for (int i = 0; i < pointContributions.Count; i++)
{
Matrix3x3 contribution;
GetPointContribution(pointWeight, ref center, pointContributions[i], out contribution);
Matrix3x3.Add(ref volumeDistribution, ref contribution, out volumeDistribution);
}
return volumeDistribution;
}
/// <summary>
/// Inverts the given matix.
/// </summary>
/// <param name="matrix">Matrix to be inverted.</param>
/// <param name="result">Inverted matrix.</param>
public static void Invert(ref Matrix3x3 matrix, out Matrix3x3 result)
{
float determinantInverse = 1 / matrix.Determinant();
float m11 = (matrix.M22 * matrix.M33 - matrix.M23 * matrix.M32) * determinantInverse;
float m12 = (matrix.M13 * matrix.M32 - matrix.M33 * matrix.M12) * determinantInverse;
float m13 = (matrix.M12 * matrix.M23 - matrix.M22 * matrix.M13) * determinantInverse;
float m21 = (matrix.M23 * matrix.M31 - matrix.M21 * matrix.M33) * determinantInverse;
float m22 = (matrix.M11 * matrix.M33 - matrix.M13 * matrix.M31) * determinantInverse;
float m23 = (matrix.M13 * matrix.M21 - matrix.M11 * matrix.M23) * determinantInverse;
float m31 = (matrix.M21 * matrix.M32 - matrix.M22 * matrix.M31) * determinantInverse;
float m32 = (matrix.M12 * matrix.M31 - matrix.M11 * matrix.M32) * determinantInverse;
float m33 = (matrix.M11 * matrix.M22 - matrix.M12 * matrix.M21) * determinantInverse;
result.M11 = m11;
result.M12 = m12;
result.M13 = m13;
result.M21 = m21;
result.M22 = m22;
result.M23 = m23;
result.M31 = m31;
result.M32 = m32;
result.M33 = m33;
}
/// <summary>
/// Initializes the constraint for the current frame.
/// </summary>
/// <param name="dt">Time between frames.</param>
public override void Update(float dt)
{
basis.rotationMatrix = entity.orientationMatrix;
basis.ComputeWorldSpaceAxes();
float updateRate = 1 / dt;
if (settings.mode == MotorMode.Servomechanism) //Only need to do the bulk of this work if it's a servo.
{
Quaternion currentRelativeOrientation;
var worldTransform = basis.WorldTransform;
Quaternion.CreateFromRotationMatrix(ref worldTransform, out currentRelativeOrientation);
//Compute the relative orientation R' between R and the target relative orientation.
Quaternion errorOrientation;
Quaternion.Conjugate(ref currentRelativeOrientation, out errorOrientation);
Quaternion.Multiply(ref settings.servo.goal, ref errorOrientation, out errorOrientation);
float errorReduction;
settings.servo.springSettings.ComputeErrorReductionAndSoftness(dt, updateRate, out errorReduction, out usedSoftness);
//Turn this into an axis-angle representation.
Quaternion.GetAxisAngleFromQuaternion(ref errorOrientation, out axis, out angle);
//Scale the axis by the desired velocity if the angle is sufficiently large (epsilon).
if (angle > Toolbox.BigEpsilon)
{
float velocity = MathHelper.Min(settings.servo.baseCorrectiveSpeed, angle * updateRate) + angle * errorReduction;
biasVelocity.X = axis.X * velocity;
biasVelocity.Y = axis.Y * velocity;
biasVelocity.Z = axis.Z * velocity;
//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 an old frame's bias velocity to sneak in.
biasVelocity = new Vector3();
}
}
else
{
usedSoftness = settings.velocityMotor.softness * updateRate;
angle = 0; //Zero out the error;
Matrix3x3 transform = basis.WorldTransform;
Matrix3x3.Transform(ref settings.velocityMotor.goalVelocity, ref transform, out biasVelocity);
}
//Compute effective mass
effectiveMassMatrix = entity.inertiaTensorInverse;
effectiveMassMatrix.M11 += usedSoftness;
effectiveMassMatrix.M22 += usedSoftness;
effectiveMassMatrix.M33 += usedSoftness;
Matrix3x3.Invert(ref effectiveMassMatrix, out effectiveMassMatrix);
//Update the maximum force
ComputeMaxForces(settings.maximumForce, dt);
}
/// <summary>
/// Inverts the largest nonsingular submatrix in the matrix, excluding 2x2's that involve M13 or M31, and excluding 1x1's that include nondiagonal elements.
/// </summary>
/// <param name="matrix">Matrix to be inverted.</param>
/// <param name="result">Inverted matrix.</param>
public static void AdaptiveInvert(ref Matrix3x3 matrix, out Matrix3x3 result)
{
int submatrix;
float determinantInverse = 1 / matrix.AdaptiveDeterminant(out submatrix);
float m11, m12, m13, m21, m22, m23, m31, m32, m33;
switch (submatrix)
{
case 0: //Full matrix.
m11 = (matrix.M22 * matrix.M33 - matrix.M23 * matrix.M32) * determinantInverse;
m12 = (matrix.M13 * matrix.M32 - matrix.M33 * matrix.M12) * determinantInverse;
m13 = (matrix.M12 * matrix.M23 - matrix.M22 * matrix.M13) * determinantInverse;
m21 = (matrix.M23 * matrix.M31 - matrix.M21 * matrix.M33) * determinantInverse;
m22 = (matrix.M11 * matrix.M33 - matrix.M13 * matrix.M31) * determinantInverse;
m23 = (matrix.M13 * matrix.M21 - matrix.M11 * matrix.M23) * determinantInverse;
m31 = (matrix.M21 * matrix.M32 - matrix.M22 * matrix.M31) * determinantInverse;
m32 = (matrix.M12 * matrix.M31 - matrix.M11 * matrix.M32) * determinantInverse;
m33 = (matrix.M11 * matrix.M22 - matrix.M12 * matrix.M21) * determinantInverse;
break;
case 1: //Upper left matrix, m11, m12, m21, m22.
m11 = matrix.M22 * determinantInverse;
m12 = -matrix.M12 * determinantInverse;
m13 = 0;
m21 = -matrix.M21 * determinantInverse;
m22 = matrix.M11 * determinantInverse;
m23 = 0;
m31 = 0;
m32 = 0;
m33 = 0;
break;
case 2: //Lower right matrix, m22, m23, m32, m33.
m11 = 0;
m12 = 0;
m13 = 0;
m21 = 0;
m22 = matrix.M33 * determinantInverse;
m23 = -matrix.M23 * determinantInverse;
m31 = 0;
m32 = -matrix.M32 * determinantInverse;
m33 = matrix.M22 * determinantInverse;
break;
case 3: //Corners, m11, m31, m13, m33.
m11 = matrix.M33 * determinantInverse;
m12 = 0;
m13 = -matrix.M13 * determinantInverse;
m21 = 0;
m22 = 0;
m23 = 0;
m31 = -matrix.M31 * determinantInverse;
m32 = 0;
m33 = matrix.M11 * determinantInverse;
break;
case 4: //M11
m11 = 1 / matrix.M11;
m12 = 0;
m13 = 0;
m21 = 0;
m22 = 0;
m23 = 0;
m31 = 0;
m32 = 0;
m33 = 0;
break;
case 5: //M22
m11 = 0;
m12 = 0;
m13 = 0;
m21 = 0;
m22 = 1 / matrix.M22;
m23 = 0;
m31 = 0;
m32 = 0;
m33 = 0;
break;
case 6: //M33
m11 = 0;
m12 = 0;
m13 = 0;
m21 = 0;
m22 = 0;
m23 = 0;
m31 = 0;
m32 = 0;
m33 = 1 / matrix.M33;
break;
default: //Completely singular.
m11 = 0; m12 = 0; m13 = 0; m21 = 0; m22 = 0; m23 = 0; m31 = 0; m32 = 0; m33 = 0;
break;
}
result.M11 = m11;
result.M12 = m12;
result.M13 = m13;
result.M21 = m21;
result.M22 = m22;
result.M23 = m23;
result.M31 = m31;
result.M32 = m32;
result.M33 = m33;
}
/// <summary>
/// Calculates necessary information for velocity solving.
/// Called automatically by space.
/// </summary>
/// <param name="dt">Time in seconds since the last update.</param>
public override void Update(float dt)
{
usedSoftness = softness / dt;
effectiveMassMatrix = entity.inertiaTensorInverse;
effectiveMassMatrix.M11 += usedSoftness;
effectiveMassMatrix.M22 += usedSoftness;
effectiveMassMatrix.M33 += usedSoftness;
Matrix3x3.Invert(ref effectiveMassMatrix, out effectiveMassMatrix);
//Determine maximum force
if (maximumForce < float.MaxValue)
{
maxForceDt = maximumForce * dt;
maxForceDtSquared = maxForceDt * maxForceDt;
}
else
{
maxForceDt = float.MaxValue;
maxForceDtSquared = float.MaxValue;
}
}
/// <summary>
/// Multiplies a transposed matrix with another matrix.
/// </summary>
/// <param name="matrix">Matrix to be multiplied.</param>
/// <param name="transpose">Matrix to be transposed and multiplied.</param>
/// <param name="result">Product of the multiplication.</param>
public static void MultiplyTransposed(ref Matrix3x3 transpose, ref Matrix3x3 matrix, out Matrix3x3 result)
{
float resultM11 = transpose.M11 * matrix.M11 + transpose.M21 * matrix.M21 + transpose.M31 * matrix.M31;
float resultM12 = transpose.M11 * matrix.M12 + transpose.M21 * matrix.M22 + transpose.M31 * matrix.M32;
float resultM13 = transpose.M11 * matrix.M13 + transpose.M21 * matrix.M23 + transpose.M31 * matrix.M33;
float resultM21 = transpose.M12 * matrix.M11 + transpose.M22 * matrix.M21 + transpose.M32 * matrix.M31;
float resultM22 = transpose.M12 * matrix.M12 + transpose.M22 * matrix.M22 + transpose.M32 * matrix.M32;
float resultM23 = transpose.M12 * matrix.M13 + transpose.M22 * matrix.M23 + transpose.M32 * matrix.M33;
float resultM31 = transpose.M13 * matrix.M11 + transpose.M23 * matrix.M21 + transpose.M33 * matrix.M31;
float resultM32 = transpose.M13 * matrix.M12 + transpose.M23 * matrix.M22 + transpose.M33 * matrix.M32;
float resultM33 = transpose.M13 * matrix.M13 + transpose.M23 * matrix.M23 + transpose.M33 * matrix.M33;
result.M11 = resultM11;
result.M12 = resultM12;
result.M13 = resultM13;
result.M21 = resultM21;
result.M22 = resultM22;
result.M23 = resultM23;
result.M31 = resultM31;
result.M32 = resultM32;
result.M33 = resultM33;
}
/// <summary>
/// Creates a quaternion from a rotation matrix.
/// </summary>
/// <param name="r">Rotation matrix used to create a new quaternion.</param>
/// <returns>System.Numerics.Quaternion representing the same rotation as the matrix.</returns>
public static System.Numerics.Quaternion CreateFromRotationMatrix(Matrix3x3 r)
{
System.Numerics.Quaternion toReturn;
CreateFromRotationMatrix(ref r, out toReturn);
return toReturn;
}
/// <summary>
/// Multiplies a matrix with a transposed matrix.
/// </summary>
/// <param name="matrix">Matrix to be multiplied.</param>
/// <param name="transpose">Matrix to be transposed and multiplied.</param>
/// <param name="result">Product of the multiplication.</param>
public static void MultiplyByTransposed(ref Matrix3x3 matrix, ref Matrix3x3 transpose, out Matrix3x3 result)
{
float resultM11 = matrix.M11 * transpose.M11 + matrix.M12 * transpose.M12 + matrix.M13 * transpose.M13;
float resultM12 = matrix.M11 * transpose.M21 + matrix.M12 * transpose.M22 + matrix.M13 * transpose.M23;
float resultM13 = matrix.M11 * transpose.M31 + matrix.M12 * transpose.M32 + matrix.M13 * transpose.M33;
float resultM21 = matrix.M21 * transpose.M11 + matrix.M22 * transpose.M12 + matrix.M23 * transpose.M13;
float resultM22 = matrix.M21 * transpose.M21 + matrix.M22 * transpose.M22 + matrix.M23 * transpose.M23;
float resultM23 = matrix.M21 * transpose.M31 + matrix.M22 * transpose.M32 + matrix.M23 * transpose.M33;
float resultM31 = matrix.M31 * transpose.M11 + matrix.M32 * transpose.M12 + matrix.M33 * transpose.M13;
float resultM32 = matrix.M31 * transpose.M21 + matrix.M32 * transpose.M22 + matrix.M33 * transpose.M23;
float resultM33 = matrix.M31 * transpose.M31 + matrix.M32 * transpose.M32 + matrix.M33 * transpose.M33;
result.M11 = resultM11;
result.M12 = resultM12;
result.M13 = resultM13;
result.M21 = resultM21;
result.M22 = resultM22;
result.M23 = resultM23;
result.M31 = resultM31;
result.M32 = resultM32;
result.M33 = resultM33;
}
/// <summary>
/// Finds the change in the rotation state quaternion provided the local inertia tensor and angular velocity.
/// </summary>
/// <param name="orientation">Orienatation of the object.</param>
/// <param name="localInertiaTensorInverse">Local-space inertia tensor of the object being updated.</param>
/// <param name="angularMomentum">Angular momentum of the object.</param>
/// <param name="orientationChange">Change in quaternion.</param>
public static void DifferentiateQuaternion(ref Quaternion orientation, ref Matrix3x3 localInertiaTensorInverse, ref Vector3 angularMomentum, out Quaternion orientationChange)
{
Quaternion normalizedOrientation;
Quaternion.Normalize(ref orientation, out normalizedOrientation);
Matrix3x3 tempRotMat;
Matrix3x3.CreateFromQuaternion(ref normalizedOrientation, out tempRotMat);
Matrix3x3 tempInertiaTensorInverse;
Matrix3x3.MultiplyTransposed(ref tempRotMat, ref localInertiaTensorInverse, out tempInertiaTensorInverse);
Matrix3x3.Multiply(ref tempInertiaTensorInverse, ref tempRotMat, out tempInertiaTensorInverse);
Vector3 halfspin;
Matrix3x3.Transform(ref angularMomentum, ref tempInertiaTensorInverse, out halfspin);
Vector3.Multiply(ref halfspin, .5f, out halfspin);
var halfspinQuaternion = new Quaternion(halfspin.X, halfspin.Y, halfspin.Z, 0);
Quaternion.Multiply(ref halfspinQuaternion, ref normalizedOrientation, out orientationChange);
}
/// <summary>
/// Scales all components of the matrix.
/// </summary>
/// <param name="matrix">Matrix to scale.</param>
/// <param name="scale">Amount to scale.</param>
/// <param name="result">Scaled matrix.</param>
public static void Multiply(ref Matrix3x3 matrix, float scale, out Matrix3x3 result)
{
result.M11 = matrix.M11 * scale;
result.M12 = matrix.M12 * scale;
result.M13 = matrix.M13 * scale;
result.M21 = matrix.M21 * scale;
result.M22 = matrix.M22 * scale;
result.M23 = matrix.M23 * scale;
result.M31 = matrix.M31 * scale;
result.M32 = matrix.M32 * scale;
result.M33 = matrix.M33 * scale;
}
protected internal override void UpdateJacobiansAndVelocityBias()
{
//Transform the anchors and offsets into world space.
Vector3 offsetA, offsetB, lineDirection;
Quaternion.Transform(ref LocalLineAnchor, ref ConnectionA.Orientation, out offsetA);
Quaternion.Transform(ref LocalLineDirection, ref ConnectionA.Orientation, out lineDirection);
Quaternion.Transform(ref LocalAnchorB, ref ConnectionB.Orientation, out offsetB);
Vector3 anchorA, anchorB;
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);
//This entire constraint is very similar to the IKDistanceLimit, except the current distance is along an axis.
float currentDistance;
Vector3.Dot(ref separation, ref lineDirection, out currentDistance);
//Compute jacobians
if (currentDistance > maximumDistance)
{
//We are exceeding the maximum limit.
velocityBias = new Vector3(errorCorrectionFactor * (currentDistance - maximumDistance), 0, 0);
}
else if (currentDistance < minimumDistance)
{
//We are exceeding the minimum limit.
velocityBias = new Vector3(errorCorrectionFactor * (minimumDistance - currentDistance), 0, 0);
//The limit can only push in one direction. Flip the jacobian!
Vector3.Negate(ref lineDirection, out lineDirection);
}
else if (currentDistance - minimumDistance > (maximumDistance - minimumDistance) * 0.5f)
{
//The objects are closer to hitting the maximum limit.
velocityBias = new Vector3(currentDistance - maximumDistance, 0, 0);
}
else
{
//The objects are closer to hitting the minimum limit.
velocityBias = new Vector3(minimumDistance - currentDistance, 0, 0);
//The limit can only push in one direction. Flip the jacobian!
Vector3.Negate(ref lineDirection, out lineDirection);
}
Vector3 angularA, angularB;
//We can't just use the offset to anchor for A's jacobian- the 'collision' location is way out there at anchorB!
Vector3 rA;
Vector3.Subtract(ref anchorB, ref ConnectionA.Position, out rA);
Vector3.Cross(ref rA, ref lineDirection, out angularA);
//linearB = -linearA, so just swap the cross product order.
Vector3.Cross(ref lineDirection, ref offsetB, out angularB);
//Put all the 1x3 jacobians into a 3x3 matrix representation.
linearJacobianA = new Matrix3x3 { M11 = lineDirection.X, M12 = lineDirection.Y, M13 = lineDirection.Z };
linearJacobianB = new Matrix3x3 { M11 = -lineDirection.X, M12 = -lineDirection.Y, M13 = -lineDirection.Z };
angularJacobianA = new Matrix3x3 { M11 = angularA.X, M12 = angularA.Y, M13 = angularA.Z };
angularJacobianB = new Matrix3x3 { M11 = angularB.X, M12 = angularB.Y, M13 = angularB.Z };
}
/// <summary>
/// Negates every element in the matrix.
/// </summary>
/// <param name="matrix">Matrix to negate.</param>
/// <param name="result">Negated matrix.</param>
public static void Negate(ref Matrix3x3 matrix, out Matrix3x3 result)
{
result.M11 = -matrix.M11;
result.M12 = -matrix.M12;
result.M13 = -matrix.M13;
result.M21 = -matrix.M21;
result.M22 = -matrix.M22;
result.M23 = -matrix.M23;
result.M31 = -matrix.M31;
result.M32 = -matrix.M32;
result.M33 = -matrix.M33;
}
/// <summary>
/// Subtracts the two matrices from each other on a per-element basis.
/// </summary>
/// <param name="a">First matrix to subtract.</param>
/// <param name="b">Second matrix to subtract.</param>
/// <param name="result">Difference of the two matrices.</param>
public static void Subtract(ref Matrix3x3 a, ref Matrix3x3 b, out Matrix3x3 result)
{
float m11 = a.M11 - b.M11;
float m12 = a.M12 - b.M12;
float m13 = a.M13 - b.M13;
float m21 = a.M21 - b.M21;
float m22 = a.M22 - b.M22;
float m23 = a.M23 - b.M23;
float m31 = a.M31 - b.M31;
float m32 = a.M32 - b.M32;
float m33 = a.M33 - b.M33;
result.M11 = m11;
result.M12 = m12;
result.M13 = m13;
result.M21 = m21;
result.M22 = m22;
result.M23 = m23;
result.M31 = m31;
result.M32 = m32;
result.M33 = m33;
}
public static void Validate(this Matrix3x3 m)
{
m.Right.Validate();
m.Up.Validate();
m.Backward.Validate();
}
