Esempio n. 1
0
 public DoublePoint[] getHeadingEdges(AForge.DoublePoint robotPosition, AForge.DoublePoint robotHeading)
 {
     ControlStrategy.isAtHeading(this, robotPosition, robotHeading);
     return(headingEdges);
 }
Esempio n. 2
0
        public void calculateNextMove(CollisionArea collisionArea, AForge.DoublePoint robotPosition, double speed, AForge.DoublePoint heading, List <Robot> neighbors, out double referenceSpeed, out AForge.DoublePoint referenceHeading)
        {
            bool go = true;

            if (ControlStrategy.isAtHeading(collisionArea, robotPosition, heading))
            {
                int distance = collisionArea.distance2Middle(robotPosition);

                if (queue.Count == 0)
                {
                    foreach (Robot neighbor in neighbors)
                    {
                        if (ControlStrategy.isAtHeading(collisionArea, neighbor.getPosition(), neighbor.getHeading()))
                        {
                            if (distance > collisionArea.distance2Robot(neighbor))
                            {
                                queue.Add(neighbor.getID());
                                go = false;
                            }
                        }
                    }
                }
                else
                {
                    foreach (Robot neighbor in neighbors)
                    {
                        if (ControlStrategy.isAtHeading(collisionArea, neighbor.getPosition(), neighbor.getHeading()))
                        {
                            foreach (int ID in queue)
                            {
                                if (ID == neighbor.getID())
                                {
                                    go = false;
                                    break;
                                }
                            }
                        }
                    }
                }
                foreach (Robot neighbor in neighbors)
                {
                    if (collisionArea.intersects(neighbor.getPosition()))
                    {
                        go = false;
                        break;
                    }
                }
            }
            else
            {
                queue.Clear();
            }

            if (go)
            {
                referenceSpeed   = speed;
                referenceHeading = heading;
            }
            else
            {
                stopStrategy.calculateNextMove(collisionArea.getNearestPoint(robotPosition), Program.robotRadius, robotPosition, speed, heading, neighbors, out referenceSpeed, out referenceHeading);
            }

            corner1            = collisionArea.getHeadingEdges(robotPosition, heading)[0];
            corner2            = collisionArea.getHeadingEdges(robotPosition, heading)[1];
            this.robotPosition = robotPosition;
        }