BlueGit

timfa timdraw

Files

timdraw / static / penpath.js

penpath.js

penpath.js 2.3 KiB Download Raw
1// De rode stip loopt over de geplande lijn, niet in een sprong tussen twee metingen.
2
3function penRouteFrom (strokes, home)
4{
5 const pts = [];
6 const push = (x, y) =>
7 {
8 const last = pts[pts.length - 1];
9 if (last && Math.hypot(last[0] - x, last[1] - y) < 0.05) return;
10 pts.push([x, y]);
11 };
12 if (home) push(home[0], home[1]);
13 for (const stroke of strokes || [])
14 {
15 for (const point of stroke || []) push(point[0], point[1]);
16 }
17 if (home) push(home[0], home[1]);
18 if (pts.length < 2) return null;
19 const cum = [0];
20 for (let i = 1; i < pts.length; i++)
21 {
22 cum.push(cum[i - 1] + Math.hypot(pts[i][0] - pts[i - 1][0], pts[i][1] - pts[i - 1][1]));
23 }
24 return { pts, cum };
25}
26
27function penProject (route, x, y, hint)
28{
29 const pts = route.pts;
30 const cum = route.cum;
31 let best = { along: 0, err: Infinity };
32 let ahead = null;
33 for (let i = 0; i < pts.length - 1; i++)
34 {
35 const ax = pts[i][0];
36 const ay = pts[i][1];
37 const bx = pts[i + 1][0];
38 const by = pts[i + 1][1];
39 const dx = bx - ax;
40 const dy = by - ay;
41 const len2 = dx * dx + dy * dy;
42 let t = len2 < 1e-9 ? 0 : ((x - ax) * dx + (y - ay) * dy) / len2;
43 if (t < 0) t = 0;
44 else if (t > 1) t = 1;
45 const px = ax + dx * t;
46 const py = ay + dy * t;
47 const err = Math.hypot(x - px, y - py);
48 const along = cum[i] + Math.hypot(px - ax, py - ay);
49 if (err < best.err) best = { along, err };
50 if (along + 1e-6 >= hint - 1 && (!ahead || err < ahead.err)) ahead = { along, err };
51 }
52 if (ahead && ahead.err <= Math.max(2.5, best.err + 0.4)) return ahead;
53 return best;
54}
55
56function penAt (route, along)
57{
58 const pts = route.pts;
59 const cum = route.cum;
60 const total = cum[cum.length - 1];
61 const dist = Math.max(0, Math.min(total, along));
62 let lo = 1;
63 let hi = cum.length - 1;
64 while (lo < hi)
65 {
66 const mid = (lo + hi) >> 1;
67 if (cum[mid] < dist) lo = mid + 1;
68 else hi = mid;
69 }
70 const span = cum[lo] - cum[lo - 1];
71 const t = span > 1e-6 ? (dist - cum[lo - 1]) / span : 0;
72 return [
73 pts[lo - 1][0] + (pts[lo][0] - pts[lo - 1][0]) * t,
74 pts[lo - 1][1] + (pts[lo][1] - pts[lo - 1][1]) * t,
75 ];
76}
77
78function penStep (along, goal, mmPerMin, dt)
79{
80 const gap = goal - along;
81 if (Math.abs(gap) < 0.02) return goal;
82 const step = Math.min(Math.abs(gap), Math.max(1, mmPerMin) / 60 * dt);
83 return along + Math.sign(gap) * step;
84}