public Quaternion(Matrix4 m) { var trace = m[0, 0] + m[1, 1] + m[2, 2]; if(trace > 0) { var s = 0.5f / (float)System.Math.Sqrt(trace + 1); w = 0.25f / s; x = (m[1, 2] - m[2, 1]) * s; y = (m[2, 0] - m[0, 2]) * s; z = (m[0, 1] - m[1, 0]) * s; } else { if(m[0, 0] > m[1, 1] && m[0, 0] > m[2, 2]) { var s = 2f * (float)System.Math.Sqrt(1 + m[0, 0] - m[1, 1] - m[2, 2]); w = (m[1, 2] - m[2, 1]) / s; x = 0.25f * s; y = (m[1, 0] + m[0, 1]) / s; z = (m[2, 0] + m[0, 2]) / s; } else if(m[1, 1] > m[2, 2]) { var s = 2f * (float)System.Math.Sqrt(1 + m[1, 1] - m[0, 0] - m[2, 2]); w = (m[2, 0] - m[0, 2]) / s; x = (m[1, 0] + m[0, 1]) / s; y = 0.25f * s; z = (m[2, 1] + m[1, 2]) / s; } else { var s = 2f * (float)System.Math.Sqrt(1 + m[2, 2] - m[0, 0] - m[1, 1]); w = (m[0, 1] - m[1, 0]) / s; x = (m[2, 0] + m[0, 2]) / s; y = (m[1, 2] + m[2, 1]) / s; z = 0.25f * s; } Normalize(); } }
public static Matrix4 InitIdentity() { var m = new Matrix4(); m[0, 0] = 1; m[1, 1] = 1; m[2, 2] = 1; m[3, 3] = 1; return m; }
public static Matrix4 operator *(Matrix4 v1, Matrix4 v2) { var m = new Matrix4(); for(var i = 0; i < 4; i++) { for(var j = 0; j < 4; j++) { m[i, j] = v1[i, 0] * v2[0, j] + v1[i, 1] * v2[1, j] + v1[i, 2] * v2[2, j] + v1[i, 3] * v2[3, j]; } } return m; }
public Matrix4 Set(Matrix4 m) { Set(m.M); return this; }
public static Matrix4 InitRotation(float x, float y, float z) { var _z = new Matrix4(); var _x = new Matrix4(); var _y = new Matrix4(); x *= (float)(System.Math.PI / 180); y *= (float)(System.Math.PI / 180); z *= (float)(System.Math.PI / 180); _z[0] = new float[4] { (float)System.Math.Cos(z), -1 * (float)System.Math.Sin(z), 0, 0 }; _z[1] = new float[4] { (float)System.Math.Sin(z), (float)System.Math.Cos(z), 0, 0 }; _z[2] = new float[4] { 0, 0, 1, 0 }; _z[3] = new float[4] { 0, 0, 0, 1 }; _x[0] = new float[4] { 1, 0, 0, 0 }; _x[1] = new float[4] { 0, (float)System.Math.Cos(x), -1 * (float)System.Math.Sin(x), 0 }; _x[2] = new float[4] { 0, (float)System.Math.Sin(x), (float)System.Math.Cos(x), 0 }; _x[3] = new float[4] { 0, 0, 0, 1 }; _y[0] = new float[4] { (float)System.Math.Cos(y), 0, -1 * (float)System.Math.Sin(y), 0 }; _y[1] = new float[4] { 0, 1, 0, 0 }; _y[2] = new float[4] { (float)System.Math.Sin(y), 0, (float)System.Math.Cos(y), 0 }; _y[3] = new float[4] { 0, 0, 0, 1 }; return _z * (_y * _x); }
public Matrix4(Matrix4 m) { this.m = new float[4, 4]; Set(m); }
public static Matrix4 InitRotation(Vector3 forward, Vector3 up, Vector3 right) { var _forward = forward; var _up = up; var _right = right; var m = new Matrix4(); m[0] = new float[4] { _right.X, _right.Y, _right.Z, 0 }; m[1] = new float[4] { _up.X, _up.Y, _up.Z, 0 }; m[2] = new float[4] { _forward.X, _forward.Y, _forward.Z, 0 }; m[3] = new float[4] { 0, 0, 0, 1 }; return m; }