File size: 5,522 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
import { describe, expect, test } from "bun:test";
import {
  ribbonArrays,
  smoothPath,
  subdivisionsFor,
  tipSide,
} from "@/lib/trail";

/** One xyz sample per frame. */
const samples = (...points: [number, number, number][]) =>
  new Float32Array(points.flat());

const pointAt = (path: { points: Float32Array }, i: number) =>
  Array.from(path.points.slice(i * 3, i * 3 + 3));

describe("smoothPath", () => {
  test("passes through every frame's sample, at that frame", () => {
    const frames: [number, number, number][] = [
      [0, 0, 0],
      [1, 0, 0],
      [2, 1, 0],
      [2, 2, 1],
      [1, 3, 1],
    ];
    const path = smoothPath(samples(...frames), 4);

    // Four points per step: every fourth one is a frame's own sample.
    expect(path.count).toBe(4 * 4 + 1);
    frames.forEach((frame, k) => {
      expect(path.along[k * 4]).toBe(k);
      pointAt(path, k * 4).forEach((v, c) =>
        expect(v).toBeCloseTo(frame[c], 5),
      );
    });
  });

  test("the frame index never decreases along the path", () => {
    const frames: [number, number, number][] = Array.from(
      { length: 50 },
      (_, k) => [Math.sin(k / 5), Math.cos(k / 7), k / 50],
    );
    const { along, count } = smoothPath(samples(...frames), 3);

    for (let i = 1; i < count; i++) {
      expect(along[i]).toBeGreaterThan(along[i - 1]);
    }
    expect(along[count - 1]).toBe(49);
  });

  test("stays between its samples on a straight line (no overshoot)", () => {
    // Uneven steps along x: a uniform Catmull–Rom would swing back past a sample here.
    const xs = [0, 0.01, 0.02, 1, 1.01, 2];
    const path = smoothPath(
      samples(...xs.map((x) => [x, 0, 0] as [number, number, number])),
      4,
    );

    for (let i = 1; i < path.count; i++) {
      expect(path.points[i * 3]).toBeGreaterThanOrEqual(
        path.points[(i - 1) * 3] - 1e-6,
      );
      expect(Math.abs(path.points[i * 3 + 1])).toBeLessThan(1e-6);
    }
  });

  test("a stretch where the robot holds still is two points, at its first and last frame", () => {
    const path = smoothPath(
      samples([0, 0, 0], [1, 0, 0], [1, 0, 0], [1, 0, 0], [1, 0, 0], [2, 0, 0]),
      1,
    );

    expect(Array.from(path.along)).toEqual([0, 1, 4, 5]);
    expect(pointAt(path, 1)).toEqual(pointAt(path, 2));
  });

  test("the motion after a pause happens on the step it was recorded on", () => {
    // Still from frame 1 to 9, then a move: the curve to the next place is drawn between frames
    // 9 and 10, not spread over the pause (which would show the move before it happened).
    const frames: [number, number, number][] = [[0, 0, 0]];
    for (let k = 1; k <= 9; k++) frames.push([1, 0, 0]);
    frames.push([2, 0, 0]);
    const path = smoothPath(samples(...frames), 4);

    const moving = Array.from(path.along).filter((a) => a > 9 && a < 10);
    expect(moving).toEqual([9.25, 9.5, 9.75]);
  });

  test("skips frames whose sample is missing", () => {
    const path = smoothPath(
      samples([0, 0, 0], [Number.NaN, 0, 0], [2, 0, 0]),
      1,
    );

    expect(path.count).toBe(2);
    expect(Array.from(path.along)).toEqual([0, 2]);
  });

  test("a still robot is one point, and an empty episode none", () => {
    expect(smoothPath(samples([1, 2, 3], [1, 2, 3], [1, 2, 3]), 4).count).toBe(
      2,
    );
    expect(smoothPath(samples([1, 2, 3]), 4).count).toBe(1);
    expect(smoothPath(new Float32Array(0), 4).count).toBe(0);
  });
});

describe("subdivisionsFor", () => {
  test("smooth for short episodes, cheaper for long ones", () => {
    expect(subdivisionsFor(300)).toBe(4);
    expect(subdivisionsFor(8_000)).toBe(3);
    expect(subdivisionsFor(30_000)).toBe(1);
    expect(subdivisionsFor(0)).toBe(4);
  });
});

describe("ribbonArrays", () => {
  test("two vertices per point, two triangles per segment", () => {
    const path = smoothPath(samples([0, 0, 0], [1, 0, 0], [2, 1, 0]), 1);
    const ribbon = ribbonArrays(path);

    expect(ribbon.position.length).toBe(3 * 2 * 3);
    expect(Array.from(ribbon.side)).toEqual([-1, 1, -1, 1, -1, 1]);
    expect(Array.from(ribbon.along)).toEqual([0, 0, 1, 1, 2, 2]);
    expect(Array.from(ribbon.index)).toEqual([
      0, 1, 2, 2, 1, 3, 2, 3, 4, 4, 3, 5,
    ]);
  });

  test("each vertex knows its neighbours; the ends repeat their own point", () => {
    const path = smoothPath(samples([0, 0, 0], [1, 0, 0], [2, 1, 0]), 1);
    const { previous, next } = ribbonArrays(path);
    const vec = (a: Float32Array, v: number) =>
      Array.from(a.slice(v * 3, v * 3 + 3));

    expect(vec(previous, 0)).toEqual([0, 0, 0]);
    expect(vec(next, 0)).toEqual([1, 0, 0]);
    expect(vec(previous, 2)).toEqual([0, 0, 0]);
    expect(vec(next, 2)).toEqual([2, 1, 0]);
    expect(vec(next, 5)).toEqual([2, 1, 0]);
  });

  test("an empty path makes an empty ribbon", () => {
    const ribbon = ribbonArrays(smoothPath(new Float32Array(0), 4));
    expect(ribbon.position.length).toBe(0);
    expect(ribbon.index.length).toBe(0);
  });
});

describe("tipSide", () => {
  test("reads the side from the prim's name", () => {
    expect(tipSide("/robot/openarm_left_hand_tcp")).toBe("left");
    expect(tipSide("/g1/right_hand_palm_link")).toBe("right");
    expect(tipSide("/g1/L_hand_base_link")).toBe("left");
    expect(tipSide("/g1/R_hand_base_link")).toBe("right");
    expect(tipSide("/so101/gripper_frame_link")).toBeNull();
    // Not fooled by a word that merely contains the letters.
    expect(tipSide("/robot/lower_link")).toBeNull();
  });
});