File size: 6,887 Bytes
9c235c0
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
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
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
/**
 * The 3D Replay trail's geometry: the path a tool point (a gripper, a hand) takes through an
 * episode, smoothed, and the triangle strip the trail shader draws it with. Pure, so it is tested
 * without a GPU; `replay-trail.ts` draws it.
 */

/** Below this distance two samples are the same place: the robot held still. */
const SAME_PLACE = 1e-6;

export interface TrailPath {
  /** xyz per point. */
  points: Float32Array;
  /**
   * The episode frame each point belongs to, fractional between frames and never decreasing. The
   * shader compares it with the current frame to tell the past, the recent tail and the future.
   */
  along: Float32Array;
  count: number;
}

/**
 * Centripetal Catmull–Rom through `p0..p3`, at `t` in [0, 1] between p1 and p2 (Barry–Goldman).
 * Centripetal, unlike the uniform kind, never overshoots into loops between close samples.
 */
function catmullRom(
  p0: ArrayLike<number>,
  p1: ArrayLike<number>,
  p2: ArrayLike<number>,
  p3: ArrayLike<number>,
  t: number,
  out: Float32Array,
  at: number,
): void {
  const knot = (a: ArrayLike<number>, b: ArrayLike<number>) =>
    Math.max(
      Math.sqrt(Math.hypot(b[0] - a[0], b[1] - a[1], b[2] - a[2])),
      1e-4,
    );
  const t1 = knot(p0, p1);
  const t2 = t1 + knot(p1, p2);
  const t3 = t2 + knot(p2, p3);
  const u = t1 + (t2 - t1) * t;
  for (let c = 0; c < 3; c++) {
    const a1 = ((t1 - u) * p0[c] + u * p1[c]) / t1;
    const a2 = ((t2 - u) * p1[c] + (u - t1) * p2[c]) / (t2 - t1);
    const a3 = ((t3 - u) * p2[c] + (u - t2) * p3[c]) / (t3 - t2);
    const b1 = ((t2 - u) * a1 + u * a2) / t2;
    const b2 = ((t3 - u) * a2 + (u - t1) * a3) / (t3 - t1);
    out[at + c] = ((t2 - u) * b1 + (u - t1) * b2) / (t2 - t1);
  }
}

/**
 * The curve through one sample per frame (`samples`, xyz each), with `subdivisions` points per step
 * between frames.
 *
 * A run of frames where the robot holds still collapses to two points, at its first and last frame:
 * the trail then knows the robot was there all that time, and draws no zero-length steps (which have
 * no direction to draw a line across).
 */
