static Matrix3D()
{
Matrix3D.identity = new Matrix3D(1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1);
}
public Quaternion(double x, double y, double z, double w)
{
this.isNotDefaultQuaternion = true;
this.x = x;
this.y = y;
this.z = z;
this.w = w;
this.value = Matrix3D.Identity;
this.valueIsDirty = true;
}
public Quaternion(Vector3D axisOfRotation, double angleInRadians)
{
double halfAngleInRadians = (angleInRadians * 0.5);
this.isNotDefaultQuaternion = true;
axisOfRotation.Normalize();
this.x = axisOfRotation.X * System.Math.Sin(halfAngleInRadians);
this.y = axisOfRotation.Y * System.Math.Sin(halfAngleInRadians);
this.z = axisOfRotation.Z * System.Math.Sin(halfAngleInRadians);
this.w = System.Math.Cos(halfAngleInRadians);
this.value = Matrix3D.Identity;
this.valueIsDirty = true;
}
public void NextFrame()
{
if (FrameCount <= 0) {
return;
}
CurrentFrame++;
if (FrameCount > 0) {
while (CurrentFrame > FrameCount) {
CurrentFrame -= FrameCount;
}
}
Matrix3D identity = new Matrix3D();
identity.SetIdentity();
Skeleton.SetFrame(Frames[(int)CurrentFrame], Quaternion.Identity);
}
public override Matrix3D GetMatrix(Point3D center)
{
//Calculations from Essential Mathematics for games & interactive applications
//James M. Van Verth & Lars M. Bishop
Vector3D a = NormalizedAxis;
//TODO: Internally use a Quaternion since it is more efficient to calculate
//TODO: Look into caching all this
double angle = AngleRad;
double t = 1.0 - System.Math.Cos(angle);
double c = System.Math.Cos(angle);
double s = System.Math.Sin(angle);
double x = a.X;
double y = a.Y;
double z = a.Z;
//TODO: Optimize, do not multiple matrices, just expand out terms
//Matrix3D m = new Matrix3D(1, 0, 0, 0,
// 0, 1, 0, 0,
// 0, 0, 1, 0,
// -center.X, -center.Y, -center.Z, 1);
//m.Append(
Matrix3D m = new Matrix3D(t * x * x + c, t * x * y + s * z, t * x * z - s * y, 0,
t * x * y - s * z, t * y * y + c, t * y * z + s * x, 0,
t * x * z + s * y, t * y * z - s * x, t * z * z + c, 0,
0, 0, 0, 1);
//m.Append(new Matrix3D(1, 0, 0, 0,
// 0, 1, 0, 0,
// 0, 0, 1, 0,
// center.X, center.Y, center.Z, 1));
return m;
}
public void Prepend(Matrix3D m)
{
this = m * this;
}
public bool Equals(Matrix3D m)
{
return Equals(this, m);
}
/// <summary>
/// Appends the matrix to the current matrix
/// </summary>
/// <param name="m"></param>
public void Append(Matrix3D m)
{
this = this * m;
}
public static Matrix3D Multiply(Matrix3D m1, Matrix3D m2)
{
return m1 * m2;
}
public static bool Equals(Matrix3D m1, Matrix3D m2)
{
if (!m1.isNotDefaultMatrix || !m2.isNotDefaultMatrix) {
return m1.IsIdentity == m2.IsIdentity;
}
else {
return (m1.m11 == m2.m11) &&
(m1.m12 == m2.m12) &&
(m1.m13 == m2.m13) &&
(m1.m14 == m2.m14) &&
(m1.m21 == m2.m21) &&
(m1.m22 == m2.m22) &&
(m1.m23 == m2.m23) &&
(m1.m24 == m2.m24) &&
(m1.m31 == m2.m31) &&
(m1.m32 == m2.m32) &&
(m1.m33 == m2.m33) &&
(m1.m34 == m2.m34) &&
(m1.offsetX == m2.offsetX) &&
(m1.offsetY == m2.offsetY) &&
(m1.offsetZ == m2.offsetZ) &&
(m1.m44 == m2.m44);
}
}
public override void Project(Matrix3D matrix)
{
Start.ScreenCoords = matrix.Project(Start);
End.ScreenCoords = matrix.Project(End);
}
/// <summary>
/// Converts a rotation matrix to a quaternion. Matrix must be orthogonal
/// </summary>
/// <returns></returns>
public static Quaternion FromRotationMatrix(Matrix3D matrix)
{
double w = System.Math.Sqrt(1 + matrix.M11 + matrix.M22 + matrix.M33) / 2.0;
double x = (matrix.M32 - matrix.M23) / (4 * w);
double y = (matrix.M13 - matrix.M31) / (4 * w);
double z = (matrix.M21 - matrix.M12) / (4 * w);
return new Quaternion(x, y, z, w);
}
public abstract void Project(Matrix3D matrix);
