public static void genomeToPath(Genome pathGenome)
{
// Initialize Path List
//LinkedList<EZPathFollowing.PathPart> output = new LinkedList<EZPathFollowing.PathPart>();
// Resets the global path
Variables.resetPath();
// Resets the gloabl genome
Variables.resetGenome();
// Initialize Variables
double length; // Length is a number n from 0 to 7. 0.5 + n * 0.5 is the length of a Pathpart
double angle; // Angle a is a number from 0 to 7, 10 + a * 5 is the angle in DEGREE
bool driveRight;
double direction = (360 - Variables.configuration_start.Theta[0]) * Math.PI / 180;
EZPathFollowing.Point2D start = Variables.start;
// Iterate over all 20 GenomeParts
for (int i = 0; i < 20; i++)
{
// Length is saved in Bits 1,2 and 3 and is required for both PathParts
length = GenomePart.getDouble(pathGenome.genome.Get(i * 8 + 1), pathGenome.genome.Get(i * 8 + 2), pathGenome.genome.Get(i * 8 + 3));
length = 0.5 + length * 0.5;
// Bit 0 says whether its a curve or line
if (pathGenome.genome.Get(i * 8) == false)
{
// The first Pathpart needs a start and direction
if (i == 0)
{
Variables.path.AddLast(EZPathFollowing.PathPrimitives.line(length, direction, start));
}
else
{
Variables.path.AddLast(EZPathFollowing.PathPrimitives.line(length));
}
}
else
{
// Angle and driveRight are only necessary for curves (Bit 0 = true)
angle = GenomePart.getDouble(pathGenome.genome.Get(i * 8 + 4), pathGenome.genome.Get(i * 8 + 5), pathGenome.genome.Get(i * 8 + 6));
angle = (10 + angle * 5) * Math.PI / 180;
driveRight = pathGenome.genome.Get(i * 8 + 7);
// Again, first PathPart needs a start and direction
if (i == 0)
{
Variables.path.AddLast(EZPathFollowing.PathPrimitives.curve(length, direction, angle, driveRight, start));
}
else
{
Variables.path.AddLast(EZPathFollowing.PathPrimitives.curve(length, angle, driveRight));
}
}
}
}
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());
}
// 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);
}
private double getAlpha(double x, double y, double orientation, double L0)
{
x *= 27;
y *= 27;
L0 *= 27;
double KV = 27;
double test = path.First.Value.pathlength();
double test2 = path.First.Value.referencePositionDefinitionValue(new EZPathFollowing.Point2D(x, y));
// remove old path parts
while (path.Count > 0 && path.First.Value.referencePositionDefinitionValue(new EZPathFollowing.Point2D(x, y)) > path.First.Value.pathlength())
{
path.RemoveFirst();
}
if (path.Count > 0)
{
// there are parts left
// berechne Treffpunkt
double distanceleft = KV + path.First.Value.referencePositionDefinitionValue(new EZPathFollowing.Point2D(x, y));
EZPathFollowing.PathPart thisPart = path.ElementAt(0);
for (int i = 0; i < path.Count - 1; i++)
{
double thislength = thisPart.pathlength();
if (distanceleft > thislength)
{
distanceleft -= thislength;
thisPart = path.ElementAt(i + 1);
}
else
{
break;
}
}
EZPathFollowing.Point2D hitpoint = thisPart.position(distanceleft);
//System.Windows.Forms.MessageBox.Show(distanceleft.ToString()+" "+hitpoint.x.ToString() + ", " + hitpoint.y.ToString());
if (Math.Sin(orientation) * (x - hitpoint.x) == Math.Cos(orientation) * (y - hitpoint.y))
{
return(0.0);
}
else
{
double radius = ((x - hitpoint.x) * (x - hitpoint.x) + (y - hitpoint.y) * (y - hitpoint.y)) / 2 / (-Math.Sin(orientation) * (x - hitpoint.x) + Math.Cos(orientation) * (y - hitpoint.y));
return(-Math.Atan2(L0, radius));
}
}
return(40.0 * Math.PI / 180.0);
}
// 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());
}
public CirclePathPart(Point2D startpoint, Point2D endpoint, Point2D center, bool driveRight, bool reverse, double speed, double angle, double direction)
: base(startpoint, endpoint, reverse, speed, direction)
{
setAttributes(startpoint, endpoint, center, driveRight, reverse, speed, angle, direction);
}
public override bool move(Point2D difference)
{
base.move(difference);
m_center = Point2D.add(m_center, difference);
return(true);
}