diff --git a/src/fastfiz_env/wrappers/action.py b/src/fastfiz_env/wrappers/action.py index 149ff1e..6cfb4cc 100644 --- a/src/fastfiz_env/wrappers/action.py +++ b/src/fastfiz_env/wrappers/action.py @@ -134,9 +134,9 @@ def action(self, action: np.ndarray[float, np.dtype[np.float32]]) -> np.ndarray[ case ActionSpaces.VECTOR_3D: x, y, z = action r, el, az = cart2sph(x, y, z) - r, theta, phi = sph2deg(r, el, az) - phi = float(np.interp(az, (0, np.rad2deg(np.pi)), (self.MIN_PHI, self.MAX_PHI))) - theta = float(np.interp(el, (-np.rad2deg(np.pi), np.rad2deg(np.pi)), (self.MIN_THETA, self.MAX_THETA))) + r, el, az = sph2deg(r, el, az) + phi = float(np.interp(az, (0, 360), (self.MIN_PHI, self.MAX_PHI))) + theta = float(np.interp(el, (0, 180), (self.MIN_THETA, self.MAX_THETA))) velocity = float(np.interp(r, (0, np.sqrt(3)), (self.MIN_VELOCITY, self.MAX_VELOCITY))) case ActionSpaces.NORM_3D: theta = float(np.interp(action[0], (-1, 1), (self.MIN_THETA, self.MAX_THETA))) diff --git a/src/fastfiz_env/wrappers/utils.py b/src/fastfiz_env/wrappers/utils.py index 0bfe321..0080d79 100644 --- a/src/fastfiz_env/wrappers/utils.py +++ b/src/fastfiz_env/wrappers/utils.py @@ -14,8 +14,9 @@ def cart2sph(x: float, y: float, z: float) -> tuple[float, float, float]: tuple[float, float, float]: A tuple containing radius (magnitude), elevation angle (theta, in radians), and azimuth angle (phi, in radians). """ r = np.sqrt(x**2 + y**2 + z**2) - el: float = np.arccos(z / r) - az: float = np.arctan2(y, x) + el = np.arccos(z / r) + az = (np.arctan2(y, x) + 2 * np.pi) % (2 * np.pi) # Using arctan2 to get correct quadrant + return r, el, az @@ -31,6 +32,6 @@ def sph2deg(r: float, el: float, az: float) -> tuple[float, float, float]: Returns: tuple[float, float, float]: A tuple containing radius (magnitude), elevation angle (theta, in degrees), and azimuth angle (phi, in degrees). """ - theta: float = np.rad2deg(el) - phi: float = np.rad2deg(az) - return r, theta, phi + el: float = np.rad2deg(el) + az: float = np.rad2deg(az) + return r, el, az