/// <summary>
/// Decomposes a transformation into a scale, rotation and translation component.
/// NOTE: The input is assumed to be a valid affine transformation.
/// The rotation output is in Euler-Angles (yaw, pitch, roll).
/// </summary>
public static void Decompose(this Trafo3d trafo, out V3d scale, out V3d rotation, out V3d translation)
{
translation = trafo.GetModelOrigin();
var rt = trafo.GetOrthoNormalOrientation();
if (rt.Forward.Det.IsTiny())
{
rotation = V3d.Zero;
}
else
{
var rot = Rot3d.FromFrame(rt.Forward.C0.XYZ, rt.Forward.C1.XYZ, rt.Forward.C2.XYZ);
rotation = rot.GetEulerAngles();
}
scale = trafo.GetScaleVector();
// if matrix is left-handed there must be some negative scale
// since rotation remains the x-axis, the y-axis must be flipped
if (trafo.Forward.Det < 0)
{
scale.Y = -scale.Y;
}
}
void TestDecompose(Trafo3d trafo, Rot3d jitter)
{
var x = (M44d)jitter;
trafo = trafo * new Trafo3d(x, x.Inverse);
V3d r_d, s_d, t_d;
trafo.Decompose(out s_d, out r_d, out t_d);
var recomposed = Trafo3d.FromComponents(s_d, r_d, t_d);
Assert.IsFalse(s_d.AnyNaN || r_d.AnyNaN || t_d.AnyNaN, "something NaN");
var dt = trafo.Forward - recomposed.Forward;
var e = dt.NormMax.Abs();
Assert.IsTrue(e < 1e-9, "DIFF");
var eq = CheckForwardBackwardConsistency(new Trafo3d(trafo.Forward, recomposed.Backward)) &&
CheckForwardBackwardConsistency(new Trafo3d(recomposed.Forward, trafo.Backward));
Assert.True(eq, "trafo not consistent");
}
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 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));
});
}
static V3d InvTransformUsingQuaternionOpt(Rot3d rot, V3d v)
{
// TODO
// Multiply(a, b)
//return new Rot3d(
// a.W * b.W - a.X * b.X - a.Y * b.Y - a.Z * b.Z,
// a.W * b.X + a.X * b.W + a.Y * b.Z - a.Z * b.Y,
// a.W * b.Y + a.Y * b.W + a.Z * b.X - a.X * b.Z,
// a.W * b.Z + a.Z * b.W + a.X * b.Y - a.Y * b.X
// );
// rot.Conungated * Rot3d(0, v)
var q = new QuaternionD(
+rot.X * v.X + rot.Y * v.Y + rot.Z * v.Z,
rot.W * v.X - rot.Y * v.Z + rot.Z * v.Y,
rot.W * v.Y - rot.Z * v.X + rot.X * v.Z,
rot.W * v.Z - rot.X * v.Y + rot.Y * v.X);
// q * rot
return(new V3d(
q.W * rot.X + q.X * rot.W + q.Y * rot.Z - q.Z * rot.Y,
q.W * rot.Y + q.Y * rot.W + q.Z * rot.X - q.X * rot.Z,
q.W * rot.Z + q.Z * rot.W + q.X * rot.Y - q.Y * rot.X
));
}
public static Trafo3d RotateInto(V3d from, V3d into)
{
var rot = new Rot3d(from, into);
var inv = rot.Inverse;
return(new Trafo3d((M44d)rot, (M44d)inv));
}
/// <summary>
/// Multiplies 2 Similarity transformations.
/// This concatenates the two similarity transformations into a single one, first b is applied, then a.
/// Attention: Multiplication is NOT commutative!
/// </summary>
public static Similarity3d Multiply(Similarity3d a, Similarity3d b)
{
//a.Scale * b.Scale, a.Rot * b.Rot, a.Trans + a.Rot * a.Scale * b.Trans
return(new Similarity3d(a.Scale * b.Scale, new Euclidean3d(
Rot3d.Multiply(a.Rot, b.Rot),
a.Trans + a.Rot.TransformDir(a.Scale * b.Trans))
));
}
/// <summary>
/// Spherical linear interpolation.
///
/// Assumes q1 and q2 are normalized and that t in [0,1].
///
/// This method does interpolate along the shortest arc
/// between q1 and q2.
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <param name="t">[0,1]</param>
/// <returns>Interpolant</returns>
public static Rot3d SlerpShortest(
Rot3d a, Rot3d b, double t
)
{
Rot3d q3 = b;
double cosomega = Rot3d.Dot(a, q3);
if (cosomega < 0.0)
{
cosomega = -cosomega;
q3 = -q3;
}
if (cosomega >= 1.0)
{
// Special case: q1 and q2 are the same, so just return one of them.
// This also catches the case where cosomega is very slightly > 1.0
return(a);
}
double sinomega = (1 - cosomega * cosomega).Sqrt();
Rot3d result;
if (sinomega * double.MaxValue > 1)
{
double omega = cosomega.Acos();
double s1 = ((1.0 - t) * omega).Sin() / sinomega;
double s2 = (t * omega).Sin() / sinomega;
result = s1 * a + s2 * q3;
}
else if (cosomega > 0)
{
// omega == 0
double s1 = 1.0 - t;
double s2 = t;
result = s1 * a + s2 * q3;
}
else
{
// omega == -pi
result = new Rot3d(a.Z, -a.Y, a.X, -a.W);
double s1 = ((0.5 - t) * Constant.Pi).Sin();
double s2 = (t * Constant.Pi).Sin();
result = s1 * a + s2 * result;
}
return(result);
}
static V3d TransformUsingQuaternionOpt2(Rot3d rot, V3d v)
{
// rot * Rot3d(0, v)
var W = -rot.X * v.X - rot.Y * v.Y - rot.Z * v.Z;
var X = rot.W * v.X + rot.Y * v.Z - rot.Z * v.Y;
var Y = rot.W * v.Y + rot.Z * v.X - rot.X * v.Z;
var Z = rot.W * v.Z + rot.X * v.Y - rot.Y * v.X;
// q * Rot3d(rot.W, -rot.V) (rot.Conungated)
return(new V3d(
-W * rot.X + X * rot.W - Y * rot.Z + Z * rot.Y,
-W * rot.Y + Y * rot.W - Z * rot.X + X * rot.Z,
-W * rot.Z + Z * rot.W - X * rot.Y + Y * rot.X
));
}
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 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);
});
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 void TrafoRotIntoCornerCase()
{
var rnd = new Random();
for (int i = 0; i < 1000; i++)
{
// some vectors will not normalize to 1.0 -> provoke numerical issues in Rot3d
var vecd = new V3d(0, 0, -rnd.NextDouble());
var rotd = new Rot3d(V3d.OOI, vecd);
var testd = rotd.TransformDir(V3d.OOI);
Assert.True((testd + V3d.OOI).Length < 1e-7);
var vecf = new V3f(0, 0, -rnd.NextDouble());
var rotf = new Rot3f(V3f.OOI, vecf);
var testf = rotf.TransformDir(V3f.OOI);
Assert.True((testf + V3f.OOI).Length < 1e-3);
}
}
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 Rot3dInvTransformTest()
{
var rnd = new RandomSystem(1);
for (int i = 0; i < 10000; i++)
{
var v = rnd.UniformV3d();
var axis = rnd.UniformV3dDirection();
var ang = rnd.UniformDouble();
var rot = Rot3d.Rotation(axis, ang);
var test1 = InvTransformUsingM33d(rot, v);
var test2 = rot.InvTransform(v);
var test3 = InvTransformUsingQuaternion(rot, v);
var test4 = InvTransformUsingQuaternionOpt(rot, v);
Assert.IsTrue(test1.ApproximateEquals(test2, 1e-5));
Assert.IsTrue(test1.ApproximateEquals(test3, 1e-5));
Assert.IsTrue(test1.ApproximateEquals(test4, 1e-5));
}
}
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);
});
}
public DistanceRot3d()
{
var rnd = new RandomSystem(1);
A.SetByIndex(i => RndRot(rnd));
angles.SetByIndex(i =>
{
double a = 0;
do
{
a = RndAngle(rnd);
}while (a == 0);
return(a);
});
B.SetByIndex(i =>
{
var r = Rot3d.Rotation(RndAxis(rnd), angles[i]);
return(r * A[i]);
});
}
static V3d TransformUsingQuaternionOpt1(Rot3d rot, V3d v)
{
// Multiply(a, b)
//return new Rot3d(
// a.W * b.W - a.X * b.X - a.Y * b.Y - a.Z * b.Z,
// a.W * b.X + a.X * b.W + a.Y * b.Z - a.Z * b.Y,
// a.W * b.Y + a.Y * b.W + a.Z * b.X - a.X * b.Z,
// a.W * b.Z + a.Z * b.W + a.X * b.Y - a.Y * b.X
// );
// rot * Rot3d(0, v)
var q = new QuaternionD(
-rot.X * v.X - rot.Y * v.Y - rot.Z * v.Z,
rot.W * v.X + rot.Y * v.Z - rot.Z * v.Y,
rot.W * v.Y + rot.Z * v.X - rot.X * v.Z,
rot.W * v.Z + rot.X * v.Y - rot.Y * v.X);
// q * Rot3d(rot.W, -rot.V) (rot.Conungated)
return(new V3d(
-q.W * rot.X + q.X * rot.W - q.Y * rot.Z + q.Z * rot.Y,
-q.W * rot.Y + q.Y * rot.W - q.Z * rot.X + q.X * rot.Z,
-q.W * rot.Z + q.Z * rot.W - q.X * rot.Y + q.Y * rot.X
));
}
public OrientedBox3d(Box3d box, Rot3d rot, V3d trans)
{
Box = box;
Trafo = new Euclidean3d(rot, trans);
}
public Trafo3d(Rot3d trafo)
{
Forward = (M44d)trafo;
Backward = (M44d)trafo.Inverse;
}
public static M33d Multiply(Scale3d scale, Rot3d rot)
{
return(Scale3d.Multiply(scale, (M33d)rot));
}
public static Rot3d GetRandomRot3(RandomSystem rnd) => Rot3d.Rotation(rnd.UniformV3dDirection(), rnd.UniformDouble() * Constant.PiTimesTwo);
public static CameraExtrinsics FromWorld2Camera(V3d angleAxis, V3d translation) => FromWorld2Camera((M33d)Rot3d.FromAngleAxis(angleAxis), translation);
/// <summary>
/// Creates rotational matrix from quaternion.
/// </summary>
/// <returns>Rotational matrix.</returns>
public static M44d Rotation(Rot3d r)
{
return((M44d)r);
}
static V3d TransformUsingQuaternion(Rot3d q, V3d v)
{
var r = q * new QuaternionD(0, v) * q.Inverse;
return(new V3d(r.X, r.Y, r.Z));
}
static V3d TransformUsingM33d(Rot3d rot, V3d v)
{
return(((M33d)rot).Transform(v));
}
public void Write(Rot3d t)
{
Write(t.W); Write(t.V);
}
public static M34d Rotation(Rot3d q)
{
return((M34d)q);
}
static V3d InvTransformUsingQuaternion(Rot3d rot, V3d v)
{
var r = rot.Inverse * new QuaternionD(0, v) * rot;
return(new V3d(r.X, r.Y, r.Z));
}
