diff options
| -rw-r--r-- | src/Camera.java | 11 | ||||
| -rw-r--r-- | src/RaceTrack.java | 178 | ||||
| -rw-r--r-- | src/RobotRace.java | 19 |
3 files changed, 129 insertions, 79 deletions
diff --git a/src/Camera.java b/src/Camera.java index 8bc07a6..c9da648 100644 --- a/src/Camera.java +++ b/src/Camera.java @@ -117,15 +117,8 @@ class Camera { // center at the chosen robot. center = track.getPointForLane(focus.getTimePos(), focus.getLane()); - /* look in the direction where the robots walks, namely the tangent - Add the actual robot position to the tangent vector, and calculate - the normal vector based on the resulting vector. This is the up vector. */ - Vector robotPos = track.getPointForLane(focus.getTimePos(), - focus.getLane()); - Vector robotTangent = track.getTangent(focus.getTimePos()); - Vector totalVector = robotTangent.add(robotPos); - - up = new Vector(-totalVector.y(), totalVector.x(), 0); + // the head (up vector) points "forwards" + up = track.getTangent(focus.getTimePos()); // "above" is 10 meters. eye = center.add(new Vector(0, 0, 10f)); diff --git a/src/RaceTrack.java b/src/RaceTrack.java index 6aadbb7..d588fbe 100644 --- a/src/RaceTrack.java +++ b/src/RaceTrack.java @@ -42,7 +42,7 @@ class RaceTrack extends BetterBase { * 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. @@ -57,37 +57,36 @@ class RaceTrack extends BetterBase { // points are chosen such that the boundaries of a quarter lay // on a straight line (to get second-order continuity). - // top #---|#---# - // left ^- - - - - - - begin here (top right) - // # # - // | | (v then continue clock-wise) + // 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) // | - - // # # <- - this point is the last of top-right, - // - | and the first of bottom-left // | bottom | // # left right # - // #----#|--# + // #--|#---# controlPointsOTrack = new Vector[] { // top-right - new Vector( 0, 15, 1), + new Vector( 15, 0, 1), + new Vector( 15, 8, 1), new Vector( 8, 15, 1), - new Vector( 15, 8, 1), - // bottom-right - new Vector( 15, 0, 1), - new Vector( 15, -8, 1), + // top-left + new Vector( 0, 15, 1), + new Vector( -8, 15, 1), + new Vector(-15, 8, 1), - new Vector( 8, -15, 1), // bottom-left - new Vector( 0, -15, 1), - new Vector( -8, -15, 1), - - new Vector(-15, -8, 1), - // top-left new Vector(-15, 0, 1), - new Vector(-15, 8, 1), + new Vector(-15, -8, 1), + new Vector( -8, -15, 1), - new Vector( -8, 15, 1), + // bottom-right + new Vector( 0, -15, 1), + new Vector( 8, -15, 1), + new Vector( 15, -8, 1), }; controlPointsLTrack = new Vector[] { @@ -170,34 +169,78 @@ class RaceTrack extends BetterBase { public void draw(int trackNr) { // The test track is selected if (0 == trackNr) { - drawTestTrack(); + // Special case: no control points, fall back to test track. + selectedControlPoints = null; } else if (1 == trackNr) { // The O-track is selected - drawTrack(controlPointsOTrack); + selectedControlPoints = controlPointsOTrack; } else if (2 == trackNr) { // The L-track is selected - drawTrack(controlPointsLTrack); + selectedControlPoints = controlPointsLTrack; } else if (3 == trackNr) { // The C-track is selected - drawTrack(controlPointsCTrack); + selectedControlPoints = controlPointsCTrack; } else if (4 == trackNr) { // The custom track is selected - drawTrack(controlPointsOTrack); + 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 <= {@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); - Vector lanes_len = new Vector(p.x(), p.y(), 0).normalized().scale(laneNo + .5); + // 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); } @@ -205,57 +248,66 @@ class RaceTrack extends BetterBase { * Returns the tangent of the curve at 0 <= {@code t} <= 1. */ public Vector getTangent(double t) { - /* - * Given a vector (-Y/B^2, X/A^2, 0) where X and Y are the coordinates - * of the point p on the ellipse. A is the HALFWIDTH of the ellipse and - * B is the HALFHEIGHT of the ellipse. - * - * Vector (X/A^2, Y/B^2, 0) is the normal vector through point p - * and the center of the ellipse. Because the X and Y coordinates - * are divided by the width and height of the ellipse, everything is - * "normalized" to a circle. Hence a line through the origin and a point - * p describes a normal vector for a point p on the ellipse. - * - * Since the dot product of the latter vector and the first (tangent) - * vector results in zero, we can say the normal vector is perpendicular - * to the tangent vector. And because of that, the first vector - * describes the tangent vector of point p. - */ + if (selectedControlPoints != null) { + // TODO: do not call func -- optimization + double u = calculateBezierParams(t); + return getCubicBezierTng(u, selectedControlPoints, bezier_start_i); + } + Vector p = getPoint(t); - return new Vector(-p.y() / (ELLIPSE_A * ELLIPSE_A), - p.x() / (ELLIPSE_B * ELLIPSE_B), + // 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(); } - private void drawTestTrack() { + /** + * 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. + * 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_C = getPoint(t); - // the outer side is located on the number of lanes (4) shifted from - // the center to the side (minus 0.5). - Vector point_D = getPointForLane(t, 3.5); + 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 = new Vector(point_E.x(), point_E.y(), 0).normalized(); + Vector norm_outside = norm_P; Vector norm_inside = norm_outside.scale(-1).normalized(); Vector norm_up = Vector.Z; @@ -406,17 +458,25 @@ class RaceTrack extends BetterBase { 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 + + // 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 + - // (3 - 3u^2) . P1 + + // = -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(3 - 3 * t * t)) - .add(P2.scale(6 * t + 9 * t * t)) + 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); + } } diff --git a/src/RobotRace.java b/src/RobotRace.java index b634cb2..81a9355 100644 --- a/src/RobotRace.java +++ b/src/RobotRace.java @@ -506,17 +506,14 @@ public class RobotRace extends Base { Vector robotPos = raceTrack.getPointForLane(robot.getTimePos(), i); gl.glTranslated(robotPos.x(), robotPos.y(), robotPos.z()); - /* Calculate angle for the robots to look at, multiply by 180/PI - * to convert the radions to degrees. - * First get the tangent of the robot, that is the real direction - * where the robot is looking to. - * Then add this vector to the actual position, and from the - * resulting vector we can calculate the angle. */ - Vector robotTangent = raceTrack.getTangent(robot.getTimePos()); - Vector totalVector = robotTangent.add(robotPos); - - double angle = atan2(totalVector.y(), totalVector.x()) * 180/PI; - + /* While the robot looks in the tangent direction, the position of + * the robot on the track is determined by the normal. For cases + * where the origin is used to create the track (circles), the + * robot position could be used as well (for direction). In the + * general case though, the normal is needed. + */ + Vector norm = raceTrack.getNormal(robot.getTimePos()); + double angle = atan2(norm.y(), norm.x()) * 180 / PI; gl.glRotated(angle, 0, 0, 1); // Draw the current robot |