|
| 1 | +import os |
| 2 | +import time |
1 | 3 | from functools import cached_property |
| 4 | +from importlib import resources |
2 | 5 |
|
3 | 6 | import matplotlib.pyplot as plt |
4 | 7 | import numpy as np |
5 | 8 |
|
| 9 | +from ..tools import import_optional_dependency |
6 | 10 | from .plot_helpers import show_or_save_plot |
7 | 11 |
|
8 | 12 |
|
@@ -133,6 +137,256 @@ def trajectory_3d(self, *, filename=None): # pylint: disable=too-many-statement |
133 | 137 | ax1.set_box_aspect(None, zoom=0.95) # 95% for label adjustment |
134 | 138 | show_or_save_plot(filename) |
135 | 139 |
|
| 140 | + def _resolve_animation_model_path(self, file_name): |
| 141 | + """Resolve model path, defaulting to the built-in STL when omitted.""" |
| 142 | + if file_name is not None: |
| 143 | + return file_name |
| 144 | + |
| 145 | + return str( |
| 146 | + resources.files("rocketpy.plots").joinpath("assets/default_rocket.stl") |
| 147 | + ) |
| 148 | + |
| 149 | + def _validate_animation_inputs(self, file_name, start, stop, time_step): |
| 150 | + """Validate shared input parameters for 3D animation methods.""" |
| 151 | + if time_step <= 0: |
| 152 | + raise ValueError( |
| 153 | + f"Invalid time_step: {time_step}. It must be greater than 0." |
| 154 | + ) |
| 155 | + |
| 156 | + if stop is None: |
| 157 | + stop = self.flight.t_final |
| 158 | + |
| 159 | + if ( |
| 160 | + start < 0 |
| 161 | + or stop < 0 |
| 162 | + or start > self.flight.t_final |
| 163 | + or stop > self.flight.t_final |
| 164 | + or start >= stop |
| 165 | + ): |
| 166 | + raise ValueError( |
| 167 | + f"Invalid animation time range: start={start}, stop={stop}. " |
| 168 | + f"Both must be within [0, {self.flight.t_final}] and start < stop." |
| 169 | + ) |
| 170 | + |
| 171 | + if not os.path.isfile(file_name): |
| 172 | + raise FileNotFoundError( |
| 173 | + f"Could not find the 3D model file: '{file_name}'. " |
| 174 | + "Provide a valid .stl file path." |
| 175 | + ) |
| 176 | + |
| 177 | + return stop |
| 178 | + |
| 179 | + @staticmethod |
| 180 | + def _rotation_from_quaternion(q0, q1, q2, q3): |
| 181 | + """Convert unit quaternion to axis-angle representation in degrees.""" |
| 182 | + norm = np.sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3) |
| 183 | + if norm == 0: |
| 184 | + return 0.0, (1.0, 0.0, 0.0) |
| 185 | + |
| 186 | + q0 = q0 / norm |
| 187 | + q1 = q1 / norm |
| 188 | + q2 = q2 / norm |
| 189 | + q3 = q3 / norm |
| 190 | + |
| 191 | + # q and -q represent the same orientation. Keep q0 non-negative to |
| 192 | + # reduce discontinuities in axis-angle interpolation across frames. |
| 193 | + if q0 < 0: |
| 194 | + q0 = -q0 |
| 195 | + q1 = -q1 |
| 196 | + q2 = -q2 |
| 197 | + q3 = -q3 |
| 198 | + |
| 199 | + q0 = np.clip(q0, -1.0, 1.0) |
| 200 | + angle = 2 * np.arccos(q0) |
| 201 | + sin_half = np.sqrt(max(1 - q0 * q0, 0.0)) |
| 202 | + |
| 203 | + if sin_half < 1e-12: |
| 204 | + return 0.0, (1.0, 0.0, 0.0) |
| 205 | + |
| 206 | + axis = (q1 / sin_half, q2 / sin_half, q3 / sin_half) |
| 207 | + return np.degrees(angle), axis |
| 208 | + |
| 209 | + def _create_animation_box(self, start, scale=1.0): |
| 210 | + """Create a world box with minimum visible dimensions.""" |
| 211 | + min_box_dim = 10.0 |
| 212 | + x_values = self.flight.x[:, 1] |
| 213 | + y_values = self.flight.y[:, 1] |
| 214 | + z_values = self.flight.z[:, 1] - self.flight.env.elevation |
| 215 | + |
| 216 | + center_x = 0.5 * (np.max(x_values) + np.min(x_values)) |
| 217 | + center_y = 0.5 * (np.max(y_values) + np.min(y_values)) |
| 218 | + center_z = max(self.flight.z(start) - self.flight.env.elevation, 0.0) |
| 219 | + |
| 220 | + length = max(np.ptp(x_values) * scale, min_box_dim) |
| 221 | + width = max(np.ptp(y_values) * scale, min_box_dim) |
| 222 | + height = max(np.ptp(z_values) * scale, min_box_dim) |
| 223 | + |
| 224 | + # Keep z center inside visible space while preserving minimum box size. |
| 225 | + center_z = max(center_z, 0.5 * min_box_dim) |
| 226 | + |
| 227 | + vedo = import_optional_dependency("vedo") |
| 228 | + Box = vedo.Box |
| 229 | + |
| 230 | + return Box( |
| 231 | + pos=[center_x, center_y, center_z], |
| 232 | + length=length, |
| 233 | + width=width, |
| 234 | + height=height, |
| 235 | + ).wireframe() |
| 236 | + |
| 237 | + def animate_trajectory( |
| 238 | + self, file_name=None, start=0, stop=None, time_step=0.1, **kwargs |
| 239 | + ): |
| 240 | + """Animate 6-DOF trajectory and attitude using vedo. |
| 241 | +
|
| 242 | + Parameters |
| 243 | + ---------- |
| 244 | + file_name : str | None, optional |
| 245 | + Path to a 3D model file representing the rocket, usually ``.stl``. |
| 246 | + If None, RocketPy uses a built-in default STL model. |
| 247 | + Default is None. |
| 248 | + start : int, float, optional |
| 249 | + Animation start time in seconds. Default is 0. |
| 250 | + stop : int, float | None, optional |
| 251 | + Animation end time in seconds. If None, uses ``flight.t_final``. |
| 252 | + Default is None. |
| 253 | + time_step : float, optional |
| 254 | + Animation frame step in seconds. Must be greater than 0. |
| 255 | + Default is 0.1. |
| 256 | + **kwargs : dict, optional |
| 257 | + Additional keyword arguments passed to ``vedo.Plotter.show``. |
| 258 | + """ |
| 259 | + |
| 260 | + vedo = import_optional_dependency("vedo") |
| 261 | + |
| 262 | + Line = vedo.Line |
| 263 | + Mesh = vedo.Mesh |
| 264 | + Plotter = vedo.Plotter |
| 265 | + settings = vedo.settings |
| 266 | + |
| 267 | + file_name = self._resolve_animation_model_path(file_name) |
| 268 | + stop = self._validate_animation_inputs(file_name, start, stop, time_step) |
| 269 | + |
| 270 | + try: |
| 271 | + settings.allow_interaction = True |
| 272 | + except AttributeError: |
| 273 | + pass |
| 274 | + |
| 275 | + world = self._create_animation_box(start, scale=1.2) |
| 276 | + base_rocket = Mesh(file_name).c("green") |
| 277 | + time_steps = np.arange(start, stop, time_step) |
| 278 | + trajectory_points = [] |
| 279 | + |
| 280 | + plt = Plotter(axes=1, interactive=False) |
| 281 | + plt.show(world, "Rocket Trajectory Animation", viewup="z", **kwargs) |
| 282 | + |
| 283 | + for t in time_steps: |
| 284 | + rocket = base_rocket.clone() |
| 285 | + x_position = self.flight.x(t) |
| 286 | + y_position = self.flight.y(t) |
| 287 | + z_position = self.flight.z(t) - self.flight.env.elevation |
| 288 | + |
| 289 | + angle_deg, axis = self._rotation_from_quaternion( |
| 290 | + self.flight.e0(t), |
| 291 | + self.flight.e1(t), |
| 292 | + self.flight.e2(t), |
| 293 | + self.flight.e3(t), |
| 294 | + ) |
| 295 | + |
| 296 | + rocket.pos(x_position, y_position, z_position) |
| 297 | + if angle_deg != 0.0: |
| 298 | + rocket.rotate(angle_deg, axis=axis) |
| 299 | + |
| 300 | + trajectory_points.append([x_position, y_position, z_position]) |
| 301 | + actors = [world, rocket] |
| 302 | + if len(trajectory_points) > 1: |
| 303 | + actors.append(Line(trajectory_points, c="k", alpha=0.5)) |
| 304 | + |
| 305 | + plt.show(*actors, resetcam=False) |
| 306 | + |
| 307 | + start_pause = time.time() |
| 308 | + while time.time() - start_pause < time_step: |
| 309 | + plt.render() |
| 310 | + |
| 311 | + if getattr(plt, "escaped", False): |
| 312 | + break |
| 313 | + |
| 314 | + plt.interactive().close() |
| 315 | + return None |
| 316 | + |
| 317 | + def animate_rotate( |
| 318 | + self, file_name=None, start=0, stop=None, time_step=0.1, **kwargs |
| 319 | + ): |
| 320 | + """Animate rocket attitude (rotation-only view) using vedo. |
| 321 | +
|
| 322 | + Parameters |
| 323 | + ---------- |
| 324 | + file_name : str | None, optional |
| 325 | + Path to a 3D model file representing the rocket, usually ``.stl``. |
| 326 | + If None, RocketPy uses a built-in default STL model. |
| 327 | + Default is None. |
| 328 | + start : int, float, optional |
| 329 | + Animation start time in seconds. Default is 0. |
| 330 | + stop : int, float | None, optional |
| 331 | + Animation end time in seconds. If None, uses ``flight.t_final``. |
| 332 | + Default is None. |
| 333 | + time_step : float, optional |
| 334 | + Animation frame step in seconds. Must be greater than 0. |
| 335 | + Default is 0.1. |
| 336 | + **kwargs : dict, optional |
| 337 | + Additional keyword arguments passed to ``vedo.Plotter.show``. |
| 338 | + """ |
| 339 | + |
| 340 | + vedo = import_optional_dependency("vedo") |
| 341 | + |
| 342 | + Mesh = vedo.Mesh |
| 343 | + Plotter = vedo.Plotter |
| 344 | + settings = vedo.settings |
| 345 | + |
| 346 | + file_name = self._resolve_animation_model_path(file_name) |
| 347 | + stop = self._validate_animation_inputs(file_name, start, stop, time_step) |
| 348 | + |
| 349 | + try: |
| 350 | + settings.allow_interaction = True |
| 351 | + except AttributeError: |
| 352 | + pass |
| 353 | + |
| 354 | + world = self._create_animation_box(start, scale=0.3) |
| 355 | + base_rocket = Mesh(file_name).c("green") |
| 356 | + time_steps = np.arange(start, stop, time_step) |
| 357 | + |
| 358 | + x_start = self.flight.x(start) |
| 359 | + y_start = self.flight.y(start) |
| 360 | + z_start = self.flight.z(start) - self.flight.env.elevation |
| 361 | + |
| 362 | + plt = Plotter(axes=1, interactive=False) |
| 363 | + plt.show(world, "Rocket Rotation Animation", viewup="z", **kwargs) |
| 364 | + |
| 365 | + for t in time_steps: |
| 366 | + rocket = base_rocket.clone() |
| 367 | + angle_deg, axis = self._rotation_from_quaternion( |
| 368 | + self.flight.e0(t), |
| 369 | + self.flight.e1(t), |
| 370 | + self.flight.e2(t), |
| 371 | + self.flight.e3(t), |
| 372 | + ) |
| 373 | + |
| 374 | + rocket.pos(x_start, y_start, z_start) |
| 375 | + if angle_deg != 0.0: |
| 376 | + rocket.rotate(angle_deg, axis=axis) |
| 377 | + |
| 378 | + plt.show(world, rocket, resetcam=False) |
| 379 | + |
| 380 | + start_pause = time.time() |
| 381 | + while time.time() - start_pause < time_step: |
| 382 | + plt.render() |
| 383 | + |
| 384 | + if getattr(plt, "escaped", False): |
| 385 | + break |
| 386 | + |
| 387 | + plt.interactive().close() |
| 388 | + return None |
| 389 | + |
136 | 390 | def linear_kinematics_data(self, *, filename=None): # pylint: disable=too-many-statements |
137 | 391 | """Prints out all Kinematics graphs available about the Flight |
138 | 392 |
|
|
0 commit comments