import java.awt.Color; import robotrace.Vector; import static java.lang.Math.*; import static javax.media.opengl.GL2.*; /** * Implementation of a race track that is made from Bezier segments. */ class RaceTrack extends BetterBase { /** * Half-width of the ellipse. */ protected static final double ELLIPSE_A = 10; /** * Half-height of the ellipse. */ protected static final double ELLIPSE_B = 14; /** * Number of segments for the race track. */ private final double SEGMENTS = 180; /** * Array with control points for the O-track. */ private final Vector[] controlPointsOTrack; /** * Array with control points for the L-track. */ private final Vector[] controlPointsLTrack; /** * Array with control points for the C-track. */ private final Vector[] controlPointsCTrack; /** * Array with control points for the custom track. */ private Vector[] controlPointsCustomTrack; private Vector[] selectedControlPoints; private final RobotRace race; /** * Debug option: set to true to show control points and center line for * Bézier spline tracks. */ boolean debugBezierTracks = false; /** * Constructs the race track, sets up display lists. */ public RaceTrack(RobotRace race) { this.race = race; // points are chosen such that the boundaries of a quarter lay // on a straight line (to get second-order continuity). // top #---#|--# this is the first control point of top-left, // left ^- - - - - - and the last of top-right // # top # // | right | (^ then continue anti-clockwise) // - | // # # <- - - begin here (top-right) // | - // | bottom | // # left right # // #--|#---# controlPointsOTrack = new Vector[] { // top-right new Vector( 15, 0, 1), new Vector( 15, 8, 1), new Vector( 8, 15, 1), // top-left new Vector( 0, 15, 1), new Vector( -8, 15, 1), new Vector(-15, 8, 1), // bottom-left new Vector(-15, 0, 1), new Vector(-15, -8, 1), new Vector( -8, -15, 1), // bottom-right new Vector( 0, -15, 1), new Vector( 8, -15, 1), new Vector( 15, -8, 1), }; // the control points are grouped per 3. The last of the array and the // first two form such a group for an edge. The track starts on the top // edge and goes counter-clockwise. Most of the edges have been // determined as follows: take a point p (the edge), a slope s. Then the // vertices for this edge group are p - ax and p+ax where x is usually // -3 or 3 and the slope s -1 or 1 (depending on the direction). controlPointsLTrack = new Vector[] { // (on the bottom, there is a vertex belonging to the top group) // top new Vector( -3, 11, 1), new Vector( -6, 14, 1), new Vector( -8, 14, 1), // left new Vector( -11, 11, 1), new Vector( -14, 8, 1), new Vector( -14, -8, 1), // bottom new Vector( -11, -11, 1), new Vector( -8, -14, 1), new Vector( 9, -13, 1), // right of L foot (bottom) new Vector( 11, -11, 1), //new Vector( 14, -8, 1), new Vector( 13, -9, 1), // more weight downwards //new Vector( 14, -8, 1), new Vector( 12, -6, 1), // more weight upwards // top of L foot new Vector( 11, -5, 1), //new Vector( 8, -2, 1), new Vector( 9, -3, 1), // more weight downwards new Vector( 0, -10, 1), // right (top) new Vector( -3, -7, 1), new Vector( -6, -4, 1), new Vector( 0, 8, 1), // ^-- belongs to top edge }; // C track, anti-clockwise again. controlPointsCTrack = new Vector[] { new Vector( 2, 15, 1), new Vector(-17, 15, 1), new Vector(-17, -12, 1), new Vector(2, -12, 1), new Vector(6.5, -12, 1), new Vector(11, -12, 1), new Vector(11, -9, 1), new Vector(11, -6, 1), new Vector(6.5, -6, 1), new Vector( 2, -6, 1), new Vector( -10, -6, 1), new Vector( -10, 9, 1), new Vector( 2, 9, 1), new Vector( 6.5, 9, 1), new Vector( 11, 9, 1), new Vector( 11, 12, 1), new Vector( 11, 15, 1), new Vector( 6.5, 15, 1), }; // custom track (O track with varying elevation) controlPointsCustomTrack = new Vector[] { // top-right new Vector( 15, 0, 1), new Vector( 15, 8, 1), new Vector( 8, 15, 4), // top-left new Vector( 0, 15, 4), new Vector( -8, 15, 4), new Vector(-15, 8, 1), // bottom-left new Vector(-15, 0, 1), new Vector(-15, -8, 1), new Vector( -8, -15, 3), // bottom-right new Vector( 0, -15, 3), new Vector( 8, -15, 3), new Vector( 15, -8, 1), }; } /** * Draws this track, based on the selected track number. */ public void draw(int trackNr) { // The test track is selected if (0 == trackNr) { // Special case: no control points, fall back to test track. selectedControlPoints = null; } else if (1 == trackNr) { // The O-track is selected selectedControlPoints = controlPointsOTrack; } else if (2 == trackNr) { // The L-track is selected selectedControlPoints = controlPointsLTrack; } else if (3 == trackNr) { // The C-track is selected selectedControlPoints = controlPointsCTrack; } else if (4 == trackNr) { // The custom track is selected selectedControlPoints = controlPointsCustomTrack; } if (selectedControlPoints != null) { assert selectedControlPoints.length % 3 == 0 : "Multiple of three control points required"; } drawTrack(); // debugging purposes: show track center and control points if (debugBezierTracks && selectedControlPoints != null) { drawCenterLineTrack(selectedControlPoints); // show a marker every 5% of the track double steps = 20; gl.glPointSize(8); for (int i = 0; i < steps; i++) { // alternate colors setColor(i % 2 == 0 ? Color.MAGENTA : Color.PINK); Vector p = getPoint(i / steps); // draw a marker gl.glPushMatrix(); gl.glTranslated(p.x(), p.y(), p.z()); glut.glutSolidCone(0.4, 1.5, 8, 16); gl.glPopMatrix(); } } } /** * 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; //assert t >= 0 && t < 1.0 : "t is invalid: " + t; int number_of_segments = selectedControlPoints.length / 3; // number of "u" units per segment double segment_size = 1.0 / number_of_segments; // 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; // 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; } /** * Returns the position of the curve at 0 <= {@code t} <= 1. */ public Vector getPoint(double t) { if (selectedControlPoints != null) { // TODO: do not call func -- optimization double u = calculateBezierParams(t); return getCubicBezierPnt(u, selectedControlPoints, bezier_start_i); } return new Vector(ELLIPSE_A * cos(2 * PI * t), ELLIPSE_B * sin(2 * PI * t), 1); } private int getNumberOfLanes() { // TODO: get robots count from race instance return 4; } /** * Returns the position of the curve at 0 <= {@code t} <= 1 and * the center of a lane at lane 1 <= laneNo <= (number of robots). */ public Vector getPointForLane(double t, double laneNo) { Vector p = getPoint(t); // relative distance from center line (positive if directed to normal) double relDist = laneNo - getNumberOfLanes() / 2 + .5; Vector lanes_len = getNormal(t).scale(relDist); return p.add(lanes_len); } /** * Returns the tangent of the curve at 0 <= {@code t} <= 1. */ public Vector getTangent(double t) { if (selectedControlPoints != null) { // TODO: do not call func -- optimization double u = calculateBezierParams(t); return getCubicBezierTng(u, selectedControlPoints, bezier_start_i); } Vector p = getPoint(t); // tangent is derivative of ellipse: // d / dt (A cos(t)) / (B sin(t)) = (-A sin(t)) / (B cos(t)) return new Vector(-ELLIPSE_A * sin(2 * PI * t), ELLIPSE_B * cos(2 * PI * t), 0).normalized(); } /** * Returns the normal vector of the curve at t. */ public Vector getNormal(double t) { Vector tangent = getTangent(t); // right-hand rule: a (tangent direction), a x b is normal (pointing // outside), so b must be positive Z vector. Vector norm = tangent.cross(Vector.Z); // for out purposes, Z is zero. assert norm.z() == 0 : "Z is not zero!"; assert tangent.dot(norm) == 0 : "Result is not normal?!"; // just to be sure, unit lengths! return norm.normalized(); } private void drawTrack() { /* A track segment looks like: * B----------------------------D "outside top" * / : /| * / G- - - - - - - - - - - - -/--H "outside bottom" * P * / / * A----------------------------C "inside top" * | | * E----------------------------F "inside bottom" * ^-- t = t0 ^-- t = t0 + 1 * Assume point A the inner point of the race track. Draw quads from * EF (starting point) to AC, BD, GH. P is a point on the center line. */ // previous points Vector point_A = null, point_B = null, point_E = null, point_G = null; for (double i = 0; i <= SEGMENTS; ++i) { double t = i / SEGMENTS; Vector point_P = getPoint(t); Vector norm_P = getNormal(t); Vector halfLaneLen = norm_P.scale(getNumberOfLanes() / 2); Vector point_C = point_P.subtract(halfLaneLen); Vector point_D = point_P.add(halfLaneLen); // Z=1 to Z=-1 Vector point_F = point_C.subtract(new Vector(0, 0, 2)); Vector point_H = point_D.subtract(new Vector(0, 0, 2)); // initially, there are no "previous" vectors to use as start. if (i > 0) { Vector norm_outside = norm_P; Vector norm_inside = norm_outside.scale(-1).normalized(); Vector norm_up = Vector.Z; // Set brick texture if (race.enableTextures) { race.getBrickTexture().bind(gl); } // Draw track walls gl.glBegin(GL_QUADS); setColor(Color.RED); // inside bottom glNormal(norm_inside); gl.glTexCoord2f(0, 0); glVertex(point_E); gl.glTexCoord2f(1, 0); glVertex(point_F); setColor(Colors.PALE_TURQOISE); // inside top glNormal(norm_up.add(norm_inside).normalized()); gl.glTexCoord2f(1, 1); glVertex(point_C); gl.glTexCoord2f(0, 1); glVertex(point_A); // outside bottom glNormal(norm_outside); gl.glTexCoord2f(0, 0); glVertex(point_G); gl.glTexCoord2f(1, 0); glVertex(point_H); // outside top glNormal(norm_up.add(norm_outside).normalized()); gl.glTexCoord2f(1, 1); glVertex(point_D); gl.glTexCoord2f(0, 1); glVertex(point_B); gl.glEnd(); if (race.enableTextures) { race.getTrackTexture().bind(gl); } // Draw track itself // Every 20 segments a distance line is drawn, // and at the start, a start line is drawn. gl.glBegin(GL_QUADS); glNormal(Vector.Z); gl.glTexCoord2f(i == 1 ? 0 : 0.2f, 0); glVertex(point_A); gl.glTexCoord2f(i % 20 == 0 && i != SEGMENTS ? 1f : 0.8f, 0); glVertex(point_C); gl.glTexCoord2f(i % 20 == 0 && i != SEGMENTS ? 1f : 0.8f, 1f); glVertex(point_D); gl.glTexCoord2f(i == 1 ? 0 : 0.2f, 1f); glVertex(point_B); gl.glEnd(); } unbindTextures(); // save points for next draw round point_E = point_F; point_A = point_C; point_B = point_D; point_G = point_H; } } /** * Draw a closed race track with control points. * * @param pts Control points. */ private void drawCenterLineTrack(Vector[] pts) { assert pts != null; assert pts.length % 3 == 0 : "Multiple of three control points required"; int number_of_segments = pts.length / 3; // number of "u" units per segment double segment_size = 1.0 / number_of_segments; gl.glPushMatrix(); // put lines and dots above track gl.glTranslated(0, 0, 0.5); gl.glLineWidth(2); gl.glBegin(GL_LINE_STRIP); for (double i = 0; i <= SEGMENTS; ++i) { double u = i / SEGMENTS; int segment_number = (int) ((i / (SEGMENTS + 1)) / segment_size); int start = 3 * segment_number; double segment_u = u - segment_number * segment_size; // scale the part to 0.0 to 0.1 segment_u *= number_of_segments; segment_u = min(segment_u, 1.0); //assert segment_u >= 0.0 && segment_u <= 1.0 : "Segment out of bounds: " + segment_u; Vector bezierPt = getCubicBezierPnt(segment_u, pts, start); gl.glColor4d(1.0, 0.0, segment_number % 2 == 0 ? 1 : 0, .8); glVertex(bezierPt); } gl.glEnd(); // draw control points gl.glPointSize(10); gl.glBegin(GL_POINTS); for (int i = 0; i < pts.length; i++) { double color = (i % 3.0) / 3.0; gl.glColor3d(0.0, color, 1.0); glVertex(pts[i]); } gl.glEnd(); // restore position gl.glPopMatrix(); } /** * Obtains a cubic Bézier segment from points P0, P1, P2 and P3 for * parameter value t. */ public static Vector getCubicBezierPnt(double t, Vector P0, Vector P1, Vector P2, Vector P3) { // the factorials for the Bézier blending functions (Bernstein // polynomials) with n=3 are pre-calculated. // P(u) = (1 - u)^3 . P0 + // 3u (1 - u)^2 . P1 + // 3u^2 (1 - u) . P2 + // u^3 . P3 // Implementation note: Vector is instantiated 7 times! return P0.scale( pow(1 - t, 3)) // k = 0 .add(P1.scale(3 * t * pow(1 - t, 2))) // k = 1 .add(P2.scale(3 * pow(t, 2) * (1 - t))) // k = 2 .add(P3.scale( pow(t, 3))); // k = 3 } /** * Obtains a point on the Bézier curve from points P, starting at index i. */ public static Vector getCubicBezierPnt(double t, Vector[] P, int i) { Vector P4 = P[(i + 3) % P.length]; return getCubicBezierPnt(t, P[i], P[i + 1], P[i + 2], P4); } /** * Evaluate the tangent of a cubic Bézier segment from points P0, P2, P2 and * P3 for parameter value t. */ public static Vector getCubicBezierTng(double t, Vector P0, Vector P1, Vector P2, Vector P3) { // The tangent is the derivative of the Bézier curve P(t). // dP(u) / du = -3 (1 - u)^2 . P0 + // (3 (1 - u)^2 - 6u (1 - u)) . P1 + // (6u (1 - u) - 3u^2) . P2 + // 3u^2 . P3 // = -3 (1 - u)^2 . P0 + // (-9u + 3) (1-u). P1 + // (6u - 9u^2) . P2 + // 3u^2 . P3 return P0.scale(-3 * pow(1 - t, 2)) .add(P1.scale((-9 * t + 3) * (1 - t))) .add(P2.scale(6 * t - 9 * t * t)) .add(P3.scale(3 * t * t)); } /** * Obtains the tangent on Bézier curve from points P, starting at index i. */ public static Vector getCubicBezierTng(double t, Vector[] P, int i) { 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; } }