1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
use super::Dynamics;
use crate::dimensions::{Matrix3, Vector3, VectorN, U3};
use std::f64;
use utils::is_diagonal;
// use alga::general::operator::Inverse;
/// `AngularMom` exposes the equations of motion for the angular momentum of a **rigid body**.
#[derive(Copy, Clone, Debug)]
pub struct AngularMom {
time: f64, // Needed to stop the integration by calling dyn.time()
tensor: Matrix3<f64>,
velocity: Vector3<f64>,
}
impl AngularMom {
/// Initializes a new AngularMom struct with an time-invariant inertia tensor of a rigid body and its original angular velocity.
///
/// Throughout this documentation [I] refers to the inertia tensor and ω to the angular velocity.
/// NOTE: The provided inertia tensor **must** be expressed in a frame such that it is a diagonal matrix.
/// There is always such a frame. If this is _not_ the primary frame desired, use the parallel axis theorem.
/// This theorem is developped in Schaub & Junkins, "Analytical Mechanics of Space Systems", 3th ed., page 163,
/// section 4.2.2 "Inertia Matrix Properties". This function will **panic!** if the inertia tensor is not diagonal.
pub fn from_tensor_matrix(tensor: &Matrix3<f64>, velocity: &Vector3<f64>) -> AngularMom {
if !is_diagonal(tensor) {
panic!("The provided inertia tensor is not diagonal.");
}
AngularMom {
time: 0.0,
tensor: *tensor,
velocity: *velocity,
}
}
/// Returns the angular momentum of the system, i.e. [I]ω
pub fn momentum(&self) -> Vector3<f64> {
self.tensor * self.velocity
}
}
impl Dynamics for AngularMom {
type StateSize = U3;
type StateType = Vector3<f64>;
fn time(&self) -> f64 {
self.time
}
/// Returns the **angular velocity** ω of the system, not its momentum.
fn state_vector(&self) -> VectorN<f64, Self::StateSize> {
self.velocity
}
fn state(&self) -> Vector3<f64> {
self.velocity
}
/// Set the **angular velocity** ω of the system and the time.
fn set_state(&mut self, new_t: f64, new_angular_velocity: &VectorN<f64, Self::StateSize>) {
self.time = new_t;
self.velocity = *new_angular_velocity;
}
/// Computes the instantaneous equations of motion of the angular velocity of a tensor (i.e. the angular acceleration).
/// [I]̲̇ω = -[̃ω][I]̲ω + ̲L
///
/// Source: Schaub & Junkins, 3th ed., eq. 4.32.
fn eom(&self, _t: f64, omega: &VectorN<f64, Self::StateSize>) -> VectorN<f64, Self::StateSize> {
let omega_dot_x = (self.tensor[(1, 1)] - self.tensor[(2, 2)]) / self.tensor[(0, 0)]
* omega[(1, 0)]
* omega[(2, 0)];
let omega_dot_y = (self.tensor[(2, 2)] - self.tensor[(0, 0)]) / self.tensor[(1, 1)]
* omega[(2, 0)]
* omega[(0, 0)];
let omega_dot_z = (self.tensor[(0, 0)] - self.tensor[(1, 1)]) / self.tensor[(2, 2)]
* omega[(0, 0)]
* omega[(1, 0)];
Vector3::new(omega_dot_x, omega_dot_y, omega_dot_z)
}
}