public Similarity3d(M44d m, double epsilon = (double)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 = (M33d)m;
var s0 = m33.C0.Norm2;
var s1 = m33.C1.Norm2;
var s2 = m33.C2.Norm2;
var s = (s0 * s1 * s2).Pow((double)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 Euclidean3d(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 Similarity3d(Euclidean3d euclideanTransformation, double scale)
{
Scale = scale;
EuclideanTransformation = new Euclidean3d(
euclideanTransformation.Rot,
scale * euclideanTransformation.Trans
);
}
/// <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 Similarity3d Multiply(Similarity3d a, Euclidean3d b)
{
return(Multiply(a, (Similarity3d)b));
}
/// <summary>
/// Creates a similarity transformation from an uniform scale by factor <paramref name="scale"/>, and a (subsequent) rigid transformation <paramref name="euclideanTransformation"/>.
/// </summary>
public Similarity3d(double scale, Euclidean3d euclideanTransformation)
{
Scale = scale;
EuclideanTransformation = euclideanTransformation;
}
public OrientedBox3d(Box3d box, Rot3d rot, V3d trans)
{
Box = box;
Trafo = new Euclidean3d(rot, trans);
}
public OrientedBox3d(Box3d box, Euclidean3d trafo)
{
Box = box;
Trafo = trafo;
}
/// <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 Euclidean3d Multiply(Euclidean3d a, Euclidean3d b)
{
//a.Rot * b.Rot, a.Trans + a.Rot * b.Trans
return(new Euclidean3d(Rot3d.Multiply(a.Rot, b.Rot), a.Trans + a.Rot.TransformDir(b.Trans)));
}
public static Similarity3d Parse(string s)
{
var x = s.NestedBracketSplitLevelOne().ToArray();
return(new Similarity3d(double.Parse(x[0]), Euclidean3d.Parse(x[1])));
}
public static Euclidean3d operator *(Euclidean3d a, Euclidean3d b)
{
return(Euclidean3d.Multiply(a, b));
}
public static bool ApproxEqual(Euclidean3d r0, Euclidean3d r1, double angleTol, double posTol)
{
return(V3d.ApproxEqual(r0.Trans, r1.Trans, posTol) && Rot3d.ApproxEqual(r0.Rot, r1.Rot, angleTol));
}
public static bool ApproxEqual(Euclidean3d r0, Euclidean3d r1)
{
return(ApproxEqual(r0, r1, Constant <double> .PositiveTinyValue, Constant <double> .PositiveTinyValue));
}
/// <summary>
/// Transforms point p (p.w is presumed 1.0) by the inverse of the rigid transformation r.
/// </summary>
public static V3d InvTransformPos(Euclidean3d r, V3d 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 V3d InvTransformDir(Euclidean3d r, V3d v)
{
return(r.Rot.InvTransformDir(v));
}
/// <summary>
/// Transforms point p (p.w is presumed 1.0) by rigid transformation r.
/// </summary>
public static V3d TransformPos(Euclidean3d r, V3d p)
{
return(r.Rot.TransformPos(p) + r.Trans);
}
public static M34d Multiply(M33d m, Euclidean3d r)
{
return(M34d.Multiply(m, (M34d)r));
}
/// <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 Similarity3d Multiply(Euclidean3d a, Similarity3d b)
{
return(Multiply((Similarity3d)a, b));
}
public static bool ApproxEqual(Similarity3d t0, Similarity3d t1, double angleTol, double posTol, double scaleTol)
{
return(t0.Scale.ApproximateEquals(t1.Scale, scaleTol) && Euclidean3d.ApproxEqual(t0.EuclideanTransformation, t1.EuclideanTransformation, angleTol, posTol));
}
/*
* public static M34d operator *(M33d m, Euclidean3d r)
* {
* return (M34d)m * (M34d)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 Euclidean3d Transformed(Euclidean3d self, Similarity3d t)
{
return(new Euclidean3d(t.Rot * self.Rot, t.TransformPos(self.Trans)));
}
public Trafo3d(Euclidean3d trafo)
{
Forward = (M44d)trafo;
Backward = (M44d)trafo.Inverse;
}
public OrientedBox3d(Box3d box, Rot3d rot)
{
Box = box;
Trafo = new Euclidean3d(rot, V3d.Zero);
}