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