/// <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))
));
}
public Trafo3d(Similarity3d trafo)
{
Forward = (M44d)trafo;
Backward = (M44d)trafo.Inverse;
}
public static Similarity3d operator *(Similarity3d a, Euclidean3d b)
{
return(Similarity3d.Multiply(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 bool ApproxEqual(Similarity3d t0, Similarity3d t1)
{
return(ApproxEqual(t0, t1, Constant <double> .PositiveTinyValue, Constant <double> .PositiveTinyValue, Constant <double> .PositiveTinyValue));
}
/// <summary>
/// Transforms point p (p.w is presumed 1.0) by the inverse of the similarity transformation t.
/// </summary>
public static V3d InvTransformPos(Similarity3d t, V3d p)
{
return(t.EuclideanTransformation.InvTransformPos(p) / t.Scale);
}
/// <summary>
/// Transforms direction vector v (v.w is presumed 0.0) by the inverse of the similarity transformation t.
/// Actually, only the rotation and scale is used.
/// </summary>
public static V3d InvTransformDir(Similarity3d t, V3d v)
{
return(t.EuclideanTransformation.InvTransformDir(v) / t.Scale);
}
/// <summary>
/// Transforms point p (p.w is presumed 1.0) by similarity transformation t.
/// </summary>
public static V3d TransformPos(Similarity3d t, V3d p)
{
return(t.EuclideanTransformation.TransformPos(t.Scale * p));
}
/// <summary>
/// Transforms direction vector v (v.w is presumed 0.0) by similarity transformation t.
/// Actually, only the rotation and scale is used.
/// </summary>
public static V3d TransformDir(Similarity3d t, V3d v)
{
return(t.EuclideanTransformation.TransformDir(t.Scale * v));
}
/// <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));
}
/// <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>
/// Returns a new Euclidean transformation by transforming this by a 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>
public Euclidean3d Transformed(Similarity3d t)
{
return(Transformed(this, t));
}
/*
* 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)));
}
