 # Multi-line time step function

Apr 8th, 2020 (edited)
285
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
1. /*
2.   This code is hereby released to the public domain.
3.   ~aaaaaa123456789, 2020-04-08, last updated 2020-06-06
4. */
5.
6. #include <stdlib.h>
7. #include <math.h>
8.
9. struct point {
10.   short x, y;
11. };
12.
13. // points, count: vertices
14. // position: relative position within the line, between 0 and 1
15. // speeding_distance: distance required to go from no speed to max and vice-versa
16. struct point compute_point (const struct point * points, unsigned count, double position, double speeding_distance) {
17.   if (!(points && count)) return (struct point) {.x = 0, .y = 0};
18.   if (!-- count) return *points;
19.   if (position <= 0) return *points;
20.   if (position >= 1) return points[count];
21.   if (speeding_distance < 0.01) speeding_distance = 0.01;
22.   double acceleration = 4 / speeding_distance;
23.   double * segment_length = malloc(sizeof *segment_length * count);
24.   unsigned p;
25.   for (p = 0; p < count; p ++)
26.     segment_length[p] = sqrt((points[p + 1].x - points[p].x) * (points[p + 1].x - points[p].x) +
27.                              (points[p + 1].y - points[p].y) * (points[p + 1].y - points[p].y));
28.   double * corner_speed = malloc(sizeof *corner_speed * (count + 1));
29.   *corner_speed = corner_speed[count] = 0;
30.   // speed at each corner: 1 - cos(angle). Easy to calculate using a dot product, without any actual trigonometry.
31.   for (p = 1; p < count; p ++)
32.     corner_speed[p] = 1 - ((points[p + 1].x - points[p].x) * (points[p - 1].x - points[p].x) +
33.                            (points[p + 1].y - points[p].y) * (points[p - 1].y - points[p].y)) /
34.                           (segment_length[p] * segment_length[p - 1]);
35.   double * segment_time = malloc(sizeof *segment_time * count);
36.   unsigned char * segment_type = malloc(sizeof *segment_type * count);
37.   double total_time = 0;
38.   double initial, final, peak, time, length; // work with local variables for simplicity
39.   for (p = 0; p < count; p ++) {
40.     initial = corner_speed[p];
41.     final = corner_speed[p + 1];
42.     length = segment_length[p];
43.     peak = sqrt((initial * initial + final * final) / 2 + length * acceleration);
44.     if (peak >= 2) {
45.       // speed capped at 2; the peak value is just a calculation aid now
46.       segment_type[p] = 0;
47.       time = (peak * peak - 2 * (initial + final) + 4) / (acceleration * 2);
48.     } else if ((peak >= initial) && (peak >= final)) {
49.       segment_type[p] = 1;
50.       time = (peak * 2 - initial - final) / acceleration;
51.     } else if (initial <= final) {
52.       // not enough distance to reach the final speed, so we lower it
53.       segment_type[p] = 2;
54.       final = sqrt(initial * initial + 2 * acceleration * length);
55.       corner_speed[p + 1] = final;
56.       time = length * 2 / (initial + final);
57.     } else {
58.       // not enough distance to stop this time, but we must stop faster or we'll overrun the segment
59.       segment_type[p] = 3;
60.       time = length * 2 / (initial + final); // same as above
61.     }
62.     segment_time[p] = time;
63.     total_time += time;
64.   }
65.   position *= total_time;
66.   for (p = 0; (p < count) && (segment_time[p] <= position); position -= segment_time[p ++]);
67.   unsigned char type = 4;
68.   if (p < count) {
69.     initial = corner_speed[p];
70.     final = corner_speed[p + 1];
71.     time = segment_time[p];
72.     length = segment_length[p];
73.     type = segment_type[p];
74.     position /= time;
75.   }
76.   // everything's in a local variable now, so get rid of those allocated arrays
77.   free(corner_speed);
78.   free(segment_time);
79.   free(segment_length);
80.   free(segment_type);
81.   double first_split, second_split;
82.   switch (type) {
83.     case 0:
84.       // two splits: when peak speed is reached and when slowdown begins
85.       first_split = (2 - initial) / acceleration;
86.       second_split = time - (2 - final) / acceleration;
87.       break;
88.     case 1:
89.       // only one split
90.       first_split = ((final - initial) / acceleration + time) / 2;
91.       second_split = first_split;
92.       break;
93.     case 2:
94.       // no splits (the split would come after the end point); assign an arbitrary large value
95.       first_split = time * 16;
96.       second_split = first_split;
97.       break;
98.     case 3:
99.       // uses a different formula, so compute the position directly (and avoid recomputing later)
100.       position *= (2 * initial + position * (final - initial)) / (initial + final);
101.       break;
102.     default:
103.       // only possible if position >= 1 due to rounding errors; just stay at the line's end point
104.       return points[count];
105.   }
106.   if (type < 3)
107.     if (position <= (first_split / time))
108.       position *= time * (initial + time * position * acceleration / 2) / length;
109.     else if (position >= (second_split / time))
110.       position = 1 + (position - 1) * time * (final - time * acceleration * (position - 1) / 2) / length;
111.     else
112.       position = (2 * (time * position - first_split) + (4 - initial * initial) / (2 * acceleration)) / length;
113.   // this relies on implicit float to short conversion
114.   // FIXME: point wraps with large negative speeds (see https://www.youtube.com/watch?v=kpk2tdsPh0A#t=632)
115.   return (struct point) {
116.     .x = round(points[p].x * (1 - position) + points[p + 1].x * position),
117.     .y = round(points[p].y * (1 - position) + points[p + 1].y * position)
118.   };
119. }
RAW Paste Data Copied