1 files changed, 16 insertions(+), 7 deletions(-)

M src/solve.rs

@@ 30,14 30,15 @@ impl Solver {
         let th1 = -v2.atan2();
         let mut dth = 0.0;
         let mut chord = 1.0f64;
-        const N: usize = 10;
+        let mut lastxy: Option<(f64, f64)> = None;
+        const N: usize = 20;
         for i in 0..N {
             let bias0 = inv_arm_len(v1.hypot(), chord);
             let bias1 = inv_arm_len(v2.hypot(), chord);
             let params = CurveParams {
-                k0: th0 + dth,
+                k0: th0 + 0.5 * dth,
                 bias0,
-                k1: th1 - dth,
+                k1: th1 - 0.5 * dth,
                 bias1,
             };
             if i == N - 1 {


@@ 46,12 47,20 @@ impl Solver {
             let result = params.compute();
             chord = result.chord;
             let th_err = mod_tau(th0 - th1 - (result.th0 - result.th1));
-            // TODO: use Newton or secant methods, this should be close to linear.
-            dth += 0.5 * th_err;
+            // Secant method
+            let nextxy = (dth, th_err);
+            let delta = if i >= 10 {
+                let lastxy = lastxy.unwrap();
+                (nextxy.0 - lastxy.0) / (nextxy.1 - lastxy.1)
+            } else {
+                -0.5
+            };
             println!(
-                "result th0={:.3}, th1={:.3}, err {:.3}",
-                result.th0, result.th1, th_err
+                "result th0={:.3}, th1={:.3}, dth {:0.3} err {:.3}",
+                result.th0, result.th1, dth, th_err
             );
+            dth -= delta * th_err;
+            lastxy = Some(nextxy);
         }
         unreachable!()
     }