aaaaaa123456789

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