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

Issue:
Rust Subproblem 6



Discussion / hints:

Fix patch:
diff --git a/rust/benches/benchmark.rs b/rust/benches/benchmark.rs
index 74ebe03..2ad53ef 100644
--- a/rust/benches/benchmark.rs
+++ b/rust/benches/benchmark.rs
@@ -1,124 +1,76 @@
-use linear_subproblem_solutions_rust::subproblems::subproblem5;
-
 use {
-    linear_subproblem_solutions_rust::subproblems::{
-        subproblem1,
-        subproblem2,
-        subproblem2extended,
-        subproblem3,
-        subproblem4,
+    linear_subproblem_solutions_rust::setups::{
+        Setup,
+        Subproblem1Setup,
+        Subproblem2Setup,
+        Subproblem2ExtendedSetup,
+        Subproblem3Setup,
+        Subproblem4Setup,
+        Subproblem5Setup,
+        Subproblem6Setup,
     },
 
-    nalgebra::Vector3,
-
     criterion::{
         Criterion,
-        black_box,
         criterion_group,
         criterion_main,
     },
 };
 
-fn random_vector3() -> Vector3<f64> {
-    Vector3::new(random(), random(), random())
-}
+pub fn subproblem1benchmark(c: &mut Criterion) {
+    let mut setup = Subproblem1Setup::new();
 
-fn random_norm_vector3() -> Vector3<f64> {
-    Vector3::new(random(), random(), random()).normalize()
-}
+    setup.setup();
 
-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),
-    )));
+    c.bench_function("Subproblem 1 Benchmark", |b| b.iter(|| setup.run()));
 }
 
 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),
-    )));
+    let mut setup = Subproblem2Setup::new();
+
+    setup.setup();
+
+    c.bench_function("Subproblem 2 Benchmark", |b| b.iter(|| setup.run()));
 }
 
 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),
-    )));
+    let mut setup = Subproblem2ExtendedSetup::new();
+
+    setup.setup();
+
+    c.bench_function("Subproblem 2 Ex Benchmark", |b| b.iter(|| setup.run()));
 }
 
 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),
-    )));
+    let mut setup = Subproblem3Setup::new();
+
+    setup.setup();
+
+    c.bench_function("Subproblem 3 Benchmark", |b| b.iter(|| setup.run()));
 }
 
 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),
-    )));
+    let mut setup = Subproblem4Setup::new();
+
+    setup.setup();
+
+    c.bench_function("Subproblem 4 Benchmark", |b| b.iter(|| setup.run()));
 }
 
 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),
