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;
#[derive(Copy, Clone, Debug)]
pub struct AngularMom {
time: f64,
tensor: Matrix3<f64>,
velocity: Vector3<f64>,
}
impl AngularMom {
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,
}
}
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
}
fn state_vector(&self) -> VectorN<f64, Self::StateSize> {
self.velocity
}
fn state(&self) -> Vector3<f64> {
self.velocity
}
fn set_state(&mut self, new_t: f64, new_angular_velocity: &VectorN<f64, Self::StateSize>) {
self.time = new_t;
self.velocity = *new_angular_velocity;
}
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)
}
}