summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorPeter Wu <lekensteyn@gmail.com>2014-01-17 18:40:05 +0100
committerPeter Wu <lekensteyn@gmail.com>2014-01-17 18:40:05 +0100
commita789954f7b9893c6bdd5ba824317cc2d09c7e56b (patch)
treefda3636cd2c50ae8440a1bc0b2b57ea7074db32c
parent0ce5a249d8b8aabc98f3dccee78cced779018594 (diff)
download2iv60-robots-a789954f7b9893c6bdd5ba824317cc2d09c7e56b.tar.gz
Account for different curve lengths
-rw-r--r--src/RaceTrack.java61
1 files changed, 55 insertions, 6 deletions
diff --git a/src/RaceTrack.java b/src/RaceTrack.java
index 8958c3d..0362f9a 100644
--- a/src/RaceTrack.java
+++ b/src/RaceTrack.java
@@ -197,6 +197,9 @@ class RaceTrack extends BetterBase {
* For internal use, only valid for drawing Bézier splines.
*/
private int bezier_start_i;
+ private Vector[] cached_controlPoints;
+ private double[] segmentLengths;
+ private double trackLength;
private double calculateBezierParams(double t) {
t = t % 1.0;
@@ -206,15 +209,39 @@ class RaceTrack extends BetterBase {
// number of "u" units per segment
double segment_size = 1.0 / number_of_segments;
- int segment_number = (int) (t / segment_size);
- // should always hold if t < 1.0
+ // TODO: this calculation is expensive, perhaps do it in constructor?
+ if (cached_controlPoints != selectedControlPoints) {
+ System.err.println("Recalculating Bézier segments...");
+ segmentLengths = new double[number_of_segments];
+ trackLength = 0;
+ // find lengths of each segment
+ for (int i = 0; i < number_of_segments; i++) {
+ double arcLen = getCubicBezierLen(selectedControlPoints, 3 * i);
+ System.err.println("Length of segment " + i + ": " + arcLen);
+ segmentLengths[i] = arcLen;
+ trackLength += arcLen;
+ }
+
+ cached_controlPoints = selectedControlPoints;
+ }
+
+ int segment_number = 0;
+ // position on the track from the start (in "meters")
+ double segmentPos = t * trackLength;
+ // starting point of segment over the full track
+ double segmentStartPos = 0;
+ while (segmentStartPos + segmentLengths[segment_number] < segmentPos) {
+ segmentStartPos += segmentLengths[segment_number];
+ segment_number++;
+ }
+
assert segment_number < number_of_segments;
bezier_start_i = 3 * segment_number;
- // drop segments before this one
- double segment_u = t - segment_number * segment_size;
- // scale the part to 0.0 to 0.1
- segment_u *= number_of_segments;
+ // position inside the current segment (in "meters")
+ double posInSegment = segmentPos - segmentStartPos;
+ // convert "meters" to progress (in unit)
+ double segment_u = posInSegment / segmentLengths[segment_number];
assert segment_u >= 0.0 && segment_u <= 1.0;
return segment_u;
}
@@ -491,4 +518,26 @@ class RaceTrack extends BetterBase {
Vector P4 = P[(i + 3) % P.length];
return getCubicBezierTng(t, P[i], P[i + 1], P[i + 2], P4);
}
+
+ /**
+ * Calculates the arc length of a cubic Bézier curve.
+ */
+ public static double getCubicBezierLen(Vector[] controlPoints, int start) {
+ // if you are brave, try http://math.stackexchange.com/a/64769/16508
+ // this implementation approximates the length
+ double len = 0;
+ double steps = 180.0; // higher is more precise, lower is faster
+ Vector a = getCubicBezierPnt(0.0, controlPoints, start);
+ for (int i = 1; i < steps; i++) {
+ double u = i / steps;
+ Vector b = getCubicBezierPnt(u, controlPoints, start);
+ // length between the previous and current point on the curve.
+ double part_len = sqrt(pow(a.x() - b.x(), 2) +
+ pow(a.y() - b.y(), 2));
+ len += part_len;
+
+ a = b;
+ }
+ return len;
+ }
}