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;
 }