export function smoothPath(
  samples: Float32Array,
  subdivisions: number,
): TrailPath {
  const frames = Math.floor(samples.length / 3);
  const steps = Math.max(1, Math.floor(subdivisions));

  // Distinct places, in order, with the frames the robot spent at each.
  const places: number[] = [];
  const firstFrame: number[] = [];
  const lastFrame: number[] = [];
  for (let k = 0; k < frames; k++) {
    const x = samples[k * 3];
    const y = samples[k * 3 + 1];
    const z = samples[k * 3 + 2];
    if (!Number.isFinite(x) || !Number.isFinite(y) || !Number.isFinite(z))
      continue;
    const n = firstFrame.length;
    if (
      n > 0 &&
      Math.hypot(
        x - places[(n - 1) * 3],
        y - places[(n - 1) * 3 + 1],
        z - places[(n - 1) * 3 + 2],
      ) < SAME_PLACE
    ) {
      lastFrame[n - 1] = k;
      continue;
    }
    places.push(x, y, z);
    firstFrame.push(k);
    lastFrame.push(k);
  }

  const n = firstFrame.length;
  const held = firstFrame.filter((f, i) => lastFrame[i] > f).length;
  const count = n + held + Math.max(0, n - 1) * (steps - 1);
  const points = new Float32Array(count * 3);
  const along = new Float32Array(count);

  const place = (i: number) => {
    const c = Math.min(n - 1, Math.max(0, i));
    return places.slice(c * 3, c * 3 + 3);
  };
  /// Past the ends, a mirrored neighbour keeps the end segments straight rather than hooked.
  const before = (i: number) => {
    if (i >= 0) return place(i);
    const a = place(0);
    const b = place(1);
    return [2 * a[0] - b[0], 2 * a[1] - b[1], 2 * a[2] - b[2]];
  };
  const after = (i: number) => {
    if (i < n) return place(i);
    const a = place(n - 1);
    const b = place(n - 2);
    return [2 * a[0] - b[0], 2 * a[1] - b[1], 2 * a[2] - b[2]];
  };

  let w = 0;
  for (let i = 0; i < n; i++) {
    const p = place(i);
    points.set(p, w * 3);
    along[w++] = firstFrame[i];
    if (lastFrame[i] > firstFrame[i]) {
      points.set(p, w * 3);
      along[w++] = lastFrame[i];
    }
    if (i === n - 1) break;
    // The step to the next place is taken between the last frame here and the next one.
    const p0 = before(i - 1);
    const p2 = place(i + 1);
    const p3 = after(i + 2);
    for (let j = 1; j < steps; j++) {
      catmullRom(p0, p, p2, p3, j / steps, points, w * 3);
      along[w++] = lastFrame[i] + j / steps;
    }
  }
  return { points, along, count: w };
}

/** Points per step between frames: smooth, within `budget` points for the whole episode. */
export function subdivisionsFor(frames: number, budget = 24_000): number {
  return Math.max(1, Math.min(4, Math.floor(budget / Math.max(1, frames))));
}

export interface RibbonArrays {
  /** Two vertices per point, one each side of the line. */
  position: Float32Array;
  /** The neighbouring points, which give the line's direction at each vertex. */
  previous: Float32Array;
  next: Float32Array;
  /** -1 or 1: which side of the line the vertex is pushed to. */
  side: Float32Array;
  along: Float32Array;
  index: Uint32Array;
}

/**
 * A triangle strip along `path`, two vertices per point. The shader pushes each vertex sideways in
 * screen space, so the line keeps its width in pixels at any zoom; the strip joins its segments
 * itself, so a transparent line has no overlapping caps that would show as beads.
 */
export function ribbonArrays(path: TrailPath): RibbonArrays {
  const { points, along, count } = path;
  const position = new Float32Array(count * 6);
  const previous = new Float32Array(count * 6);
  const next = new Float32Array(count * 6);
  const side = new Float32Array(count * 2);
  const alongOut = new Float32Array(count * 2);
  for (let i = 0; i < count; i++) {
    const p = i * 3;
    const before = Math.max(0, i - 1) * 3;
    const ahead = Math.min(count - 1, i + 1) * 3;
    for (let s = 0; s < 2; s++) {
      const v = i * 2 + s;
      for (let c = 0; c < 3; c++) {
        position[v * 3 + c] = points[p + c];
        previous[v * 3 + c] = points[before + c];
        next[v * 3 + c] = points[ahead + c];
      }
      side[v] = s === 0 ? -1 : 1;
      alongOut[v] = along[i];
    }
  }
  const segments = Math.max(0, count - 1);
  const index = new Uint32Array(segments * 6);
  for (let i = 0; i < segments; i++) {
    const a = i * 2;
    index.set([a, a + 1, a + 2, a + 2, a + 1, a + 3], i * 6);
  }
  return { position, previous, next, side, along: alongOut, index };
}

/** Which side of the robot a tool point is on, from its prim path; `null` when it doesn't say. */
export function tipSide(path: string): "left" | "right" | null {
  const name = path.slice(path.lastIndexOf("/") + 1);
  if (/(^|_)(left|l)(_|$)/i.test(name)) return "left";
  if (/(^|_)(right|r)(_|$)/i.test(name)) return "right";
  return null;
}