~raph/bezoid

ref: 8bfe25c0030d271103988ca2acb3364ce1964dac bezoid/src/solve.rs -rw-r--r-- 2.2 KiB
8bfe25c0Raph Levien Simpler arm length formula 1 year, 6 months ago
                                                                                
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
//! Solver from cubic params to curve params.

use druid::kurbo::Point;

use crate::bezoid::CurveParams;

pub struct Solver;

impl Solver {
    /// Solve for curve params, given bezier control points.
    ///
    /// The points are given relative to p0 at (0, 0) and p3 at
    /// (1, 0).
    pub fn solve(&self, p1: Point, p2: Point) -> CurveParams {
        println!("({:.3}, {:.3}) ({:.3}, {:.3})", p1.x, p1.y, p2.x, p2.y);
        fn inv_arm_len(h: f64, th: f64) -> f64 {
            // This formula ensures that bezier parameters approximating
            // a circular arc map to a bias of 1.0.
            let a = h * 1.5 * (th.cos() + 1.0);
            if a < 1.0 {
                2.0 - a.powf(2.0)
            } else {
                1.0 + 2.0 * (0.5 * (1.0 - a)).tanh()
            }
        }
        let v1 = p1.to_vec2();
        let v2 = Point::new(1.0, 0.0) - p2;
        let th0 = v1.atan2();
        let th1 = -v2.atan2();
        let mut dth = 0.0;
        let mut lastxy: Option<(f64, f64)> = None;
        const N: usize = 10;
        for i in 0..N {
            let bias0 = inv_arm_len(v1.hypot(), th0);
            let bias1 = inv_arm_len(v2.hypot(), th1);
            let params = CurveParams {
                k0: th0 + 0.5 * dth,
                bias0,
                k1: th1 - 0.5 * dth,
                bias1,
            };
            if i == N - 1 {
                return params;
            }
            let result = params.compute();
            let th_err = mod_tau(th0 - th1 - (result.th0 - result.th1));
            if th_err.abs() < 1e-3 {
                return params;
            }
            // Secant method
            let nextxy = (dth, th_err);
            let delta = if let Some(lastxy) = lastxy {
                (nextxy.0 - lastxy.0) / (nextxy.1 - lastxy.1)
            } else {
                -0.5
            };
            println!(
                "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!()
    }
}

fn mod_tau(x: f64) -> f64 {
    // Do this in terms of euclidean remainder instead?
    x - std::f64::consts::TAU * (x * (1.0 / std::f64::consts::TAU)).round()
}