|
public FindThisSystemRelativeToWorldSystemCurrentlyRelativeToPassedSystem ( |
||
| thisRelativeTo | The coordinate System this coordinate system is currently based on | |
| return | ||
private LegoBlock createBlockInGivenCoordinateSystem(Distance xDimension, Distance yDimension, Distance zDimension, CoordinateSystem blocksCoordinateSystem)
{
LegoBlock toReturn = new LegoBlock();
//make our geometry
Point basePoint = Point.Origin;
Point topLeftPoint = new Point(Distance.ZeroDistance, yDimension, Distance.ZeroDistance);
Point bottomRightPoint = new Point(xDimension, Distance.ZeroDistance, Distance.ZeroDistance);
Point topRightPoint = new Point(xDimension, yDimension, Distance.ZeroDistance);
Point backBasePoint = new Point(Distance.ZeroDistance, Distance.ZeroDistance, zDimension);
Point backTopLeftPoint = new Point(Distance.ZeroDistance, yDimension, zDimension);
Point backBottomRightPoint = new Point(xDimension, Distance.ZeroDistance, zDimension);
Point backTopRightPoint = new Point(xDimension, yDimension, zDimension);
List<Polygon> planes = new List<Polygon>();
planes.Add(new Polygon(new List<Point> { basePoint, topLeftPoint, topRightPoint, bottomRightPoint }));
planes.Add(new Polygon(new List<Point> { backBasePoint, backTopLeftPoint, backTopRightPoint, backBottomRightPoint }));
planes.Add(new Polygon(new List<Point> { topLeftPoint, topRightPoint, backTopRightPoint, backTopLeftPoint }));
planes.Add(new Polygon(new List<Point> { basePoint, bottomRightPoint, backBottomRightPoint, backBasePoint }));
planes.Add(new Polygon(new List<Point> { basePoint, topLeftPoint, backTopLeftPoint, backBasePoint }));
planes.Add(new Polygon(new List<Point> { bottomRightPoint, topRightPoint, backTopRightPoint, backBottomRightPoint }));
toReturn.Geometry = new Polyhedron(planes);
//now figure out is base coordinate system relative to the world and store it
toReturn.BlockSystem = blocksCoordinateSystem.FindThisSystemRelativeToWorldSystemCurrentlyRelativeToPassedSystem(CURRENT_COORDINATE_SYSTEM);
//now move it so it is in ouur current coordinate system
toReturn.Geometry = toReturn.Geometry.Shift(blocksCoordinateSystem.ShiftFromThisTo());
//then return it
return toReturn;
}
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();
}