-    )));
+    let mut setup = Subproblem5Setup::new();
+
+    setup.setup();
+
+    c.bench_function("Subproblem 5 Benchmark", |b| b.iter(|| setup.run()));
+}
+
+pub fn subproblem6benchmark(c: &mut Criterion) {
+    let mut setup = Subproblem6Setup::new();
+
+    setup.setup();
+
+    c.bench_function("Subproblem 6 Benchmark", |b| b.iter(|| setup.run()));
 }
 
 criterion_group!(
@@ -129,6 +81,7 @@ criterion_group!(
     subproblem3benchmark,
     subproblem4benchmark,
     subproblem5benchmark,
+    subproblem6benchmark,
 );
 
 criterion_main!(benches);
diff --git a/rust/src/auxiliary.rs b/rust/src/auxiliary.rs
new file mode 100644
index 0000000..1088b88
--- /dev/null
+++ b/rust/src/auxiliary.rs
@@ -0,0 +1,178 @@
+use {
+    crate::solutionset::{ SolutionSet4, SolutionSet2 },
+
+    nalgebra::{
+        ComplexField,
+        Complex,
+        DVector,
+        Vector3,
+        Vector2,
+        Vector4,
+        Vector5,
+        Matrix2,
+        Matrix2x4,
+        Matrix3,
+        Matrix4
+    },
+};
+
+/// Creates a 3x3 rotation matrix
+pub fn rot(k: Vector3<f64>, theta: f64) -> Matrix3<f64> {
+    let k = k.normalize();
+    Matrix3::identity() + hat(k) * theta.sin() + hat(k) * hat(k) * (1.0 - theta.cos())
+}
+
+pub fn random_vector3() -> Vector3<f64> {
+    Vector3::new(random(), random(), random())
+}
+
+pub fn random_norm_vector3() -> Vector3<f64> {
+    Vector3::new(random(), random(), random()).normalize()
+}
+
+pub fn random_angle() -> f64 {
+    random() * std::f64::consts::PI
+}
+
+pub 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);
+
+    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)
+}
+
+pub 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)
+}
+
+pub 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)
+}
+
+pub 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)
+}
+
+pub 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()
+}
+
+pub fn null_space_matrix2x4(a: &Matrix2x4<f64>, epsilon: f64) -> SolutionSet2<Vector4<f64>> {
+    let eigen = (a.transpose() * a).symmetric_eigen();
+
+    let mut zero_indices = [0, 0];
+    let mut i = 0;
+
+    for (j, v) in eigen.eigenvalues.iter().enumerate() {
+        if v.abs() < epsilon {
+            zero_indices[i] = j;
+            i += 1;
+
+            if i >= zero_indices.len() {
+                break;
+            }
+        }
+    }
+
+    match i {
+        0 => SolutionSet2::Zero,
+        2 => SolutionSet2::Two(eigen.eigenvectors.column(zero_indices[0]).into() , eigen.eigenvectors.column(zero_indices[1]).into()),
+        _ => unreachable!(),
+    }
+}
+
+pub fn solve_2_ellipse_numeric(xm1: &Vector2<f64>, xn1: &Matrix2<f64>, xm2: &Vector2<f64>, xn2: &Matrix2<f64>) -> SolutionSet4<(f64, f64)> {
+    let a_1 = xn1.transpose() * xn1;
+    let a = a_1[0];
+    let b = 2.0 * a_1[(1, 0)];
+    let c = a_1[(1, 1)];
+    let b_1 = 2.0 * xm1.transpose() * xn1;
+    let d = b_1[0];
+    let e = b_1[1];
+    let f = (xm1.transpose() * xm1)[0] - 1.0;
+
+    let a_2 = xn2.transpose() * xn2;
+    let a1 = a_2[0];
+    let b1 = 2.0 * a_2[(1, 0)];
+    let c1 = a_2[(1, 1)];
+    let b_2 = 2.0 * xm2.transpose() * xn2;
+    let d1 = b_2[0];
+    let e1 = b_2[1];
+    let fq = (xm2.transpose() * xm2)[0] - 1.0;
+
+    let z0 = f * a * (d1 * d1) + (a * a) * (fq * fq) - d * a * d1 * fq + (a1*a1) * (f * f)
+        - 2.0 * a * fq * a1 * f - d * d1 * a1 * f + a1 * (d * d) * fq;
+
+    let z1 = e1 * (d * d) * a1 - fq * d1 * a * b - 2.0 * a * fq * a1 * e - f * a1 * b1 * d
+        + 2.0 * d1 * b1 * a * f + 2.0 * e1 * fq * (a * a) + (d1 * d1) * a * e - e1 * d1 * a * d
+        - 2.0 * a * e1 * a1 * f - f * a1 * d1 * b + 2.0 * f * e * (a1 * a1) - fq * b1 * a * d - e
+        * a1 * d1 * d + 2.0 * fq * b * a1 * d;
+
+    let z2 = (e1 * e1) * (a * a) + 2.0 * c1 * fq * (a * a) - e * a1 * d1 * b + fq * a1
+        * (b * b) - e * a1 * b1 * d - fq * b1 * a * b - 2.0 * a * e1 * a1 * e + 2.0 * d1 * b1 * a
+        * e - c1 * d1 * a * d - 2.0 * a * c1 * a1 * f + (b1 * b1) * a * f + 2.0 * e1 * b * a1 * d
+        + (e * e) * (a1 * a1) - c * a1 * d1 * d - e1 * b1 * a * d + 2.0 * f * c * (a1 * a1) - f * a1
+        * b1 * b + c1 * (d * d) * a1 + (d1 * d1) * a * c - e1 * d1 * a * b - 2.0 * a * fq * a1 * c;
+
+    let z3 = -2.0 * a * a1 * c * e1 + e1 * a1 * (b * b) + 2.0 * c1 * b * a1 * d - c * a1 * b1
+        * d + (b1 * b1) * a * e - e1 * b1 * a * b - 2.0 * a * c1 * a1 * e - e * a1 * b1 * b - c1
+        * b1 * a * d + 2.0 * e1 * c1 * (a * a) + 2.0 * e * c * (a1 * a1) - c * a1 * d1 * b + 2.0
+        * d1 * b1 * a * c - c1 * d1 * a * b;
+
+    let z4 = (a * a) * (c1 * c1) - 2.0 * a * c1 * a1 * c + (a1 * a1) * (c * c) - b * a * b1
+        * c1 - b * b1 * a1 * c + (b * b) * a1 * c1 + c * a * (b1 * b1);
+
+    let y = approximate_quartic_roots(Vector5::new(
+        Complex::from_real(z4),
+        Complex::from_real(z3),
+        Complex::from_real(z2),
+        Complex::from_real(z1),
+        Complex::from_real(z0)
+    ));
+
+    let y_sq = y.iter().filter(|z| z.im == 0.0).map(|z| z.re * z.re).collect::<Vec<f64>>();
+    let y = y.iter().filter(|z| z.im == 0.0).map(|z| z.re).collect::<Vec<f64>>();
+    let y_sq = DVector::from_vec(y_sq);
+    let y = DVector::from_vec(y);
+
+
+    let x = -(((a * c1 * y_sq.clone()).add_scalar(a * fq) - a1 * c * y_sq.clone() + a * e1 * y.clone() - a1 * e * y.clone()).add_scalar(-a1 * f)).component_div(&(((a * b1 * y.clone()).add_scalar(a * d1) - a1 * b * y.clone()).add_scalar(-a1 * d)));
+
+    SolutionSet4::from_vec(&x.iter().zip(y.iter()).map(|(u, v)| (*u, *v)).collect::<Vec<(f64, f64)>>())
+}
+
+fn random() -> f64 {
+    fastrand::f64() * 2.0 - 1.0
+}
+
+// Matrix cross-product for a 3 x 3 vector
+fn hat(k: Vector3<f64>) -> Matrix3<f64> {
+    Matrix3::new(
+        0.0, -k.z, k.y,
+        k.z, 0.0, -k.x,
+        -k.y, k.x, 0.0,
+    )
+}
diff --git a/rust/src/tests/correctness.rs b/rust/src/correctness.rs
similarity index 95%
rename from rust/src/tests/correctness.rs
rename to rust/src/correctness.rs
index 84605e7..e124b10 100644
--- a/rust/src/tests/correctness.rs
+++ b/rust/src/correctness.rs
@@ -6,6 +6,7 @@ use super::setups::{
     Subproblem3Setup,
     Subproblem4Setup,
     Subproblem5Setup,
+    Subproblem6Setup,
 };
 
 const TEST_ITERATIONS: usize = 1000;
