/// <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 __e3t__ Multiply(__e3t__ a, __e3t__ b)
{
//a.Rot * b.Rot, a.Trans + a.Rot * b.Trans
return(new __e3t__(__r3t__.Multiply(a.Rot, b.Rot), a.Trans + a.Rot.TransformDir(b.Trans)));
}
public static bool ApproxEqual(__e3t__ r0, __e3t__ r1, __ft__ angleTol, __ft__ posTol)
{
return(__v3t__.ApproxEqual(r0.Trans, r1.Trans, posTol) && __r3t__.ApproxEqual(r0.Rot, r1.Rot, angleTol));
}
/*
* public static M3__s4f__ operator *(M3__s3f__ m, __e3t__ r)
* {
* return (M3__s4f__)m * (M3__s4f__)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 __e3t__ Transformed(__e3t__ self, Similarity__s3f__ t)
{
return(new __e3t__(t.Rot * self.Rot, t.TransformPos(self.Trans)));
}
public static bool ApproxEqual(__e3t__ r0, __e3t__ r1)
{
return(ApproxEqual(r0, r1, Constant <__ft__> .PositiveTinyValue, Constant <__ft__> .PositiveTinyValue));
}
/// <summary>
/// Transforms point p (p.w is presumed 1.0) by the inverse of the rigid transformation r.
/// </summary>
public static __v3t__ InvTransformPos(__e3t__ r, __v3t__ 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 __v3t__ InvTransformDir(__e3t__ r, __v3t__ v)
{
return(r.Rot.InvTransformDir(v));
}
/// <summary>
/// Transforms point p (p.w is presumed 1.0) by rigid transformation r.
/// </summary>
public static __v3t__ TransformPos(__e3t__ r, __v3t__ p)
{
return(r.Rot.TransformPos(p) + r.Trans);
}
public static M3__s4f__ Multiply(M3__s3f__ m, __e3t__ r)
{
return(M3__s4f__.Multiply(m, (M3__s4f__)r));
}
