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