|
public ShiftToThisFrom ( |
||
| systemToShiftFrom | The CoordinateSystem to shift from to this CoordinateSystem. Defaults to the WorldCoordinateSystem if left out | |
| return | ||
public void Shift_ShiftToCoordinateSystem()
{
CoordinateSystem system = new CoordinateSystem(Point.MakePointWithInches(1, -2, -4), Angle.RightAngle, new Angle(new Degree(), -45), Angle.ZeroAngle);
Point testPoint = Point.MakePointWithInches(0, 3, 0);
Point shifted = testPoint.Shift(system.ShiftToThisFrom());
Point expected = Point.MakePointWithInches(2.12132034356, 3.53553391, -5);
shifted.Should().Be(expected);
}
public void CoordinateSystem_Shift_FromAndTo()
{
CoordinateSystem test = new CoordinateSystem(Point.MakePointWithInches(1, -2, -3), new Angle(new Degree(), -45), new Angle(new Degree(), 23.6), new Angle(new Degree(), 243));
Shift resultFrom = test.ShiftFromThisTo();
Shift resultTo = test.ShiftToThisFrom();
List<Rotation> expectedRotations = new List<Rotation>();
expectedRotations.Add(new Rotation(Line.XAxis, new Angle(new Degree(), -45)));
expectedRotations.Add(new Rotation(Line.YAxis, new Angle(new Degree(), 23.6)));
expectedRotations.Add(new Rotation(Line.ZAxis, new Angle(new Degree(), 243)));
Shift expected = new Shift(expectedRotations, Point.MakePointWithInches(1, -2, -3));
resultFrom.Should().Be(expected);
//it ends up being what we inputted because it shift reflects the movement of the objects so it is by definition the negative of the
//coordinate system. Therfore, whenever we want to revert it we are taking a "double negative" in implementation, giving us the origina
resultTo.Should().Be(expected.Inverse());
}
public void Shift_RotateToCoordinateSystem()
{
CoordinateSystem system = new CoordinateSystem(Point.Origin, Angle.RightAngle / 2, Angle.ZeroAngle, Angle.RightAngle / 2);
Point testPoint = Point.MakePointWithInches(0, 3, 0);
Point shifted = testPoint.Shift(system.ShiftToThisFrom());
Point expected = Point.MakePointWithInches(2.12132034, 1.5, -1.5);
shifted.Should().Be(expected);
//try to get the point we had before we switched order of shifting
CoordinateSystem system2 = new CoordinateSystem(Point.Origin, new Angle(new Degree(), 30), new Angle(new Degree(), 54.7356104), Angle.ZeroAngle);
Point shifted2 = testPoint.Shift(system2.ShiftToThisFrom());
Point expected2 = Point.MakePointWithInches(0, 2.59807621, -1.5);
shifted2.Should().Be(expected2);
}
public void CoordinateSystem_RotationMatrix()
{
Point origin = Point.Origin;
Angle angleX = new Angle(-46.775, Degrees);
Angle angleY = new Angle(23.6, Degrees);
Angle angleZ = new Angle(213, Degrees);
CoordinateSystem testSystem = new CoordinateSystem(origin, angleX, angleY, angleZ);
Matrix test1 = Matrix.RotationMatrixAboutZ(angleZ) * Matrix.RotationMatrixAboutY(angleY) * Matrix.RotationMatrixAboutX(angleX);
List<Angle> results1 = test1.EulerAngles();
Matrix test2 = Matrix.RotationMatrixAboutX(angleX) * Matrix.RotationMatrixAboutY(angleY) * Matrix.RotationMatrixAboutZ(angleZ);
List<Angle> results2 = test2.EulerAngles();
Matrix testMatrix = testSystem.RotationMatrixFromThisToWorld();
List<Angle> results = testMatrix.EulerAngles();
List<Rotation> resultRotations = new List<Rotation>();
resultRotations.Add(new Rotation(Line.XAxis, results[0]));
resultRotations.Add(new Rotation(Line.YAxis, results[1]));
resultRotations.Add(new Rotation(Line.ZAxis, results[2]));
Shift resultShift = new Shift(resultRotations, origin);
List<Rotation> resultRotations2 = new List<Rotation>();
resultRotations2.Add(new Rotation(Line.ZAxis, results[2]));
resultRotations2.Add(new Rotation(Line.YAxis, results[1]));
resultRotations2.Add(new Rotation(Line.XAxis, results[0]));
Shift resultShift2 = new Shift(resultRotations2, origin);
Point testPoint = Point.MakePointWithInches(3, -1, -20);
Point expectedPoint = testPoint.Shift(testSystem.ShiftToThisFrom());
Point expectedPoint2 = testPoint.Shift(testSystem.ShiftFromThisTo());
Point resultPoint = testPoint.Shift(resultShift);
Point resultPoint2 = testPoint.Shift(resultShift.Inverse());
Point resultPoint3 = testPoint.Shift(resultShift2);
(results[0] == angleX.ProperAngle).Should().BeTrue();
(results[1] == angleY).Should().BeTrue();
(results[2] == angleZ).Should().BeTrue();
}
public void Shift_TranslateUsingToCoordinateSystem()
{
CoordinateSystem system = new CoordinateSystem(Point.MakePointWithInches(1, -2, -4));
List<LineSegment> bounds = new List<LineSegment>();
bounds.Add(new LineSegment(Point.MakePointWithInches(0, 1, 0), Point.MakePointWithInches(0, 3, 0)));
bounds.Add(new LineSegment(Point.MakePointWithInches(0, 1, 0), Point.MakePointWithInches(4, 1, 0)));
bounds.Add(new LineSegment(Point.MakePointWithInches(0, 3, 0), Point.MakePointWithInches(4, 3, 0)));
bounds.Add(new LineSegment(Point.MakePointWithInches(4, 1, 0), Point.MakePointWithInches(4, 3, 0)));
Polygon testPolygon = new Polygon(bounds);
Polygon shifted = testPolygon.Shift(system.ShiftToThisFrom());
List<LineSegment> expectedBounds = new List<LineSegment>();
expectedBounds.Add(new LineSegment(Point.MakePointWithInches(-1, 3, 4), Point.MakePointWithInches(-1, 5, 4)));
expectedBounds.Add(new LineSegment(Point.MakePointWithInches(-1, 3, 4), Point.MakePointWithInches(3, 3, 4)));
expectedBounds.Add(new LineSegment(Point.MakePointWithInches(-1, 5, 4), Point.MakePointWithInches(3, 5, 4)));
expectedBounds.Add(new LineSegment(Point.MakePointWithInches(3, 3, 4), Point.MakePointWithInches(3, 5, 4)));
Polygon expectedPolygon = new Polygon(expectedBounds);
shifted.Should().Be(expectedPolygon);
}
public void CoordinateSystem_Shift_UndoShift()
{
CoordinateSystem system = new CoordinateSystem(Point.MakePointWithInches(-1, 2, 4), new Angle(new Degree(), 123), new Angle(new Degree(), -22), new Angle(new Degree(), 78));
List<LineSegment> bounds = new List<LineSegment>();
bounds.Add(new LineSegment(Point.MakePointWithInches(0, 1, 0), Point.MakePointWithInches(0, 3, 0)));
bounds.Add(new LineSegment(Point.MakePointWithInches(0, 1, 0), Point.MakePointWithInches(4, 1, 0)));
bounds.Add(new LineSegment(Point.MakePointWithInches(0, 3, 0), Point.MakePointWithInches(4, 3, 0)));
bounds.Add(new LineSegment(Point.MakePointWithInches(4, 1, 0), Point.MakePointWithInches(4, 3, 0)));
Polygon testPolygon = new Polygon(bounds);
Polygon shifted = testPolygon.Shift(system.ShiftToThisFrom());
Polygon shifted2 = shifted.Shift(system.ShiftFromThisTo());
testPolygon.Should().Be(shifted2);
}
public void CoordinateSystem_TwoSystemsEqualExpectedCombinedSystemShift()
{
Point expectedPoint = Point.MakePointWithInches(1, 2, 3);
//try our first one
CoordinateSystem testCurrent = new CoordinateSystem(Point.MakePointWithInches(1, 2, 3), Angle.ZeroAngle, Angle.ZeroAngle, Angle.RightAngle);
CoordinateSystem testRelativeToCurrent = new CoordinateSystem(Point.MakePointWithInches(1, -2, 1), Angle.ZeroAngle, Angle.ZeroAngle, Angle.RightAngle / 2);
Point expectedOrigin1 = Point.MakePointWithInches(3, 3, 4);
Angle xExpected1 = Angle.ZeroAngle;
Angle yExpected1 = Angle.ZeroAngle;
Angle zExpected1 = new Angle(new Degree(), 135);
CoordinateSystem expectedCombined1 = new CoordinateSystem(expectedOrigin1, xExpected1, yExpected1, zExpected1);
Point testPointWorld1 = Point.MakePointWithInches(3 - 2.12132034356, 3 - 0.70710678118, 7);
Point point1InCurrent = testPointWorld1.Shift(testCurrent.ShiftToThisFrom());
Point point1DoubleShifted = point1InCurrent.Shift(testRelativeToCurrent.ShiftToThisFrom());
Point point1Combined = testPointWorld1.Shift(expectedCombined1.ShiftToThisFrom());
(expectedPoint == point1DoubleShifted).Should().BeTrue();
(expectedPoint == point1Combined).Should().BeTrue();
//now another one
CoordinateSystem testCurrent2 = new CoordinateSystem(Point.MakePointWithInches(1, 1, -1), Angle.StraightAngle, Angle.ZeroAngle, -1 * Angle.RightAngle);
CoordinateSystem testRelativeToCurrent2 = new CoordinateSystem(Point.MakePointWithInches(-2, 0, 1), new Angle(new Degree(), -45), -1 * Angle.RightAngle, Angle.ZeroAngle);
Point expectedOrigin2 = Point.MakePointWithInches(1, 3, -2); //1, 3, -2
Angle xExpected2 = Angle.ZeroAngle;
Angle yExpected2 = Angle.RightAngle;
Angle zExpected2 = new Angle(new Degree(), 135);
CoordinateSystem expectedCombined2 = new CoordinateSystem(expectedOrigin2, xExpected2, yExpected2, zExpected2);
Point testPointWorld2 = Point.MakePointWithInches(1 - 3.53553390611, 3 + 0.70710678032, -3);
Point point2InCurrent = testPointWorld2.Shift(testCurrent2.ShiftToThisFrom());
Point point2DoubleShifted = point2InCurrent.Shift(testRelativeToCurrent2.ShiftToThisFrom());
Point point2Combined = testPointWorld2.Shift(expectedCombined2.ShiftToThisFrom());
(expectedPoint == point2DoubleShifted).Should().BeTrue();
(expectedPoint == point2Combined).Should().BeTrue();
//and one last one that negates out the anles
CoordinateSystem testCurrent3 = new CoordinateSystem(Point.MakePointWithInches(2, -2, 0), Angle.RightAngle, Angle.ZeroAngle, Angle.RightAngle);
CoordinateSystem testRelativeToCurrent3 = new CoordinateSystem(Point.MakePointWithInches(-2, 0, 0), -1 * Angle.RightAngle, -1 * Angle.RightAngle, Angle.ZeroAngle);
CoordinateSystem expectedCombined3 = new CoordinateSystem(Point.MakePointWithInches(2, -4, 0), Angle.ZeroAngle, Angle.ZeroAngle, Angle.ZeroAngle);
Point testPointWorld3 = Point.MakePointWithInches(3, -2, 3);
Point point3InCurrent = testPointWorld3.Shift(testCurrent3.ShiftToThisFrom());
Point point3DoubleShifted = point3InCurrent.Shift(testRelativeToCurrent3.ShiftToThisFrom());
Point point3Combined = testPointWorld3.Shift(expectedCombined3.ShiftToThisFrom());
(expectedPoint == point3DoubleShifted).Should().BeTrue();
(expectedPoint == point3Combined).Should().BeTrue();
}
public void CoordinateSystem_FindThisSystemRelativeToWorldSystemCurrentlyRelativeToPassedSystem()
{
//test one that really is more just an origin test
CoordinateSystem testCurrent = new CoordinateSystem(Point.MakePointWithInches(1, 2, 3), Angle.ZeroAngle, Angle.ZeroAngle, Angle.RightAngle);
CoordinateSystem testRelativeToCurrent = new CoordinateSystem(Point.MakePointWithInches(1, -2, 1), Angle.ZeroAngle, Angle.ZeroAngle, Angle.RightAngle / 2);
CoordinateSystem basedOnWorld1 = testRelativeToCurrent.FindThisSystemRelativeToWorldSystemCurrentlyRelativeToPassedSystem(testCurrent);
Point expectedOrigin = Point.MakePointWithInches(3, 3, 4);
Angle xExpected = Angle.ZeroAngle;
Angle yExpected = Angle.ZeroAngle;
Angle zExpected = new Angle(new Degree(), 135);
CoordinateSystem expectedCombined1 = new CoordinateSystem(expectedOrigin, xExpected, yExpected, zExpected);
//this just helps with debugging and is redundent
Point testPointWorld1 = Point.MakePointWithInches(3 - 2.12132034356, 3 - 0.70710678118, 7);
Point resultPoint1 = testPointWorld1.Shift(basedOnWorld1.ShiftToThisFrom()); //1,2,3
Point expectedPoint1 = testPointWorld1.Shift(expectedCombined1.ShiftToThisFrom()); //1,2,3
(basedOnWorld1 == expectedCombined1).Should().BeTrue();
//test that negates the coordinate system (this works in both intrinsic and extrensic scenarios
CoordinateSystem testCurrent2 = new CoordinateSystem(Point.MakePointWithInches(2, -2, 0), Angle.RightAngle, Angle.ZeroAngle, Angle.RightAngle);
CoordinateSystem testRelativeToCurrent2 = new CoordinateSystem(Point.MakePointWithInches(-2, 0, 0), -1 * Angle.RightAngle, -1 * Angle.RightAngle, Angle.ZeroAngle);
CoordinateSystem basedOnWorld2 = testRelativeToCurrent2.FindThisSystemRelativeToWorldSystemCurrentlyRelativeToPassedSystem(testCurrent2);
CoordinateSystem expectedCombined2 = new CoordinateSystem(Point.MakePointWithInches(2, -4, 0), Angle.ZeroAngle, Angle.ZeroAngle, Angle.ZeroAngle);
//this just helps with debugging and is redundent
Point testPointWorld2 = Point.MakePointWithInches(3, -2, 3);
Point resultPoint2 = testPointWorld2.Shift(basedOnWorld2.ShiftToThisFrom()); //1,2,3
Point expectedPoint2 = testPointWorld2.Shift(expectedCombined2.ShiftToThisFrom()); //1,2,3
basedOnWorld2.Should().Be(expectedCombined2);
//test one that onlt works when using the current coords axes for the second one and not the worlds
CoordinateSystem testCurrent3 = new CoordinateSystem(Point.Origin, Angle.RightAngle, Angle.RightAngle, Angle.ZeroAngle);
CoordinateSystem testRelativeToCurrent3 = new CoordinateSystem(Point.Origin, Angle.RightAngle / 2, Angle.ZeroAngle, Angle.ZeroAngle);
CoordinateSystem basedOnWorld3 = testRelativeToCurrent3.FindThisSystemRelativeToWorldSystemCurrentlyRelativeToPassedSystem(testCurrent3);
Point expectedOrigin3 = Point.Origin; //1, 3, -2
Angle xExpected3 = Angle.RightAngle;
Angle yExpected3 = Angle.RightAngle;
Angle zExpected3 = new Angle(new Degree(), -45);
CoordinateSystem expectedCombined3 = new CoordinateSystem(expectedOrigin3, xExpected3, yExpected3, zExpected3);
// Matrix e = expectedCombined3.RotationMatrixFromThisToWorld();
var e = expectedCombined3.ShiftFromThisToWorld.Matrix;
List<Angle> ea = e.EulerAngles();
Matrix r = basedOnWorld3.RotationMatrixFromThisToWorld();
List<Angle> ra = r.EulerAngles();
(basedOnWorld3 == expectedCombined3).Should().BeTrue();
//now try another test just for a more robust case
CoordinateSystem testCurrent4 = new CoordinateSystem(Point.MakePointWithInches(1, 1, -1), Angle.StraightAngle, Angle.ZeroAngle, -1 * Angle.RightAngle);
CoordinateSystem testRelativeToCurrent4 = new CoordinateSystem(Point.MakePointWithInches(-2, 0, 1), new Angle(new Degree(), -45), -1 * Angle.RightAngle, Angle.ZeroAngle);
CoordinateSystem basedOnWorld4 = testRelativeToCurrent4.FindThisSystemRelativeToWorldSystemCurrentlyRelativeToPassedSystem(testCurrent4);
Point expectedOrigin4 = Point.MakePointWithInches(1, 3, -2); //1, 3, -2
Angle xExpected4 = Angle.ZeroAngle;
Angle yExpected4 = Angle.RightAngle;
Angle zExpected4 = new Angle(new Degree(), 135);
CoordinateSystem expectedCombined4 = new CoordinateSystem(expectedOrigin4, xExpected4, yExpected4, zExpected4);
//this just helps with debugging and is redundent
Point testPointWorld4 = Point.MakePointWithInches(1 - 3.53553390611, 3 + 0.70710678032, -3);
Point resultPoint4 = testPointWorld4.Shift(basedOnWorld4.ShiftToThisFrom()); //1,2,3
Point expectedPoint4 = testPointWorld4.Shift(expectedCombined4.ShiftToThisFrom()); //1,2,3
(basedOnWorld4 == expectedCombined4).Should().BeTrue();
}
