/// <summary>Finite test of matrix elements</summary>
public static bool IsFinite(m3x4 m)
{
return
(IsFinite(m.x) &&
IsFinite(m.y) &&
IsFinite(m.z));
}
/// <summary>Return true if any components of 'm' are NaN</summary>
public static bool IsNaN(m3x4 m)
{
return
(IsNaN(m.x) ||
IsNaN(m.y) ||
IsNaN(m.z));
}
/// <summary>Return the maximum element value in 'mm'</summary>
public static float MaxElement(m3x4 m)
{
return(Max(
MaxElement(m.x),
MaxElement(m.y),
MaxElement(m.z)));
}
/// <summary>Return the minimum element value in 'm'</summary>
public static float MinElement(m3x4 m)
{
return(Min(
MinElement(m.x),
MinElement(m.y),
MinElement(m.z)));
}
/// <summary>Approximate equal</summary>
public static bool FEqlAbsolute(m3x4 a, m3x4 b, float tol)
{
return
(FEqlAbsolute(a.x, b.x, tol) &&
FEqlAbsolute(a.y, b.y, tol) &&
FEqlAbsolute(a.z, b.z, tol));
}
/// <summary>Absolute value of 'x'</summary>
public static m3x4 Abs(m3x4 x)
{
return(new m3x4(
Abs(x.x),
Abs(x.y),
Abs(x.z)));
}
/// <summary>Create a rotation from an axis and angle</summary>
public static m3x4 Rotation(v4 axis_norm, v4 axis_sine_angle, float cos_angle)
{
Debug.Assert(Math_.FEql(axis_norm.LengthSq, 1f), "'axis_norm' should be normalised");
var mat = new m3x4();
v4 trace_vec = axis_norm * (1.0f - cos_angle);
mat.x.x = trace_vec.x * axis_norm.x + cos_angle;
mat.y.y = trace_vec.y * axis_norm.y + cos_angle;
mat.z.z = trace_vec.z * axis_norm.z + cos_angle;
trace_vec.x *= axis_norm.y;
trace_vec.z *= axis_norm.x;
trace_vec.y *= axis_norm.z;
mat.x.y = trace_vec.x + axis_sine_angle.z;
mat.x.z = trace_vec.z - axis_sine_angle.y;
mat.x.w = 0.0f;
mat.y.x = trace_vec.x - axis_sine_angle.z;
mat.y.z = trace_vec.y + axis_sine_angle.x;
mat.y.w = 0.0f;
mat.z.x = trace_vec.z + axis_sine_angle.y;
mat.z.y = trace_vec.y - axis_sine_angle.x;
mat.z.w = 0.0f;
return(mat);
}
/// <summary>True if 'm' is orthonormal</summary>
public static bool IsOrthonormal(m3x4 m)
{
return
(FEql(m.x.LengthSq, 1f) &&
FEql(m.y.LengthSq, 1f) &&
FEql(m.z.LengthSq, 1f) &&
FEql(Cross(m.x, m.y) - m.z, v4.Zero));
}
// Constructors
public m6x8(m3x4 m00, m3x4 m01, m3x4 m10, m3x4 m11)
: this()
{
this.m00 = m00;
this.m01 = m01;
this.m10 = m10;
this.m11 = m11;
}
/// <summary>Create a shear matrix</summary>
public static m3x4 Shear(float sxy, float sxz, float syx, float syz, float szx, float szy)
{
var mat = new m3x4 {
};
mat.x = new v4(1.0f, sxy, sxz, 0.0f);
mat.y = new v4(syx, 1.0f, syz, 0.0f);
mat.z = new v4(szx, szy, 1.0f, 0.0f);
return(mat);
}
/// <summary>
/// Permute the vectors in a rotation matrix by 'n'.<para/>
/// n == 0 : x y z<para/>
/// n == 1 : z x y<para/>
/// n == 2 : y z x<para/></summary>
public static m3x4 PermuteRotation(m3x4 mat, int n)
{
switch (n % 3)
{
default: return(mat);
case 1: return(new m3x4(mat.z, mat.x, mat.y));
case 2: return(new m3x4(mat.y, mat.z, mat.x));
}
}
/// <summary>Invert the matrix 'm'</summary>
public static m3x4 Invert(m3x4 m)
{
if (!IsInvertible(m))
{
throw new Exception("Matrix is singular");
}
var det = Determinant(m);
var tmp = new m3x4(
Cross(m.y, m.z) / det,
Cross(m.z, m.x) / det,
Cross(m.x, m.y) / det);
return(Transpose(tmp));
}
public static bool FEqlRelative(m3x4 a, m3x4 b, float tol)
{
var max_a = MaxElement(Abs(a));
var max_b = MaxElement(Abs(b));
if (max_b == 0)
{
return(max_a < tol);
}
if (max_a == 0)
{
return(max_b < tol);
}
var abs_max_element = Max(max_a, max_b);
return(FEqlAbsolute(a, b, tol * abs_max_element));
}
/// <summary>Create a quaternion from a rotation matrix</summary>
public quat(m3x4 m)
: this()
{
Debug.Assert(Math_.IsOrthonormal(m), "Only orientation matrices can be converted into quaternions");
var trace = m.x.x + m.y.y + m.z.z;
if (trace >= 0.0f)
{
var s = 0.5f / (float)Math.Sqrt(1.0f + trace);
x = (m.y.z - m.z.y) * s;
y = (m.z.x - m.x.z) * s;
z = (m.x.y - m.y.x) * s;
w = (0.25f / s);
}
else if (m.x.x > m.y.y && m.x.x > m.z.z)
{
var s = 0.5f / (float)Math.Sqrt(1.0f + m.x.x - m.y.y - m.z.z);
x = (0.25f / s);
y = (m.x.y + m.y.x) * s;
z = (m.z.x + m.x.z) * s;
w = (m.y.z - m.z.y) * s;
}
else if (m.y.y > m.z.z)
{
var s = 0.5f / (float)Math.Sqrt(1.0f - m.x.x + m.y.y - m.z.z);
x = (m.x.y + m.y.x) * s;
y = (0.25f / s);
z = (m.y.z + m.z.y) * s;
w = (m.z.x - m.x.z) * s;
}
else
{
var s = 0.5f / (float)Math.Sqrt(1.0f - m.x.x - m.y.y + m.z.z);
x = (m.z.x + m.x.z) * s;
y = (m.y.z + m.z.y) * s;
z = (0.25f / s);
w = (m.x.y - m.y.x) * s;
}
}
/// <summary>Return the trace of 'm'</summary>
public static float Trace(m3x4 m)
{
return(m.x.x + m.y.y + m.z.z);
}
/// <summary>Return possible Euler angles for the rotation matrix 'mat'</summary>
public static v4 EulerAngles(m3x4 mat)
{
var q = new quat(mat);
return(EulerAngles(q));
}
/// <summary>Spherically interpolate between two rotations</summary>
public static m3x4 Slerp(m3x4 lhs, m3x4 rhs, double frac)
{
return(new m3x4(Slerp(new quat(lhs), new quat(rhs), frac)));
}
/// <summary>Return an orthonormalised version of 'm'</summary>
public static m3x4 Orthonormalise(m3x4 m)
{
Orthonormalise(ref m);
return(m);
}
public m4x4(quat q, v4 pos) : this()
{
this.rot = new m3x4(q);
this.pos = pos;
}
public static bool FEql(m3x4 a, m3x4 b)
{
return
(FEqlRelative(a, b, TinyF));
}
/// <summary>Orthonormalise 'm' in-place</summary>
public static void Orthonormalise(ref m3x4 m)
{
Normalise(ref m.x);
m.y = Normalise(Cross(m.z, m.x));
m.z = Cross(m.x, m.y);
}
/// <summary>Return the inverse of 'm' assuming m is orthonormal</summary>
public static m3x4 InvertFast(m3x4 m)
{
InvertFast(ref m);
return(m);
}
/// <summary>True if 'm' can be inverted</summary>
public static bool IsInvertible(m3x4 m)
{
return(Determinant(m) != 0);
}
/// <summary>Return the transpose of 'm'</summary>
public static m3x4 Transpose(m3x4 m)
{
Transpose(ref m);
return(m);
}
/// <summary>Invert 'm' in-place assuming m is orthonormal</summary>
public static void InvertFast(ref m3x4 m)
{
Debug.Assert(IsOrthonormal(m), "Matrix is not orthonormal");
Transpose(ref m);
}
/// <summary>Transpose 'm' in-place</summary>
public static void Transpose(ref m3x4 m)
{
Swap(ref m.x.y, ref m.y.x);
Swap(ref m.x.z, ref m.z.x);
Swap(ref m.y.z, ref m.z.y);
}
/// <summary>Return the diagonal elements of 'm'</summary>
public static v4 Diagonal(m3x4 m)
{
return(new v4(m.x.x, m.y.y, m.z.z, 0));
}
/// <summary>Return the kernel of 'm'</summary>
public static v4 Kernel(m3x4 m)
{
return(new v4(m.y.y * m.z.z - m.y.z * m.z.y, -m.y.x * m.z.z + m.y.z * m.z.x, m.y.x * m.z.y - m.y.y * m.z.x, 0));
}
public m4x4(m3x4 rot, v4 pos) : this()
{
this.rot = rot;
this.pos = pos;
}
/// <summary>Return the determinant of 'm'</summary>
public static float Determinant(m3x4 m)
{
return(Triple(m.x, m.y, m.z));
}