// Rates how close the given configuration at the end of the path is to the desired ending configuration. // This function values the first point higher than the rest and has a maximum of 100 public static double rateConfiguration(configuration wantedConfiguration, configuration givenConfiguration) { double distance; double[] distances = new double[Variables.vehicle_size - 1]; double rating = 100; // Distance between first axle of wanted and given configuration ("start" and "end" point of our path) EZPathFollowing.LinePathPart line = new EZPathFollowing.LinePathPart(wantedConfiguration.getPoint(0), givenConfiguration.getPoint(0), false, 0, 0); distance = line.pathlength(); // If the distance of start and end is over 100 the path is so bad we don't even care anymore about the rest if (distance < 100) { // Iterates over all axles except the first one and writes the distance to distances[i - 1] for (int i = 1; i < Variables.vehicle_size; i++) { line = new EZPathFollowing.LinePathPart(wantedConfiguration.getPoint(i), givenConfiguration.getPoint(i), false, 0, 0); distances[i - 1] = line.pathlength(); } // The rating is 50% distance between the start/end point and 50% the average distance between all other points rating = (distance + average(distances)) / 2; } // Makes sure the rating does not go above 100. This could happen if the distances between the other points of the configuration are > 100 but the // Distance between the 0 axles are not. if (rating > 100) rating = 100; // Returns rating, which is either 100 (worst case value) if distance is > 100 or the average of all distances. // Thus 100 is the worst and the lower the number the better return rating; }
// Rates how close the given configuration at the end of the path is to the desired ending configuration public static double rateConfigurationNoMaximum(configuration wantedConfiguration, configuration givenConfiguration) { double distances; double rating; EZPathFollowing.LinePathPart line = null; line = new EZPathFollowing.LinePathPart(wantedConfiguration.getPoint(0), givenConfiguration.getPoint(0), false, 0, 0); distances = line.pathlength(); rating = distances; return rating; }
// Draws the given configuration. public static void drawConfiguration(configuration config) { GL.PointSize(3); GL.Color3(Color.Red); // Iterates over the configuration for (int i = 0; i < Variables.vehicle_size; i++){ // Draws the Lines GL.Begin(BeginMode.Lines); // For the first element the starting point is L since there is no previous M if (i == 0) GL.Vertex2(config.Lx,config.Ly); // Otherwise it's the previous M else GL.Vertex2(config.Mx[i-1],config.My[i-1]); GL.Vertex2(config.Mx[i],config.My[i]); GL.End(); // Draws the points last so they are above the lines GL.Begin(BeginMode.Points); if (i == 0) GL.Vertex2(config.Lx,config.Ly); GL.Vertex2(config.X[i], config.Y[i]); GL.Vertex2(config.Mx[i],config.My[i]); GL.End(); } }
// Calculates an entire configuration from a given startpoint and orientation public static configuration getConfig(EZPathFollowing.Point2D start, double[] orientation) { // New config configuration configuration = new configuration(); // Current X (first axle point) is the given start EZPathFollowing.Point2D X = start; EZPathFollowing.Point2D M; EZPathFollowing.Point2D L; // Iterates over all pathparts for (int i = 0; i < Variables.vehicle_size; i++) { // Writes the axle point to the configuration configuration.X[i] = X.x; configuration.Y[i] = X.y; // Writes the angle to the configuration configuration.Theta[i] = Convert.ToInt32(orientation[i]); // M is a new point to the left of X, distance M[i] M = new EZPathFollowing.Point2D(X.x - Variables.vehicle.M[i], X.y); // Rotates M around X by Theta (clockwise, starting at 9 o'clock) M = EZPathFollowing.Point2D.rotateAround(M, X, configuration.Theta[i]); // Writes M to the configuration configuration.Mx[i] = M.x; configuration.My[i] = M.y; // L only has to be calculated for the first Vehicle Part since otherwise it is the same as M[i-1] if (i == 0) { // L is a new point to the right of X, distance L[i] L = new EZPathFollowing.Point2D(X.x + Variables.vehicle.L[i], X.y); // Rotates L around X by Theta (clockwise, starting at 3 o'clock) L = EZPathFollowing.Point2D.rotateAround(L, X, configuration.Theta[i]); configuration.Lx = L.x; configuration.Ly = L.y; } // If there is a new Vehicle Part, calculate the next X if (i < Variables.vehicle_size - 1) { // X[i+1] is a point to the left of the current M with the distance L[i+1] X = new EZPathFollowing.Point2D(M.x - Variables.vehicle.L[i + 1], M.y); // Rotates X around M by Theta[i+1] (clockwise, starting at 9 o'clock) X = EZPathFollowing.Point2D.rotateAround(X, M, configuration.Theta[i + 1]); } } return configuration; }
// Rates how close the given configuration at the end of the path is to the desired ending configuration public static double rateConfigurationNoMaximum(configuration wantedConfiguration, configuration givenConfiguration) { double distances; double rating; EZPathFollowing.LinePathPart line = null; line = new EZPathFollowing.LinePathPart(wantedConfiguration.getPoint(0), givenConfiguration.getPoint(0), false, 0, 0); distances = line.pathlength(); rating = distances; return(rating); }