public DoublePoint[] getHeadingEdges(AForge.DoublePoint robotPosition, AForge.DoublePoint robotHeading) { ControlStrategy.isAtHeading(this, robotPosition, robotHeading); return(headingEdges); }
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; }