|
public DistanceTo ( |
||
| anotherPoint | Point to calculate distance to. | |
| return | double | |
public AForge.DoublePoint getNearestPoint(AForge.DoublePoint point)
{
DoublePoint closestPoint = points[0];
double distance2P, minDistance = closestPoint.DistanceTo(point);
foreach (DoublePoint p in points)
{
distance2P = point.DistanceTo(p);
if (distance2P < minDistance)
{
closestPoint = p;
minDistance = closestPoint.DistanceTo(point);
}
}
return(closestPoint);
}
/*
* This function firstly calculates the closest point, drives towards it and finally starts the path.
*/
public override void calculateNextMove(AForge.DoublePoint robotPosition, double speed, AForge.DoublePoint heading, List <Robot> neighbors, out double referenceSpeed, out AForge.DoublePoint referenceHeading)
{
if (!onLine)
{
pointCount = 0;
double distance = robotPosition.DistanceTo(points[pointCount]);
for (int i = 0; i < points.Count(); i++)
{
if (distance > points[i].DistanceTo(robotPosition))
{
pointCount = i;
distance = points[pointCount].DistanceTo(robotPosition);
}
}
if (distance < closeUpLimit)
{
onLine = true;
}
else
{
goToPoint.calculateNextMove(points[pointCount], robotPosition, speed, heading, neighbors, out referenceSpeed, out referenceHeading);
return;
}
}
if (onLine)
{
double distance = robotPosition.DistanceTo(points[pointCount]);
if (distance <= closeUpLimit)
{
if (direction == 1)
{
pointCount = (pointCount >= points.Count() - 1 ? 0 : pointCount + direction);
}
else
{
pointCount = (pointCount >= 1 ? points.Count() - 1 : pointCount + direction);
//pointCount = (pointCount > 1 ? pointCount - 1 : points.Count() - 1); //Shouldn't it be this?
}
}
else
{
onLine = false;
}
goToPoint.calculateNextMove(points[pointCount], robotPosition, speed, heading, neighbors, out referenceSpeed, out referenceHeading);
if (fifoStrategy != null)
{
fifoStrategy.calculateNextMove(robotPosition, referenceSpeed, referenceHeading, neighbors, out referenceSpeed, out referenceHeading);
}
return;
}
//referenceSpeed = speed; //uncomment if using m3pi robots
//$$$$$Changes/Additions made for RC cars$$$$$//
referenceSpeed = Program.testSpeed;
//$$$$$$$$$$//
referenceHeading = heading;
}