/// <summary>
/// Calculates an inverse of the matrix if it exists.
/// </summary>
public void Invert()
{
float[,] invVals = new float[3, 3];
invVals[0, 0] = m11 * m22 - m12 * m21;
invVals[1, 0] = m12 * m20 - m10 * m22;
invVals[2, 0] = m10 * m21 - m11 * m20;
float det = m00 * invVals[0, 0] + m01 * invVals[1, 0] + m02 * invVals[2, 0];
if (MathEx.Abs(det) <= 1e-06f)
{
throw new DivideByZeroException("Matrix determinant is zero. Cannot invert.");
}
invVals[0, 1] = m02 * m21 - m01 * m22;
invVals[0, 2] = m01 * m12 - m02 * m11;
invVals[1, 1] = m00 * m22 - m02 * m20;
invVals[1, 2] = m02 * m10 - m00 * m12;
invVals[2, 1] = m01 * m20 - m00 * m21;
invVals[2, 2] = m00 * m11 - m01 * m10;
float invDet = 1.0f / det;
for (int row = 0; row < 3; row++)
{
for (int col = 0; col < 3; col++)
{
invVals[row, col] *= invDet;
}
}
}
/// <summary>
/// Performs spherical interpolation between two quaternions. Spherical interpolation neatly interpolates between
/// two rotations without modifying the size of the vector it is applied to (unlike linear interpolation).
/// </summary>
/// <param name="from">Start quaternion.</param>
/// <param name="to">End quaternion.</param>
/// <param name="t">Interpolation factor in range [0, 1] that determines how much to interpolate between
/// <paramref name="from"/> and <paramref name="to"/>.</param>
/// <param name="shortestPath">Should the interpolation be performed between the shortest or longest path between
/// the two quaternions.</param>
/// <returns>Interpolated quaternion representing a rotation between <paramref name="from"/> and
/// <paramref name="to"/>.</returns>
public static Quaternion Slerp(Quaternion from, Quaternion to, float t, bool shortestPath = true)
{
float dot = Dot(from, to);
Quaternion quat;
if (dot < 0.0f && shortestPath)
{
dot = -dot;
quat = -to;
}
else
{
quat = to;
}
if (MathEx.Abs(dot) < (1 - epsilon))
{
float sin = MathEx.Sqrt(1 - (dot * dot));
Radian angle = MathEx.Atan2(sin, dot);
float invSin = 1.0f / sin;
float a = MathEx.Sin((1.0f - t) * angle) * invSin;
float b = MathEx.Sin(t * angle) * invSin;
return(a * from + b * quat);
}
else
{
Quaternion ret = (1.0f - t) * from + t * quat;
ret.Normalize();
return(ret);
}
}
/// <summary>
/// Updates the dimensions of the controller by taking account scale of the parent scene object.
/// </summary>
private void UpdateDimensions()
{
Vector3 scale = SceneObject.Scale;
float height = serializableData.height * MathEx.Abs(scale.y);
float radius = serializableData.radius * MathEx.Abs(MathEx.Max(scale.x, scale.z));
native.Height = height;
native.Radius = radius;
}
/// <summary>
/// Transforms the bounding box by the given matrix.
///
/// As the resulting box will no longer be axis aligned, an axis align box is instead created by encompassing the
/// transformed oriented bounding box. Retrieving the value as an actual OBB would provide a tighter fit.
/// </summary>
/// <param name="tfrm">Affine matrix to transform the box with.</param>
public void TransformAffine(Matrix4 tfrm)
{
Vector3 center = Center;
Vector3 halfSize = Size * 0.5f;
Vector3 newCenter = tfrm.MultiplyAffine(center);
Vector3 newHalfSize = new Vector3(
MathEx.Abs(tfrm.m00) * halfSize.x + MathEx.Abs(tfrm.m01) * halfSize.y + MathEx.Abs(tfrm.m02) * halfSize.z,
MathEx.Abs(tfrm.m10) * halfSize.x + MathEx.Abs(tfrm.m11) * halfSize.y + MathEx.Abs(tfrm.m12) * halfSize.z,
MathEx.Abs(tfrm.m20) * halfSize.x + MathEx.Abs(tfrm.m21) * halfSize.y + MathEx.Abs(tfrm.m22) * halfSize.z);
minimum = newCenter - newHalfSize;
maximum = newCenter + newHalfSize;
}
/// <summary>
/// Calculates two vectors orthonormal to the first vector.
/// </summary>
/// <param name="x">Normalized vector to calculate orthonormal vectors for.</param>
/// <param name="y">First orthonormal vector.</param>
/// <param name="z">Second orthonormal vector.</param>
public static void OrthogonalComplement(Vector3 x, out Vector3 y, out Vector3 z)
{
if (MathEx.Abs(x.x) > MathEx.Abs(x.y))
{
y = new Vector3(-x.z, 0, x.x);
}
else
{
y = new Vector3(0, x.z, -x.y);
}
z = Cross(x, y);
Orthonormalize(ref x, ref y, ref z);
}
/// <summary>
/// Wraps an angle in [0, 360) range. Values lower than zero, or higher or equal to 360
/// will get wrapped around back into [0, 360) range.
/// </summary>
/// <param name="angle">Angle to wrap.</param>
/// <returns>Angle in [0, 360) range.</returns>
public static Degree WrapAngle(Degree angle)
{
const float inv360 = 1.0f / 360.0f;
float angleVal = angle.Degrees;
float wrapCount = (float)MathEx.Floor(MathEx.Abs(angleVal) * inv360);
if (angleVal > 0.0f)
{
angleVal -= 360.0f * wrapCount;
}
else if (angleVal < 0.0f)
{
angleVal += 360.0f * (wrapCount + 1);
}
return(new Degree(angleVal));
}
/// <summary>
/// Calculates an angle between two rotations.
/// </summary>
/// <param name="a">First rotation.</param>
/// <param name="b">Second rotation.</param>
/// <returns>Angle between the rotations, in degrees.</returns>
public static Degree Angle(Quaternion a, Quaternion b)
{
return(MathEx.Acos(MathEx.Min(MathEx.Abs(Dot(a, b)), 1.0f)) * 2.0f);
}