koobas.hobune.stream
Generate a 3x3 rotation matrix from a Matrix4x4
I'm working through Real Time Collision Detection, and generating my own Collision system based off of Unity's Colliders. (Mostly to learn more about collision algorithms) I'm just having a hard time figuring out how to convert the built in Matrix4x4 to a 3x3. Right now I'm using the 4x4 Matrix to Update the the center and HalfWidths of my boxes.
This is what that code looks like (I know it needs some redesign for readability):
public void UpdateAABB(AABB3D a, float[][] m, Vector3 t, ref AABB3D b)
{
Vector3 modify = b.Center;
modify.x = t.x;
modify.y = t.y;
modify.z = t.z;
b.HalfHeight = 0.0f;
b.HalfWidth = 0.0f;
b.HalfDepth = 0.0f;
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
if (i == 0)
{
if(j == 0)
{
modify.x += m[j][i] * a.Center.x;
b.HalfWidth += Math.Abs(m[j][i]) * a.HalfWidth;
}
else if (j == 1)
{
modify.x += m[j][i]*a.center.y;
b.HalfWidth += Math.Abs(m[j][i]) * a.HalfHeight;
}
else if (j == 2)
{
modify.x += m[j][i]*a.center.z;
b.HalfWidth += Math.Abs(m[j][i])*a.HalfDepth;
}
}
else if (i == 1)
{
if(j == 0)
{
modify.y += m[j][i] * a.Center.x;
b.HalfHeight += Math.Abs(m[j][i]) * a.HalfWidth;
}
else if(j == 1)
{
modify.y += m[j][i] * a.Center.y;
b.HalfHeight += Math.Abs(m[j][i]) * a.HalfHeight;
}
else if (j == 2)
{
modify.y += m[j][i]*a.Center.z;
b.HalfHeight += Math.Abs(m[j][i])*a.HalfDepth;
}
}
else if (i == 2)
{
if (j == 0)
{
modify.z += m[j][i]*a.Center.x;
b.HalfDepth += Math.Abs(m[j][i])*a.HalfWidth;
}
else if (j == 1)
{
modify.z += m[j][i]*a.Center.y;
b.HalfDepth += Math.Abs(m[j][i])*a.HalfHeight;
}
else if (j == 2)
{
modify.z += m[j][i]*a.Center.z;
b.HalfDepth += Math.Abs(m[j][i])*a.HalfDepth;
}
}
}
}
b.Center = modify;
}
This is how it gets called:
matrix4X4 = Matrix4x4.TRS(transform.position, transform.rotation, Vector3.one);
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
rotation[i][j] = matrix4X4[i, j];
}
}
boundingBox.UpdateAABB(rotationBox, rotation, transform.position, ref boundingBox);
It works fine for re-sizing boxes when I had the algorithm set up for 2D, but won't update the z axis correctly in 3D. I believe it comes from Unity's 4x4 Matrix. So, how could I go about building a 3x3 Matrix from either the Matrix4x4 class or a Quaternion?
Any direct mapping is going to be dependent on unpublished and thus unstable assumptions about the internal layout of Unity's matrices.
I suggest you extract a quaternion from the 4x4 and then convert back to your 3x3 representation. That will at least be stable.
Its more work but if you intelligently cache then you shouldn't have to do it that often.
If you REALLY want to write unstable code, then you should be able to find out easily yourself how the matrix is organized. Put known values for the X, Y, Z rotations and then dump the matrix and see where they are.
In case anyone else needs a 3x3 Matrix here's how I did it:
private static float[][] identityMatrix =
{
new [] {1.0f, 0.0f, 0.0f},
new []{0.0f, 1.0f, 0.0f},
new []{0.0f, 0.0f, 1.0f}
};
public static float[][] QuaternionTo3x3(this Quaternion value)
{
float[][] matrix3x3 =
{
new float[3],
new float[3],
new float[3],
};
float[][] symetricalMatrix =
{
new float[3] {(-(value.y * value.y) - (value.z * value.z)), value.x * value.y, value.x * value.z},
new float[3] {value.x * value.y, (-(value.x * value.x) - (value.z * value.z)), value.y * value.z},
new float[3] {value.x * value.z, value.y * value.z, (-(value.x * value.x) - (value.y * value.y))}
};
float[][] antiSymetricalMatrix =
{
new[] {0.0f, -value.z, value.y},
new []{value.z, 0.0f, -value.x},
new []{-value.y, value.x, 0.0f}
};
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
matrix3x3[i][j] = identityMatrix[i][j] +
(2.0f*symetricalMatrix[i][j]) +
(2.0f*value.w * antiSymetricalMatrix[i][j]);
}
}
return matrix3x3;
}
Recommend you investigate the use of Math.NET numerics libraries in (C# solutions). For me to get the numerics library to work with the Unity3D C3(v3) scripts, I had to
download Math.NET Numerics via NUGET to a full .NET 4+ project (alternatively you could download from GITHUB at https://github.com/mathnet/mathnet-numerics ).
Then move the MathNet.Numerics.dll to the Assets/Plugins directory
Close and (re) open Unity, for Unity to load the dll as a reference within its older Visual Studio project version. You can delete or unload the the previously mentioned .NET 4.5 project, thereafter.
From there, it's possible to use their matrix and vector libraries (with so much more as well).
Cross reference on how to include a dll in Unity: http://answers.unity3d.com/questions/333829/including-a-dll-in-unity.html
Explanation for using a Math.NET Matrix: http://numerics.mathdotnet.com/Matrix.html
Some links on using Matrices with their Linear Regression examples.
http://christoph.ruegg.name/blog/linear-regression-mathnet-numerics.html
http://www.imagingshop.com/linear-and-nonlinear-least-squares-with-math-net/
I hope that's of use?
