path: root/src/RaceTrack.java

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(  -7,  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(  -7, -14, 1),

            new Vector(   8, -14, 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(  13,  -7, 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
        };

        controlPointsCTrack = new Vector[] {
            // CORRECT
            new Vector(  2,  15, 1),

            new Vector( 6.5,   15, 1),
            new Vector( 11,   15, 1),

            new Vector( 11, 12, 1),

            new Vector( 11, 9, 1),
            new Vector( 6.5, 9, 1),


            new Vector( 2, 9, 1),

            new Vector( -10, 9, 1),
            new Vector( -10, -6, 1),

            new Vector( 2, -6, 1),

            new Vector(6.5, -6, 1),
            new Vector(11, -6, 1),

            new Vector(11, -9, 1),

            new Vector(11, -12, 1),
            new Vector(6.5, -12, 1),

            new Vector(2, -12, 1),

            new Vector(-17, -12, 1),
            new Vector(-17, 15, 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();
    }

    /**
     * For internal use, only valid for drawing Bézier splines.
     */
    private int bezier_start_i;

    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;

        int segment_number = (int) (t / segment_size);
        // should always hold if t < 1.0
        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;
        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 &lt;= {@code t} &lt;= 1 and
     * the center of a lane at lane 1 &lt;= laneNo &lt;= (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;
        }

        // debugging purposes: show track center and control points
        if (debugBezierTracks && selectedControlPoints != null) {
            drawCenterLineTrack(selectedControlPoints);
        }
    }

    /**
     * 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;

        // put lines and dots above track
        gl.glTranslated(0, 0, 1);

        gl.glLineWidth(5);
        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 = ((double) i / 3.0);
            gl.glColor3d(0.0, color, 1.0);
            glVertex(pts[i]);
        }
        gl.glEnd();

        // restore position
        gl.glTranslated(0, 0, -1);
    }

    /**
     * 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);
    }
}