public static void FromInto()
{
TrafoTesting.GenericTest(rnd =>
{
var dir = rnd.UniformV3d().Normalized;
var rotId = Rot3d.RotateInto(dir, dir);
var matId = (M33d)rotId;
Assert.IsTrue(matId.IsIdentity(0));
var rot = Rot3d.RotateInto(dir, -dir);
var invDir = rot.Transform(dir);
Assert.IsTrue(invDir.ApproximateEquals(-dir, 1e-14));
var dirF = rnd.UniformV3f().Normalized;
var rotIdF = Rot3f.RotateInto(dirF, dirF);
var matIdF = (M33f)rotIdF;
Assert.IsTrue(matIdF.IsIdentity(0));
var rotF = Rot3f.RotateInto(dirF, -dirF);
var invDirF = rotF.Transform(dirF);
Assert.IsTrue(invDirF.ApproximateEquals(-dirF, 1e-6f));
});
}
public static void OrthoNormalOrientation()
{
// Previous implementation
Func <Trafo3d, Trafo3d> reference = trafo =>
{
var x = trafo.Forward.C0.XYZ.Normalized; // TransformDir(V3f.XAxis)
var y = trafo.Forward.C1.XYZ.Normalized; // TransformDir(V3f.YAxis)
var z = trafo.Forward.C2.XYZ.Normalized; // TransformDir(V3f.ZAxis)
y = z.Cross(x).Normalized;
z = x.Cross(y).Normalized;
return(Trafo3d.FromBasis(x, y, z, V3d.Zero));
};
var rnd = new RandomSystem(1);
TrafoTesting.GenericTest(rnd =>
{
var trafo = TrafoTesting.GetRandomTrafo(rnd);
var res = trafo.GetOrthoNormalOrientation();
var res_ref = reference(trafo);
TrafoTesting.AreEqual(res, res_ref);
});
}
public static void MultiplicationScaleTest()
=> TrafoTesting.GenericTest(rnd =>
{
var a = TrafoTesting.GetRandomRot2(rnd);
var b = TrafoTesting.GetRandomScale2(rnd);
var p = rnd.UniformV2d() * rnd.UniformInt(1000);
{
var trafo = a * b;
var res = trafo.TransformPos(p);
var trafoRef = (M33d)a * (M33d)b;
var resRef = trafoRef.TransformPos(p);
TrafoTesting.AreEqual(res, resRef);
}
{
var trafo = b * a;
var res = trafo.TransformPos(p);
var trafoRef = (M33d)b * (M33d)a;
var resRef = trafoRef.TransformPos(p);
TrafoTesting.AreEqual(res, resRef);
}
});
public static void FromEuler()
{
TrafoTesting.GenericTest(rnd =>
{
var euler = rnd.UniformV3dFull() * Constant.PiTimesFour - Constant.PiTimesTwo;
var rot = Rot3d.RotationEuler(euler);
var qx = Rot3d.RotationX(euler.X);
var qy = Rot3d.RotationY(euler.Y);
var qz = Rot3d.RotationZ(euler.Z);
var test = qz * qy * qx;
TrafoTesting.AreEqual(rot, test);
var euler2 = rot.GetEulerAngles();
var rot2 = Rot3d.RotationEuler(euler2);
var rot2M = (M33d)rot2;
var rotM = (M33d)rot;
if (!Fun.ApproximateEquals(rot2M, rotM, 1e-6))
{
Report.Line("FA");
}
Assert.IsTrue(Fun.ApproximateEquals(rot2M, rotM, 1e-6));
});
}
public static void MultiplicationFull2x3Test()
{
TrafoTesting.GenericTest(rnd =>
{
var m = TrafoTesting.GetRandom2x3(rnd);
var r = TrafoTesting.GetRandomRot2(rnd);
var mr = m * r;
var rm = r * m;
var mr_ref = m * (M33d)r;
var rm_ref = (M23d)r * (M33d)m;
{
var p = rnd.UniformV2d() * rnd.UniformInt(1000);
var res_mr = mr.TransformPos(p);
var res_mr_ref = mr_ref.TransformPos(p);
var res_rm = rm.TransformPos(p);
var res_rm_ref = rm_ref.TransformPos(p);
TrafoTesting.AreEqual(res_mr, res_mr_ref);
TrafoTesting.AreEqual(res_rm, res_rm_ref);
}
});
}
public static void Multiplication3x3Test()
=> TrafoTesting.GenericMatrixMultiplicationTest <Affine3d, M33d, M34d>(
rnd => TrafoTesting.GetRandomAffine(rnd, false),
Affine.TransformPos,
Mat.Transform,
Mat.TransformPos,
(a, v) => (a * new V4d(v, 1)).XYZ,
(m, v) => m * v,
(m, v) => m * new V4d(v, 1));
public static void FromM33dAndV3d()
=> TrafoTesting.GenericTest(rnd =>
{
var a = TrafoTesting.GetRandomEuclidean(rnd);
var m = (M33d)a.Rot;
var t = a.Trans;
var restored = Euclidean3d.FromM33dAndV3d(m, t);
TrafoTesting.AreEqual(a, restored);
});
public static void FromM33dAndV3d()
=> TrafoTesting.GenericTest(rnd =>
{
var a = TrafoTesting.GetRandomSimilarity(rnd);
var tmp = (M34d)a;
var m = (M33d)tmp;
var t = tmp.C3;
var restored = Similarity3d.FromM33dAndV3d(m, t);
TrafoTesting.AreEqual(a, restored);
});
public void TrafoDecomposeTest()
{
TrafoTesting.GenericTest((rnd, i) =>
{
var rot = rnd.UniformV3dFull() * Constant.PiTimesFour - Constant.PiTimesTwo;
var trans = rnd.UniformV3dFull() * 10 - 5;
var scale = rnd.UniformV3dFull() * 4 - 2;
TestDecompose(scale, rot, trans);
});
}
public void TrafoRotIntoTest()
{
TrafoTesting.GenericTest((rnd, i) =>
{
var rx = new V3d(rnd.UniformDouble() * 1e-17, 0, 0) * (rnd.UniformDouble() > 0.5 ? 1 : -1);
var ry = new V3d(0, rnd.UniformDouble() * 1e-17, 0) * (rnd.UniformDouble() > 0.5 ? 1 : -1);
var rz = new V3d(0, 0, rnd.UniformDouble() * 1e-17) * (rnd.UniformDouble() > 0.5 ? 1 : -1);
// equal cases
var req = new[]
{
Trafo3d.RotateInto(V3d.XAxis, (V3d.XAxis + ry).Normalized),
Trafo3d.RotateInto(V3d.YAxis, (V3d.YAxis + rz).Normalized),
Trafo3d.RotateInto(V3d.ZAxis, (V3d.ZAxis + rx).Normalized),
};
foreach (var r in req)
{
Assert.True(CheckForwardBackwardConsistency(r));
}
// 180° cases
var r180 = new[]
{
Trafo3d.RotateInto(-V3d.XAxis, (V3d.XAxis + ry).Normalized),
Trafo3d.RotateInto(V3d.XAxis, (-V3d.XAxis + rz).Normalized),
Trafo3d.RotateInto(-V3d.YAxis, (V3d.YAxis + rz).Normalized),
Trafo3d.RotateInto(V3d.YAxis, (-V3d.YAxis + rx).Normalized),
Trafo3d.RotateInto(-V3d.ZAxis, (V3d.ZAxis + rx).Normalized),
Trafo3d.RotateInto(V3d.ZAxis, (-V3d.ZAxis + ry).Normalized)
};
foreach (var r in r180)
{
Assert.True(CheckForwardBackwardConsistency(r));
}
// 90° cases
var r90 = new[]
{
Trafo3d.RotateInto((-V3d.XAxis + rz).Normalized, V3d.ZAxis),
Trafo3d.RotateInto((V3d.XAxis + ry).Normalized, V3d.ZAxis),
Trafo3d.RotateInto((-V3d.YAxis + rx).Normalized, V3d.ZAxis),
Trafo3d.RotateInto((V3d.YAxis + rz).Normalized, V3d.ZAxis),
Trafo3d.RotateInto(-V3d.XAxis, (-V3d.ZAxis + rx).Normalized),
Trafo3d.RotateInto(V3d.XAxis, (-V3d.ZAxis + rx).Normalized),
Trafo3d.RotateInto(-V3d.YAxis, (-V3d.ZAxis + ry).Normalized),
Trafo3d.RotateInto(V3d.YAxis, (-V3d.ZAxis + ry).Normalized),
};
foreach (var r in r90)
{
Assert.True(CheckForwardBackwardConsistency(r));
}
});
}
public static void DistanceTest()
{
TrafoTesting.GenericTest(rnd =>
{
var delta = (rnd.UniformDouble() - 0.5) * 2.0 * Constant.Pi;
var r1 = TrafoTesting.GetRandomRot2(rnd);
var r2 = new Rot2d(r1.Angle + (delta + Constant.PiTimesTwo * rnd.UniformInt(10)));
var dist = r1.Distance(r2);
TrafoTesting.AreEqual(dist, Fun.Abs(delta));
});
}
public static void MultiplicationAffine3x4Test()
=> TrafoTesting.GenericTest(rnd =>
{
var a = TrafoTesting.GetRandomAffine(rnd);
var b = TrafoTesting.GetRandomAffine(rnd);
var ma = (M34d)a;
var mb = (M34d)b;
var a_x_b = a * b;
var ma_x_mb = Mat.MultiplyAffine(ma, mb);
TrafoTesting.AreEqual((M34d)a_x_b, ma_x_mb);
});
public static void ConsistentWithMatrixRotationTest()
{
TrafoTesting.GenericTest(rnd =>
{
var angle = rnd.UniformDouble() * Constant.PiTimesTwo;
var m = M22d.Rotation(angle);
var r = new Rot2d(angle);
var p = rnd.UniformV2d() * rnd.UniformInt(1000);
var res = m.Transform(p);
var res2 = r.Transform(p);
TrafoTesting.AreEqual(res, res2);
});
}
public static void ConsistentWithMatrixScaleTest()
{
TrafoTesting.GenericTest(rnd =>
{
var scale = rnd.UniformV3d() * 10;
var m = M33d.Scale(scale);
var r = new Scale3d(scale);
var p = rnd.UniformV3d() * rnd.UniformInt(1000);
var res = m.Transform(p);
var res2 = r.Transform(p);
TrafoTesting.AreEqual(res, res2);
});
}
public static void ConsistentWithMatrixShiftTest()
{
TrafoTesting.GenericTest(rnd =>
{
var shift = rnd.UniformV3d() * 10;
var m = M34d.Translation(shift);
var r = new Shift3d(shift);
var p = rnd.UniformV3d() * rnd.UniformInt(1000);
var res = m.TransformPos(p);
var res2 = r.Transform(p);
TrafoTesting.AreEqual(res, res2);
});
}
public static void RotIntoCornerCase()
{
TrafoTesting.GenericTest(rnd =>
{
// some vectors will not normalize to 1.0 -> provoke numerical issues in Rot3d
var vecd = new V3d(0, 0, -rnd.UniformDouble());
var rotd = Rot3d.RotateInto(V3d.OOI, vecd.Normalized);
var testd = rotd.Transform(V3d.OOI);
Assert.True((testd + V3d.OOI).Length < 1e-8);
var vecf = new V3f(0, 0, -rnd.UniformDouble());
var rotf = Rot3f.RotateInto(V3f.OOI, vecf.Normalized);
var testf = rotf.Transform(V3f.OOI);
Assert.True((testf + V3f.OOI).Length < 1e-5);
});
}
public static void FromM33d()
{
TrafoTesting.GenericTest(rnd =>
{
var rot = rnd.UniformV3dFull() * Constant.PiTimesFour - Constant.PiTimesTwo;
var mat = M44d.RotationEuler(rot);
var mat2 = (M44d)Rot3d.FromM33d((M33d)mat);
Assert.IsFalse(mat.Elements.Any(x => x.IsNaN()), "NaN");
if (!Fun.ApproximateEquals(mat, mat2, 1e-9))
{
Assert.Fail("FAIL");
}
});
}
public static void NormalFrame()
{
TrafoTesting.GenericTest((rnd, i) =>
{
var n = rnd.UniformV3d().Normalized;
var basis = M33d.NormalFrame(n);
Assert.IsTrue(basis.C0.Length.ApproximateEquals(1, 1e-7));
Assert.IsTrue(basis.C1.Length.ApproximateEquals(1, 1e-7));
Assert.IsTrue(basis.C2.Length.ApproximateEquals(1, 1e-7));
Assert.IsTrue(basis.C0.AngleBetween(basis.C1).ApproximateEquals(Constant.PiHalf, 1e-7));
Assert.IsTrue(basis.C0.AngleBetween(basis.C2).ApproximateEquals(Constant.PiHalf, 1e-7));
Assert.IsTrue(basis.C1.AngleBetween(basis.C2).ApproximateEquals(Constant.PiHalf, 1e-7));
});
}
public static void InverseTest()
=> TrafoTesting.GenericTest(rnd =>
{
var t = TrafoTesting.GetRandomTrafo(rnd, false);
var p = rnd.UniformV3d() * rnd.UniformInt(1000);
var q = t.Forward.TransformPos(p);
// Inverse property
var res = t.Inverse.Forward.TransformPos(q);
// Backward
var res2 = t.Backward.TransformPos(q);
TrafoTesting.AreEqual(p, res);
TrafoTesting.AreEqual(p, res2);
});
public static void ConsistentWithMatrixRotationAndShiftTest()
=> TrafoTesting.GenericTest(rnd =>
{
var trans = rnd.UniformV3d() * 10;
var axis = rnd.UniformV3dDirection();
var angle = rnd.UniformDouble() * Constant.PiTimesTwo;
var m = M44d.Translation(trans) * M44d.Rotation(axis, angle);
var e = new Euclidean3d(Rot3d.Rotation(axis, angle), trans);
var p = rnd.UniformV3d() * rnd.UniformInt(1000);
var res = m.TransformPos(p);
var res2 = e.TransformPos(p);
TrafoTesting.AreEqual(res, res2);
});
/// <summary>
/// Tests multiplication of T trafos with their M matrix representations.
/// </summary>
public static void GenericMatrixMultiplicationTest <T, M, M2>(
Func <RandomSystem, T> frnd,
Func <T, V3d, V3d> trafoTransform,
Func <M, V3d, V3d> matrixTransform,
Func <M2, V3d, V3d> matrixTransform2,
Func <T, V3d, V3d> trafoMul,
Func <M, V3d, V3d> matrixMul,
Func <M2, V3d, V3d> matrixMul2,
bool squareMatrix = true)
=> GenericTest(rnd =>
{
dynamic a = frnd(rnd);
dynamic b = frnd(rnd);
dynamic ma = (M)a;
dynamic mb = (M)b;
var a_x_b = a * b;
var ma_x_b = ma * b;
var a_x_mb = a * mb;
var ma_x_mb = (squareMatrix) ? ma * mb : null;
var p = rnd.UniformV3d() * rnd.UniformInt(1000);
{
var res = trafoTransform(a_x_b, p);
var res2 = matrixTransform2(ma_x_b, p);
var res3 = matrixTransform2(a_x_mb, p);
var res4 = squareMatrix ? matrixTransform(ma_x_mb, p) : res3;
TrafoTesting.AreEqual(res, res2);
TrafoTesting.AreEqual(res, res3);
TrafoTesting.AreEqual(res, res4);
}
{
var res = trafoMul(a_x_b, p);
var res2 = matrixMul2(ma_x_b, p);
var res3 = matrixMul2(a_x_mb, p);
var res4 = squareMatrix ? matrixMul(ma_x_mb, p) : res3;
TrafoTesting.AreEqual(res, res2);
TrafoTesting.AreEqual(res, res3);
TrafoTesting.AreEqual(res, res4);
}
});
public static void InverseTest()
=> TrafoTesting.GenericTest(rnd =>
{
var e = TrafoTesting.GetRandomEuclidean(rnd);
var p = rnd.UniformV3d() * rnd.UniformInt(1000);
var q = e.TransformPos(p);
// Inverse property
var res = e.Inverse.TransformPos(q);
// Invert method
Euclidean.Invert(ref e);
var res2 = e.TransformPos(q);
TrafoTesting.AreEqual(p, res);
TrafoTesting.AreEqual(p, res2);
});
public static void InverseTest()
=> TrafoTesting.GenericTest(rnd =>
{
var s = TrafoTesting.GetRandomSimilarity(rnd);
var p = rnd.UniformV3d() * rnd.UniformInt(1000);
var q = s.TransformPos(p);
// Inverse property
var res = s.Inverse.TransformPos(q);
// Invert method
Similarity.Invert(ref s);
var res2 = s.TransformPos(q);
TrafoTesting.AreEqual(p, res);
TrafoTesting.AreEqual(p, res2);
});
public static void InverseTest()
=> TrafoTesting.GenericTest(rnd =>
{
var a = TrafoTesting.GetRandomAffine(rnd);
var p = rnd.UniformV3d() * rnd.UniformInt(1000);
var q = a.TransformPos(p);
// Inverse property
var res = a.Inverse.TransformPos(q);
// Invert method
Affine.Invert(ref a);
var res2 = a.TransformPos(q);
TrafoTesting.AreEqual(p, res);
TrafoTesting.AreEqual(p, res2);
});
public static void FromIntoEpislon()
{
TrafoTesting.GenericTest((rnd, i) =>
{
var dir = rnd.UniformV3d().Normalized;
var eps = rnd.UniformV3d() * (i / 100) * 1e-22;
var rotId = Rot3d.RotateInto(dir, (dir + eps).Normalized);
var matId = (M33d)rotId;
Assert.IsTrue(matId.IsIdentity(1e-10));
var dirF = rnd.UniformV3f().Normalized;
var epsF = rnd.UniformV3f() * (i / 100) * 1e-12f;
var rotIdF = Rot3f.RotateInto(dirF, (dirF + epsF).Normalized);
var matIdF = (M33f)rotIdF;
Assert.IsTrue(matIdF.IsIdentity(1e-7f));
});
}
public static void YawPitchRoll()
{
TrafoTesting.GenericTest(rnd =>
{
var yaw = rnd.UniformDouble() * Constant.PiTimesFour - Constant.PiTimesTwo;
var pitch = rnd.UniformDouble() * Constant.PiTimesFour - Constant.PiTimesTwo;
var roll = rnd.UniformDouble() * Constant.PiTimesFour - Constant.PiTimesTwo;
// Aardvark euler angles: roll (X), pitch (Y), yaw (Z). Ther are applied in reversed order.
var mat = (M33d)(M44d.RotationZ(yaw) * M44d.RotationY(pitch) * M44d.RotationX(roll));
var mat2 = (M33d)M44d.RotationEuler(roll, pitch, yaw);
var mat3 = (M33d)(Rot3d.RotationZ(yaw) * Rot3d.RotationY(pitch) * Rot3d.RotationX(roll));
var mat4 = (M33d)Rot3d.RotationEuler(roll, pitch, yaw);
Assert.IsTrue(Fun.ApproximateEquals(mat, mat2, 1e-7));
Assert.IsTrue(Fun.ApproximateEquals(mat, mat3, 1e-7));
Assert.IsTrue(Fun.ApproximateEquals(mat, mat4, 1e-7));
});
}
public static void InverseTest()
{
TrafoTesting.GenericTest(rnd =>
{
var r = TrafoTesting.GetRandomRot2(rnd);
var p = rnd.UniformV2d() * rnd.UniformInt(1000);
var q = r.Transform(p);
// Inverse property
var res = r.Inverse.Transform(q);
// Invert method
Rot.Invert(ref r);
var res2 = r.Transform(q);
TrafoTesting.AreEqual(p, res);
TrafoTesting.AreEqual(p, res2);
});
}
/// <summary>
/// Tests multiplication of T trafos with random M matrices.
/// </summary>
public static void GenericFullMatrixMultiplicationTest <T, M, MAugmented>(
Func <RandomSystem, T> frnd,
Func <RandomSystem, M> frndMatrix,
Func <M, V3d, V3d> matrixTransform,
Func <M, V3d, V3d> matrixMul)
=> GenericTest(rnd =>
{
dynamic m = frndMatrix(rnd);
dynamic t = frnd(rnd);
dynamic maugm = (MAugmented)m;
dynamic maugt = (MAugmented)t;
dynamic mt = (M)t;
var m_x_t = m * t;
var t_x_m = t * m;
var m_x_t_ref = m * maugt;
var t_x_m_ref = mt * maugm;
var p = rnd.UniformV3d() * rnd.UniformInt(1000);
{
var res_mt = matrixTransform(m_x_t, p);
var res_mt_ref = matrixTransform(m_x_t_ref, p);
var res_tm = matrixTransform(t_x_m, p);
var res_tm_ref = matrixTransform(t_x_m_ref, p);
TrafoTesting.AreEqual(res_mt, res_mt_ref);
TrafoTesting.AreEqual(res_tm, res_tm_ref);
}
{
var res_mt = matrixMul(m_x_t, p);
var res_mt_ref = matrixMul(m_x_t_ref, p);
var res_tm = matrixMul(t_x_m, p);
var res_tm_ref = matrixMul(t_x_m_ref, p);
TrafoTesting.AreEqual(res_mt, res_mt_ref);
TrafoTesting.AreEqual(res_tm, res_tm_ref);
}
});
public static void Multiplication2x3Test()
{
TrafoTesting.GenericTest(rnd =>
{
var r1 = TrafoTesting.GetRandomRot2(rnd);
var r2 = TrafoTesting.GetRandomRot2(rnd);
var r = r1 * r2;
var rm = (M23d)r1 * r2;
var mr = r1 * (M23d)r2;
{
var p = rnd.UniformV2d() * rnd.UniformInt(1000);
var res = r.Transform(p);
var res2 = rm.TransformPos(p);
var res3 = mr.TransformPos(p);
TrafoTesting.AreEqual(res, res2);
TrafoTesting.AreEqual(res, res3);
}
});
}
public static void RotationXYZ()
{
TrafoTesting.GenericTest(rnd =>
{
var angle = rnd.UniformDouble() * Constant.PiTimesFour - Constant.PiTimesTwo;
var rotX1 = Rot3d.RotationX(angle);
var rotX2 = Rot3d.Rotation(V3d.XAxis, angle);
TrafoTesting.AreEqual(rotX1, rotX2);
var rotY1 = Rot3d.RotationY(angle);
var rotY2 = Rot3d.Rotation(V3d.YAxis, angle);
TrafoTesting.AreEqual(rotY1, rotY2);
var rotZ1 = Rot3d.RotationZ(angle);
var rotZ2 = Rot3d.Rotation(V3d.ZAxis, angle);
TrafoTesting.AreEqual(rotZ1, rotZ2);
});
}
