public static Plane ApplyInvToPlaneXY(this RotoTranslation3 @this)
{
var normal = @this.Rotation.RowZ();
var freeTerm = @this.Translation.Z;
return(new Plane(freeTerm, normal));
}
public static Plane ApplyToPlaneXY(this RotoTranslation3 @this)
{
var normal = @this.Rotation.ColZ();
var freeTerm = -normal * @this.Translation;
return(new Plane(freeTerm, normal));
}
public static Plane ApplyInv(this RotoTranslation3 @this, Plane plane)
{
var normal = @this.Rotation.ApplyInv(plane.Normal);
var freeTerm = plane.FreeTerm + plane.Normal * @this.Translation;
return(new Plane(freeTerm, normal));
}
public void Applying_a_RotoTranslation3_followed_by_applying_its_precomputed_Inv_results_in_the_original_vector()
{
var x = new RotoTranslation3(new Rotation3(new Quaternion(1.1, -2.2, 3.3, -4.4)), new Vector3(4.1, -2.4, 1.3));
var v = new Vector3(3.14, 2.88, -1.79);
var roundTrip = x.Inv.Apply(x.Apply(v));
Expect(Vector3.Distance(roundTrip, v), Is.LessThan(_tolerance));
}
public void ApplyInvToPlaneXY_yields_a_Plane_passing_through_inverse_images_of_points_on_plane_XY_with_correct_orientation()
{
var x = new RotoTranslation3(new Rotation3(new Quaternion(1.1, -2.2, 3.3, -4.4)), new Vector3(4.1, -2.4, 1.3));
var plane = x.ApplyInvToPlaneXY();
Expect(x.ApplyInv(Vector3.Zero).DistanceTo(plane), Is.LessThan(_tolerance));
Expect(x.ApplyInv(Vector3.UnitX).DistanceTo(plane), Is.LessThan(_tolerance));
Expect(x.ApplyInv(Vector3.UnitY).DistanceTo(plane), Is.LessThan(_tolerance));
Expect(x.ApplyInv(Vector3.UnitZ).SignedDistanceTo(plane), Is.Positive); // orientation test
}
public void Applying_the_product_of_two_RotoTranslation3_is_equivalent_to_applying_them_in_succession()
{
var x = new RotoTranslation3(new Rotation3(new Quaternion(1.1, -2.2, 3.3, -4.4)), new Vector3(4.1, -2.4, 1.3));
var y = new RotoTranslation3(new Rotation3(new Quaternion(-0.1, -1.2, -5.3, -3.4)), new Vector3(2.1, -1.4, -0.3));
var v = new Vector3(3.14, 2.88, -1.79);
var expected = x.Apply(y.Apply(v));
var actual = (x * y).Apply(v);
Expect(Vector3.Distance(actual, expected), Is.LessThan(_tolerance));
}
public void Applying_inverse_RotoTranslation3_commutes_with_taking_PointAt_given_coordinate_from_Ray3()
{
var ray = new Ray3(origin: new Vector3(3.141, 2.718, 0.577), direction: new Vector3(1.23, 4.56, -3.21));
var trans = new RotoTranslation3(new Rotation3(new Quaternion(1.1, -2.2, 3.3, -4.4)), new Vector3(4.1, -2.4, 1.3));
var coord = 5.77;
var expected = trans.ApplyInv(ray.PointAt(coord));
var actual = trans.ApplyInv(ray).PointAt(coord);
Expect(Vector3.Distance(actual, expected), Is.LessThan(_tolerance));
}
public void Apply_to_Plane_given_in_parameteric_form_yields_a_Plane_passing_through_images_of_points_on_original_Plane_with_correct_orientation()
{
var x = new RotoTranslation3(new Rotation3(new Quaternion(1.1, -2.2, 3.3, -4.4)), new Vector3(4.1, -2.4, 1.3));
var p = new Vector3(3.14, 2.88, 0.57);
var u = new Vector3(-1, -2, 3);
var v = new Vector3(-0.88, 0.49, 4.05);
var origPlane = Plane.Parametric(p, u, v);
var newPlane = x.Apply(origPlane);
Expect(x.Apply(p).DistanceTo(newPlane), Is.LessThan(_tolerance));
Expect(x.Apply(p + u).DistanceTo(newPlane), Is.LessThan(_tolerance));
Expect(x.Apply(p + v).DistanceTo(newPlane), Is.LessThan(_tolerance));
var q = new Vector3(0.17, -0.46, 0.83);
Expect(x.Apply(q).SignedDistanceTo(newPlane), Is.EqualTo(q.SignedDistanceTo(origPlane)).Within(_tolerance)); // orientation test
}
public static IPolygon3 ApplyInv(this RotoTranslation3 @this, IPolygon3 polygon)
{
return(new Polygon3(@this.Inv * polygon.CoordinateSystem, polygon.InPlane));
}
public static Segment3 ApplyInv(this RotoTranslation3 @this, Segment3 segment)
{
return(new Segment3(@this.ApplyInv(segment.Start), @this.ApplyInv(segment.End)));
}
public static Ray3 ApplyInv(this RotoTranslation3 @this, Ray3 ray)
{
return(new Ray3(@this.ApplyInv(ray.Origin), @this.Rotation.ApplyInv(ray.Direction)));
}
public YamlSerialization(RotoTranslation3 rotoTranslation3)
{
Rotation = new Rotation3Utils.YamlSerialization(rotoTranslation3.Rotation);
Translation = new Vector3Utils.YamlSerialization(rotoTranslation3.Translation);
}