/// <summary>
/// Transforms direction vector v (v.w is presumed 0.0) by rigid transformation r.
/// Actually, only the rotation is used.
/// </summary>
public static V2f TransformDir(Euclidean2f r, V2f v)
{
return(r.Rot.TransformDir(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 Euclidean2f Multiply(Euclidean2f a, Euclidean2f b)
{
//a.Rot * b.Rot, a.Trans + a.Rot * b.Trans
return(new Euclidean2f(Rot2f.Multiply(a.Rot, b.Rot), a.Trans + a.Rot.TransformDir(b.Trans)));
}
public static M23f Multiply(M22f m, Euclidean2f r)
{
return(M23f.Multiply(m, (M23f)r));
}
public static Euclidean2f operator *(Euclidean2f a, Euclidean2f b)
{
return(Euclidean2f.Multiply(a, b));
}
public static bool ApproxEqual(Euclidean2f r0, Euclidean2f r1, float angleTol, float posTol)
{
return(V2f.ApproxEqual(r0.Trans, r1.Trans, posTol) && Rot2f.ApproxEqual(r0.Rot, r1.Rot, angleTol));
}
public static bool ApproxEqual(Euclidean2f r0, Euclidean2f 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 V2f InvTransformPos(Euclidean2f r, V2f p)
{
return(r.Rot.InvTransformPos(p - r.Trans));
}
/// <summary>
/// Transforms point p (p.w is presumed 1.0) by rigid transformation r.
/// </summary>
public static V2f TransformPos(Euclidean2f r, V2f p)
{
return(r.Rot.TransformPos(p) + r.Trans);
}