public FlightPath BeginFlight()
{
//Start our index at 1 because 0 will always be our origin and we want to fly towards the first node
pathNodeIndex = 1;
path = new FlightPath();
xPosition = xGrid.startX;
yPosition = xGrid.startY;
direction = float.NaN;
prevCollidingEdge = null;
prevCollidingCorner = null;
eliminatedCorners = new List <Grid.Corner>();
//main control loop for a single flight
do
{
//determine the direction in which we should move via our estimated path
CalculateDirection(preComputeFlightPath);
//move in said direction and increment the path node if we have arrived at one
Move(preComputeFlightPath);
//terminate loop if we have reached the last node, i.e. the end
} while (pathNodeIndex < preComputeFlightPath.path.Count());
//add that end node since we didn't get a chance to do it earlier
path.addNode(xGrid.endX, xGrid.endY, true);
return(path);
}
public Drone(float droneRadius, float flightSpeed, Grid.Grid parentGrid)
{
//just setting some local variables, properties, things of that nature
radius = droneRadius; speed = flightSpeed; xPosition = parentGrid.startX; yPosition = parentGrid.startY;
xGrid = parentGrid;
prevDirection = float.NaN;
preComputeFlightPath = new FlightPath();
preComputeFlightPath.addNode(xGrid.startX, xGrid.startY, false);
preComputeFlightPath.addNode(xGrid.endX, xGrid.endY, true);
}
public FlightPath OptimizeFlightPath()
{
FlightPath copyPath = new FlightPath();
foreach (FlightPath.PathNode x in path.path)
{
copyPath.addNode(x.node.X, x.node.Y, x.optimal);
}
float maxDistance = 0;
for (int i = path.path.Count - 1; i > 0; i--)
{
if (!copyPath.path.ElementAt(i).optimal)
{
copyPath.path.RemoveAt(i);
continue;
}
float d = Length(new PointF(copyPath.path.ElementAt(i).node.X - copyPath.path.ElementAt(copyPath.path.Count - 1).node.X, copyPath.path.ElementAt(i).node.Y - copyPath.path.ElementAt(copyPath.path.Count - 1).node.Y));
bool removed = false;
if (d < maxDistance)
{
copyPath.path.RemoveAt(i);
removed = true;
}
maxDistance = d;
if (removed)
{
continue;
}
if (i < path.path.Count - 2)
{
float l1 = Length(new PointF(copyPath.path.ElementAt(i + 1).node.X - copyPath.path.ElementAt(i - 1).node.X, copyPath.path.ElementAt(i + 1).node.Y - copyPath.path.ElementAt(i - 1).node.Y));
float l2 = Length(new PointF(copyPath.path.ElementAt(i).node.X - copyPath.path.ElementAt(i - 1).node.X, copyPath.path.ElementAt(i).node.Y - copyPath.path.ElementAt(i - 1).node.Y));
if (l2 > l1)
{
copyPath.path.RemoveAt(i);
continue;
}
}
}
preComputeFlightPath = copyPath;
return(copyPath);
}
public bool isEqual(FlightPath x)
{
if (x.path.Count != path.Count)
{
return(false);
}
else
{
for (int i = 0; i < path.Count; i++)
{
if (!(Math.Abs(x.path.ElementAt(i).node.X - path.ElementAt(i).node.X) < .1 && Math.Abs(x.path.ElementAt(i).node.Y - path.ElementAt(i).node.Y) < .1 && x.path.ElementAt(i).optimal == path.ElementAt(i).optimal))
{
return(false);
}
}
return(true);
}
}
private void Move(FlightPath estimatedFlightPath)
{
float xNew;
float yNew;
Forecast(out xNew, out yNew);
//see if this step takes us through our current target node
float targetX = estimatedFlightPath.path.ElementAt(pathNodeIndex).node.X;
float targetY = estimatedFlightPath.path.ElementAt(pathNodeIndex).node.Y;
if (hasArrived(targetX, targetY))
{
//increment to next node if yes
pathNodeIndex++;
}
//update our position
xPosition = xNew;
yPosition = yNew;
}
private void CalculateDirection(FlightPath estimatedFlightPath)
{
bool isOptimalDirection = true;
float targetX = estimatedFlightPath.path.ElementAt(pathNodeIndex).node.X;
float targetY = estimatedFlightPath.path.ElementAt(pathNodeIndex).node.Y;
direction = (float)Math.Atan2(targetY - yPosition, targetX - xPosition);
//check if we are hitting any of the rectangles
//since we don't want to actually smack one, we will use a circle size of 5 units larger than our drone radius
//this can probably be implemented with a 'sensor' class later, but this will suffice for now
float collisionRadius = radius + 5;
Grid.Edge collidingEdge = null;
Grid.Corner collidingCorner = null;
//loop through obstructions, break on first edge detection encountered
foreach (Grid.Obstruction rect in xGrid.objList)
{
collidingCorner = circleCornerCollision(xPosition, yPosition, collisionRadius, rect);
collidingEdge = circleEdgeCollision(xPosition, yPosition, collisionRadius, rect);
if (collidingEdge != null || collidingCorner != null)
{
//flight path is no longer optimal, we will remove these points later when trying to optimize
isOptimalDirection = false;
break;
}
}
bool isEliminated = false;
if (collidingCorner != null)
{
foreach (Grid.Corner cor in eliminatedCorners)
{
if (collidingCorner.Vertex.X == cor.Vertex.X && collidingCorner.Vertex.Y == cor.Vertex.Y && collidingCorner.Center.X == cor.Center.X && collidingCorner.Center.Y == cor.Center.Y)
{
isEliminated = true;
break;
}
}
}
if (collidingCorner != null && isEliminated)
{
collidingCorner = null;
if (collidingEdge == null)
{
isOptimalDirection = true;
}
}
if (collidingEdge != null)
{
//we had a collision on an edge, now we have to determine the direction in which we should go along the wall until we reach the end of the obstruction
//if we are still colliding with an edge, previous edge will be populated, which means we must just edge lock and set our previous direction to our new one
if (prevCollidingEdge != null)
{
direction = prevDirection;
}
else
{
//there are two possible angles here, pick the one that will take us closer to the next node
//note: this will not always work! there are cases when this becomes counterintuitive, but for our simple test case this will probably work 95% of the time
//define two points, these will serve as our vectors
//first point is just our direction already, with magnitude 1
PointF V1 = new PointF((float)Math.Cos(direction), (float)Math.Sin(direction));
//second point is our normalized edge
PointF V2 = new PointF(collidingEdge.P2.X - collidingEdge.P1.X, collidingEdge.P2.Y - collidingEdge.P1.Y);
//this is the result of our angle calculation between the two edges, if this angle is smaller than its complement, our direction will be the same
//if not, we will need to reverse the direction of our colliding edge
float resultantAngle = calculateAngle(V1, V2);
float resultantCompliment = (float)Math.PI - resultantAngle;
if (resultantAngle <= resultantCompliment)
{
direction = (float)Math.Atan2(collidingEdge.P2.Y - collidingEdge.P1.Y, collidingEdge.P2.X - collidingEdge.P1.X);
}
else
{
direction = (float)Math.Atan2(collidingEdge.P1.Y - collidingEdge.P2.Y, collidingEdge.P1.X - collidingEdge.P2.X);
}
prevCollidingEdge = collidingEdge;
}
}
else if (collidingCorner != null)
{
if (prevCollidingCorner != null || prevCollidingEdge != null)
{
direction = prevDirection;
}
else
{
//we've hit a corner, set a course to fly straight out to the corner of the obstruction
PointF outVector = new PointF(collidingCorner.Vertex.X - collidingCorner.Center.X, collidingCorner.Vertex.Y - collidingCorner.Center.Y);
direction = calculateAngle(new PointF(1, 0), outVector);
if (outVector.Y < 0)
{
direction = direction * -1;
}
prevCollidingCorner = collidingCorner;
}
}
else
{
prevCollidingEdge = null;
if (prevCollidingCorner != null)
{
eliminatedCorners.Add(prevCollidingCorner);
}
prevCollidingCorner = null;
}
//if direction has changed more than a degree (.017rad = 1deg), add a node to our flight path
if (float.IsNaN(prevDirection) || Math.Abs(direction - prevDirection) > .017)
{
path.addNode(xPosition, yPosition, isOptimalDirection);
}
prevDirection = direction;
}
