Skip to content

Commit b36e272

Browse files
otac0notac0n
committed
Add (preliminary, possibly subtly wrong) implementations of Matrix4X4.Decompose and VectorXD.Transform(Vector, Quaternion).
1 parent 8e06b11 commit b36e272

8 files changed

Lines changed: 523 additions & 239 deletions

File tree

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

Lines changed: 104 additions & 161 deletions
Original file line numberDiff line numberDiff line change
@@ -1199,7 +1199,6 @@ private static Vector128<T> Permute(Vector128<T> value, byte control)
11991199
}
12001200
}*/
12011201

1202-
/*
12031202
/// <summary>Attempts to extract the scale, translation, and rotation components from the given scale/rotation/translation matrix.
12041203
/// If successful, the out parameters will contained the extracted values.</summary>
12051204
/// <param name="matrix">The source matrix.</param>
@@ -1212,193 +1211,137 @@ public static bool Decompose<T>(Matrix4X4<T> matrix, out Vector3D<T> scale, out
12121211
{
12131212
bool result = true;
12141213

1215-
unsafe
1216-
{
1217-
fixed (Vector3D<T>* scaleBase = &scale)
1218-
{
1219-
T* pfScales = (T*) scaleBase;
1220-
T det;
1221-
1222-
VectorBasis<T> vectorBasis;
1223-
Vector3D<T>** pVectorBasis = (Vector3D<T>**) &vectorBasis;
1214+
translation = new Vector3D<T>(matrix.M41, matrix.M42, matrix.M43);
12241215

1225-
Matrix4X4<T> matTemp = Matrix4X4<T>.Identity;
1226-
CanonicalBasis<T> canonicalBasis = default;
1227-
Vector3D<T>* pCanonicalBasis = &canonicalBasis.Row0;
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);
12281221

1229-
canonicalBasis.Row0 = new Vector3D<T>(T.One, T.Zero, T.Zero);
1230-
canonicalBasis.Row1 = new Vector3D<T>(T.Zero, T.One, T.Zero);
1231-
canonicalBasis.Row2 = new Vector3D<T>(T.Zero, T.Zero, T.One);
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;
12321227

1233-
translation = new Vector3D<T>(
1234-
matrix.M41,
1235-
matrix.M42,
1236-
matrix.M43);
1228+
scale = new Vector3D<T>(scales[0], scales[1], scales[2]);
12371229

1238-
pVectorBasis[0] = (Vector3D<T>*) &matTemp.Row1;
1239-
pVectorBasis[1] = (Vector3D<T>*) &matTemp.Row2;
1240-
pVectorBasis[2] = (Vector3D<T>*) &matTemp.Row3;
1241-
1242-
*(pVectorBasis[0]) = new Vector3D<T>(matrix.M11, matrix.M12, matrix.M13);
1243-
*(pVectorBasis[1]) = new Vector3D<T>(matrix.M21, matrix.M22, matrix.M23);
1244-
*(pVectorBasis[2]) = new Vector3D<T>(matrix.M31, matrix.M32, matrix.M33);
1245-
1246-
scale.X = pVectorBasis[0]->Length;
1247-
scale.Y = pVectorBasis[1]->Length;
1248-
scale.Z = pVectorBasis[2]->Length;
1230+
// Rank axes by scale magnitude
1231+
uint a, b, c;
1232+
{
1233+
T x = scales[0], y = scales[1], z = scales[2];
12491234

1250-
uint a, b, c;
1251-
#region Ranking
1252-
T x = pfScales[0], y = pfScales[1], z = pfScales[2];
1253-
if (!(x >= y))
1235+
if (!(x >= y))
1236+
{
1237+
if (!(y >= z))
1238+
{ a = 2; b = 1; c = 0; }
1239+
else
12541240
{
1255-
if (!(y >= z))
1256-
{
1257-
a = 2;
1258-
b = 1;
1259-
c = 0;
1260-
}
1241+
a = 1;
1242+
if (!(x >= z))
1243+
{ b = 2; c = 0; }
12611244
else
1262-
{
1263-
a = 1;
1264-
1265-
if (!(x >= z))
1266-
{
1267-
b = 2;
1268-
c = 0;
1269-
}
1270-
else
1271-
{
1272-
b = 0;
1273-
c = 2;
1274-
}
1275-
}
1245+
{ b = 0; c = 2; }
12761246
}
1247+
}
1248+
else
1249+
{
1250+
if (!(x >= z))
1251+
{ a = 2; b = 0; c = 1; }
12771252
else
12781253
{
1279-
if (!(x >= z))
1280-
{
1281-
a = 2;
1282-
b = 0;
1283-
c = 1;
1284-
}
1254+
a = 0;
1255+
if (!(y >= z))
1256+
{ b = 2; c = 1; }
12851257
else
1286-
{
1287-
a = 0;
1288-
1289-
if (!(y >= z))
1290-
{
1291-
b = 2;
1292-
c = 1;
1293-
}
1294-
else
1295-
{
1296-
b = 1;
1297-
c = 2;
1298-
}
1299-
}
1258+
{ b = 1; c = 2; }
13001259
}
1301-
#endregion
1260+
}
1261+
}
13021262

1303-
if (!(pfScales[a] >= T.CreateTruncating(DecomposeEpsilon)))
1304-
{
1305-
*(pVectorBasis[a]) = pCanonicalBasis[a];
1306-
}
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+
};
13071269

1308-
*pVectorBasis[a] = Vector3D.Normalize(*pVectorBasis[a]);
1270+
T eps = T.CreateTruncating(DecomposeEpsilon);
13091271

1310-
if (!(pfScales[b] >= T.CreateTruncating(DecomposeEpsilon)))
1311-
{
1312-
uint cc;
1313-
T fAbsX, fAbsY, fAbsZ;
1314-
1315-
fAbsX = T.Abs(pVectorBasis[a]->X);
1316-
fAbsY = T.Abs(pVectorBasis[a]->Y);
1317-
fAbsZ = T.Abs(pVectorBasis[a]->Z);
1318-
1319-
#region Ranking
1320-
if (!(fAbsX >= fAbsY))
1321-
{
1322-
if (!(fAbsY >= fAbsZ))
1323-
{
1324-
cc = 0;
1325-
}
1326-
else
1327-
{
1328-
if (!(fAbsX >= fAbsZ))
1329-
{
1330-
cc = 0;
1331-
}
1332-
else
1333-
{
1334-
cc = 2;
1335-
}
1336-
}
1337-
}
1338-
else
1339-
{
1340-
if (!(fAbsX >= fAbsZ))
1341-
{
1342-
cc = 1;
1343-
}
1344-
else
1345-
{
1346-
if (!(fAbsY >= fAbsZ))
1347-
{
1348-
cc = 1;
1349-
}
1350-
else
1351-
{
1352-
cc = 2;
1353-
}
1354-
}
1355-
}
1356-
#endregion
1357-
1358-
*pVectorBasis[b] = Vector3D.Cross(*pVectorBasis[a], *(pCanonicalBasis + cc));
1359-
}
1272+
if (!(scales[a] >= eps))
1273+
basis[a] = canonical[a];
13601274

1361-
*pVectorBasis[b] = Vector3D.Normalize(*pVectorBasis[b]);
1275+
basis[a] = Vector3D.Normalize(basis[a]);
13621276

1363-
if (!(pfScales[c] >= T.CreateTruncating(DecomposeEpsilon)))
1364-
{
1365-
*pVectorBasis[c] = Vector3D.Cross(*pVectorBasis[a], *pVectorBasis[b]);
1366-
}
1277+
if (!(scales[b] >= eps))
1278+
{
1279+
T ax = T.Abs(basis[a].X);
1280+
T ay = T.Abs(basis[a].Y);
1281+
T az = T.Abs(basis[a].Z);
13671282

1368-
*pVectorBasis[c] = Vector3D.Normalize(*pVectorBasis[c]);
1283+
uint cc;
1284+
if (!(ax >= ay))
1285+
{
1286+
if (!(ay >= az))
1287+
cc = 0;
1288+
else
1289+
cc = (!(ax >= az)) ? 0u : 2u;
1290+
}
1291+
else
1292+
{
1293+
if (!(ax >= az))
1294+
cc = 1;
1295+
else
1296+
cc = (!(ay >= az)) ? 1u : 2u;
1297+
}
13691298

1370-
det = matTemp.GetDeterminant();
1299+
basis[b] = Vector3D.Cross(basis[a], canonical[cc]);
1300+
}
13711301

1372-
// use Kramer's rule to check for handedness of coordinate system
1373-
if (!(det >= T.Zero))
1374-
{
1375-
// switch coordinate system by negating the scale and inverting the basis vector on the x-axis
1376-
pfScales[a] = -pfScales[a];
1377-
*pVectorBasis[a] = -(*pVectorBasis[a]);
1302+
basis[b] = Vector3D.Normalize(basis[b]);
13781303

1379-
det = -det;
1380-
}
1304+
if (!(scales[c] >= eps))
1305+
basis[c] = Vector3D.Cross(basis[a], basis[b]);
13811306

1382-
det = det - T.One;
1383-
det = det * det;
1307+
basis[c] = Vector3D.Normalize(basis[c]);
13841308

1385-
if (!(T.CreateTruncating(DecomposeEpsilon) >= det))
1386-
{
1387-
// Non-SRT matrix encountered
1388-
rotation = Quaternion<T>.Identity;
1389-
result = false;
1390-
}
1391-
else
1392-
{
1393-
// generate the quaternion from the matrix
1394-
rotation = Quaternion<T>.CreateFromRotationMatrix(matTemp);
1395-
}
1396-
}
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);
1315+
1316+
T det = rotationMatrix.GetDeterminant();
1317+
1318+
// Fix handedness
1319+
if (!(det >= T.Zero))
1320+
{
1321+
scales[a] = -scales[a];
1322+
det = -det;
1323+
1324+
rotationMatrix.Row1 = -rotationMatrix.Row1;
1325+
rotationMatrix.Row2 = -rotationMatrix.Row2;
1326+
rotationMatrix.Row3 = -rotationMatrix.Row3;
13971327
}
13981328

1329+
T diff = det - T.One;
1330+
diff *= diff;
1331+
1332+
if (!(eps >= diff))
1333+
{
1334+
rotation = Quaternion<T>.Identity;
1335+
result = false;
1336+
}
1337+
else
1338+
{
1339+
rotation = Quaternion<T>.CreateFromRotationMatrix(rotationMatrix);
1340+
}
1341+
1342+
scale = new Vector3D<T>(scales[0], scales[1], scales[2]);
13991343
return result;
14001344
}
1401-
*/
14021345

14031346
/// <summary>Transforms the given matrix by applying the given Quaternion rotation.</summary>
14041347
/// <param name="value">The source matrix to transform.</param>

0 commit comments

Comments
 (0)