public static Matrix4 CreateFromTranslation(Vector3 t)
{
Matrix4 m = new Matrix4(new float[4, 4]
{
{1,0,0,t.X},
{0,1,0,t.Y},
{0,0,1,t.Z},
{0,0,0,1}
});
return m;
}
public void SetInverse(Matrix4 m)
{
float det = GetDeterminant();
if (det == 0) return;
det = (1.0f) / det;
data[0] = (-m.data[9] * m.data[6] + m.data[5] * m.data[10]) * det;
data[4] = (m.data[8] * m.data[6] - m.data[4] * m.data[10]) * det;
data[8] = (-m.data[8] * m.data[5] + m.data[4] * m.data[9]) * det;
data[1] = (m.data[9] * m.data[2] - m.data[1] * m.data[10]) * det;
data[5] = (-m.data[8] * m.data[2] + m.data[0] * m.data[10]) * det;
data[9] = (m.data[8] * m.data[1] - m.data[0] * m.data[9]) * det;
data[2] = (-m.data[5] * m.data[2] + m.data[1] * m.data[6]) * det;
data[6] = (+m.data[4] * m.data[2] - m.data[0] * m.data[6]) * det;
data[10] = (-m.data[4] * m.data[1] + m.data[0] * m.data[5]) * det;
data[3] = (m.data[9] * m.data[6] * m.data[3]
- m.data[5] * m.data[10] * m.data[3]
- m.data[9] * m.data[2] * m.data[7]
+ m.data[1] * m.data[10] * m.data[7]
+ m.data[5] * m.data[2] * m.data[11]
- m.data[1] * m.data[6] * m.data[11]) * det;
data[7] = (-m.data[8] * m.data[6] * m.data[3]
+ m.data[4] * m.data[10] * m.data[3]
+ m.data[8] * m.data[2] * m.data[7]
- m.data[0] * m.data[10] * m.data[7]
- m.data[4] * m.data[2] * m.data[11]
+ m.data[0] * m.data[6] * m.data[11]) * det;
data[11] = (m.data[8] * m.data[5] * m.data[3]
- m.data[4] * m.data[9] * m.data[3]
- m.data[8] * m.data[1] * m.data[7]
+ m.data[0] * m.data[9] * m.data[7]
+ m.data[4] * m.data[1] * m.data[11]
- m.data[0] * m.data[5] * m.data[11]) * det;
}
public static Vector3 WorldToLocalDirn(Vector3 world, Matrix4 transform)
{
return transform.TransformInverseDirection(world);
}
public static Vector3 LocalToWorldDirn(Vector3 local, Matrix4 transform)
{
return transform.TransformDirection(local);
}
public static Vector3 WorldToLocal(Vector3 world, Matrix4 transform)
{
return transform.TransformInverse(world);
}
public static Vector3 LocalToWorld(Vector3 local, Matrix4 transform)
{
return Vector3.Transform((local), transform);
}
public void Translate(Vector3 trans)
{
Matrix4 inv_translation_matrix = Matrix4.CreateTranslation(-trans);
inv_matrix = inv_matrix * inv_translation_matrix;
Matrix4 translation_matrix = Matrix4.CreateTranslation(trans);
forward_matrix = translation_matrix * forward_matrix;
}
static Matrix3 _transformInertiaTensor(Quaternion q, Matrix3 iitBody, Matrix4 rotmat)
{
float t4 = rotmat[0] * iitBody[0] + rotmat[1] * iitBody[3] + rotmat[2] * iitBody[6];
float t9 = rotmat[0] * iitBody[1] + rotmat[1] * iitBody[4] + rotmat[2] * iitBody[7];
float t14 = rotmat[0] * iitBody[2] + rotmat[1] * iitBody[5] + rotmat[2] * iitBody[8];
float t28 = rotmat[4] * iitBody[0] + rotmat[5] * iitBody[3] + rotmat[6] * iitBody[6];
float t33 = rotmat[4] * iitBody[1] + rotmat[5] * iitBody[4] + rotmat[6] * iitBody[7];
float t38 = rotmat[4] * iitBody[2] + rotmat[5] * iitBody[5] + rotmat[6] * iitBody[8];
float t52 = rotmat[8] * iitBody[0] + rotmat[9] * iitBody[3] + rotmat[10] * iitBody[6];
float t57 = rotmat[8] * iitBody[1] + rotmat[9] * iitBody[4] + rotmat[10] * iitBody[7];
float t62 = rotmat[8] * iitBody[2] + rotmat[9] * iitBody[5] + rotmat[10] * iitBody[8];
Matrix3 iitWorld = Matrix3.Identity;
iitWorld[0] = t4 * rotmat[0] + t9 * rotmat[1] + t14 * rotmat[2];
iitWorld[1] = t4 * rotmat[4] + t9 * rotmat[5] + t14 * rotmat[6];
iitWorld[2] = t4 * rotmat[8] + t9 * rotmat[9] + t14 * rotmat[10];
iitWorld[3] = t28 * rotmat[0] + t33 * rotmat[1] + t38 * rotmat[2];
iitWorld[4] = t28 * rotmat[4] + t33 * rotmat[5] + t38 * rotmat[6];
iitWorld[5] = t28 * rotmat[8] + t33 * rotmat[9] + t38 * rotmat[10];
iitWorld[6] = t52 * rotmat[0] + t57 * rotmat[1] + t62 * rotmat[2];
iitWorld[7] = t52 * rotmat[4] + t57 * rotmat[5] + t62 * rotmat[6];
iitWorld[8] = t52 * rotmat[8] + t57 * rotmat[9] + t62 * rotmat[10];
return iitWorld;
}
public void CalculateInternals()
{
transform = Body.GetTransform() * Offset;
}
public static Matrix4 Inverse(Matrix4 m)
{
Matrix4 result = Matrix4.Identity;
result.SetInverse(m);
return result;
}
public static Vector3 Transform(Vector3 p, Matrix4 m)
{
return new Vector3(m[0, 0] * p.X + m[0, 1] * p.Y + m[0, 2] * p.Z + m[0, 3],
m[1, 0] * p.X + m[1, 1] * p.Y + m[1, 2] * p.Z + m[1, 3],
m[2, 0] * p.X + m[2, 1] * p.Y + m[2, 2] * p.Z + m[2, 3]);
}
public static Vector3 TransformNormal(Vector3 v, Matrix4 m)
{
return (new Vector3(m[0, 0] * v.X + m[0, 1] * v.Y + m[0, 2] * v.Z,
m[1, 0] * v.X + m[1, 1] * v.Y + m[1, 2] * v.Z,
m[2, 0] * v.X + m[2, 1] * v.Y + m[2, 2] * v.Z));
}
public virtual void ComputeBoundingBox()
{
BBox object_bbox = Object.GetBoundingBox();
Vector3[] v = new Vector3[8];
v[0].X = object_bbox.Min.X; v[0].Y = object_bbox.Min.Y; v[0].Z = object_bbox.Min.Z;
v[1].X = object_bbox.Max.X; v[1].Y = object_bbox.Min.Y; v[1].Z = object_bbox.Min.Z;
v[2].X = object_bbox.Max.X; v[2].Y = object_bbox.Max.Y; v[2].Z = object_bbox.Min.Z;
v[3].X = object_bbox.Min.X; v[3].Y = object_bbox.Max.Y; v[3].Z = object_bbox.Min.Z;
v[4].X = object_bbox.Min.X; v[4].Y = object_bbox.Min.Y; v[4].Z = object_bbox.Max.Z;
v[5].X = object_bbox.Max.X; v[5].Y = object_bbox.Min.Y; v[5].Z = object_bbox.Max.Z;
v[6].X = object_bbox.Max.X; v[6].Y = object_bbox.Max.Y; v[6].Z = object_bbox.Max.Z;
v[7].X = object_bbox.Min.X; v[7].Y = object_bbox.Max.Y; v[7].Z = object_bbox.Max.Z;
v[0] = Vector3.Transform(v[0], forward_matrix);
v[1] = Vector3.Transform(v[1], forward_matrix);
v[2] = Vector3.Transform(v[2],forward_matrix);
v[3] = Vector3.Transform(v[3], forward_matrix);
v[4] = Vector3.Transform( v[4], forward_matrix);
v[5] = Vector3.Transform( v[5], forward_matrix);
v[6] = Vector3.Transform(v[6], forward_matrix);
v[7] = Vector3.Transform( v[7], forward_matrix);
forward_matrix = Matrix4.Identity;
float x0 = float.MaxValue;
float y0 = float.MaxValue;
float z0 = float.MaxValue;
for (int j = 0; j <= 7; j++)
{
if (v[j].X < x0)
x0 = v[j].X;
}
for (int j = 0; j <= 7; j++)
{
if (v[j].Y < y0)
y0 = v[j].Y;
}
for (int j = 0; j <= 7; j++)
{
if (v[j].Z < z0)
z0 = v[j].Z;
}
float x1 = float.MinValue;
float y1 = float.MinValue;
float z1 = float.MinValue;
for (int j = 0; j <= 7; j++)
{
if (v[j].X > x1)
x1 = v[j].X;
}
for (int j = 0; j <= 7; j++)
{
if (v[j].Y > y1)
y1 = v[j].Y;
}
for (int j = 0; j <= 7; j++)
{
if (v[j].Z > z1)
z1 = v[j].Z;
}
bbox.Min.X = x0;
bbox.Min.Y = y0;
bbox.Min.Z = z0;
bbox.Max.X = x1;
bbox.Max.Y = y1;
bbox.Max.Z = z1;
}
//public void RotateX(float theta)
//{
// Matrix4 rotation_matrix = Matrix4.CreateRotation(new Vector3(1, 0, 0), theta);
// Matrix4 inv_rotation_matrix = Matrix4.CreateRotation(new Vector3(1, 0, 0), -theta);
// inv_matrix = inv_matrix * inv_rotation_matrix;
// forward_matrix = rotation_matrix * forward_matrix;
//}
//public void RotateY(float theta)
//{
// Matrix4 rotation_matrix = Matrix4.CreateRotation(new Vector3(0, 1, 0), theta);
// Matrix4 inv_rotation_matrix = Matrix4.CreateRotation(new Vector3(0, 1, 0), -theta);
// inv_matrix = inv_matrix * inv_rotation_matrix;
// forward_matrix = rotation_matrix * forward_matrix;
// //Console.WriteLine(inv_matrix);
// //Console.WriteLine(Matrix4.Inverse(forward_matrix));
// //float sin_theta = MathHelper.Sin(theta * MathHelper.PI / 180.0f);
// //float cos_theta = MathHelper.Cos(theta * MathHelper.PI / 180.0f);
// //Matrix4 inv_y_rotation_matrix = Matrix4.Identity; // temporary inverse rotation matrix about y axis
// //inv_y_rotation_matrix[0, 0] = cos_theta;
// //inv_y_rotation_matrix[0, 2] = -sin_theta;
// //inv_y_rotation_matrix[2, 0] = sin_theta;
// //inv_y_rotation_matrix[2, 2] = cos_theta;
// //inv_matrix = inv_matrix * inv_y_rotation_matrix;
// //Matrix4 y_rotation_matrix = Matrix4.Identity; // temporary rotation matrix about x axis
// //y_rotation_matrix[0, 0] = cos_theta;
// //y_rotation_matrix[0, 2] = sin_theta;
// //y_rotation_matrix[2, 0] = -sin_theta;
// //y_rotation_matrix[2, 2] = cos_theta;
// //forward_matrix = y_rotation_matrix * forward_matrix;
//}
//public void RotateZ(float theta)
//{
// Matrix4 rotation_matrix = Matrix4.CreateRotation(new Vector3(0, 0, 1), theta);
// Matrix4 inv_rotation_matrix = Matrix4.CreateRotation(new Vector3(0, 0, 1), -theta);
// inv_matrix = inv_matrix * inv_rotation_matrix;
// forward_matrix = rotation_matrix * forward_matrix;
//}
//public void Rotate(Vector3 axis, float theta)
//{
// //Matrix4 rotation_matrix = Matrix4.CreateRotation(axis, theta);
// //Matrix4 inv_rotation_matrix = Matrix4.CreateRotation(axis, -theta);
// //inv_matrix = inv_matrix * inv_rotation_matrix;
// //forward_matrix = rotation_matrix * forward_matrix;
//}
//public void Scale(Vector3 scale)
//{
// Matrix4 scale_matrix = Matrix4.CreateScale(scale);
// Matrix4 inv_scale_matrix = Matrix4.CreateScale(new Vector3(1.0f / scale.X, 1.0f / scale.Y, 1.0f / scale.Z));
// inv_matrix = inv_matrix * inv_scale_matrix;
// forward_matrix = scale_matrix * forward_matrix;
//}
public void Transform(Matrix4 m)
{
Matrix4 inv = m.Inverse();
inv_matrix *= inv;
forward_matrix = m * forward_matrix;
}
public static Vector3 Transform(Vector3 vector, Matrix4 matrix)
{
float[] data = matrix;
return new Vector3(
vector.X * data[0] + vector.Y * data[1] + vector.Z * data[2] + data[3],
vector.X * data[4] + vector.Y * data[5] + vector.Z * data[6] + data[7],
vector.X * data[8] + vector.Y * data[9] + vector.Z * data[10] + data[11]);
}
public static Vector3 TransformDirection(Vector3 vector, Matrix4 matrix)
{
return matrix.TransformDirection(vector);
}
public static Matrix4 CreateFromScale(Vector3 scale)
{
Matrix4 ret = new Matrix4(new float[12]
{
scale.X,0,0,0,
0,scale.Y,0,0,
0,0,scale.Z,0
});
return ret;
}
public void GetOrientation(ref Matrix4 matrix)
{
matrix[0] = TransformMatrix[0];
matrix[1] = TransformMatrix[1];
matrix[2] = TransformMatrix[2];
matrix[3] = TransformMatrix[4];
matrix[4] = TransformMatrix[5];
matrix[5] = TransformMatrix[6];
matrix[6] = TransformMatrix[8];
matrix[7] = TransformMatrix[9];
matrix[8] = TransformMatrix[10];
}
static internal void _calculateTransformMatrix(out Matrix4 transformMatrix, Vector3 position, Quaternion orientation)
{
transformMatrix = Matrix4.Identity;
transformMatrix[0] = 1.0f - 2.0f * orientation.Z * orientation.Z -
2.0f * orientation.W * orientation.W;
transformMatrix[1] = 2.0f * orientation.Y * orientation.Z -
2.0f * orientation.X * orientation.W;
transformMatrix[2] = 2.0f * orientation.Y * orientation.W +
2.0f * orientation.X * orientation.Z;
transformMatrix[3] = position.X;
transformMatrix[4] = 2.0f * orientation.Y * orientation.Z +
2.0f * orientation.X * orientation.W;
transformMatrix[5] = 1.0f - 2.0f * orientation.Y * orientation.Y -
2.0f * orientation.W * orientation.W;
transformMatrix[6] = 2.0f * orientation.Z * orientation.W -
2.0f * orientation.X * orientation.Y;
transformMatrix[7] = position.Y;
transformMatrix[8] = 2.0f * orientation.Y * orientation.W -
2.0f * orientation.X * orientation.Z;
transformMatrix[9] = 2.0f * orientation.Z * orientation.W +
2.0f * orientation.X * orientation.Y;
transformMatrix[10] = 1.0f - 2.0f * orientation.Y * orientation.Y -
2.0f * orientation.Z * orientation.Z;
transformMatrix[11] = position.Z;
}
public void CalculateDerivedData()
{
Orientation.Normalize();
TransformMatrix = _calculateTransformMatrix(Position, Orientation);
InverseInertiaTensorWorld = _transformInertiaTensor(Orientation, InverseInertiaTensor, TransformMatrix);
}
static string toStringMatrix(Matrix4 m)
{
string s = "";
for (int r = 0; r < 4; r++)
{
for (int c = 0; c < 4; c++)
{
s += m[r, c] + ",";
}
}
s = s.Remove(s.Length - 1);
return s;
}
