public void calculateNextMove(DoublePoint referencePoint, int offset, DoublePoint robotPosition, double speed, DoublePoint heading, List <Robot> neighbors, out double referenceSpeed, out DoublePoint referenceHeading)
{
this.referencePoint = referencePoint;
if (robotPosition != referencePoint)
{
referenceHeading = ControlStrategy.vector2Point(robotPosition, referencePoint);
double distance = robotPosition.DistanceTo(referencePoint);
referenceSpeed = speedRegulator(distance - offset);
}
else
{
referenceSpeed = Program.neutralSpeed;
referenceHeading = heading;
}
}