use super::chain::{cross, rot, scale, sub, RevoluteJoint, SerialChain};
use super::{dot, norm, unit, Goal, Ik, Solution, V3};
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Bearing {
pub cant: f64,
pub station: f64,
}
#[derive(Debug, Clone, PartialEq)]
pub struct SwivelNozzle {
pub bearings: Vec<Bearing>,
pub exit: f64,
pub masses: Vec<f64>,
pub com: Vec<V3>,
pub reference: V3,
pub aim: f64,
pub breakout: f64,
}
impl SwivelNozzle {
pub fn three_bearing() -> Self {
Self {
bearings: vec![
Bearing {
cant: 0.0,
station: 0.000,
},
Bearing {
cant: -23.750,
station: 0.560,
},
Bearing {
cant: 23.750,
station: 1.280,
},
],
exit: 1.680,
masses: vec![463.5, 463.3, 306.7],
com: vec![
[0.269, 0.093, -0.107],
[0.246, 0.093, 0.105],
[0.106, 0.002, -0.045],
],
reference: [0.0, 0.0, 0.0],
aim: 1.0,
breakout: 30.0,
}
}
pub fn masses(mut self, masses: impl Into<Vec<f64>>) -> Self {
self.masses = masses.into();
self
}
pub fn com_offsets(mut self, com: impl Into<Vec<V3>>) -> Self {
self.com = com.into();
self
}
pub fn from_bearings(bearings: impl Into<Vec<Bearing>>, exit: f64) -> Self {
let bearings = bearings.into();
let n = bearings.len();
let mut nozzle = Self {
bearings,
exit,
masses: vec![0.0; n],
com: vec![[0.0; 3]; n],
..Self::three_bearing()
};
let segments = nozzle.segments();
nozzle.com = segments.iter().map(|l| [l * 0.5, 0.0, 0.0]).collect();
nozzle
}
pub fn reference(mut self, reference: V3) -> Self {
self.reference = reference;
self
}
pub fn n(&self) -> usize {
self.bearings.len()
}
pub fn axes(&self) -> Vec<V3> {
self.bearings
.iter()
.map(|b| {
let c = b.cant.to_radians();
[c.cos(), 0.0, -c.sin()]
})
.collect()
}
pub fn segments(&self) -> Vec<f64> {
let n = self.n();
(0..n)
.map(|i| {
let next = if i + 1 < n {
self.bearings[i + 1].station
} else {
self.exit
};
next - self.bearings[i].station
})
.collect()
}
pub fn envelope(&self) -> f64 {
2.0 * self
.bearings
.iter()
.enumerate()
.map(|(i, b)| if i % 2 == 0 { b.cant } else { -b.cant })
.sum::<f64>()
}
pub fn chain(&self) -> SerialChain {
let axes = self.axes();
let segments = self.segments();
let joints = (0..self.n())
.map(|i| {
let len = segments[i];
RevoluteJoint {
axis: axes[i],
link: [len, 0.0, 0.0],
mass: self.masses.get(i).copied().unwrap_or(0.0),
com: self.com.get(i).copied().unwrap_or([len * 0.5, 0.0, 0.0]),
limits: (-180.0, 180.0),
}
})
.collect();
SerialChain {
joints,
gravity: [0.0, 0.0, -9.81],
metres: 1.0,
}
}
pub fn exhaust(&self, q: &[f64]) -> (V3, V3) {
self.chain().tool(q)
}
pub fn deflection(&self, q: &[f64]) -> f64 {
let (_, e) = self.exhaust(q);
unit(e)[0].clamp(-1.0, 1.0).acos().to_degrees()
}
pub fn azimuth(&self, q: &[f64]) -> f64 {
let e = unit(self.exhaust(q).1);
if e[1].abs() < 1e-12 && e[2].abs() < 1e-12 {
return 0.0;
}
e[1].atan2(-e[2]).to_degrees()
}
pub fn lateral(&self, q: &[f64]) -> f64 {
unit(self.exhaust(q).1)[1]
.clamp(-1.0, 1.0)
.asin()
.to_degrees()
}
pub fn direction(deflection: f64, azimuth: f64) -> V3 {
let (d, a) = (deflection.to_radians(), azimuth.to_radians());
[d.cos(), d.sin() * a.sin(), -d.sin() * a.cos()]
}
pub fn direction_jacobian(&self, q: &[f64]) -> [[f64; 3]; 3] {
let chain = self.chain();
let poses = chain.poses(q);
let e = unit(chain.tool(q).1);
let mut j = [[0.0; 3]; 3];
for (c, pose) in poses.iter().enumerate().take(3) {
let col = cross(pose.axis, e);
for r in 0..3 {
j[r][c] = col[r];
}
}
j
}
pub fn authority(&self, q: &[f64]) -> f64 {
let j = self.direction_jacobian(q);
let mut a = [[0.0f64; 3]; 3];
for r in 0..3 {
for c in 0..3 {
a[r][c] = (0..3).map(|k| j[r][k] * j[c][k]).sum();
}
}
sym3_eigenvalues(a)[1].max(0.0).sqrt()
}
pub fn solver(&self) -> Ik {
Ik {
lambda: 0.02 * self.aim,
max_step: 4.0,
tol: 1e-6 * self.aim,
max_iters: 200,
limits: (-180.0, 180.0),
..Ik::default()
}
}
pub fn solve(&self, seed: &[f64], deflection: f64, azimuth: f64) -> Solution {
let want = Self::direction(deflection, azimuth);
let chain = self.chain();
let aim = self.aim;
let mut sol = self
.solver()
.solve(seed, &Goal::Point(scale(want, aim)), |q| {
let e = unit(chain.tool(q).1);
(scale(e, aim), e)
});
sol.axis_err = 2.0
* (sol.position_err / (2.0 * aim))
.clamp(0.0, 1.0)
.asin()
.to_degrees();
sol
}
pub fn has_roll_bearing(&self) -> bool {
self.bearings.first().is_some_and(|b| b.cant.abs() < 1e-9)
}
pub fn roll_schedule(&self, to: f64, steps: usize) -> Schedule {
let steps = steps.max(1);
let mut points = Vec::with_capacity(steps + 1);
for i in 0..=steps {
let t = 180.0 * i as f64 / steps as f64;
let mut q = vec![t; self.n()];
q[0] = 0.0;
q[0] = -self.azimuth(&q);
let deflection = self.deflection(&q);
points.push(self.sample(&q, 0.0));
if deflection >= to {
break;
}
}
Schedule {
points,
breakout: 0.0,
}
}
pub fn schedule(&self, to: f64, steps: usize) -> Schedule {
if self.has_roll_bearing() {
self.roll_schedule(to, steps)
} else {
self.searched_schedule(to, steps)
}
}
pub fn searched_schedule(&self, to: f64, steps: usize) -> Schedule {
let n = self.n();
let steps = steps.max(1);
let mut points = Vec::with_capacity(steps + 17);
const BREAKOUT_POINTS: usize = 16;
let mut q = vec![0.0; n];
for i in 0..=BREAKOUT_POINTS {
let t = self.breakout * i as f64 / BREAKOUT_POINTS as f64;
q = vec![t; n];
points.push(self.sample(&q, 0.0));
}
let start = points.last().map(|p| p.deflection).unwrap_or(0.0);
let to = to.min(self.envelope());
if to > start {
for i in 1..=steps {
let want = start + (to - start) * i as f64 / steps as f64;
let sol = self.solve(&q, want, 0.0);
q = sol.joints;
points.push(self.sample(&q, sol.axis_err));
}
}
Schedule {
points,
breakout: start,
}
}
fn sample(&self, q: &[f64], residual: f64) -> SchedulePoint {
SchedulePoint {
deflection: self.deflection(q),
lateral: self.lateral(q),
bearings: q.to_vec(),
authority: self.authority(q),
residual,
}
}
pub fn thrust(&self, q: &[f64], thrust: f64) -> ThrustState {
let (exit, dir) = self.exhaust(q);
let dir = unit(dir);
let force = scale(dir, -thrust);
let arm = sub(exit, self.reference);
ThrustState {
direction: dir,
exit,
deflection: dir[0].clamp(-1.0, 1.0).acos().to_degrees(),
lateral: dir[1].clamp(-1.0, 1.0).asin().to_degrees(),
force,
moment: cross(arm, force),
thrust,
}
}
pub fn holding_torque(&self, q: &[f64]) -> Vec<f64> {
let n = self.n();
self.chain()
.inverse_dynamics(q, &vec![0.0; n], &vec![0.0; n], true)
}
pub fn slew_torque(&self, q: &[f64], rate: &[f64], accel: &[f64]) -> Vec<f64> {
let rad: Vec<f64> = rate.iter().map(|v| v.to_radians()).collect();
let acc: Vec<f64> = accel.iter().map(|v| v.to_radians()).collect();
self.chain().inverse_dynamics(q, &rad, &acc, true)
}
}
#[derive(Debug, Clone, PartialEq)]
pub struct SchedulePoint {
pub deflection: f64,
pub lateral: f64,
pub bearings: Vec<f64>,
pub authority: f64,
pub residual: f64,
}
#[derive(Debug, Clone, PartialEq)]
pub struct Schedule {
pub points: Vec<SchedulePoint>,
pub breakout: f64,
}
impl Schedule {
pub fn bearings_at(&self, deflection: f64) -> Vec<f64> {
if self.points.is_empty() {
return Vec::new();
}
let first = &self.points[0];
if deflection <= first.deflection {
return first.bearings.clone();
}
let mut key = first.deflection;
for w in self.points.windows(2) {
let (a, b) = (&w[0], &w[1]);
let next = b.deflection.max(key);
if deflection <= next {
let span = next - key;
let t = if span > 1e-12 {
(deflection - key) / span
} else {
0.0
};
return a
.bearings
.iter()
.zip(&b.bearings)
.map(|(x, y)| x + (y - x) * t)
.collect();
}
key = next;
}
self.points.last().unwrap().bearings.clone()
}
pub fn travel(&self) -> Vec<f64> {
let mut out = Vec::with_capacity(self.points.len());
let mut total = 0.0;
out.push(0.0);
for w in self.points.windows(2) {
total += w[0]
.bearings
.iter()
.zip(&w[1].bearings)
.fold(0.0f64, |m, (a, b)| m.max((b - a).abs()));
out.push(total);
}
out
}
pub fn bearings_at_travel(&self, u: f64) -> Vec<f64> {
if self.points.is_empty() {
return Vec::new();
}
let travel = self.travel();
let total = travel.last().copied().unwrap_or(0.0);
if total <= 0.0 {
return self.points[0].bearings.clone();
}
let want = (u.clamp(0.0, 1.0)) * total;
for (i, w) in travel.windows(2).enumerate() {
if want <= w[1] {
let span = w[1] - w[0];
let t = if span > 1e-12 {
(want - w[0]) / span
} else {
0.0
};
return self.points[i]
.bearings
.iter()
.zip(&self.points[i + 1].bearings)
.map(|(a, b)| a + (b - a) * t)
.collect();
}
}
self.points.last().unwrap().bearings.clone()
}
pub fn breakout_reversal(&self) -> f64 {
let mut peak = f64::NEG_INFINITY;
let mut worst = 0.0f64;
for p in &self.points {
peak = peak.max(p.deflection);
worst = worst.max(peak - p.deflection);
}
worst
}
pub fn reach(&self) -> f64 {
self.points.last().map(|p| p.deflection).unwrap_or(0.0)
}
pub fn worst_lateral(&self) -> f64 {
self.points
.iter()
.fold(0.0f64, |m, p| m.max(p.lateral.abs()))
}
pub fn worst_residual(&self) -> f64 {
self.points.iter().fold(0.0f64, |m, p| m.max(p.residual))
}
pub fn worst_authority(&self) -> (f64, f64) {
self.points.iter().fold((f64::INFINITY, 0.0), |(a, d), p| {
if p.authority < a {
(p.authority, p.deflection)
} else {
(a, d)
}
})
}
pub fn is_monotonic(&self) -> bool {
self.points
.iter()
.filter(|p| p.deflection >= self.breakout - 1e-9)
.collect::<Vec<_>>()
.windows(2)
.all(|w| w[1].deflection >= w[0].deflection - 1e-9)
}
pub fn worst_step(&self) -> f64 {
self.points.windows(2).fold(0.0f64, |m, w| {
w[0].bearings
.iter()
.zip(&w[1].bearings)
.fold(m, |m, (a, b)| m.max((b - a).abs()))
})
}
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct ThrustState {
pub direction: V3,
pub exit: V3,
pub deflection: f64,
pub lateral: f64,
pub force: V3,
pub moment: V3,
pub thrust: f64,
}
impl ThrustState {
pub fn lift(&self) -> f64 {
self.force[2]
}
pub fn axial(&self) -> f64 {
-self.force[0]
}
pub fn side(&self) -> f64 {
self.force[1]
}
pub fn pitch(&self) -> f64 {
self.moment[1]
}
pub fn lift_fraction(&self) -> f64 {
if self.thrust.abs() < 1e-12 {
0.0
} else {
self.lift() / self.thrust
}
}
}
fn sym3_eigenvalues(a: [[f64; 3]; 3]) -> [f64; 3] {
let p1 = a[0][1] * a[0][1] + a[0][2] * a[0][2] + a[1][2] * a[1][2];
let q = (a[0][0] + a[1][1] + a[2][2]) / 3.0;
if p1 < 1e-30 {
let mut d = [a[0][0], a[1][1], a[2][2]];
d.sort_by(|x, y| y.partial_cmp(x).unwrap_or(std::cmp::Ordering::Equal));
return d;
}
let p2 = (a[0][0] - q).powi(2) + (a[1][1] - q).powi(2) + (a[2][2] - q).powi(2) + 2.0 * p1;
let p = (p2 / 6.0).sqrt();
let mut b = a;
for (i, row) in b.iter_mut().enumerate() {
row[i] -= q;
for v in row.iter_mut() {
*v /= p;
}
}
let det = b[0][0] * (b[1][1] * b[2][2] - b[1][2] * b[2][1])
- b[0][1] * (b[1][0] * b[2][2] - b[1][2] * b[2][0])
+ b[0][2] * (b[1][0] * b[2][1] - b[1][1] * b[2][0]);
let phi = (det / 2.0).clamp(-1.0, 1.0).acos() / 3.0;
let e1 = q + 2.0 * p * phi.cos();
let e3 = q + 2.0 * p * (phi + 2.0 * std::f64::consts::PI / 3.0).cos();
[e1, 3.0 * q - e1 - e3, e3]
}
pub fn bearing_rotation(cant: f64, angle: f64) -> [[f64; 3]; 3] {
let c = cant.to_radians();
rot([c.cos(), 0.0, -c.sin()], angle)
}
pub fn cant_of(axis: V3) -> f64 {
let a = unit(axis);
(-a[2]).atan2(a[0]).to_degrees()
}
pub fn angle_between(a: V3, b: V3) -> f64 {
dot(unit(a), unit(b)).clamp(-1.0, 1.0).acos().to_degrees()
}
pub fn magnitude(v: V3) -> f64 {
norm(v)
}
#[cfg(test)]
mod tests {
use super::*;
fn nozzle() -> SwivelNozzle {
SwivelNozzle::three_bearing()
}
#[test]
fn stowed_is_a_straight_pipe() {
let n = nozzle();
let (exit, dir) = n.exhaust(&[0.0; 3]);
assert!((exit[0] - n.exit).abs() < 1e-12, "exit at {exit:?}");
assert!(
exit[1].abs() < 1e-12 && exit[2].abs() < 1e-12,
"exit at {exit:?}"
);
assert!(angle_between(dir, [1.0, 0.0, 0.0]) < 1e-9);
assert!(n.deflection(&[0.0; 3]) < 1e-9);
}
#[test]
fn half_a_turn_on_every_bearing_reaches_the_envelope() {
let n = nozzle();
assert!(
(n.envelope() - 95.0).abs() < 1e-9,
"envelope {}",
n.envelope()
);
let q = [180.0; 3];
assert!(
(n.deflection(&q) - 95.0).abs() < 1e-6,
"reached {}",
n.deflection(&q)
);
assert!(n.lateral(&q).abs() < 1e-6);
assert!(n.exhaust(&q).1[2] < 0.0, "it deflected upward");
}
#[test]
fn nothing_in_the_envelope_exceeds_the_envelope() {
let n = nozzle();
let mut worst: f64 = 0.0;
let mut q = [0.0f64; 3];
for a in (0..360).step_by(17) {
for b in (0..360).step_by(19) {
for c in (0..360).step_by(23) {
q = [a as f64, b as f64, c as f64];
worst = worst.max(n.deflection(&q));
}
}
}
let _ = q;
assert!(
worst <= n.envelope() + 1e-6,
"found {worst} past {}",
n.envelope()
);
assert!(
worst > n.envelope() - 3.0,
"the sweep never got near it: {worst}"
);
}
#[test]
fn signs_have_to_alternate_for_the_bends_to_add() {
let mut same = nozzle();
for b in &mut same.bearings {
b.cant = b.cant.abs();
}
assert!(
same.envelope().abs() < 1e-9,
"equal signs cancel: {}",
same.envelope()
);
assert!(same.deflection(&[180.0; 3]) < 1e-6);
}
#[test]
fn the_stowed_pose_has_no_first_order_pitch_authority() {
let n = nozzle();
let j = n.direction_jacobian(&[0.0; 3]);
for (c, (axial, pitch)) in j[0].iter().zip(j[2].iter()).enumerate() {
assert!(axial.abs() < 1e-12, "column {c} has an axial part");
assert!(pitch.abs() < 1e-12, "column {c} can pitch: {pitch}");
}
assert!(
(0..3).all(|r| j[r][0].abs() < 1e-12),
"a roll bearing should move a jet on its own axis nowhere: {:?}",
[j[0][0], j[1][0], j[2][0]]
);
for (c, yaw) in j[1].iter().enumerate().skip(1) {
assert!(yaw.abs() > 1e-3, "canted column {c} does nothing either");
}
assert!(
n.authority(&[0.0; 3]) < 1e-9,
"authority {}",
n.authority(&[0.0; 3])
);
let s = n.roll_schedule(95.0, 400);
let q = s.bearings_at(45.0);
assert!(
n.authority(&q) > 0.2,
"authority at 45 deg: {}",
n.authority(&q)
);
}
#[test]
fn the_roll_schedule_is_exact() {
let n = nozzle();
assert!(n.has_roll_bearing());
let s = n.roll_schedule(95.0, 400);
assert!(s.is_monotonic(), "the table doubles back");
assert!((s.reach() - 95.0).abs() < 1e-6, "reached {}", s.reach());
assert_eq!(s.breakout, 0.0, "a closed form needs no break-out");
assert!(
s.worst_lateral() < 1e-9,
"worst lateral {:e}",
s.worst_lateral()
);
let end = &s.points.last().unwrap().bearings;
assert!((end[1].abs() - 180.0).abs() < 1e-6, "{end:?}");
assert!((end[2].abs() - 180.0).abs() < 1e-6, "{end:?}");
assert!(
(end[1] - end[2]).abs() < 1e-6,
"the two canted joints move together"
);
}
#[test]
fn the_search_and_the_closed_form_agree() {
let n = nozzle();
let exact = n.roll_schedule(95.0, 400);
for want in [10.0, 25.0, 45.0, 65.0, 85.0] {
let seed = exact.bearings_at(want);
let sol = n.solve(&seed, want, 0.0);
assert!(sol.axis_err < 1e-3, "the search missed by {}", sol.axis_err);
let moved = seed
.iter()
.zip(&sol.joints)
.fold(0.0f64, |m, (a, b)| m.max((a - b).abs()));
assert!(moved < 0.5, "at {want} deg the search moved {moved} deg");
}
}
#[test]
fn the_direction_jacobian_matches_finite_differences() {
let n = nozzle();
for q in [
[20.0, 35.0, 60.0],
[120.0, -40.0, 150.0],
[-70.0, 90.0, 10.0],
] {
let j = n.direction_jacobian(&q);
let base = unit(n.exhaust(&q).1);
const H: f64 = 1e-6; for c in 0..3 {
let mut probe = q;
probe[c] += H.to_degrees();
let moved = unit(n.exhaust(&probe).1);
for r in 0..3 {
let fd = (moved[r] - base[r]) / H;
assert!(
(fd - j[r][c]).abs() < 1e-5,
"q={q:?} row {r} col {c}: analytic {} vs {fd}",
j[r][c]
);
}
}
}
}
#[test]
fn driving_the_bearings_in_step_takes_the_jet_out_of_the_plane() {
let n = nozzle();
let worst = (0..=180)
.map(|t| n.lateral(&[t as f64; 3]).abs())
.fold(0.0f64, f64::max);
assert!(
worst > 60.0,
"the ganged path only wandered {worst} degrees"
);
}
#[test]
fn the_solved_schedule_holds_the_plane_across_the_envelope() {
let n = nozzle();
let s = n.searched_schedule(95.0, 600);
assert!(s.is_monotonic(), "the closed loop doubled back");
assert!(s.reach() > 94.9, "only reached {}", s.reach());
assert!(s.breakout < 8.0, "break-out ran to {} degrees", s.breakout);
for p in s.points.iter().filter(|p| p.deflection > s.breakout + 1e-9) {
assert!(
p.lateral.abs() < 0.01,
"at {} deg the jet was {} deg out of plane",
p.deflection,
p.lateral
);
assert!(p.residual < 0.01, "missed by {} deg", p.residual);
}
assert!(
s.worst_lateral() < 8.0,
"worst lateral {}",
s.worst_lateral()
);
assert!(
n.roll_schedule(95.0, 600).worst_lateral() < 1e-9,
"the closed form should not wander at all"
);
}
#[test]
fn travel_parametrises_the_deployment_evenly() {
let n = nozzle();
let s = n.schedule(95.0, 400);
let travel = s.travel();
assert_eq!(travel.len(), s.points.len());
assert!(
travel.windows(2).all(|w| w[1] >= w[0]),
"travel went backwards"
);
let total = *travel.last().unwrap();
assert!(
total > 180.0,
"the deployment only moved {total} deg of bearing"
);
assert!(s.bearings_at_travel(0.0).iter().all(|v| v.abs() < 1e-9));
assert_eq!(s.bearings_at_travel(1.0), s.points.last().unwrap().bearings);
let mut prev = s.bearings_at_travel(0.0);
let mut worst: f64 = 0.0;
for i in 1..=100 {
let q = s.bearings_at_travel(i as f64 / 100.0);
worst = worst.max(
q.iter()
.zip(&prev)
.fold(0.0f64, |m, (a, b)| m.max((a - b).abs())),
);
prev = q;
}
assert!(
worst < 0.02 * total,
"a lurch of {worst} deg in one percent of travel"
);
assert!(n.deflection(&s.bearings_at_travel(1.0)) > 94.9);
}
#[test]
fn the_schedule_is_singular_at_both_ends_and_not_in_between() {
let n = nozzle();
let s = n.schedule(95.0, 400);
let (worst, at) = s.worst_authority();
assert!(
worst < 1e-6,
"the ends should be singular, got {worst} at {at}"
);
let middle = s
.points
.iter()
.filter(|p| p.deflection > 20.0 && p.deflection < 80.0)
.fold(f64::INFINITY, |m, p| m.min(p.authority));
assert!(
middle > 0.10,
"the middle of the envelope is weak: {middle}"
);
}
#[test]
fn the_table_interpolates_and_clamps() {
let n = nozzle();
let s = n.schedule(95.0, 200);
let at_zero = s.bearings_at(-5.0);
assert!(at_zero.iter().all(|v| v.abs() < 1e-9), "{at_zero:?}");
let past_end = s.bearings_at(120.0);
assert_eq!(past_end, s.points.last().unwrap().bearings);
for want in [10.0, 30.0, 55.0, 80.0, 92.0] {
let q = s.bearings_at(want);
assert!(
(n.deflection(&q) - want).abs() < 0.5,
"asked {want}, table gives {}",
n.deflection(&q)
);
assert!(n.lateral(&q).abs() < 0.5, "interpolation left the plane");
}
}
#[test]
fn full_deflection_is_lift_and_a_little_braking() {
let n = nozzle();
let q = [180.0; 3];
let t = n.thrust(&q, 80_000.0);
assert!(
t.lift_fraction() > 0.99,
"lift fraction {}",
t.lift_fraction()
);
assert!(t.axial() < 0.0, "axial {}", t.axial());
assert!(
t.axial() > -0.15 * t.thrust,
"braking far too hard: {}",
t.axial()
);
assert!(t.side().abs() < 1.0, "side force {}", t.side());
let s = n.thrust(&[0.0; 3], 80_000.0);
assert!((s.axial() - 80_000.0).abs() < 1e-3);
assert!(s.lift().abs() < 1e-6);
assert!(
s.pitch().abs() < 1e-6,
"a straight jet on the axis has no moment"
);
}
#[test]
fn a_deflected_jet_pitches_the_aircraft() {
let n = nozzle().reference([-4.0, 0.0, 0.0]);
let s = n.schedule(95.0, 200);
let q = s.bearings_at(90.0);
let t = n.thrust(&q, 80_000.0);
assert!(t.lift() > 0.9 * t.thrust);
assert!(
t.pitch() < 0.0,
"lift behind the CG is nose-down: {}",
t.pitch()
);
let arm = t.exit[0] - (-4.0);
assert!(
(-t.pitch() - arm * t.lift()).abs() < 0.02 * t.pitch().abs() + 1.0,
"moment {} vs lift*arm {}",
t.pitch(),
arm * t.lift()
);
let stowed = n.thrust(&[0.0; 3], 80_000.0);
assert!(stowed.pitch().abs() < 1e-6, "{}", stowed.pitch());
}
#[test]
fn holding_torque_peaks_where_the_nozzle_hangs_furthest_out() {
let n = nozzle();
let stowed = n.holding_torque(&[0.0; 3]);
assert!(stowed.iter().all(|t| t.abs() < 6000.0), "{stowed:?}");
let s = n.schedule(95.0, 200);
let worst = (0..=95)
.map(|d| {
let q = s.bearings_at(d as f64);
n.holding_torque(&q)
.iter()
.fold(0.0f64, |m, t| m.max(t.abs()))
})
.fold(0.0f64, f64::max);
assert!(
(500.0..50_000.0).contains(&worst),
"peak holding torque {worst} N.m looks wrong"
);
}
#[test]
fn accelerating_a_bearing_costs_torque_on_top_of_holding_it() {
let n = nozzle();
let s = n.schedule(95.0, 200);
let q = s.bearings_at(45.0);
let hold = n.holding_torque(&q);
let still = n.slew_torque(&q, &[0.0; 3], &[0.0; 3]);
for (a, b) in still.iter().zip(&hold) {
assert!((a - b).abs() < 1e-9, "{still:?} vs {hold:?}");
}
let one = n.slew_torque(&q, &[0.0; 3], &[60.0; 3]);
let two = n.slew_torque(&q, &[0.0; 3], &[120.0; 3]);
for i in 0..3 {
let d1 = one[i] - hold[i];
let d2 = two[i] - hold[i];
assert!(d1.abs() > 1e-6, "joint {i} felt no acceleration at all");
assert!(
(d2 / d1 - 2.0).abs() < 1e-6,
"joint {i}: {d1} then {d2}, not double"
);
}
let slow = n.slew_torque(&q, &[20.0; 3], &[0.0; 3]);
let fast = n.slew_torque(&q, &[40.0; 3], &[0.0; 3]);
for i in 0..3 {
let d1 = slow[i] - hold[i];
let d2 = fast[i] - hold[i];
assert!(d1.abs() > 1e-6, "joint {i} felt no rotation at all");
assert!(
(d2 / d1 - 4.0).abs() < 1e-5,
"joint {i}: {d1} then {d2}, not four times"
);
}
}
#[test]
fn the_solver_lands_on_a_commanded_direction() {
let n = nozzle();
let mut seed = vec![n.breakout; 3];
for want in [5.0, 20.0, 45.0, 70.0, 90.0] {
let sol = n.solve(&seed, want, 0.0);
assert!(
sol.axis_err < 1e-3,
"asked {want} deg, missed by {} ({:?})",
sol.axis_err,
sol.stop
);
seed = sol.joints;
}
let sol = n.solve(&seed, 40.0, 25.0);
assert!(sol.axis_err < 1e-3, "off-plane miss {}", sol.axis_err);
assert!((n.azimuth(&sol.joints) - 25.0).abs() < 0.01);
}
#[test]
fn symmetric_3x3_eigenvalues_come_back_descending_and_right() {
let a = [[3.0, 1.0, 1.0], [1.0, 3.0, 1.0], [1.0, 1.0, 4.0]];
let e = sym3_eigenvalues(a);
assert!(e[0] >= e[1] && e[1] >= e[2]);
let trace = a[0][0] + a[1][1] + a[2][2];
assert!((e.iter().sum::<f64>() - trace).abs() < 1e-9);
let d = sym3_eigenvalues([[1.0, 0.0, 0.0], [0.0, 5.0, 0.0], [0.0, 0.0, 3.0]]);
assert_eq!(d, [5.0, 3.0, 1.0]);
}
#[test]
fn a_bearing_rotation_is_the_chain_in_one_line() {
use super::super::chain::apply;
for cant in [5.0, 11.875, 23.75, 40.0] {
let r = bearing_rotation(cant, 180.0);
let bent = apply(r, [1.0, 0.0, 0.0]);
assert!(
(angle_between(bent, [1.0, 0.0, 0.0]) - 2.0 * cant).abs() < 1e-9,
"cant {cant} folded {}",
angle_between(bent, [1.0, 0.0, 0.0])
);
}
}
#[test]
fn segments_and_masses_line_up_with_the_stations() {
let n = nozzle();
let seg = n.segments();
assert_eq!(seg.len(), 3);
let stations: Vec<f64> = n.bearings.iter().map(|b| b.station).collect();
assert!((seg[0] - (stations[1] - stations[0])).abs() < 1e-12);
assert!((seg[1] - (stations[2] - stations[1])).abs() < 1e-12);
assert!((seg[2] - (n.exit - stations[2])).abs() < 1e-12);
assert!((seg.iter().sum::<f64>() - n.exit).abs() < 1e-12);
let heavy = nozzle().masses([900.0, 900.0, 600.0]);
let a = n.holding_torque(&[0.0, 45.0, 0.0]);
let b = heavy.holding_torque(&[0.0, 45.0, 0.0]);
assert!(b[0].abs() > a[0].abs(), "{b:?} vs {a:?}");
}
}