public static bool ApproxEqual(Euclidean2d r0, Euclidean2d r1, double angleTol, double posTol)
{
return(V2d.ApproxEqual(r0.Trans, r1.Trans, posTol) && Rot2d.ApproxEqual(r0.Rot, r1.Rot, angleTol));
}
public static Euclidean2d operator *(Euclidean2d a, Euclidean2d b)
{
return(Euclidean2d.Multiply(a, b));
}
/// <summary>
/// Transforms point p (p.w is presumed 1.0) by the inverse of the rigid transformation r.
/// </summary>
public static V2d InvTransformPos(Euclidean2d r, V2d p)
{
return(r.Rot.InvTransformPos(p - r.Trans));
}
public static bool ApproxEqual(Euclidean2d r0, Euclidean2d r1)
{
return(ApproxEqual(r0, r1, Constant <double> .PositiveTinyValue, Constant <double> .PositiveTinyValue));
}
/// <summary>
/// Transforms point p (p.w is presumed 1.0) by rigid transformation r.
/// </summary>
public static V2d TransformPos(Euclidean2d r, V2d p)
{
return(r.Rot.TransformPos(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 V2d InvTransformDir(Euclidean2d r, V2d v)
{
return(r.Rot.InvTransformDir(v));
}
public static M23d Multiply(M22d m, Euclidean2d r)
{
return(M23d.Multiply(m, (M23d)r));
}
/// <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 Euclidean2d Multiply(Euclidean2d a, Euclidean2d b)
{
//a.Rot * b.Rot, a.Trans + a.Rot * b.Trans
return(new Euclidean2d(Rot2d.Multiply(a.Rot, b.Rot), a.Trans + a.Rot.TransformDir(b.Trans)));
}
public OrientedBox2d(Box2d box, Euclidean2d trafo)
{
Box = box;
Trafo = trafo;
}
public OrientedBox2d(Box2d box, V2d trans)
{
Box = box;
Trafo = new Euclidean2d(Rot2d.Identity, trans);
}
public OrientedBox2d(Box2d box, Rot2d rot)
{
Box = box;
Trafo = new Euclidean2d(rot, V2d.Zero);
}
