public static __m33t__ Multiply(__rot2t__ rot, __m33t__ mat)
{
__ft__ a = (__ft__)System.Math.Cos(rot.Angle);
__ft__ b = (__ft__)System.Math.Sin(rot.Angle);
return(new __m33t__(a * mat.M00 +
b * mat.M10,
a * mat.M01 +
b * mat.M11,
a * mat.M02 +
b * mat.M12,
-b * mat.M00 +
a * mat.M10,
-b * mat.M01 +
a * mat.M11,
-b * mat.M02 +
a * mat.M12,
mat.M20,
mat.M21,
mat.M22));
}
/// <summary>
/// Creates a quaternion from a rotation matrix
/// </summary>
/// <param name="m"></param>
/// <param name="epsilon"></param>
public static __rot3t__ From__m33t__(__m33t__ m, __ft__ epsilon = (__ft__)1e-6)
{
if (!m.IsOrthonormal(epsilon))
{
throw new ArgumentException("Matrix is not orthonormal.");
}
var t = 1 + m.M00 + m.M11 + m.M22;
if (t > epsilon)
{
__ft__ s = t.Sqrt() * 2;
__ft__ x = (m.M21 - m.M12) / s;
__ft__ y = (m.M02 - m.M20) / s;
__ft__ z = (m.M10 - m.M01) / s;
__ft__ w = s / 4;
return(new __rot3t__(w, x, y, z).Normalized);
}
else
{
if (m.M00 > m.M11 && m.M00 > m.M22)
{
__ft__ s = Fun.Sqrt(1 + m.M00 - m.M11 - m.M22) * 2;
__ft__ x = s / 4;
__ft__ y = (m.M01 + m.M10) / s;
__ft__ z = (m.M02 + m.M20) / s;
__ft__ w = (m.M21 - m.M12) / s;
return(new __rot3t__(w, x, y, z).Normalized);
}
else if (m.M11 > m.M22)
{
__ft__ s = Fun.Sqrt(1 + m.M11 - m.M00 - m.M22) * 2;
__ft__ x = (m.M01 + m.M10) / s;
__ft__ y = s / 4;
__ft__ z = (m.M12 + m.M21) / s;
__ft__ w = (m.M20 - m.M02) / s;
return(new __rot3t__(w, x, y, z).Normalized);
}
else
{
__ft__ s = Fun.Sqrt(1 + m.M22 - m.M00 - m.M11) * 2;
__ft__ x = (m.M20 + m.M02) / s;
__ft__ y = (m.M12 + m.M21) / s;
__ft__ z = s / 4;
__ft__ w = (m.M01 - m.M10) / s;
return(new __rot3t__(w, x, y, z).Normalized);
}
}
}
/// <summary>
/// </summary>
public static __m33t__ operator *(__rot3t__ r3, __rot2t__ r2)
{
__m33t__ m33 = (__m33t__)r3;
__m22t__ m22 = (__m22t__)r2;
return(new __m33t__(
m33.M00 * m22.M00 + m33.M01 * m22.M10,
m33.M00 * m22.M01 + m33.M01 * m22.M11,
m33.M02,
m33.M10 * m22.M00 + m33.M11 * m22.M10,
m33.M10 * m22.M01 + m33.M11 * m22.M11,
m33.M12,
m33.M20 * m22.M00 + m33.M21 * m22.M10,
m33.M20 * m22.M01 + m33.M21 * m22.M11,
m33.M22
));
}
