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