| 1 | // De rode stip loopt over de geplande lijn, niet in een sprong tussen twee metingen. |
| 2 | |
| 3 | function 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 | |
| 27 | function 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 | |
| 56 | function 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 | |
| 78 | function 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 | } |