diff --git a/src/double_pendulum.rs b/src/double_pendulum.rs new file mode 100644 index 0000000..8606229 --- /dev/null +++ b/src/double_pendulum.rs @@ -0,0 +1,109 @@ +use crate::hamiltonian::*; + +use std::ops::{Add, Sub, Mul, Div, AddAssign}; + +const G: f64 = 9.81; + +#[derive(Clone, Copy, Debug)] +pub struct DoublePendulum { + pub theta: f64, + pub phi: f64, +} + +fn sq(x: f64) -> f64 { + x * x +} + +impl Hamiltonian for State { + fn energy(&self) -> f64 { + let cos_theta = self.q.theta.cos(); + let cos_phi = self.q.phi.cos(); + let cos_sum = (self.q.theta + self.q.phi).cos(); + + let t1 = sq(self.p.theta); + let t2 = (2.0 * cos_phi + 3.0) * sq(self.p.phi); + let t3 = 2.0 * (cos_phi + 1.0) * self.p.theta * self.p.phi; + + let kinetic = (t1 + t2 - t3) / (4.0 - 2.0 * sq(cos_phi)); + let potential = - G * (2.0 * cos_theta + cos_sum); + kinetic + potential + } + + fn dots(&self) -> Self { + let c = self.q.phi.cos(); + let s = self.q.phi.sin(); + let ssum = (self.q.theta + self.q.phi).sin(); + + let qdots = DoublePendulum { + theta: (self.p.theta - (c + 1.0) * self.p.phi) / (2.0 - sq(c)), + phi: { + let t1 = (2.0 * c + 3.0) * self.p.phi; + let t2 = (c + 1.0) * self.p.theta; + (t1 - t2) / (2.0 - sq(c)) + }, + }; + let pdots = DoublePendulum { + theta: - G * (2.0 * self.q.theta.sin() + ssum), + phi: { + let t1 = sq(self.p.theta) * c * s; + let t2 = sq(self.p.phi) * s * (c + 1.0) * (c + 2.0); + let t3 = self.p.theta * self.p.phi * s * (sq(c + 1.0) + 1.0); + (t1 + t2 - t3) / sq(2.0 - sq(c)) - G * ssum + } + }; + + Self { q: qdots, p: pdots } + } +} + +impl Add for DoublePendulum { + type Output = Self; + + fn add(self, rhs: Self) -> Self { + Self { + theta: self.theta + rhs.theta, + phi: self.phi + rhs.phi, + } + } +} + +impl Sub for DoublePendulum { + type Output = Self; + + fn sub(self, rhs: Self) -> Self { + Self { + theta: self.theta - rhs.theta, + phi: self.phi - rhs.phi, + } + } +} + + +impl Mul for DoublePendulum { + type Output = Self; + + fn mul(self, rhs: f64) -> Self { + Self { + theta: self.theta * rhs, + phi: self.phi * rhs, + } + } +} + +impl Div for DoublePendulum { + type Output = Self; + + fn div(self, rhs: f64) -> Self { + Self { + theta: self.theta / rhs, + phi: self.phi / rhs, + } + } +} + +impl AddAssign for DoublePendulum { + fn add_assign(&mut self, other: Self) { + self.theta += other.theta; + self.phi += other.phi; + } +} diff --git a/src/lib.rs b/src/lib.rs index 8716395..99d08de 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -1,5 +1,7 @@ pub mod hamiltonian; pub mod augmented; +pub mod double_pendulum; pub use hamiltonian::*; pub use augmented::*; +pub use double_pendulum::*;