// 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 end configuration. Only measures distance, resemblance
public static double rateDistance()
{
EZPathFollowing.Point2D point1 = Variables.end;
EZPathFollowing.Point2D point2 = new EZPathFollowing.Point2D(Variables.x*27, Variables.y*27);
return (point1 - point2).length();
}
private void button_output_current_ending_position_Click(object sender, EventArgs e)
{
EZPathFollowing.Point2D point1 = Variables.end;
EZPathFollowing.Point2D point2 = new EZPathFollowing.Point2D(Variables.x*27, Variables.y*27);
double blubb = (point1 - point2).length();
MessageBox.Show(blubb.ToString());
// MessageBox.Show(Variables.end.x.ToString() + "," + Variables.end.y.ToString());
}