|
|
@ -297,6 +297,16 @@ function dotn(a, b) { |
|
|
|
return r; |
|
|
|
return r; |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
function dotn3(a, b, c) { |
|
|
|
|
|
|
|
let r = 0; |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
for (let i = 0; i < a.length; ++i) { |
|
|
|
|
|
|
|
r += a[i] * b[i] * c[i]; |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
return r; |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
function mix(a, b, t) { |
|
|
|
function mix(a, b, t) { |
|
|
|
return a * t + b * (1 - t); |
|
|
|
return a * t + b * (1 - t); |
|
|
|
} |
|
|
|
} |
|
|
@ -478,9 +488,75 @@ function stroke_intersects_capsule(state, stroke, a, b, radius) { |
|
|
|
return false; |
|
|
|
return false; |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
function estimate_next_point(ts, xs, ys, dt=0) { |
|
|
|
function proj_remove_x_from_y(xs, ys, ws) { |
|
|
|
|
|
|
|
const dxx = dotn3(xs, xs, ws); |
|
|
|
|
|
|
|
const dxy = dotn3(xs, ys, ws); |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if (dxx < 1e-6) { |
|
|
|
|
|
|
|
return [0, ys]; |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
const c = dxy / dxx; |
|
|
|
|
|
|
|
const res = []; |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
for (let i = 0; i < xs.length; ++i) { |
|
|
|
|
|
|
|
res.push(ys[i] - c * xs[i]); |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
return [c, res]; |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
function estimate_next_point2(ts, xs, ys, wr=1.0) { |
|
|
|
const N = ts.length; |
|
|
|
const N = ts.length; |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if (N < 2) { |
|
|
|
|
|
|
|
return [xs[N - 1], ys[N - 1]]; |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// inner product weight, power-law ramp over time from W0 to 1.0 (max)
|
|
|
|
|
|
|
|
const t0 = ts[0]; |
|
|
|
|
|
|
|
const t1 = ts[N - 1]; |
|
|
|
|
|
|
|
const ws = []; |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// constant and quadratic basis terms
|
|
|
|
|
|
|
|
const q0 = []; |
|
|
|
|
|
|
|
const ss = []; |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
for (const t of ts) { |
|
|
|
|
|
|
|
ws.push(1 + ((wr - 1) * (t - t0) / (t1 - t0))); |
|
|
|
|
|
|
|
q0.push(1); |
|
|
|
|
|
|
|
ss.push(t * t); |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// constant term
|
|
|
|
|
|
|
|
const [c0x, xs_0] = proj_remove_x_from_y(q0, xs, ws); |
|
|
|
|
|
|
|
const [c0y, ys_0] = proj_remove_x_from_y(q0, ys, ws); |
|
|
|
|
|
|
|
const [c0t, ts_0] = proj_remove_x_from_y(q0, ts, ws); |
|
|
|
|
|
|
|
const [c0s, ss_0] = proj_remove_x_from_y(q0, ss, ws); |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// linear term
|
|
|
|
|
|
|
|
const [c01x, xs_01] = proj_remove_x_from_y(ts_0, xs_0, ws); |
|
|
|
|
|
|
|
const [c01y, ys_01] = proj_remove_x_from_y(ts_0, ys_0, ws); |
|
|
|
|
|
|
|
// don't need to do ts here because it's guaranteed to go to zero
|
|
|
|
|
|
|
|
const [c01s, ss_01] = proj_remove_x_from_y(ts_0, ss_0, ws); |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// quadratic term
|
|
|
|
|
|
|
|
const [c012x, xs_012] = proj_remove_x_from_y(ss_01, xs_0, ws); |
|
|
|
|
|
|
|
const [c012y, ys_012] = proj_remove_x_from_y(ss_01, ys_0, ws); |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
const reconstructed_x = c0x * 1 + c01x * ts_0[N - 1] + c012x * ss_01[N - 1]; |
|
|
|
|
|
|
|
const reconstructed_y = c0y * 1 + c01y * ts_0[N - 1] + c012y * ss_01[N - 1]; |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
return [reconstructed_x, reconstructed_y]; |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
function estimate_next_point1(ts, xs, ys, dt=0) { |
|
|
|
|
|
|
|
const N = ts.length; |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if (N < 2) { |
|
|
|
|
|
|
|
return [xs[N - 1], ys[N - 1]]; |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
// mean values
|
|
|
|
// mean values
|
|
|
|
let mx = 0, my = 0, mt = 0; |
|
|
|
let mx = 0, my = 0, mt = 0; |
|
|
|
|
|
|
|
|
|
|
@ -494,10 +570,6 @@ function estimate_next_point(ts, xs, ys, dt=0) { |
|
|
|
mx /= N; |
|
|
|
mx /= N; |
|
|
|
my /= N; |
|
|
|
my /= N; |
|
|
|
|
|
|
|
|
|
|
|
if (N < 2) { |
|
|
|
|
|
|
|
return [xs[N - 1], ys[N - 1]]; |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// orthogonalize against constant term
|
|
|
|
// orthogonalize against constant term
|
|
|
|
for (let i = 0; i < N; ++i) { |
|
|
|
for (let i = 0; i < N; ++i) { |
|
|
|
ts[i] -= mt; |
|
|
|
ts[i] -= mt; |
|
|
@ -548,7 +620,7 @@ function smooth_curve(points, window_ms=config.demetri_ms, dt=0) { |
|
|
|
y_window.push(p.y); |
|
|
|
y_window.push(p.y); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
const [nx, ny] = estimate_next_point(t_window, x_window, y_window, dt); |
|
|
|
const [nx, ny] = estimate_next_point2(t_window, x_window, y_window, config.wr); |
|
|
|
|
|
|
|
|
|
|
|
result.push({ |
|
|
|
result.push({ |
|
|
|
'x': nx, |
|
|
|
'x': nx, |
|
|
|