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; }