~raph/bezoid

d68d7f138c50dfe5e5b0bdefaa68d59816f1cca3 — Raph Levien 1 year, 6 months ago 84eb183
Attempt secant solving

Doesn't work, the function is too nonlinear :/
1 files changed, 16 insertions(+), 7 deletions(-)

M src/solve.rs
M src/solve.rs => src/solve.rs +16 -7
@@ 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!()
    }