Repository: rpiRobotics/ik-geo (language: python)

Issue:
Rust Subproblem 5



Discussion / hints:

Fix patch:
diff --git a/rust/Cargo.toml b/rust/Cargo.toml
index facc291..1a675c6 100644
--- a/rust/Cargo.toml
+++ b/rust/Cargo.toml
@@ -7,5 +7,13 @@ edition = "2021"
 nalgebra = "0.31.1"
 fastrand = "1.8.0"
 
+[dev-dependencies.criterion]
+version = "0.3"
+features = [ "html_reports" ]
+
+[[bench]]
+name = "benchmark"
+harness = false
+
 [lib]
 doctest = false
diff --git a/rust/README.md b/rust/README.md
index 3f303e3..2eeef11 100644
--- a/rust/README.md
+++ b/rust/README.md
@@ -18,8 +18,11 @@
     use subproblems::nalgebra as na;
     ```
 
-## Testing
+### Testing
 
-1. Clone the rust directory
-2. Run `cargo test`\
+Run `cargo test`
 Run `cargo test -- --nocapture` for diagnostic info
+
+### Benchmarking
+
+Run `cargo bench`
diff --git a/rust/benches/benchmark.rs b/rust/benches/benchmark.rs
new file mode 100644
index 0000000..74ebe03
--- /dev/null
+++ b/rust/benches/benchmark.rs
@@ -0,0 +1,134 @@
+use linear_subproblem_solutions_rust::subproblems::subproblem5;
+
+use {
+    linear_subproblem_solutions_rust::subproblems::{
+        subproblem1,
+        subproblem2,
+        subproblem2extended,
+        subproblem3,
+        subproblem4,
+    },
+
+    nalgebra::Vector3,
+
+    criterion::{
+        Criterion,
+        black_box,
+        criterion_group,
+        criterion_main,
+    },
+};
+
+fn random_vector3() -> Vector3<f64> {
+    Vector3::new(random(), random(), random())
+}
+
+fn random_norm_vector3() -> Vector3<f64> {
+    Vector3::new(random(), random(), random()).normalize()
+}
+
+fn random() -> f64 {
+    fastrand::f64() * 2.0 - 1.0
+}
+
+pub fn subproblem1benchmark(c: &mut Criterion) {
+    let p1 = random_vector3();
+    let p2 = random_vector3();
+    let k = random_norm_vector3();
+
+    c.bench_function("Subproblem 1 Benchmark", |b| b.iter(|| subproblem1(
+        black_box(&p1),
+        black_box(&p2),
+        black_box(&k),
+    )));
+}
+
+pub fn subproblem2benchmark(c: &mut Criterion) {
+    let p1 = random_vector3();
+    let p2 = random_vector3();
+    let k1 = random_norm_vector3();
+    let k2 = random_norm_vector3();
+
+    c.bench_function("Subproblem 2 Benchmark", |b| b.iter(|| subproblem2(
+        black_box(&p1),
+        black_box(&p2),
+        black_box(&k1),
+        black_box(&k2),
+    )));
+}
+
+pub fn subproblem2extended_benchmark(c: &mut Criterion) {
+    let p0 = random_vector3();
+    let p1 = random_vector3();
+    let p2 = random_vector3();
+    let k1 = random_norm_vector3();
+    let k2 = random_norm_vector3();
+
+    c.bench_function("Subproblem 2 Ex Benchmark", |b| b.iter(|| subproblem2extended(
+        black_box(&p0),
+        black_box(&p1),
+        black_box(&p2),
+        black_box(&k1),
+        black_box(&k2),
+    )));
+}
+
+pub fn subproblem3benchmark(c: &mut Criterion) {
+    let p1 = random_vector3();
+    let p2 = random_vector3();
+    let k = random_norm_vector3();
+    let d = random();
+
+    c.bench_function("Subproblem 3 Benchmark", |b| b.iter(|| subproblem3(
+        black_box(&p1),
+        black_box(&p2),
+        black_box(&k),
+        black_box(d),
+    )));
+}
+
+pub fn subproblem4benchmark(c: &mut Criterion) {
+    let h = random_norm_vector3();
+    let p = random_vector3();
+    let k = random_norm_vector3();
+    let d = random();
+
+    c.bench_function("Subproblem 4 Benchmark", |b| b.iter(|| subproblem4(
+        black_box(&h),
+        black_box(&p),
+        black_box(&k),
+        black_box(d),
+    )));
+}
+
+pub fn subproblem5benchmark(c: &mut Criterion) {
+    let p0 = random_vector3();
+    let p1 = random_vector3();
+    let p2 = random_vector3();
+    let p3 = random_vector3();
+    let k1 = random_norm_vector3();
+    let k2 = random_norm_vector3();
+    let k3 = random_norm_vector3();
+
+    c.bench_function("Subproblem 5 Benchmark", |b| b.iter(|| subproblem5(
+        black_box(&p0),
+        black_box(&p1),
+        black_box(&p2),
+        black_box(&p3),
+        black_box(&k1),
+        black_box(&k2),
+        black_box(&k3),
+    )));
+}
+
+criterion_group!(
+    benches,
+    subproblem1benchmark,
+    subproblem2benchmark,
+    subproblem2extended_benchmark,
+    subproblem3benchmark,
+    subproblem4benchmark,
+    subproblem5benchmark,
+);
+
+criterion_main!(benches);
diff --git a/rust/src/lib.rs b/rust/src/lib.rs
index 6416dcf..a657c6c 100644
--- a/rust/src/lib.rs
+++ b/rust/src/lib.rs
@@ -1,5 +1,6 @@
 pub use nalgebra;
 
+pub mod solutionset;
 pub mod subproblems;
 
 #[cfg(test)]
diff --git a/rust/src/solutionset.rs b/rust/src/solutionset.rs
new file mode 100644
index 0000000..62ddb75
--- /dev/null
+++ b/rust/src/solutionset.rs
@@ -0,0 +1,117 @@
+use std::fmt::{ Display, Formatter, Result };
+
+#[derive(Debug, Clone)]
+pub enum SolutionSet2<T> {
+    One(T),
+    Two(T, T),
+}
+
+#[derive(Debug, Clone)]
+pub enum SolutionSet4<T> {
+    Zero,
+    One(T),
+    Two(T, T),
+    Three(T, T, T),
+    Four(T, T, T, T),
+}
+
+impl<T> SolutionSet2<T> where T: Copy {
+    pub fn expect_one(&self) -> T {
+        match self {
+            Self::One(s) => *s,
+            Self::Two(..) => panic!("Found two solutions where one was expected"),
+        }
+    }
+
+    pub fn expect_two(&self) -> (T, T) {
+        match self {
+            Self::One(_) => panic!("Found one solution where two were expected"),
+            Self::Two(s1, s2) => (*s1, *s2),
+        }
+    }
+
+    pub fn get_first(&self) -> T {
+        match self {
+            Self::One(s) => *s,
+            Self::Two(s, _) => *s,
+        }
+    }
+
+    pub fn get_all(&self) -> Vec<T> {
+        match self {
+            Self::One(s) => vec![*s],
+            Self::Two(s1, s2) => vec![*s1, *s2],
+        }
+    }
+}
+
+impl<T> SolutionSet4<T> where T: Copy {
+    pub fn expect_one(&self) -> T {
+        match self {
+            Self::One(s) => *s,
+            _ => panic!("Expected one solution"),
+        }
+    }
+
+    pub fn expect_two(&self) -> (T, T) {
+        match self {
+            Self::Two(s1, s2) => (*s1, *s2),
+            _ => panic!("Expected two solutions"),
+        }
+    }
+
+    pub fn expect_three(&self) -> (T, T, T) {
+        match self {
+            Self::Three(s1, s2, s3) => (*s1, *s2, *s3),
+            _ => panic!("Expected two solutions"),
+        }
+    }
+
+    pub fn expect_four(&self) -> (T, T, T, T) {
+        match self {
+            Self::Four(s1, s2, s3, s4) => (*s1, *s2, *s3, *s4),
+            _ => panic!("Expected two solutions"),
+        }
+    }
+
+    pub fn get_first(&self) -> T {
+        match self {
+            Self::Zero => panic!("No solutions"),
+            Self::One(s) => *s,
+            Self::Two(s, ..) => *s,
+            Self::Three(s, ..) => *s,
+            Self::Four(s, ..) => *s,
+        }
+    }
+
+    pub fn get_all(&self) -> Vec<T> {
+        match self {
+            Self::Zero => vec![],
+            Self::One(s) => vec![*s],
+            Self::Two(s1, s2) => vec![*s1, *s2],
+            Self::Three(s1, s2, s3) => vec![*s1, *s2, *s3],
+            Self::Four(s1, s2, s3, s4) => vec![*s1, *s2, *s3, *s4],
+        }
+    }
+}
+
+impl<T: Display> Display for SolutionSet2<T> {
+    fn fmt(&self, f: &mut Formatter) -> Result {
+        match self {
+            Self::One(s) => write!(f, "{{ {} }}", s),
+            Self::Two(s1, s2) => write!(f, "{{ {} {} }}", s1, s2),
+        }
+    }
+}
+
+impl<T: Display> Display for SolutionSet4<T> {
+    fn fmt(&self, f: &mut Formatter) -> Result {
+        match self {
+            Self::Zero => write!(f, "{{ }}"),
+            Self::One(s) => write!(f, "{{ {} }}", s),
+            Self::Two(s1, s2) => write!(f, "{{ {} {} }}", s1, s2),
+            Self::Three(s1, s2, s3) => write!(f, "{{ {} {} {} }}", s1, s2, s3),
+            Self::Four(s1, s2, s3, s4) => write!(f, "{{ {} {} {} {} }}", s1, s2, s3, s4),
+        }
+    }
+}
diff --git a/rust/src/subproblems.rs b/rust/src/subproblems.rs
index f56a371..9552c19 100644
--- a/rust/src/subproblems.rs
+++ b/rust/src/subproblems.rs
@@ -1,40 +1,15 @@
-use nalgebra::{ Vector3, Matrix3x2, Matrix4x3, Matrix4x1, Matrix3x4, Matrix3, Vector2 };
-
-#[derive(Clone)]
-pub enum SolutionSet2<T> {
-    One(T),
-    Two(T, T),
-}
-
-impl<T> SolutionSet2<T> where T: Copy {
-    pub fn expect_one(&self) -> T {
-        match self {
-            Self::One(s) => *s,
-            Self::Two(..) => panic!("Found two solutions where one was expected"),
-        }
-    }
-
-    pub fn expect_two(&self) -> [T; 2] {
-        match self {
-            Self::One(_) => panic!("Found one solution where two were expected"),
-            Self::Two(s1, s2) => [*s1, *s2],
-        }
+use {
+    super::solutionset::{ SolutionSet2, SolutionSet4 },
+
+    nalgebra::{
+        Vector2, Vector3, Vector4, Vector5,
+        Matrix, Matrix3, Matrix4, Matrix3x2, Matrix3x4, Matrix4x1, Matrix4x3,
+        ArrayStorage, U1, U8,
+        Complex, DVector, Matrix2,
     }
+};
 
-    pub fn get_first(&self) -> T {
-        match self {
-            Self::One(s) => *s,
-            Self::Two(s, _) => *s,
-        }
-    }
-
-    pub fn get_all(&self) -> Vec<T> {
-        match self {
-            Self::One(s) => vec![*s],
-            Self::Two(s1, s2) => vec![*s1, *s2],
-        }
-    }
-}
+pub type Vector8<T> = Matrix<T, U8, U1, ArrayStorage<T, 8, 1>>;
 
 /**
 Solves for `theta` where `rot(k, theta) * p1 = p2` if possible.
@@ -52,6 +27,8 @@ pub fn subproblem1(p1: &Vector3<f64>, p2: &Vector3<f64>, k: &Vector3<f64>) -> (f
     (theta, is_ls)
 }
 
+// NOTE: Maybe we should be using types like SolutionSet2<(f64, f64)> rather than (SolutionSet2<f64>, SolutionSet2<f64>)
+
 /**
 Solves for `theta1` and `theta2` where `rot(k1, theta1) * p1 = rot(k2, theta2) * p2` if possible.
 If not, minimizes `|| rot(k1, theta1) * p1 - rot(k2, theta2) * p2 ||`.
@@ -224,3 +201,160 @@ pub fn subproblem4(h: &Vector3<f64>, p: &Vector3<f64>, k: &Vector3<f64>, d: f64)
         )
     }
 }
+
+/**
+Solves for theta1, theta2, and theta3 where p0 + rot(k1, theta1) * p1 = rot(k2, theta2) * (p2 + rot(k3, theta3) * p3) if possible.
+There can be up to 4 solutions.
+ */
+pub fn subproblem5(p0: &Vector3<f64>, p1: &Vector3<f64>, p2: &Vector3<f64>, p3: &Vector3<f64>, k1: &Vector3<f64>, k2: &Vector3<f64>, k3: &Vector3<f64>) -> (SolutionSet4<f64>, SolutionSet4<f64>, SolutionSet4<f64>) {
+    let mut theta1 = [0.0; 8];
+    let mut theta2 = [0.0; 8];
+    let mut theta3 = [0.0; 8];
+
+    let mut i_soln = 0;
+
+    let p1_s = p0 + k1 * k1.transpose() * p1;
+    let p3_s = p2 + k3 * k3.transpose() * p3;
+
+    let delta1 = k2.dot(&p1_s);
+    let delta3 = k2.dot(&p3_s);
+
+    let (p_1, r_1) = cone_polynomials(p0, k1, p1, &p1_s, k2);
+    let (p_3, r_3) = cone_polynomials(p2, k3, p3, &p3_s, k2);
+
+    let p_13 = p_1 - p_3;
+    let p_13_sq = vec_self_convolve_2(&p_13);
+
+    let rhs = r_3 - r_1 - p_13_sq;
+
+    let eqn_real = vec_self_convolve_3(&rhs) - 4.0 * vec_convolve_3(&p_13_sq, &r_1);
+    let mut eqn: Vector5<Complex<f64>> = Vector5::zeros();
+
+    for (complex, real) in eqn.iter_mut().zip(eqn_real.into_iter()) {
+        complex.re = *real;
+    }
+
+    let all_roots = approximate_quartic_roots(eqn).transpose();
+    let h_vec = DVector::from_vec(all_roots.into_iter().filter(|c| c.im == 0.0).map(|c| c.re).collect());
+
+    let kxp1 = k1.cross(p1);
+    let kxp3 = k3.cross(p3);
+    let a_1 = Matrix3x2::from_columns(&[kxp1, -k1.cross(&kxp1)]);
+    let a_3 = Matrix3x2::from_columns(&[kxp3, -k3.cross(&kxp3)]);
+
+    let signs_1 = [1.0,  1.0, -1.0, -1.0];
+    let signs_3 = [1.0, -1.0,  1.0, -1.0];
+
+    let j = Matrix2::new(
+         0.0, 1.0,
+        -1.0, 0.0,
+    );
+
+    for &h in h_vec.into_iter() {
+        let const_1 = a_1.transpose() * k2 * (h - delta1);
+        let const_3 = a_3.transpose() * k2 * (h - delta3);
+        let pm_1 = j * a_1.transpose() * k2 * ((a_1.transpose() * k2).norm_squared() - (h - delta1).powi(2)).sqrt();
+        let pm_3 = j * a_3.transpose() * k2 * ((a_3.transpose() * k2).norm_squared() - (h - delta3).powi(2)).sqrt();
+
+        for (&sign_1, &sign_3) in signs_1.iter().zip(signs_3.iter()) {
+            let sc1 = const_1 + sign_1 * pm_1;
+            let sc1 = sc1 / (a_1.transpose() * k2).norm_squared();
+
+            let sc3 = const_3 + sign_3 * pm_3;
+            let sc3 = sc3 / (a_3.transpose() * k2).norm_squared();
+
+            let v1 = a_1 * sc1 + p1_s;
+            let v3 = a_3 * sc3 + p3_s;
+
+            if ((v1 - h * k2).norm() - (v3 - h * k2).norm()).abs() < 1e-5 {
+                theta1[i_soln] = sc1[0].atan2(sc1[1]);
+                (theta2[i_soln], _) = subproblem1(&v3, &v1, &k2);
+                theta3[i_soln] = sc3[0].atan2(sc3[1]);
+
+                i_soln += 1;
+            }
+        }
+    }
+
+    match i_soln {
+        1 => (
+            SolutionSet4::One(theta1[0]),
+            SolutionSet4::One(theta2[0]),
+            SolutionSet4::One(theta3[0]),
+        ),
+
+        2 => (
+            SolutionSet4::Two(theta1[0], theta1[1]),
+            SolutionSet4::Two(theta2[0], theta2[1]),
+            SolutionSet4::Two(theta3[0], theta3[1]),
+        ),
+
+        3 => (
+            SolutionSet4::Three(theta1[0], theta1[1], theta1[2]),
+            SolutionSet4::Three(theta2[0], theta2[1], theta2[2]),
+            SolutionSet4::Three(theta3[0], theta3[1], theta3[2]),
+        ),
+
+        4 => (
+            SolutionSet4::Four(theta1[0], theta1[1], theta1[2], theta1[3]),
+            SolutionSet4::Four(theta2[0], theta2[1], theta2[2], theta2[3]),
+            SolutionSet4::Four(theta3[0], theta3[1], theta3[2], theta3[3]),
+        ),
+
+        _ => (
+            SolutionSet4::Zero,
+            SolutionSet4::Zero,
+            SolutionSet4::Zero,
+        ),
+    }
+}
+
+fn cone_polynomials(p0_i: &Vector3<f64>, k_i: &Vector3<f64>, p_i: &Vector3<f64>, p_i_s: &Vector3<f64>, k2: &Vector3<f64>) -> (Vector2<f64>, Vector3<f64>) {
+    let ki_x_k2 = k_i.cross(k2);
+    let ki_x_ki_x_k2 = k_i.cross(&ki_x_k2);
+    let norm_ki_x_k2_sq = ki_x_k2.dot(&ki_x_k2);
+
+    let ki_x_pi = k_i.cross(p_i);
+    let norm_ki_x_pi_sq = ki_x_pi.dot(&ki_x_pi);
+
+    let alpha = p0_i.transpose() * ki_x_ki_x_k2 / norm_ki_x_k2_sq;
+    let delta = k2.dot(p_i_s);
+    let beta =  p0_i.transpose() *  ki_x_k2  / norm_ki_x_k2_sq;
+
+    let p_const = norm_ki_x_pi_sq + p_i_s.norm_squared() + 2.0 * alpha[0] * delta;
+    let p = Vector2::new(-2.0 * alpha[0], p_const);
+/*
+    R = [-1 2*delta -delta^2]; % -(H-delta_i)^2
+    R(end) = R(end) + norm_kiXpi_sq*norm_kiXk2_sq; % ||A_i' k_2||^2 - (H-delta_i)^2
+    R = (2*beta)^2 * R
+    */
+    let r = Vector3::new(-1.0, 2.0 * delta, -delta * delta + norm_ki_x_pi_sq * norm_ki_x_k2_sq);
+    let r = (2.0 * beta[0]).powi(2) * r;
+
+    (p, r)
+}
+
+fn vec_self_convolve_2(v: &Vector2<f64>) -> Vector3<f64> {
+    let (a, b) = (v[0], v[1]);
+    Vector3::new(a * a, 2.0 * a * b, b * b)
+}
+
+fn vec_self_convolve_3(v: &Vector3<f64>) -> Vector5<f64> {
+    let (a, b, c) = (v[0], v[1], v[2]);
+    Vector5::new(a * a, 2.0 * a * b, 2.0 * a * c + b * b, 2.0 * b * c, c * c)
+}
+
+fn vec_convolve_3(v1: &Vector3<f64>, v2: &Vector3<f64>) -> Vector5<f64> {
+    let (a, b, c) = (v1[0], v1[1], v1[2]);
+    let (x, y, z) = (v2[0], v2[1], v2[2]);
+    Vector5::new(a * x, b * x + a * y, a * z + b * y + c * x, b * z + c * y, c * z)
+}
+
+fn approximate_quartic_roots(p: Vector5<Complex<f64>>) -> Vector4<Complex<f64>> {
+    Matrix4::from_columns(&[
+        p.fixed_rows::<4>(1) / -p[0],
+        Vector4::new(Complex::new(1.0, 0.0), Complex::new(0.0, 0.0), Complex::new(0.0, 0.0), Complex::new(0.0, 0.0)),
+        Vector4::new(Complex::new(0.0, 0.0), Complex::new(1.0, 0.0), Complex::new(0.0, 0.0), Complex::new(0.0, 0.0)),
+        Vector4::new(Complex::new(0.0, 0.0), Complex::new(0.0, 0.0), Complex::new(1.0, 0.0), Complex::new(0.0, 0.0)),
+    ]).transpose().eigenvalues().unwrap()
+}


Test patch:
diff --git a/rust/src/tests/correctness.rs b/rust/src/tests/correctness.rs
index 271e035..84605e7 100644
--- a/rust/src/tests/correctness.rs
+++ b/rust/src/tests/correctness.rs
@@ -5,20 +5,22 @@ use super::setups::{
     Subproblem2ExtendedSetup,
     Subproblem3Setup,
     Subproblem4Setup,
+    Subproblem5Setup,
 };
 
 const TEST_ITERATIONS: usize = 1000;
 
 #[test]
 fn run_tests() {
-    const ERROR_THRESHOLD: f64 = 1e-9;
+    const ERROR_THRESHOLD: f64 = 1e-5;
 
     let setups: Vec<Box<dyn Setup>> = vec![
-        Box::new(Subproblem1Setup::new()),
-        Box::new(Subproblem2Setup::new()),
-        Box::new(Subproblem2ExtendedSetup::new()),
-        Box::new(Subproblem3Setup::new()),
+        Box::new(Subproblem5Setup::new()),
         Box::new(Subproblem4Setup::new()),
+        Box::new(Subproblem3Setup::new()),
+        Box::new(Subproblem2ExtendedSetup::new()),
+        Box::new(Subproblem2Setup::new()),
+        Box::new(Subproblem1Setup::new()),
     ];
 
     for mut setup in setups {
diff --git a/rust/src/tests/setups.rs b/rust/src/tests/setups.rs
index edb103e..8b5d8ff 100644
--- a/rust/src/tests/setups.rs
+++ b/rust/src/tests/setups.rs
@@ -4,12 +4,21 @@ use {
     crate::{
         tests::auxiliary::{
             random_vector3,
+            random_norm_vector3,
             random_angle,
             rot,
-            random_norm_vector3
         },
 
-        subproblems::{ SolutionSet2, subproblem1, subproblem2, subproblem2extended, subproblem3 }
+        subproblems::{
+            subproblem1,
+            subproblem2,
+            subproblem2extended,
+            subproblem3,
+            subproblem4,
+            subproblem5,
+        },
+
+        solutionset::{ SolutionSet2, SolutionSet4 },
     },
 
     nalgebra::Vector3,
@@ -41,7 +50,6 @@ pub struct Subproblem1Setup {
     theta: f64,
 }
 
-#[derive(Clone)]
 pub struct Subproblem2Setup {
     p1: Vector3<f64>,
     p2: Vector3<f64>,
@@ -62,6 +70,7 @@ pub struct Subproblem2ExtendedSetup {
     theta1: f64,
     theta2: f64,
 }
+
 pub struct Subproblem3Setup {
     p1: Vector3<f64>,
     p2: Vector3<f64>,
@@ -80,13 +89,19 @@ pub struct Subproblem4Setup {
     theta: SolutionSet2<f64>,
 }
 
-pub struct Subproblem3Setup {
+pub struct Subproblem5Setup {
+    p0: Vector3<f64>,
     p1: Vector3<f64>,
     p2: Vector3<f64>,
-    k: Vector3<f64>,
-    d: f64,
+    p3: Vector3<f64>,
 
-    theta: SolutionSet2<f64>,
+    k1: Vector3<f64>,
+    k2: Vector3<f64>,
+    k3: Vector3<f64>,
+
+    theta1: SolutionSet4<f64>,
+    theta2: SolutionSet4<f64>,
+    theta3: SolutionSet4<f64>,
 }
 
 impl Subproblem1Setup {
@@ -119,12 +134,14 @@ impl Subproblem2Setup {
     }
 
     fn calculate_error(&self, theta1: &[f64], theta2: &[f64]) -> f64 {
+        assert!(theta1.len() == theta2.len());
+
         theta1.iter()
             .zip(theta2.iter())
             .map(|(&t1, &t2)| {
                 (rot(self.k2, t2) * self.p2 - rot(self.k1,t1) * self.p1).norm()
             })
-            .sum()
+            .sum::<f64>() / theta1.len() as f64
     }
 }
 
@@ -155,13 +172,54 @@ impl Subproblem3Setup {
         }
     }
 
-    pub fn calculate_error(&self, theta: &[f64]) -> f64 {
+    fn calculate_error(&self, theta: &[f64]) -> f64 {
         theta
             .iter()
             .map(|&t| {
                 ((self.p2 - rot(self.k, t) * self.p1).norm() - self.d).abs()
             })
-            .sum()
+            .sum::<f64>() / theta.len() as f64
+    }
+}
+
+impl Subproblem4Setup {
+    pub fn new() -> Self {
+        Self {
+            h: Vector3::zeros(),
+            p: Vector3::zeros(),
+            k: Vector3::zeros(),
+            d: 0.0,
+
+            theta: SolutionSet2::One(0.0),
+        }
+    }
+
+    fn calculate_error(&self, theta: &[f64]) -> f64 {
+        theta
+            .iter()
+            .map(|&t| {
+                ((self.h.transpose() * rot(self.k, t) * self.p)[0] - self.d).abs()
+            })
+            .sum::<f64>() / theta.len() as f64
+    }
+}
+
+impl Subproblem5Setup {
+    pub fn new() -> Self {
+        Self {
+            p0: Vector3::zeros(),
+            p1: Vector3::zeros(),
+            p2: Vector3::zeros(),
+            p3: Vector3::zeros(),
+
+            k1: Vector3::zeros(),
+            k2: Vector3::zeros(),
+            k3: Vector3::zeros(),
+
+            theta1: SolutionSet4::One(0.0),
+            theta2: SolutionSet4::One(0.0),
+            theta3: SolutionSet4::One(0.0),
+        }
     }
 }
 
@@ -429,3 +487,63 @@ impl Setup for Subproblem4Setup {
         "Subproblem 4"
     }
 }
+
+impl Setup for Subproblem5Setup {
+    fn setup(&mut self) {
+        let theta1 = random_angle();
+        let theta2 = random_angle();
+        let theta3 = random_angle();
+
+        self.p1 = random_vector3();
+        self.p2 = random_vector3();
+        self.p3 = random_vector3();
+
+        self.k1 = random_norm_vector3();
+        self.k2 = random_norm_vector3();
+        self.k3 = random_norm_vector3();
+
+        self.theta1 = SolutionSet4::One(theta1);
+        self.theta2 = SolutionSet4::One(theta2);
+        self.theta3 = SolutionSet4::One(theta3);
+
+        self.p0 = -(rot(self.k1, theta1) * self.p1 - rot(self.k2, theta2) * (self.p2 + rot(self.k3, theta3) * self.p3));
+    }
+
+    fn setup_ls(&mut self) {
+        unimplemented!()
+    }
+
+    fn run(&mut self) {
+        (self.theta1, self.theta2, self.theta3) = subproblem5(&self.p0, &self.p1, &self.p2, &self.p3, &self.k1, &self.k2, &self.k3);
+    }
+
+    fn error(&self) -> f64 {
+        let theta1 = self.theta1.get_all();
+        let len = theta1.len();
+
+        if len == 0 {
+            return 0.0;
+        }
+
+        theta1
+            .into_iter()
+            .zip(self.theta2
+                .get_all()
+                .into_iter())
+            .zip(self.theta3
+                .get_all()
+                .into_iter())
+            .map(|((t1, t2), t3)| {
+                (self.p0 + rot(self.k1, t1) * self.p1 - rot(self.k2, t2) * (self.p2 + rot(self.k3, t3) * self.p3)).norm()
+            })
+            .sum::<f64>() / len as f64
+    }
+
+    fn is_at_local_min(&self) -> bool {
+        unimplemented!()
+    }
+
+    fn name(&self) -> &'static str {
+        "Subproblem 5"
+    }
+}