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