from aerosandbox.dynamics.point_mass.common_point_mass import (
_DynamicsPointMassBaseClass,
)
from aerosandbox.weights.mass_properties import MassProperties
import aerosandbox.numpy as np
from typing import Union, Dict, Tuple
[docs]class DynamicsPointMass3DCartesian(_DynamicsPointMassBaseClass):
"""
Dynamics instance:
* simulating a point mass
* in 3D
* with velocity parameterized in Cartesian coordinates
State variables:
x_e: x-position, in Earth axes. [meters]
y_e: y-position, in Earth axes. [meters]
z_e: z-position, in Earth axes. [meters]
u_e: x-velocity, in Earth axes. [m/s]
v_e: v-velocity, in Earth axes. [m/s]
w_e: z-velocity, in Earth axes. [m/s]
Indirect control variables:
alpha: Angle of attack. [degrees]
beta: Sideslip angle. [degrees]
bank: Bank angle. [radians]
Control variables:
Fx_e: Force along the Earth-x axis. [N]
Fy_e: Force along the Earth-y axis. [N]
Fz_e: Force along the Earth-z axis. [N]
"""
def __init__(
self,
mass_props: MassProperties = None,
x_e: Union[float, np.ndarray] = 0,
y_e: Union[float, np.ndarray] = 0,
z_e: Union[float, np.ndarray] = 0,
u_e: Union[float, np.ndarray] = 0,
v_e: Union[float, np.ndarray] = 0,
w_e: Union[float, np.ndarray] = 0,
alpha: Union[float, np.ndarray] = 0,
beta: Union[float, np.ndarray] = 0,
bank: Union[float, np.ndarray] = 0,
):
# Initialize state variables
[docs] self.mass_props = MassProperties() if mass_props is None else mass_props
# Initialize indirect control variables
# Initialize control variables
@property
[docs] def state(self) -> Dict[str, Union[float, np.ndarray]]:
return {
"x_e": self.x_e,
"y_e": self.y_e,
"z_e": self.z_e,
"u_e": self.u_e,
"v_e": self.v_e,
"w_e": self.w_e,
}
@property
[docs] def control_variables(self) -> Dict[str, Union[float, np.ndarray]]:
return {
"alpha": self.alpha,
"beta": self.beta,
"bank": self.bank,
"Fx_e": self.Fx_e,
"Fy_e": self.Fy_e,
"Fz_e": self.Fz_e,
}
[docs] def state_derivatives(self) -> Dict[str, Union[float, np.ndarray]]:
return {
"x_e": self.u_e,
"y_e": self.v_e,
"z_e": self.w_e,
"u_e": self.Fx_e / self.mass_props.mass,
"v_e": self.Fy_e / self.mass_props.mass,
"w_e": self.Fz_e / self.mass_props.mass,
}
@property
[docs] def speed(self) -> float:
return (
self.u_e**2
+ self.v_e**2
+ self.w_e**2
+ 1e-200 # To avoid gradient singularities
) ** 0.5
@property
[docs] def gamma(self):
"""
Returns the flight path angle, in radians.
Positive flight path angle indicates positive vertical speed.
"""
return np.arctan2(
-self.w_e,
(self.u_e**2 + self.v_e**2 + 1e-200) # To avoid gradient singularities
** 0.5,
)
@property
[docs] def track(self):
"""
Returns the track angle, in radians.
* Track of 0 == North == aligned with x_e axis
* Track of np.pi / 2 == East == aligned with y_e axis
"""
return np.arctan2(
self.v_e, self.u_e + 1e-200 # To avoid gradient singularities,
)
[docs] def convert_axes(
self,
x_from: float,
y_from: float,
z_from: float,
from_axes: str,
to_axes: str,
) -> Tuple[float, float, float]:
if from_axes == to_axes:
return x_from, y_from, z_from
if not (from_axes == "earth" and to_axes == "earth"):
rot_w_to_e = np.rotation_matrix_from_euler_angles(
roll_angle=self.bank,
pitch_angle=self.gamma,
yaw_angle=self.track,
as_array=False,
)
if from_axes == "earth":
x_e = x_from
y_e = y_from
z_e = z_from
elif from_axes == "wind":
x_e = (
rot_w_to_e[0][0] * x_from
+ rot_w_to_e[0][1] * y_from
+ rot_w_to_e[0][2] * z_from
)
y_e = (
rot_w_to_e[1][0] * x_from
+ rot_w_to_e[1][1] * y_from
+ rot_w_to_e[1][2] * z_from
)
z_e = (
rot_w_to_e[2][0] * x_from
+ rot_w_to_e[2][1] * y_from
+ rot_w_to_e[2][2] * z_from
)
else:
x_w, y_w, z_w = self.op_point.convert_axes(
x_from, y_from, z_from, from_axes=from_axes, to_axes="wind"
)
x_e = (
rot_w_to_e[0][0] * x_w + rot_w_to_e[0][1] * y_w + rot_w_to_e[0][2] * z_w
)
y_e = (
rot_w_to_e[1][0] * x_w + rot_w_to_e[1][1] * y_w + rot_w_to_e[1][2] * z_w
)
z_e = (
rot_w_to_e[2][0] * x_w + rot_w_to_e[2][1] * y_w + rot_w_to_e[2][2] * z_w
)
if to_axes == "earth":
x_to = x_e
y_to = y_e
z_to = z_e
elif to_axes == "wind":
x_to = (
rot_w_to_e[0][0] * x_e + rot_w_to_e[1][0] * y_e + rot_w_to_e[2][0] * z_e
)
y_to = (
rot_w_to_e[0][1] * x_e + rot_w_to_e[1][1] * y_e + rot_w_to_e[2][1] * z_e
)
z_to = (
rot_w_to_e[0][2] * x_e + rot_w_to_e[1][2] * y_e + rot_w_to_e[2][2] * z_e
)
else:
x_w = (
rot_w_to_e[0][0] * x_e + rot_w_to_e[1][0] * y_e + rot_w_to_e[2][0] * z_e
)
y_w = (
rot_w_to_e[0][1] * x_e + rot_w_to_e[1][1] * y_e + rot_w_to_e[2][1] * z_e
)
z_w = (
rot_w_to_e[0][2] * x_e + rot_w_to_e[1][2] * y_e + rot_w_to_e[2][2] * z_e
)
x_to, y_to, z_to = self.op_point.convert_axes(
x_w, y_w, z_w, from_axes="wind", to_axes=to_axes
)
return x_to, y_to, z_to
[docs] def add_force(
self,
Fx: Union[float, np.ndarray] = 0,
Fy: Union[float, np.ndarray] = 0,
Fz: Union[float, np.ndarray] = 0,
axes="earth",
) -> None:
Fx_e, Fy_e, Fz_e = self.convert_axes(
x_from=Fx, y_from=Fy, z_from=Fz, from_axes=axes, to_axes="earth"
)
self.Fx_e = self.Fx_e + Fx_e
self.Fy_e = self.Fy_e + Fy_e
self.Fz_e = self.Fz_e + Fz_e
if __name__ == "__main__":
[docs] dyn = DynamicsPointMass3DCartesian()