def test_given_eight_turn_right_when_converting_absolute_then_coordinate_is_correct(
self) -> None:
orientation = Orientation(pi / 4)
start = AbsoluteCoordinate(5, 3, orientation)
stop = AbsoluteCoordinate(7, 1, orientation)
transform = RobotCoordinateTransform(start)
actual_coordinate = transform.from_absolute(stop)
self.assertRobotCoordIsCorrect((0, sqrt(8)), actual_coordinate)
def test_given_quarter_turn_left_when_converting_absolute_then_coordinate_is_correct(
self) -> None:
orientation = Orientation(-pi / 2)
start = AbsoluteCoordinate(5, 3, orientation)
stop = AbsoluteCoordinate(7, 1, orientation)
transform = RobotCoordinateTransform(start)
actual_coordinate = transform.from_absolute(stop)
self.assertRobotCoordIsCorrect((2, -2), actual_coordinate)
def test_given_quarter_turn_right_from_south_when_converting_absolute_then_orientation_change_is_correct(
self) -> None:
orientation = Orientation(pi)
start = AbsoluteCoordinate(0, 0, orientation)
orientation_change = Orientation(pi / 2)
stop = AbsoluteCoordinate(0, 0, orientation + orientation_change)
transform = RobotCoordinateTransform(start)
actual_coordinate = transform.from_absolute(stop)
self.assertEqual(orientation_change,
actual_coordinate.orientation_change)
def _calculate_absolute(movement: AbsoluteMovement) -> List[Operation]:
robot_coordinate_transform = RobotCoordinateTransform(movement.start)
robot_coordinate = robot_coordinate_transform.from_absolute(
movement.stop)
distance = Distance(robot_coordinate.length)
rotation = Angle(robot_coordinate.orientation_change.radians)
operations = []
if rotation != Angle.zero():
rotation = Angle.to_effective(rotation)
operations.append(RotateOperation(rotation))
if distance != Distance.zero():
angle = Angle(robot_coordinate.direction.radians)
angle = Angle.to_effective(angle)
operations.append(TranslateOperation(angle, distance))
return operations