public DistanceTo ( |
||
anotherPoint | Point to calculate distance to. | |
Результат | 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; }