@@ -15,6 +16,7 @@ fn run_tests() {
     const ERROR_THRESHOLD: f64 = 1e-5;
 
     let setups: Vec<Box<dyn Setup>> = vec![
+        Box::new(Subproblem6Setup::new()),
         Box::new(Subproblem5Setup::new()),
         Box::new(Subproblem4Setup::new()),
         Box::new(Subproblem3Setup::new()),
diff --git a/rust/src/lib.rs b/rust/src/lib.rs
index a657c6c..6001040 100644
--- a/rust/src/lib.rs
+++ b/rust/src/lib.rs
@@ -2,6 +2,9 @@ pub use nalgebra;
 
 pub mod solutionset;
 pub mod subproblems;
+pub mod setups;
+
+mod auxiliary;
 
 #[cfg(test)]
-mod tests;
+mod correctness;
diff --git a/rust/src/tests/setups.rs b/rust/src/setups.rs
similarity index 83%
rename from rust/src/tests/setups.rs
rename to rust/src/setups.rs
index 8b5d8ff..a9ed915 100644
--- a/rust/src/tests/setups.rs
+++ b/rust/src/setups.rs
@@ -1,8 +1,6 @@
-use crate::subproblems::subproblem4;
-
 use {
     crate::{
-        tests::auxiliary::{
+        auxiliary::{
             random_vector3,
             random_norm_vector3,
             random_angle,
@@ -16,12 +14,13 @@ use {
             subproblem3,
             subproblem4,
             subproblem5,
+            subproblem6,
         },
 
         solutionset::{ SolutionSet2, SolutionSet4 },
     },
 
-    nalgebra::Vector3,
+    nalgebra::{ Vector3, Vector2 },
 };
 
 const DELTA: f64 = 1e-12;
@@ -71,6 +70,7 @@ pub struct Subproblem2ExtendedSetup {
     theta2: f64,
 }
 
+
 pub struct Subproblem3Setup {
     p1: Vector3<f64>,
     p2: Vector3<f64>,
@@ -104,6 +104,18 @@ pub struct Subproblem5Setup {
     theta3: SolutionSet4<f64>,
 }
 
+pub struct Subproblem6Setup {
+    h: [Vector3<f64>; 4],
+    k: [Vector3<f64>; 4],
+    p: [Vector3<f64>; 4],
+
+    d1: f64,
+    d2: f64,
+
+    theta1: SolutionSet4<f64>,
+    theta2: SolutionSet4<f64>,
+}
+
 impl Subproblem1Setup {
     pub fn new() -> Self {
         Self {
@@ -172,13 +184,13 @@ impl Subproblem3Setup {
         }
     }
 
-    fn calculate_error(&self, theta: &[f64]) -> f64 {
+    pub fn calculate_error(&self, theta: &[f64]) -> f64 {
         theta
             .iter()
             .map(|&t| {
                 ((self.p2 - rot(self.k, t) * self.p1).norm() - self.d).abs()
             })
-            .sum::<f64>() / theta.len() as f64
+            .sum()
     }
 }
 
@@ -223,25 +235,19 @@ impl Subproblem5Setup {
     }
 }
 
-impl Subproblem4Setup {
+impl Subproblem6Setup {
     pub fn new() -> Self {
         Self {
-            h: Vector3::zeros(),
-            p: Vector3::zeros(),
-            k: Vector3::zeros(),
-            d: 0.0,
+            h: [Vector3::zeros(); 4],
+            k: [Vector3::zeros(); 4],
+            p: [Vector3::zeros(); 4],
 
-            theta: SolutionSet2::One(0.0),
-        }
-    }
+            d1: 0.0,
+            d2: 0.0,
 
-    pub 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()
+            theta1: SolutionSet4::One(0.0),
+            theta2: SolutionSet4::One(0.0),
+        }
     }
 }
 
@@ -547,3 +553,67 @@ impl Setup for Subproblem5Setup {
         "Subproblem 5"
     }
 }
+
+impl Setup for Subproblem6Setup {
+    fn setup(&mut self) {
+        let theta1 = random_angle();
+        let theta2 = random_angle();
+
+        self.h = [Vector3::zeros(); 4];
+        self.k = [Vector3::zeros(); 4];
+        self.p = [Vector3::zeros(); 4];
+
+        for (h, (k, p)) in self.h.iter_mut().zip(self.k.iter_mut().zip(self.p.iter_mut())) {
+            *h = random_norm_vector3();
+            *k = random_norm_vector3();
+            *p = random_vector3();
+        }
+
+        self.d1 = (
+            self.h[0].transpose() * rot(self.k[0], theta1) * self.p[0] +
+            self.h[1].transpose() * rot(self.k[1], theta2) * self.p[1]
+        )[0];
+
+        self.d2 = (
+            self.h[2].transpose() * rot(self.k[2], theta1) * self.p[2] +
+            self.h[3].transpose() * rot(self.k[3], theta2) * self.p[3]
+        )[0];
+
+        self.theta1 = SolutionSet4::One(theta1);
+        self.theta2 = SolutionSet4::One(theta2);
+    }
+
+    fn setup_ls(&mut self) {
+        unimplemented!()
+    }
+
+    fn run(&mut self) {
+        (self.theta1, self.theta2) = subproblem6(
+            [&self.h[0], &self.h[1], &self.h[2], &self.h[3]],
+            [&self.k[0], &self.k[1], &self.k[2], &self.k[3]],
+            [&self.p[0], &self.p[1], &self.p[2], &self.p[3]],
+            self.d1,
+            self.d2,
+        );
+    }
+
+    fn error(&self) -> f64 {
+        let theta1 = self.theta1.get_all();
+        let len = theta1.len();
+
+        theta1.into_iter().zip(self.theta2.get_all().iter()).map(|(t1, t2)| {
+            (Vector2::new(
+                (self.h[0].transpose() * rot(self.k[0], t1) * self.p[0] + self.h[1].transpose() * rot(self.k[1], *t2) * self.p[1])[0] - self.d1,
+                (self.h[2].transpose() * rot(self.k[2], t1) * self.p[2] + self.h[3].transpose() * rot(self.k[3], *t2) * self.p[3])[0] - self.d2
+            )).norm()
+        }).sum::<f64>() / len as f64
+    }
+
+    fn is_at_local_min(&self) -> bool {
+        unimplemented!()
+    }
+
+    fn name(&self) -> &'static str {
+        "Subproblem 6"
+    }
+}
diff --git a/rust/src/solutionset.rs b/rust/src/solutionset.rs
index 62ddb75..f394626 100644
--- a/rust/src/solutionset.rs
+++ b/rust/src/solutionset.rs
@@ -2,6 +2,7 @@ use std::fmt::{ Display, Formatter, Result };
 
 #[derive(Debug, Clone)]
 pub enum SolutionSet2<T> {
+    Zero,
     One(T),
     Two(T, T),
 }
@@ -15,9 +16,19 @@ pub enum SolutionSet4<T> {
     Four(T, T, T, T),
 }
 
-impl<T> SolutionSet2<T> where T: Copy {
+impl<T: Copy> SolutionSet2<T> {
+    pub fn from_vec(vec: &Vec<T>) -> Self {
+        match vec.len() {
+            0 => Self::Zero,
+            1 => Self::One(vec[0]),
+            2 => Self::Two(vec[0], vec[1]),
+            i => panic!("Vector contains too many solutions: {}", i)
+        }
+    }
+
     pub fn expect_one(&self) -> T {
         match self {
+            Self::Zero => panic!("Found no soliutions where one was expected"),
             Self::One(s) => *s,
             Self::Two(..) => panic!("Found two solutions where one was expected"),
         }
@@ -25,6 +36,7 @@ impl<T> SolutionSet2<T> where T: Copy {
 
     pub fn expect_two(&self) -> (T, T) {
         match self {
+            Self::Zero => panic!("Found no soliutions where two were expected"),
             Self::One(_) => panic!("Found one solution where two were expected"),
             Self::Two(s1, s2) => (*s1, *s2),
         }
@@ -32,6 +44,7 @@ impl<T> SolutionSet2<T> where T: Copy {
 
     pub fn get_first(&self) -> T {
         match self {
+            Self::Zero => panic!("No solutions"),
             Self::One(s) => *s,
             Self::Two(s, _) => *s,
         }
@@ -39,13 +52,25 @@ impl<T> SolutionSet2<T> where T: Copy {
 
     pub fn get_all(&self) -> Vec<T> {
         match self {
+            Self::Zero => vec![],
             Self::One(s) => vec![*s],
             Self::Two(s1, s2) => vec![*s1, *s2],
         }
     }
 }
 
-impl<T> SolutionSet4<T> where T: Copy {
+impl<T: Copy> SolutionSet4<T> {
+    pub fn from_vec(vec: &Vec<T>) -> Self {
+        match vec.len() {
+            0 => Self::Zero,
+            1 => Self::One(vec[0]),
+            2 => Self::Two(vec[0], vec[1]),
+            3 => Self::Three(vec[0], vec[1], vec[2]),
+            4 => Self::Four(vec[0], vec[1], vec[2], vec[3]),
+            i => panic!("Vector contains too many solutions: {}", i)
+        }
+    }
+
     pub fn expect_one(&self) -> T {
         match self {
             Self::One(s) => *s,
@@ -98,6 +123,7 @@ impl<T> SolutionSet4<T> where T: Copy {
 impl<T: Display> Display for SolutionSet2<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),
         }
diff --git a/rust/src/subproblems.rs b/rust/src/subproblems.rs
index 9552c19..ed1b61d 100644
--- a/rust/src/subproblems.rs
+++ b/rust/src/subproblems.rs
@@ -1,12 +1,24 @@
 use {
-    super::solutionset::{ SolutionSet2, SolutionSet4 },
+    super::{
+        solutionset::{ SolutionSet2, SolutionSet4 },
+
+        auxiliary::{
+            vec_self_convolve_2,
+            vec_self_convolve_3,
+            vec_convolve_3,
+            solve_2_ellipse_numeric,
+            null_space_matrix2x4,
+            cone_polynomials,
+            approximate_quartic_roots,
+        },
+    },
 
     nalgebra::{
         Vector2, Vector3, Vector4, Vector5,
-        Matrix, Matrix3, Matrix4, Matrix3x2, Matrix3x4, Matrix4x1, Matrix4x3,
+        Matrix, Matrix3, Matrix2x4, Matrix3x2, Matrix3x4, Matrix4x1, Matrix4x3,
         ArrayStorage, U1, U8,
         Complex, DVector, Matrix2,
-    }
+    },
 };
 
 pub type Vector8<T> = Matrix<T, U8, U1, ArrayStorage<T, 8, 1>>;
@@ -133,9 +145,9 @@ pub fn subproblem2extended(p0: &Vector3<f64>, p1: &Vector3<f64>, p2: &Vector3<f6
 }
 
 /**
-Solves for theta where || rot(k, theta) * p1 - p2 || = d if possibble.
-If not, minimizes | || rot(k, theta)*p1 - p2 || - d |.
-Also returns a boolean of whether or not theta is a least-squares solution.
+Solves for `theta` where `|| rot(k, theta) * p1 - p2 || = d` if possibble.
+If not, minimizes `| || rot(k, theta)*p1 - p2 || - d |`.
+Also returns a boolean of whether or not `theta` is a least-squares solution.
 */
 pub fn subproblem3(p1: &Vector3<f64>, p2: &Vector3<f64>, k: &Vector3<f64>, d: f64) -> (SolutionSet2<f64>, bool) {
     let kxp = k.cross(p1);
@@ -167,9 +179,9 @@ pub fn subproblem3(p1: &Vector3<f64>, p2: &Vector3<f64>, k: &Vector3<f64>, d: f6
 }
 
 /**
-Solves for theta where h' * rot(k, theta) * p = d if possible.
-If not minimizes | h' * rot(k, theta) * p - d |.
-Also returns a boolean of whether or not theta is a least-squares solution.
+Solves for `theta` where `h' * rot(k, theta) * p = d` if possible.
+If not minimizes `| h' * rot(k, theta) * p - d |`.
+Also returns a boolean of whether or not `theta` is a least-squares solution.
  */
 pub fn subproblem4(h: &Vector3<f64>, p: &Vector3<f64>, k: &Vector3<f64>, d: f64) -> (SolutionSet2<f64>, bool) {
     let a_11 = k.cross(p);
@@ -203,15 +215,13 @@ 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.
+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 mut theta1 = Vec::with_capacity(8);
+    let mut theta2 = Vec::with_capacity(8);
+    let mut theta3 = Vec::with_capacity(8);
 
     let p1_s = p0 + k1 * k1.transpose() * p1;
     let p3_s = p2 + k3 * k3.transpose() * p3;
@@ -266,95 +276,73 @@ pub fn subproblem5(p0: &Vector3<f64>, p1: &Vector3<f64>, p2: &Vector3<f64>, p3:
             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]);
+            if ((v1 - h * k2).norm() - (v3 - h * k2).norm()).abs() < 1e-6 {
+                let (theta2_value, _) = subproblem1(&v3, &v1, &k2);
 
-                i_soln += 1;
+                theta1.push(sc1[0].atan2(sc1[1]));
+                theta2.push(theta2_value);
+                theta3.push(sc3[0].atan2(sc3[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,
-        ),
-    }
+    (
+        SolutionSet4::from_vec(&theta1),
+        SolutionSet4::from_vec(&theta2),
+        SolutionSet4::from_vec(&theta3),
+    )
 }
 
-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)
-}
+/**
+Solves for `theta1` and `theta2` where `h1' * rot(k1, theta1) + h2' * rot(k2, theta2) = d1` and `h3' * rot(k3, theta1) + h4' * rot(k4, theta2) = d2`
+There can be up to 4 solutions
+ */
+pub fn subproblem6(h: [&Vector3<f64>; 4], k: [&Vector3<f64>; 4], p: [&Vector3<f64>; 4], d1: f64, d2: f64) -> (SolutionSet4<f64>, SolutionSet4<f64>) {
+    let k1xp1 = k[0].cross(p[0]);
+    let k2xp2 = k[1].cross(p[1]);
+    let k3xp3 = k[2].cross(p[2]);
+    let k4xp4 = k[3].cross(p[3]);
+
+    let a1 = Matrix3x2::from_columns(&[k1xp1, -k[0].cross(&k1xp1)]);
+    let a2 = Matrix3x2::from_columns(&[k2xp2, -k[1].cross(&k2xp2)]);
+    let a3 = Matrix3x2::from_columns(&[k3xp3, -k[2].cross(&k3xp3)]);
+    let a4 = Matrix3x2::from_columns(&[k4xp4, -k[3].cross(&k4xp4)]);
+
+    let h1_a1 = h[0].transpose() * a1;
+    let h2_a2 = h[1].transpose() * a2;
+    let h3_a3 = h[2].transpose() * a3;
+    let h4_a4 = h[3].transpose() * a4;
+
+    let a = Matrix2x4::from_rows(&[
+        Vector4::new(h1_a1[0], h1_a1[1], h2_a2[0], h2_a2[1]).transpose(),
+        Vector4::new(h3_a3[0], h3_a3[1], h4_a4[0], h4_a4[1]).transpose(),
+    ]);
+
+    let b = Vector2::new(
+        d1 - (h[0].transpose() * k[0] * k[0].transpose() * p[0])[0] - (h[1].transpose() * k[1] * k[1].transpose() * p[1])[0],
+        d2 - (h[2].transpose() * k[2] * k[2].transpose() * p[2])[0] - (h[3].transpose() * k[3] * k[3].transpose() * p[3])[0],
+    );
 
-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)
-}
+    let x_min = a.svd(true, true).solve(&b, 1e-9).unwrap();
 
-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)
-}
+    let (x_null_1, x_null_2) = null_space_matrix2x4(&a, 1e-9).expect_two();
 
-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)
-}
+    let xi_i = solve_2_ellipse_numeric(
+        &x_min.fixed_rows::<2>(0).into(),
+        &Matrix2::new(x_null_1[0], x_null_2[0], x_null_1[1], x_null_2[1]),
+        &x_min.fixed_rows::<2>(2).into(),
+        &Matrix2::new(x_null_1[2], x_null_2[2], x_null_1[3], x_null_2[3]),
+    ).get_all();
+
+    let mut theta1 = vec![0.0; xi_i.len()];
+    let mut theta2 = vec![0.0; xi_i.len()];
+
+    for (i, xi) in xi_i.iter().enumerate() {
+        let x = x_min + x_null_1 * xi.0 + x_null_2 * xi.1;
+
+        theta1[i] = x[0].atan2(x[1]);
+        theta2[i] = x[2].atan2(x[3]);
+    }
 
-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()
+    (SolutionSet4::from_vec(&theta1), SolutionSet4::from_vec(&theta2))
 }
diff --git a/rust/timing_results.md b/rust/timing_results.md
new file mode 100644
index 0000000..fa8cc55
--- /dev/null
+++ b/rust/timing_results.md
@@ -0,0 +1,9 @@
+| Subproblem | Time (ns)   |
+| :--------- | ----------: |
+| 1          |    26.782e0 |
+| 2          |    121.52e0 |
+| 2 Ex       |    92.794e0 |
+| 3          |    58.857e0 |
+| 4          |    50.471e0 |
+| 5          |    2.9110e3 |
+| 6          |    3.9013e3 |
\ No newline at end of file


Test patch:
diff --git a/rust/src/tests/auxiliary.rs b/rust/src/tests/auxiliary.rs
deleted file mode 100644
index 339e2e4..0000000
--- a/rust/src/tests/auxiliary.rs
+++ /dev/null
@@ -1,32 +0,0 @@
-use nalgebra::{ Vector3, Matrix3 };
-
-/// Creates a 3x3 rotation matrix
-pub fn rot(k: Vector3<f64>, theta: f64) -> Matrix3<f64> {
-    let k = k.normalize();
-    Matrix3::identity() + hat(k) * theta.sin() + hat(k) * hat(k) * (1.0 - theta.cos())
-}
-
-pub fn random_vector3() -> Vector3<f64> {
-    Vector3::new(random(), random(), random())
-}
-
-pub fn random_norm_vector3() -> Vector3<f64> {
-    Vector3::new(random(), random(), random()).normalize()
-}
-
-pub fn random_angle() -> f64 {
-    random() * std::f64::consts::PI
-}
-
-fn random() -> f64 {
-    fastrand::f64() * 2.0 - 1.0
-}
-
-// Matrix cross-product for a 3 x 3 vector
-fn hat(k: Vector3<f64>) -> Matrix3<f64> {
-    Matrix3::new(
-        0.0, -k.z, k.y,
-        k.z, 0.0, -k.x,
-        -k.y, k.x, 0.0,
-    )
-}
diff --git a/rust/src/tests/mod.rs b/rust/src/tests/mod.rs
deleted file mode 100644
index 4810479..0000000
--- a/rust/src/tests/mod.rs
+++ /dev/null
@@ -1,3 +0,0 @@
-mod correctness;
-mod setups;
-mod auxiliary;