public static V3f Original(Rot3f r)
{
var test = r.W * r.Y - r.X * r.Z;
if (test > 0.5f - Constant <float> .PositiveTinyValue) // singularity at north pole
{
return(new V3f(
2 * Fun.Atan2(r.X, r.W),
(float)Constant.PiHalf,
0));
}
if (test < -0.5f + Constant <float> .PositiveTinyValue) // singularity at south pole
{
return(new V3f(
2 * Fun.Atan2(r.X, r.W),
-(float)Constant.PiHalf,
0));
}
// From Wikipedia, conversion between quaternions and Euler angles.
return(new V3f(
Fun.Atan2(2 * (r.W * r.X + r.Y * r.Z),
1 - 2 * (r.X * r.X + r.Y * r.Y)),
Fun.AsinClamped(2 * test),
Fun.Atan2(2 * (r.W * r.Z + r.X * r.Y),
1 - 2 * (r.Y * r.Y + r.Z * r.Z))));
}
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));
});
}
/// <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="q1"></param>
/// <param name="q2"></param>
/// <param name="t">[0,1]</param>
/// <returns>Interpolant</returns>
public static Rot3f SlerpShortest(
Rot3f q1, Rot3f q2, double t
)
{
Rot3f q3 = q2;
float cosomega = Fun.Clamp(Rot3f.Dot(q1, q3), -1, 1);
if (cosomega < 0.0)
{
cosomega = -cosomega;
q3 *= -1; //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(q1);
}
double sinomega = System.Math.Sqrt(1 - cosomega * cosomega);
Rot3f result = new Rot3f();
if (sinomega * double.MaxValue > 1)
{
double omega = System.Math.Acos(cosomega);
float s1 = (float)(System.Math.Sin((1.0 - t) * omega) / sinomega);
float s2 = (float)(System.Math.Sin(t * omega) / sinomega);
result = s1 * q1 + s2 * q3;
}
else if (cosomega > 0)
{
// omega == 0
float s1 = 1.0f - (float)t;
float s2 = (float)t;
result = s1 * q1 + s2 * q3;
}
else
{
// omega == -pi
result.X = -q1.Y;
result.Y = q1.X;
result.Z = -q1.W;
result.W = q1.Z;
float s1 = (float)System.Math.Sin((0.5 - t) * System.Math.PI);
float s2 = (float)System.Math.Sin(t * System.Math.PI);
result = s1 * q1 + s2 * result;
}
return(result);
}
/// <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 Similarity3f Multiply(Similarity3f a, Similarity3f b)
{
//a.Scale * b.Scale, a.Rot * b.Rot, a.Trans + a.Rot * a.Scale * b.Trans
return(new Similarity3f(a.Scale * b.Scale, new Euclidean3f(
Rot3f.Multiply(a.Rot, b.Rot),
a.Trans + a.Rot.TransformDir(a.Scale * b.Trans))
));
}
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 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 Rot3f Original(V3f from, V3f into)
{
var angle = from.AngleBetween(into);
if (angle < 1e-6f)
{
return(Rot3f.Identity);
}
else if (Constant.PiF - angle < 1e-6f)
{
return(new Rot3f(0, from.AxisAlignedNormal()));
}
else
{
V3f axis = Vec.Cross(from, into).Normalized;
return(Rot3f.Rotation(axis, angle));
}
}
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 V3f CopySign(Rot3f r)
{
var test = r.W * r.Y - r.X * r.Z;
if (test.Abs() >= 0.5f - Constant <float> .PositiveTinyValue)
{
return(new V3f(
2 * Fun.Atan2(r.X, r.W),
Fun.CopySign((float)Constant.PiHalf, test),
0));
}
else
{
return(new V3f(
Fun.Atan2(2 * (r.W * r.X + r.Y * r.Z),
1 - 2 * (r.X * r.X + r.Y * r.Y)),
Fun.AsinClamped(2 * test),
Fun.Atan2(2 * (r.W * r.Z + r.X * r.Y),
1 - 2 * (r.Y * r.Y + r.Z * r.Z))));
}
}
public DistanceRot3f()
{
var rnd = new RandomSystem(1);
A.SetByIndex(i => RndRot(rnd));
angles.SetByIndex(i =>
{
float a = 0;
do
{
a = RndAngle(rnd);
}while (a == 0);
return(a);
});
B.SetByIndex(i =>
{
var r = Rot3f.Rotation(RndAxis(rnd), angles[i]);
return(r * A[i]);
});
}
/// <summary>
/// Creates a rigid transformation from a rotation matrix <paramref name="rot"/> and a (subsequent) translation <paramref name="trans"/>.
/// </summary>
public Euclidean3f(M33f rot, V3f trans, float epsilon = 1e-5f)
{
Rot = Rot3f.FromM33f(rot, epsilon);
Trans = trans;
}
public static M33f Multiply(Scale3f scale, Rot3f rot)
{
return(Scale3f.Multiply(scale, (M33f)rot));
}
/// <summary>
/// Creates rotational matrix from quaternion.
/// </summary>
/// <returns>Rotational matrix.</returns>
public static M44f Rotation(Rot3f r)
{
return((M44f)r);
}
private static Rot3f RndRot(RandomSystem rnd) => Rot3f.Rotation(RndAxis(rnd), RndAngle(rnd));
public static M34f Rotation(Rot3f q)
{
return((M34f)q);
}
public void Write(Rot3f t)
{
Write(t.W); Write(t.V);
}
/// <summary>
/// Multiplies 2 Euclidean transformations.
/// This concatenates the two rigid transformations into a single one, first b is applied, then a.
/// Attention: Multiplication is NOT commutative!
/// </summary>
public static Euclidean3f Multiply(Euclidean3f a, Euclidean3f b)
{
//a.Rot * b.Rot, a.Trans + a.Rot * b.Trans
return(new Euclidean3f(Rot3f.Multiply(a.Rot, b.Rot), a.Trans + a.Rot.TransformDir(b.Trans)));
}
public static Euclidean3f Parse(string s)
{
var x = s.NestedBracketSplitLevelOne().ToArray();
return(new Euclidean3f(Rot3f.Parse(x[0]), V3f.Parse(x[1])));
}
public static M33f Multiply(Rot2f rot2, Rot3f rot3)
{
return(Rot2f.Multiply(rot2, (M33f)rot3));
}
/// <summary>
/// Creates a rigid transformation from a rotation <paramref name="rot"/> and a (subsequent) translation <paramref name="trans"/>.
/// </summary>
public Euclidean3f(Rot3f rot, V3f trans)
{
Rot = rot;
Trans = trans;
}
public static bool ApproxEqual(Euclidean3f r0, Euclidean3f r1, float angleTol, float posTol)
{
return(V3f.ApproxEqual(r0.Trans, r1.Trans, posTol) && Rot3f.ApproxEqual(r0.Rot, r1.Rot, angleTol));
}
