public Similarity3f(M44f m, float epsilon = (float)0.00001)
{
if (!(m.M30.IsTiny(epsilon) && m.M31.IsTiny(epsilon) && m.M32.IsTiny(epsilon)))
{
throw new ArgumentException("Matrix contains perspective components.");
}
if (m.M33.IsTiny(epsilon))
{
throw new ArgumentException("Matrix is not homogeneous.");
}
m /= m.M33; //normalize it
var m33 = (M33f)m;
var s0 = m33.C0.Norm2;
var s1 = m33.C1.Norm2;
var s2 = m33.C2.Norm2;
var s = (s0 * s1 * s2).Pow((float)1.0 / 3); //geometric mean of scale
if (!((s0 / s - 1).IsTiny(epsilon) && (s1 / s - 1).IsTiny(epsilon) && (s2 / s - 1).IsTiny(epsilon)))
{
throw new ArgumentException("Matrix features non-uniform scaling");
}
m33 /= s;
Scale = s;
EuclideanTransformation = new Euclidean3f(m33, m.C3.XYZ);
}
/// <summary>
/// Creates a similarity transformation from a rigid transformation <paramref name="euclideanTransformation"/> and an (subsequent) uniform scale by factor <paramref name="scale"/>.
/// </summary>
public Similarity3f(Euclidean3f euclideanTransformation, float scale)
{
Scale = scale;
EuclideanTransformation = new Euclidean3f(
euclideanTransformation.Rot,
scale * euclideanTransformation.Trans
);
}
/// <summary>
/// Creates a similarity transformation from an uniform scale by factor <paramref name="scale"/>, and a (subsequent) rigid transformation <paramref name="euclideanTransformation"/>.
/// </summary>
public Similarity3f(float scale, Euclidean3f euclideanTransformation)
{
Scale = scale;
EuclideanTransformation = euclideanTransformation;
}
public static Similarity3f Parse(string s)
{
var x = s.NestedBracketSplitLevelOne().ToArray();
return(new Similarity3f(float.Parse(x[0]), Euclidean3f.Parse(x[1])));
}
public static bool ApproxEqual(Similarity3f t0, Similarity3f t1, float angleTol, float posTol, float scaleTol)
{
return(t0.Scale.ApproximateEquals(t1.Scale, scaleTol) && Euclidean3f.ApproxEqual(t0.EuclideanTransformation, t1.EuclideanTransformation, angleTol, posTol));
}
/// <summary>
/// Multiplies an Euclidean transformation by a Similarity transformation.
/// This concatenates the two transformations into a single one, first b is applied, then a.
/// Attention: Multiplication is NOT commutative!
/// </summary>
public static Similarity3f Multiply(Euclidean3f a, Similarity3f b)
{
return(Multiply((Similarity3f)a, b));
}
/// <summary>
/// Multiplies a Similarity transformation by an Euclidean transformation.
/// This concatenates the two transformations into a single one, first b is applied, then a.
/// Attention: Multiplication is NOT commutative!
/// </summary>
public static Similarity3f Multiply(Similarity3f a, Euclidean3f b)
{
return(Multiply(a, (Similarity3f)b));
}
/// <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 M34f operator *(M33f m, Euclidean3f r)
* {
* return (M34f)m * (M34f)r;
* }
*/
#endregion
#region Transformations yielding a Euclidean transformation
/// <summary>
/// Returns a new Euclidean transformation by transforming self by a Trafo t.
/// Note: This is not a concatenation.
/// t is fully applied to the Translation and Rotation,
/// but the scale is not reflected in the resulting Euclidean transformation.
/// </summary>
// [todo ISSUE 20090810 andi : andi] Rethink this notation. Maybe write Transformed methods for all transformations.
public static Euclidean3f Transformed(Euclidean3f self, Similarity3f t)
{
return(new Euclidean3f(t.Rot * self.Rot, t.TransformPos(self.Trans)));
}
public static Euclidean3f operator *(Euclidean3f a, Euclidean3f b)
{
return(Euclidean3f.Multiply(a, b));
}
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));
}
public static bool ApproxEqual(Euclidean3f r0, Euclidean3f r1)
{
return(ApproxEqual(r0, r1, Constant <float> .PositiveTinyValue, Constant <float> .PositiveTinyValue));
}
/// <summary>
/// Transforms point p (p.w is presumed 1.0) by the inverse of the rigid transformation r.
/// </summary>
public static V3f InvTransformPos(Euclidean3f r, V3f p)
{
return(r.Rot.InvTransformPos(p - r.Trans));
}
/// <summary>
/// Transforms direction vector v (v.w is presumed 0.0) by the inverse of the rigid transformation r.
/// Actually, only the rotation is used.
/// </summary>
public static V3f InvTransformDir(Euclidean3f r, V3f v)
{
return(r.Rot.InvTransformDir(v));
}
/// <summary>
/// Transforms point p (p.w is presumed 1.0) by rigid transformation r.
/// </summary>
public static V3f TransformPos(Euclidean3f r, V3f p)
{
return(r.Rot.TransformPos(p) + r.Trans);
}
public static M34f Multiply(M33f m, Euclidean3f r)
{
return(M34f.Multiply(m, (M34f)r));
}
