## ~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()
}
```