private void generateNextLocation(ArrayList borders, ArrayList frontiers, PointF botLocation)
{
float totalX = 0; float totalY = 0; int numberOfPoints = 0;
foreach (OrderedPair p in borders)
{
numberOfPoints++;
totalX += p.toCartesian().X;
totalY += p.toCartesian().Y;
}
float avgOppositeObstacleX = -totalX / numberOfPoints; float avgOppositeObstacleY = totalY / numberOfPoints;
OrderedPair closestFrontier = (OrderedPair)frontiers[0];
foreach (OrderedPair p in frontiers)
{
if (Math.Pow((p.toCartesian().X - avgOppositeObstacleX), 2) + Math.Pow((p.toCartesian().Y - avgOppositeObstacleY), 2) <
Math.Pow((closestFrontier.toCartesian().X - avgOppositeObstacleX), 2) +
Math.Pow((closestFrontier.toCartesian().Y - avgOppositeObstacleY), 2))
{
closestFrontier = p;
}
}
PointF closestLine = new PointF(0, 0);
int closestI = 0;
int closestH = 0;
for (int i = 0; i < 360; i++)
{
bool checkingLine = true;
int h = 1;
while (checkingLine)
{
bool tooClose = false;
foreach (OrderedPair p in borders)
{
if ((Math.Pow((h * Math.Cos(i * Math.PI / 180) - p.toCartesian().X), 2) + Math.Pow((h * Math.Sin(i * Math.PI / 180) - p.toCartesian().Y), 2)) < 10)
{
tooClose = true;
}
}
if (tooClose)
{
checkingLine = false;
}
if ((Math.Pow((h * Math.Cos(i * Math.PI / 180) - closestFrontier.toCartesian().X), 2) +
Math.Pow((h * Math.Sin(i * Math.PI / 180) - closestFrontier.toCartesian().Y), 2)) <
(Math.Pow((closestLine.X - closestFrontier.toCartesian().X), 2) +
Math.Pow((closestLine.Y - closestFrontier.toCartesian().Y), 2)))
{
closestLine = new PointF((float)(h * Math.Cos(i * Math.PI / 180)), (float)(h * Math.Sin(i * Math.PI / 180)));
closestI = i;
closestH = h;
}
}
}
botLocation = new PointF(botLocation.X + closestLine.X, botLocation.Y + closestLine.Y);
sendNextLocation(closestH, closestI);
}
void client_MqttMsgPublishReceived(object sender, MqttMsgPublishEventArgs e)
{
if (e.Topic.Equals("/scanning")) // if this set of data is said to done by MQTT
{
scanning = false; // done scanning for this pass
}
else
{
scanning = true;
string[] obstacle = Encoding.UTF8.GetString(e.Message).Split(','); // split the string from MQTT into polar coordinates
float r = float.Parse(obstacle[0], CultureInfo.InvariantCulture.NumberFormat); // get the distance
float theta = float.Parse(obstacle[0], CultureInfo.InvariantCulture.NumberFormat); // get the angle
int id = (r <= MAX) ? 0 : 1; // obstacle if within range, horizon if out of range
OrderedPair newObstacle = new OrderedPair(r, theta, 0, botLocation); // create new OrderedPair to add to rawData
rawData.Add(newObstacle);
}
}
public Shape(ArrayList points) // PLEASE MAKE SHAPES USING POLAR COORDINATES THEN CALL SHAPE_TO_PLOT AFTER
{
foreach (OrderedPair p in points)
{
perimeter.Add(p);
}
OrderedPair temp;
foreach (OrderedPair p in perimeter)
{
int index = perimeter.IndexOf(p);
if (((OrderedPair)(perimeter[index])).getPoint().Y == ((OrderedPair)(perimeter[index + 1])).getPoint().Y)
{
while (Math.Abs(((OrderedPair)(perimeter[index])).getPoint().X - ((OrderedPair)(perimeter[index + 1])).getPoint().X) != 0)
{
temp = new OrderedPair(((OrderedPair)(perimeter[index + 1])).getPoint().X - 1, ((OrderedPair)(perimeter[index])).getPoint().Y, 1, ((OrderedPair)(perimeter[index])).botLoc); // Create frontier points in polar
perimeter.Insert(index + 1, temp);
}
}
}
}