/// <summary>
/// Calculate the inverse of the given matrix
/// </summary>
/// <param name="mat">The matrix to invert</param>
/// <param name="result">The inverse of the given matrix if it has one, or the input if it is singular</param>
/// <exception cref="InvalidOperationException">Thrown if the Matrix4 is singular.</exception>
public static void Invert(ref Matrix4x3d mat, out Matrix4x3d result)
{
Matrix3d inverseRotation = new Matrix3d(mat.Column0.Xyz, mat.Column1.Xyz, mat.Column2.Xyz);
inverseRotation.Row0 /= inverseRotation.Row0.LengthSquared;
inverseRotation.Row1 /= inverseRotation.Row1.LengthSquared;
inverseRotation.Row2 /= inverseRotation.Row2.LengthSquared;
Vector3d translation = mat.Row3;
result.Row0 = inverseRotation.Row0;
result.Row1 = inverseRotation.Row1;
result.Row2 = inverseRotation.Row2;
result.Row3 = new Vector3d(-Vector3d.Dot(inverseRotation.Row0, translation), -Vector3d.Dot(inverseRotation.Row1, translation), -Vector3d.Dot(inverseRotation.Row2, translation));
}
/// <summary>
/// Do Spherical linear interpolation between two quaternions
/// </summary>
/// <param name="q1">The first Quaterniond</param>
/// <param name="q2">The second Quaterniond</param>
/// <param name="blend">The blend factor</param>
/// <returns>A smooth blend between the given quaternions</returns>
public static Quaterniond Slerp(Quaterniond q1, Quaterniond q2, double blend)
{
// if either input is zero, return the other.
if (q1.LengthSquared == 0.0f)
{
if (q2.LengthSquared == 0.0f)
{
return(Identity);
}
return(q2);
}
else if (q2.LengthSquared == 0.0f)
{
return(q1);
}
double cosHalfAngle = q1.W * q2.W + Vector3d.Dot(q1.Xyz, q2.Xyz);
if (cosHalfAngle >= 1.0f || cosHalfAngle <= -1.0f)
{
// angle = 0.0f, so just return one input.
return(q1);
}
else if (cosHalfAngle < 0.0f)
{
q2.Xyz = -q2.Xyz;
q2.W = -q2.W;
cosHalfAngle = -cosHalfAngle;
}
double blendA;
double blendB;
if (cosHalfAngle < 0.99f)
{
// do proper slerp for big angles
double halfAngle = (double)System.Math.Acos(cosHalfAngle);
double sinHalfAngle = (double)System.Math.Sin(halfAngle);
double oneOverSinHalfAngle = 1.0f / sinHalfAngle;
blendA = (double)System.Math.Sin(halfAngle * (1.0f - blend)) * oneOverSinHalfAngle;
blendB = (double)System.Math.Sin(halfAngle * blend) * oneOverSinHalfAngle;
}
else
{
// do lerp if angle is really small.
blendA = 1.0f - blend;
blendB = blend;
}
Quaterniond result = new Quaterniond(blendA * q1.Xyz + blendB * q2.Xyz, blendA * q1.W + blendB * q2.W);
if (result.LengthSquared > 0.0f)
{
return(Normalize(result));
}
else
{
return(Identity);
}
}
public static void Slerp(ref Quaterniond start, ref Quaterniond end, double blend, out Quaterniond result)
{
if (start.W == 0 && start.X == 0 && start.Y == 0 && start.Z == 0)
{
if (end.W == 0 && end.X == 0 && end.Y == 0 && end.Z == 0)
{
result.W = 1;
result.X = 0;
result.Y = 0;
result.Z = 0;
}
else
{
result = end;
}
}
else if (end.W == 0 && end.X == 0 && end.Y == 0 && end.Z == 0)
{
result = start;
}
Vector3d startVector = new Vector3d(start.X, start.Y, start.Z);
Vector3d endVector = new Vector3d(end.X, end.Y, end.Z);
double cosHalfAngle = start.W * end.W + Vector3d.Dot(startVector, endVector);
if (cosHalfAngle >= 1.0f || cosHalfAngle <= -1.0f)
{
// angle = 0.0f, so just return one input.
result = start;
}
else if (cosHalfAngle < 0.0f)
{
end.W = -end.W;
end.X = -end.X;
end.Y = -end.Y;
end.Z = -end.Z;
cosHalfAngle = -cosHalfAngle;
}
double blendA;
double blendB;
if (cosHalfAngle < 0.99f)
{
// do proper slerp for big angles
double halfAngle = (double)System.Math.Acos(cosHalfAngle);
double sinHalfAngle = (double)System.Math.Sin(halfAngle);
double oneOverSinHalfAngle = 1.0f / sinHalfAngle;
blendA = (double)System.Math.Sin(halfAngle * (1.0f - blend)) * oneOverSinHalfAngle;
blendB = (double)System.Math.Sin(halfAngle * blend) * oneOverSinHalfAngle;
}
else
{
// do lerp if angle is really small.
blendA = 1.0f - blend;
blendB = blend;
}
result.W = blendA * start.W + blendB * end.W;
result.X = blendA * start.X + blendB * end.X;
result.Y = blendA * start.Y + blendB * end.Y;
result.Z = blendA * start.Z + blendB * end.Z;
if (result.W != 0 || result.X != 0 || result.Y != 0 || result.Z != 0)
{
result.Normalize();
}
else
{
result.W = 1;
result.X = 0;
result.Y = 0;
result.Z = 0;
}
}
/// <summary>
/// Multiplies two instances.
/// </summary>
/// <param name="left">The first instance.</param>
/// <param name="right">The second instance.</param>
/// <param name="result">A new instance containing the result of the calculation.</param>
public static void Multiply(ref Quaterniond left, ref Quaterniond right, out Quaterniond result)
{
result = new Quaterniond(
right.W * left.Xyz + left.W * right.Xyz + Vector3d.Cross(left.Xyz, right.Xyz),
left.W * right.W - Vector3d.Dot(left.Xyz, right.Xyz));
}
public static Quaterniond Mult(Quaterniond left, Quaterniond right)
{
return new Quaterniond(
right.W * left.Xyz + left.W * right.Xyz + Vector3d.Cross(left.Xyz, right.Xyz),
left.W * right.W - Vector3d.Dot(left.Xyz, right.Xyz));
}