Skip to content

Commit 10a1266

Browse files
otac0notac0n
committed
Fix Matrix Decompose.
1 parent 69c3403 commit 10a1266

1 file changed

Lines changed: 120 additions & 95 deletions

File tree

‎sources/Maths/Maths/Matrix4X4.Ops.cs‎

Lines changed: 120 additions & 95 deletions
Original file line numberDiff line numberDiff line change
@@ -20,26 +20,6 @@ public static partial class Matrix4X4
2020
#endif
2121
private const float DecomposeEpsilon = 0.0001f;
2222

23-
/*
24-
private struct CanonicalBasis<T>
25-
where T : INumberBase<T>
26-
{
27-
public Vector3D<T> Row0;
28-
public Vector3D<T> Row1;
29-
public Vector3D<T> Row2;
30-
};
31-
32-
private struct VectorBasis<T>
33-
where T : INumberBase<T>
34-
{
35-
#pragma warning disable 649
36-
public unsafe Vector3D<T>* Element0;
37-
public unsafe Vector3D<T>* Element1;
38-
public unsafe Vector3D<T>* Element2;
39-
#pragma warning restore 649
40-
}
41-
*/
42-
4323
/// <summary>Creates a spherical billboard that rotates around a specified object position.</summary>
4424
/// <param name="objectPosition">Position of the object the billboard will rotate around.</param>
4525
/// <param name="cameraPosition">Position of the camera.</param>
@@ -1210,136 +1190,181 @@ public static bool Decompose<T>(Matrix4X4<T> matrix, out Vector3D<T> scale, out
12101190
where T : INumber<T>, IRootFunctions<T>, ITrigonometricFunctions<T>
12111191
{
12121192
bool result = true;
1193+
scale = default;
12131194

1214-
translation = new Vector3D<T>(matrix.M41, matrix.M42, matrix.M43);
1195+
Vector3D<T>[] vectorBasis = new Vector3D<T>[3];
1196+
Matrix3X3<T> canonicalBasis = Matrix3X3<T>.Identity;
1197+
Matrix4X4<T> matTemp;
12151198

1216-
// Extract basis vectors (rows)
1217-
var basis = new Vector3D<T>[3];
1218-
basis[0] = new Vector3D<T>(matrix.M11, matrix.M12, matrix.M13);
1219-
basis[1] = new Vector3D<T>(matrix.M21, matrix.M22, matrix.M23);
1220-
basis[2] = new Vector3D<T>(matrix.M31, matrix.M32, matrix.M33);
1199+
translation = new Vector3D<T>(
1200+
matrix.M41,
1201+
matrix.M42,
1202+
matrix.M43);
12211203

1222-
// Compute scales
1223-
var scales = new T[3];
1224-
scales[0] = basis[0].Length;
1225-
scales[1] = basis[1].Length;
1226-
scales[2] = basis[2].Length;
1204+
matTemp = new Matrix4X4<T>(
1205+
matrix.M11, matrix.M12, matrix.M13, T.Zero,
1206+
matrix.M21, matrix.M22, matrix.M23, T.Zero,
1207+
matrix.M31, matrix.M32, matrix.M33, T.Zero,
1208+
T.Zero, T.Zero, T.Zero, T.One);
12271209

1228-
scale = new Vector3D<T>(scales[0], scales[1], scales[2]);
1210+
T x = scale.X = matTemp.Row1.Length;
1211+
T y = scale.Y = matTemp.Row2.Length;
1212+
T z = scale.Z = matTemp.Row3.Length;
12291213

1230-
// Rank axes by scale magnitude
1231-
uint a, b, c;
1214+
int a, b, c;
1215+
if (!(x >= y))
12321216
{
1233-
T x = scales[0], y = scales[1], z = scales[2];
1234-
1235-
if (!(x >= y))
1217+
if (!(y >= z))
12361218
{
1237-
if (!(y >= z))
1238-
{ a = 2; b = 1; c = 0; }
1219+
a = 2;
1220+
b = 1;
1221+
c = 0;
1222+
}
1223+
else
1224+
{
1225+
a = 1;
1226+
1227+
if (!(x >= z))
1228+
{
1229+
b = 2;
1230+
c = 0;
1231+
}
12391232
else
12401233
{
1241-
a = 1;
1242-
if (!(x >= z))
1243-
{ b = 2; c = 0; }
1244-
else
1245-
{ b = 0; c = 2; }
1234+
b = 0;
1235+
c = 2;
12461236
}
12471237
}
1238+
}
1239+
else
1240+
{
1241+
if (!(x >= z))
1242+
{
1243+
a = 2;
1244+
b = 0;
1245+
c = 1;
1246+
}
12481247
else
12491248
{
1250-
if (!(x >= z))
1251-
{ a = 2; b = 0; c = 1; }
1249+
a = 0;
1250+
1251+
if (!(y >= z))
1252+
{
1253+
b = 2;
1254+
c = 1;
1255+
}
12521256
else
12531257
{
1254-
a = 0;
1255-
if (!(y >= z))
1256-
{ b = 2; c = 1; }
1257-
else
1258-
{ b = 1; c = 2; }
1258+
b = 1;
1259+
c = 2;
12591260
}
12601261
}
12611262
}
12621263

1263-
var canonical = new[]
1264-
{
1265-
new Vector3D<T>(T.One, T.Zero, T.Zero),
1266-
new Vector3D<T>(T.Zero, T.One, T.Zero),
1267-
new Vector3D<T>(T.Zero, T.Zero, T.One)
1268-
};
1269-
12701264
T eps = T.CreateTruncating(DecomposeEpsilon);
12711265

1272-
if (!(scales[a] >= eps))
1273-
basis[a] = canonical[a];
1266+
if (!(scale[a] >= eps))
1267+
{
1268+
var normalA = canonicalBasis[a];
1269+
matTemp[a][0] = normalA[0];
1270+
matTemp[a][1] = normalA[1];
1271+
matTemp[a][2] = normalA[2];
1272+
}
12741273

1275-
basis[a] = Vector3D.Normalize(basis[a]);
1274+
matTemp[a] = matTemp[a].Normalize();
12761275

1277-
if (!(scales[b] >= eps))
1276+
if (!(scale[b] >= eps))
12781277
{
1279-
T ax = T.Abs(basis[a].X);
1280-
T ay = T.Abs(basis[a].Y);
1281-
T az = T.Abs(basis[a].Z);
1278+
int cc;
1279+
T absX, absY, absZ;
1280+
1281+
absX = T.Abs(matTemp[a].X);
1282+
absY = T.Abs(matTemp[a].Y);
1283+
absZ = T.Abs(matTemp[a].Z);
12821284

1283-
uint cc;
1284-
if (!(ax >= ay))
1285+
if (!(absX >= absY))
12851286
{
1286-
if (!(ay >= az))
1287+
if (!(absY >= absZ))
1288+
{
12871289
cc = 0;
1290+
}
12881291
else
1289-
cc = (!(ax >= az)) ? 0u : 2u;
1292+
{
1293+
if (!(absX >= absZ))
1294+
{
1295+
cc = 0;
1296+
}
1297+
else
1298+
{
1299+
cc = 2;
1300+
}
1301+
}
12901302
}
12911303
else
12921304
{
1293-
if (!(ax >= az))
1305+
if (!(absX >= absZ))
1306+
{
12941307
cc = 1;
1308+
}
12951309
else
1296-
cc = (!(ay >= az)) ? 1u : 2u;
1310+
{
1311+
if (!(absY >= absZ))
1312+
{
1313+
cc = 1;
1314+
}
1315+
else
1316+
{
1317+
cc = 2;
1318+
}
1319+
}
12971320
}
12981321

1299-
basis[b] = Vector3D.Cross(basis[a], canonical[cc]);
1322+
var normalB = Vector3D.Cross(new Vector3D<T>(matTemp[a].AsSpan()[^-1]), canonicalBasis[cc]);
1323+
matTemp[b][0] = normalB[0];
1324+
matTemp[b][1] = normalB[1];
1325+
matTemp[b][2] = normalB[2];
13001326
}
13011327

1302-
basis[b] = Vector3D.Normalize(basis[b]);
1303-
1304-
if (!(scales[c] >= eps))
1305-
basis[c] = Vector3D.Cross(basis[a], basis[b]);
1328+
matTemp[b] = matTemp[b].Normalize();
13061329

1307-
basis[c] = Vector3D.Normalize(basis[c]);
1330+
if (!(scale[c] >= eps))
1331+
{
1332+
var normalC = Vector3D.Cross(new Vector3D<T>(matTemp[a].AsSpan()), new Vector3D<T>(matTemp[b].AsSpan()));
1333+
matTemp[c][0] = normalC[0];
1334+
matTemp[c][1] = normalC[1];
1335+
matTemp[c][2] = normalC[2];
1336+
}
13081337

1309-
// Rebuild orthonormal rotation matrix
1310-
var rotationMatrix = new Matrix4X4<T>(
1311-
basis[0].X, basis[0].Y, basis[0].Z, T.Zero,
1312-
basis[1].X, basis[1].Y, basis[1].Z, T.Zero,
1313-
basis[2].X, basis[2].Y, basis[2].Z, T.Zero,
1314-
T.Zero, T.Zero, T.Zero, T.Zero);
1338+
matTemp[c] = matTemp[c].Normalize();
13151339

1316-
T det = rotationMatrix.GetDeterminant();
1340+
T det = matTemp.GetDeterminant();
13171341

1318-
// Fix handedness
1342+
// use Kramer's rule to check for handedness of coordinate system
13191343
if (!(det >= T.Zero))
13201344
{
1321-
scales[a] = -scales[a];
1345+
// switch coordinate system by negating the scale and inverting the basis vector on the x-axis
1346+
scale[a] = -scale[a];
1347+
matTemp[a][0] = -matTemp[a][0];
1348+
matTemp[a][1] = -matTemp[a][1];
1349+
matTemp[a][2] = -matTemp[a][2];
13221350
det = -det;
1323-
1324-
rotationMatrix.Row1 = -rotationMatrix.Row1;
1325-
rotationMatrix.Row2 = -rotationMatrix.Row2;
1326-
rotationMatrix.Row3 = -rotationMatrix.Row3;
13271351
}
13281352

1329-
T diff = det - T.One;
1330-
diff *= diff;
1353+
det = det - T.One;
1354+
det = det * det;
13311355

1332-
if (!(eps >= diff))
1356+
if (!(eps >= det))
13331357
{
1358+
// Non-SRT matrix encountered
13341359
rotation = Quaternion<T>.Identity;
13351360
result = false;
13361361
}
13371362
else
13381363
{
1339-
rotation = Quaternion<T>.CreateFromRotationMatrix(rotationMatrix);
1364+
// generate the quaternion from the matrix
1365+
rotation = Quaternion<T>.CreateFromRotationMatrix(matTemp);
13401366
}
13411367

1342-
scale = new Vector3D<T>(scales[0], scales[1], scales[2]);
13431368
return result;
13441369
}
13451370

0 commit comments

Comments
 (0)