Exemple #1
0
        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));
                    }
                }
            }
        }
Exemple #2
0
        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);
        }
Exemple #4
0
        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);
        }
Exemple #5
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());
 }
Exemple #6
0
 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);
 }
Exemple #7
0
 public override bool move(Point2D difference)
 {
     base.move(difference);
     m_center = Point2D.add(m_center, difference);
     return(true);
 }