Define double pendulum system

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<DoublePendulum> {
+	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<f64> for DoublePendulum {
+	type Output = Self;
+
+	fn mul(self, rhs: f64) -> Self {
+		Self {
+			theta: self.theta * rhs,
+			phi: self.phi * rhs,
+		}
+	}
+}
+
+impl Div<f64> 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;
+	}
+}

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::*;