public bool OnCircle(CoordD coord)
{
if (Radius - 1 < GetDistance(coord, Center) && GetDistance(coord, Center) < Radius)
{
return(true);;
}
return(false);
}
public static bool Intersects(Circle circle1, Circle circle2, out CoordD[] intersections)
{
double a1 = circle1.Center.x;
double b1 = circle1.Center.y;
double a2 = circle2.Center.x;
double b2 = circle2.Center.y;
double r1 = circle1.Radius;
double r2 = circle2.Radius;
intersections = new CoordD[2];
double distance = Math.Sqrt(Math.Pow(a1 - a2, 2) + Math.Pow(b1 - b2, 2));
if (distance > r1 + r2)
{
return(false);
}
if (distance < Math.Abs(r1 - r2))
{
return(false);
}
if (distance == 0 && (r2 == r1))
{
return(false);
}
//http://csharphelper.com/blog/2014/09/determine-where-two-circles-intersect-in-c/
double a = ((r1 * r1) - (r2 * r2) + (distance * distance)) / (2 * distance);
double b = ((r2 * r2) - (r1 * r1) + (distance * distance)) / (2 * distance);
double h = Math.Sqrt((r1 * r1) - (a * a));
double centerX = a1 + a * (a2 - a1) / distance;
double centerY = b1 + a * (b2 - b1) / distance;
double x1 = centerX + h * (b2 - b1) / distance;
double y1 = centerY - h * (a2 - a1) / distance;
double x2 = centerX - h * (b2 - b1) / distance;
double y2 = centerY + h * (a2 - a1) / distance;
CoordD intersection1 = new CoordD((x1), (y1));
CoordD intersection2 = new CoordD(x2, y2);
intersections[0] = intersection1;
intersections[1] = intersection2;
return(true);
}
public double GetDistance(Coord coord1, Coord coord2)
{
CoordD centreOfCircle1 = new CoordD((coord1.x - (turnRadius * Math.Cos(coord1.heading))), (coord1.y + (turnRadius * Math.Sin(coord1.heading))));
CoordD centreOfCircle2 = new CoordD((coord1.x + (turnRadius * Math.Cos(coord1.heading))), (coord1.y - (turnRadius * Math.Sin(coord1.heading))));
double lengthOfTangent1 = Math.Sqrt(Math.Pow(GetEuclidDistance(centreOfCircle1, coord2), 2) - (turnRadius * turnRadius));
Circle turnCircle1 = new Circle(centreOfCircle1, turnRadius);
CoordD midpoint = new CoordD(coord2.x + 0.5 * (turnCircle1.Center.x - coord2.x), (coord2.y + 0.5 * (turnCircle1.Center.y - coord2.y)));
Circle constructedCircle1 = new Circle(midpoint, GetEuclidDistance(midpoint, centreOfCircle1));
CoordD[] interestions1;
if (!Circle.Intersects(turnCircle1, constructedCircle1, out interestions1))
{
return(double.PositiveInfinity);
}
double gradient1_1 = (centreOfCircle1.x - interestions1[0].x) / (interestions1[0].y - centreOfCircle1.y);
double gradient1_2 = (centreOfCircle1.x - interestions1[1].x) / (interestions1[1].y - centreOfCircle1.y);
double arcLength1_1 = turnRadius * (Math.PI - coord1.heading - Math.Atan(1 / gradient1_1));
double arcLength1_2 = turnRadius * (Math.PI - coord1.heading - Math.Atan(1 / gradient1_2));
double candidate1 = lengthOfTangent1 + Math.Max(arcLength1_1, arcLength1_2);
double lengthOfTangent2 = Math.Sqrt(Math.Pow(GetEuclidDistance(centreOfCircle2, coord2), 2) - (turnRadius * turnRadius));
Circle turnCircle2 = new Circle(centreOfCircle2, turnRadius);
CoordD midpoint2 = new CoordD(coord2.x + 0.5 * (turnCircle2.Center.x - coord2.x), (coord2.y + 0.5 * (turnCircle2.Center.y - coord2.y)));
Circle constructedCircle2 = new Circle(midpoint2, GetEuclidDistance(midpoint2, centreOfCircle2));
CoordD[] interestions2;
if (!Circle.Intersects(turnCircle2, constructedCircle2, out interestions2))
{
return(double.PositiveInfinity);
}
double gradient2_1 = (centreOfCircle2.x - interestions2[0].x) / (interestions2[0].y - centreOfCircle2.y);
double gradient2_2 = (centreOfCircle2.x - interestions2[1].x) / (interestions2[1].y - centreOfCircle2.y);
double arcLength2_1 = turnRadius * (Math.PI - coord1.heading - Math.Atan(1 / gradient1_1));
double arcLength2_2 = turnRadius * (Math.PI - coord1.heading - Math.Atan(1 / gradient1_2));
double candidate2 = lengthOfTangent2 + Math.Max(arcLength2_1, arcLength2_2);
return(Math.Min(candidate1, candidate2));
}
private bool Goal(Coord coord1, Coord coord2)
{
CoordD currentPos = coord1;
CoordD change = new CoordD(coord2.x - coord1.x, coord2.y - coord1.y) / GetDistance(coord1, coord2);
for (int i = 0; i <= GetDistance(coord1, coord2); i++)
{
currentPos = coord1 + change * i;
Coord integerCoord = currentPos;
if (integerCoord.x >= 500 || integerCoord.x < 0 || integerCoord.y >= 500 || integerCoord.y < 0)
{
return(false);
}
bool x = (obsitcleMap[integerCoord.x, integerCoord.y] == 1);
if (x == true)
{
return(true);
}
}
return(false);
}
private double GetEuclidDistance(CoordD coord1, CoordD coord2)
{
return(Math.Sqrt(Math.Pow(coord1.x - coord2.x, 2) + Math.Pow(coord1.y - coord2.y, 2)));
}
public Circle(CoordD center, double radius)
{
Center = center;
Radius = radius;
}