/* FromRotationMatrix
*/
public void Quaternionf(Matrix4f m)
{
float trace = m.Trace();
if (trace > Single.Epsilon)
{
float s = (float)System.Math.Sqrt(trace + 1f);
Vector.W = s * 0.5f;
s = 0.5f / s;
Vector.X = (m.M23 - m.M32) * s;
Vector.Y = (m.M31 - m.M13) * s;
Vector.Z = (m.M12 - m.M21) * s;
}
else
{
if (m.M11 > m.M22 && m.M11 > m.M33)
{
float s = (float)System.Math.Sqrt(1f + m.M11 - m.M22 - m.M33);
Vector.X = 0.5f * s;
s = 0.5f / s;
Vector.Y = (m.M12 + m.M21) * s;
Vector.Z = (m.M13 + m.M31) * s;
Vector.W = (m.M23 - m.M32) * s;
}
else if (m.M22 > m.M33)
{
float s = (float)System.Math.Sqrt(1f + m.M22 - m.M11 - m.M33);
Vector.Y = 0.5f * s;
s = 0.5f / s;
Vector.X = (m.M21 + m.M12) * s;
Vector.Z = (m.M32 + m.M23) * s;
Vector.W = (m.M31 - m.M13) * s;
}
else
{
float s = (float)System.Math.Sqrt(1f + m.M33 - m.M11 - m.M22);
Vector.Z = 0.5f * s;
s = 0.5f / s;
Vector.X = (m.M31 + m.M13) * s;
Vector.Y = (m.M32 + m.M23) * s;
Vector.W = (m.M12 - m.M21) * s;
}
}
}
public void Yaw(float angle)
{
Quaternionf q = new Quaternionf();
q.FromAxisAngle(zAxis, angle);
Matrix4f m = new Matrix4f();
m.FromQuaternion(q);
Rotate(m);
if (!xAxis.IsFinite() || !yAxis.IsFinite())
throw new Exception("Non-finite in CoordinateFrame.Yaw()");
}
public void Rotate(Matrix4f m)
{
xAxis = xAxis.Transform(ref m);
yAxis = yAxis.Transform(ref m);
Orthonormalize();
if (!IsFinite())
throw new Exception("Non-finite in CoordinateFrame.Rotate()");
}
public void Rotate(Quaternionf q)
{
Matrix4f m = new Matrix4f();
m.FromQuaternion(q);
Rotate(m);
}
public CoordinateFrame(Vector3f origin, Quaternionf rotation)
{
Matrix4f m = new Matrix4f();
m.FromQuaternion(rotation);
this.origin = origin;
xAxis = new Vector3f(m.M11, m.M21, m.M31);
yAxis = new Vector3f(m.M12, m.M22, m.M32);
zAxis = new Vector3f(m.M13, m.M23, m.M33);
if (!IsFinite())
throw new ArgumentException("Non-finite in CoordinateFrame constructor");
}
public CoordinateFrame(Vector3f origin, Matrix4f rotation)
{
this.origin = origin;
xAxis = new Vector3f(rotation.M11, rotation.M21, rotation.M31);
yAxis = new Vector3f(rotation.M12, rotation.M22, rotation.M32);
zAxis = new Vector3f(rotation.M13, rotation.M23, rotation.M33);
if (!IsFinite())
throw new ArgumentException("Non-finite in CoordinateFrame constructor");
}
public Vector3f TransformNormal(ref Matrix4f matrix)
{
return new Vector3f(
(X * matrix.M11) + (Y * matrix.M21) + (Z * matrix.M31),
(X * matrix.M12) + (Y * matrix.M22) + (Z * matrix.M32),
(X * matrix.M13) + (Y * matrix.M23) + (Z * matrix.M33));
}
public Vector3f Transform(ref Matrix4f matrix)
{
return new Vector3f(
(X * matrix.M11) + (Y * matrix.M21) + (Z * matrix.M31) + matrix.M41,
(X * matrix.M12) + (Y * matrix.M22) + (Z * matrix.M32) + matrix.M42,
(X * matrix.M13) + (Y * matrix.M23) + (Z * matrix.M33) + matrix.M43);
}
public Matrix4f(Matrix4f m)
{
R0 = m.R0;
R1 = m.R1;
R2 = m.R2;
R3 = m.R3;
}
public void FromQuaternion(Quaternionf quaternion)
{
Matrix4f m1 = new Matrix4f(
quaternion.W, quaternion.Z, -quaternion.Y, quaternion.X,
-quaternion.Z, quaternion.W, quaternion.X, quaternion.Y,
quaternion.Y, -quaternion.X, quaternion.W, quaternion.Z,
-quaternion.X, -quaternion.Y, -quaternion.Z, quaternion.W);
Matrix4f m2 = new Matrix4f(
quaternion.W, quaternion.Z, -quaternion.Y, -quaternion.X,
-quaternion.Z, quaternion.W, quaternion.X, -quaternion.Y,
quaternion.Y, -quaternion.X, quaternion.W, -quaternion.Z,
quaternion.X, quaternion.Y, quaternion.Z, quaternion.W);
this = m1 * m2;
}
