def _run(
self,
rotation: Tuple[float, float, float, float],
origin: Tuple[float, float, float],
offset: Tuple[float, float, float],
along_pos_x_answer: np.ndarray,
along_pos_y_answer: np.ndarray,
along_neg_x_answer: np.ndarray,
along_neg_y_answer: np.ndarray,
) -> None:
offset_as_list = [[offset[0], offset[1]]]
# Testing path along pos x direction
answer = convert_global_coords_to_local(self.along_pos_x, origin,
rotation)
np.testing.assert_allclose(answer, along_pos_x_answer, atol=1e-4)
answer = convert_global_coords_to_local(
self.along_pos_x + offset_as_list, offset, rotation)
np.testing.assert_allclose(answer, along_pos_x_answer, atol=1e-4)
# Testing path along pos y direction
answer = convert_global_coords_to_local(self.along_pos_y, origin,
rotation)
np.testing.assert_allclose(answer, along_pos_y_answer, atol=1e-4)
answer = convert_global_coords_to_local(
self.along_pos_y + offset_as_list, offset, rotation)
np.testing.assert_allclose(answer, along_pos_y_answer, atol=1e-4)
# Testing path along neg x direction
answer = convert_global_coords_to_local(self.along_neg_x, origin,
rotation)
np.testing.assert_allclose(answer, along_neg_x_answer, atol=1e-4)
answer = convert_global_coords_to_local(
self.along_neg_x + offset_as_list, offset, rotation)
np.testing.assert_allclose(answer, along_neg_x_answer, atol=1e-4)
# Testing path along neg y direction
answer = convert_global_coords_to_local(self.along_neg_y, origin,
rotation)
np.testing.assert_allclose(answer, along_neg_y_answer, atol=1e-4)
answer = convert_global_coords_to_local(
self.along_neg_y + offset_as_list, offset, rotation)
np.testing.assert_allclose(answer, along_neg_y_answer, atol=1e-4)
def test_heading_0(self):
rotation = (1, 0, 0, 0)
origin = (0, 0, 0)
offset = (50, 25, 0)
# Testing path along pos x direction
answer = convert_global_coords_to_local(self.along_pos_x, origin,
rotation)
np.testing.assert_allclose(answer, self.along_pos_y, atol=1e-4)
np.testing.assert_allclose(convert_local_coords_to_global(
answer, origin, rotation),
self.along_pos_x,
atol=1e-4)
answer = convert_global_coords_to_local(self.along_pos_x + [[50, 25]],
offset, rotation)
np.testing.assert_allclose(answer, self.along_pos_y, atol=1e-4)
np.testing.assert_allclose(convert_local_coords_to_global(
answer, offset, rotation),
self.along_pos_x + [[50, 25]],
atol=1e-4)
# Testing path along pos y direction
answer = convert_global_coords_to_local(self.along_pos_y, origin,
rotation)
np.testing.assert_allclose(answer, self.along_neg_x, atol=1e-4)
np.testing.assert_allclose(convert_local_coords_to_global(
answer, origin, rotation),
self.along_pos_y,
atol=1e-4)
answer = convert_global_coords_to_local(self.along_pos_y + [[50, 25]],
offset, rotation)
np.testing.assert_allclose(answer, self.along_neg_x, atol=1e-4)
np.testing.assert_allclose(convert_local_coords_to_global(
answer, offset, rotation),
self.along_pos_y + [[50, 25]],
atol=1e-4)
# Testing path along neg x direction
answer = convert_global_coords_to_local(self.along_neg_x, origin,
rotation)
np.testing.assert_allclose(answer, self.along_neg_y, atol=1e-4)
np.testing.assert_allclose(convert_local_coords_to_global(
answer, origin, rotation),
self.along_neg_x,
atol=1e-4)
answer = convert_global_coords_to_local(self.along_neg_x + [[50, 25]],
offset, rotation)
np.testing.assert_allclose(answer, self.along_neg_y, atol=1e-4)
np.testing.assert_allclose(convert_local_coords_to_global(
answer, offset, rotation),
self.along_neg_x + [[50, 25]],
atol=1e-4)
# Testing path along neg y direction
answer = convert_global_coords_to_local(self.along_neg_y, origin,
rotation)
np.testing.assert_allclose(answer, self.along_pos_x, atol=1e-4)
np.testing.assert_allclose(convert_local_coords_to_global(
answer, origin, rotation),
self.along_neg_y,
atol=1e-4)
answer = convert_global_coords_to_local(self.along_neg_y + [[50, 25]],
offset, rotation)
np.testing.assert_allclose(answer, self.along_pos_x, atol=1e-4)
np.testing.assert_allclose(convert_local_coords_to_global(
answer, offset, rotation),
self.along_neg_y + [[50, 25]],
atol=1e-4)
def test_heading_neg_pi_over_4(self):
rotation = (np.cos(-np.pi / 8), 0, 0, np.sin(-np.pi / 8))
origin = (0, 0, 0)
offset = (50, 25, 0)
# Testing path along pos x direction
answer = convert_global_coords_to_local(self.along_pos_x, origin,
rotation)
np.testing.assert_allclose(
answer,
self.y_equals_x *
[[-np.sqrt(2) / 2, np.sqrt(2) / 2]],
atol=1e-4)
np.testing.assert_allclose(convert_local_coords_to_global(
answer, origin, rotation),
self.along_pos_x,
atol=1e-4)
answer = convert_global_coords_to_local(self.along_pos_x + [[50, 25]],
offset, rotation)
np.testing.assert_allclose(
answer,
self.y_equals_x *
[[-np.sqrt(2) / 2, np.sqrt(2) / 2]],
atol=1e-4)
np.testing.assert_allclose(convert_local_coords_to_global(
answer, offset, rotation),
self.along_pos_x + [[50, 25]],
atol=1e-4)
# Testing path along pos y direction
answer = convert_global_coords_to_local(self.along_pos_y, origin,
rotation)
np.testing.assert_allclose(answer,
self.y_equals_x *
[[-np.sqrt(2) / 2, -np.sqrt(2) / 2]],
atol=1e-4)
np.testing.assert_allclose(convert_local_coords_to_global(
answer, origin, rotation),
self.along_pos_y,
atol=1e-4)
answer = convert_global_coords_to_local(self.along_pos_y + [[50, 25]],
offset, rotation)
np.testing.assert_allclose(answer,
self.y_equals_x *
[[-np.sqrt(2) / 2, -np.sqrt(2) / 2]],
atol=1e-4)
np.testing.assert_allclose(convert_local_coords_to_global(
answer, offset, rotation),
self.along_pos_y + [[50, 25]],
atol=1e-4)
# Testing path along neg x direction
answer = convert_global_coords_to_local(self.along_neg_x, origin,
rotation)
np.testing.assert_allclose(answer,
self.y_equals_x *
[[np.sqrt(2) / 2, -np.sqrt(2) / 2]],
atol=1e-4)
np.testing.assert_allclose(convert_local_coords_to_global(
answer, origin, rotation),
self.along_neg_x,
atol=1e-4)
answer = convert_global_coords_to_local(self.along_neg_x + [[50, 25]],
offset, rotation)
np.testing.assert_allclose(answer,
self.y_equals_x *
[[np.sqrt(2) / 2, -np.sqrt(2) / 2]],
atol=1e-4)
np.testing.assert_allclose(convert_local_coords_to_global(
answer, offset, rotation),
self.along_neg_x + [[50, 25]],
atol=1e-4)
# Testing path along neg y direction
answer = convert_global_coords_to_local(self.along_neg_y, origin,
rotation)
np.testing.assert_allclose(
answer,
self.y_equals_x * [[np.sqrt(2) / 2, np.sqrt(2) / 2]],
atol=1e-4)
np.testing.assert_allclose(convert_local_coords_to_global(
answer, origin, rotation),
self.along_neg_y,
atol=1e-4)
answer = convert_global_coords_to_local(self.along_neg_y + [[50, 25]],
offset, rotation)
np.testing.assert_allclose(
answer,
self.y_equals_x * [[np.sqrt(2) / 2, np.sqrt(2) / 2]],
atol=1e-4)
np.testing.assert_allclose(convert_local_coords_to_global(
answer, offset, rotation),
self.along_neg_y + [[50, 25]],
atol=1e-4)