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)