@Test void lineSegmentCircleIntersection() { assertEquals( MathUtil.lineSegmentCircleIntersection( new Point(-5, -5), new Point(5, 5), new Point(0, 0), 2), new Point(Math.sqrt(2), Math.sqrt(2))); }
public static Point lineSegmentCircleIntersection(Point ul1, Point ul2, Point o, double radius) { Point l1 = new Point(ul1.x - o.x, ul1.y - o.y); Point l2 = new Point(ul2.x - o.x, ul2.y - o.y); double d_x = l2.x - l1.x; double d_y = l2.y - l1.y; double d_r = Math.hypot(d_x, d_y); double determinant = l1.x * l2.y - l2.x * l1.y; double discriminant = Math.pow(radius, 2) * Math.pow(d_r, 2) - Math.pow(determinant, 2); List<Point> offsets = new LinkedList<>(); if (MathUtil.approxEquals(discriminant, 0)) { offsets.add(new Point(0, 0)); } else if (discriminant > 0) { double x_determinant = sgn(d_y) * d_x * Math.sqrt(discriminant); double y_determinant = Math.abs(d_y) * Math.sqrt(discriminant); offsets.add(new Point(x_determinant, y_determinant)); offsets.add(new Point(-x_determinant, -y_determinant)); } List<Point> intersections = new LinkedList<>(); for (Point offset : offsets) { intersections.add(new Point ( (determinant * d_y + offset.x) / Math.pow(d_r, 2) + o.x, (-determinant * d_x + offset.y) / Math.pow(d_r, 2) + o.y )); } if (intersections.size() == 2 && (intersections.get(0).distance(ul2) > intersections.get(1).distance(ul2))) { intersections.add(intersections.remove(0)); } if (intersections.size() > 0) { return intersections.get(0); } else { return null; } }
MathUtil { public static Point lineSegmentCircleIntersection(Point ul1, Point ul2, Point o, double radius) { Point l1 = new Point(ul1.x - o.x, ul1.y - o.y); Point l2 = new Point(ul2.x - o.x, ul2.y - o.y); double d_x = l2.x - l1.x; double d_y = l2.y - l1.y; double d_r = Math.hypot(d_x, d_y); double determinant = l1.x * l2.y - l2.x * l1.y; double discriminant = Math.pow(radius, 2) * Math.pow(d_r, 2) - Math.pow(determinant, 2); List<Point> offsets = new LinkedList<>(); if (MathUtil.approxEquals(discriminant, 0)) { offsets.add(new Point(0, 0)); } else if (discriminant > 0) { double x_determinant = sgn(d_y) * d_x * Math.sqrt(discriminant); double y_determinant = Math.abs(d_y) * Math.sqrt(discriminant); offsets.add(new Point(x_determinant, y_determinant)); offsets.add(new Point(-x_determinant, -y_determinant)); } List<Point> intersections = new LinkedList<>(); for (Point offset : offsets) { intersections.add(new Point ( (determinant * d_y + offset.x) / Math.pow(d_r, 2) + o.x, (-determinant * d_x + offset.y) / Math.pow(d_r, 2) + o.y )); } if (intersections.size() == 2 && (intersections.get(0).distance(ul2) > intersections.get(1).distance(ul2))) { intersections.add(intersections.remove(0)); } if (intersections.size() > 0) { return intersections.get(0); } else { return null; } } }
MathUtil { public static Point lineSegmentCircleIntersection(Point ul1, Point ul2, Point o, double radius) { Point l1 = new Point(ul1.x - o.x, ul1.y - o.y); Point l2 = new Point(ul2.x - o.x, ul2.y - o.y); double d_x = l2.x - l1.x; double d_y = l2.y - l1.y; double d_r = Math.hypot(d_x, d_y); double determinant = l1.x * l2.y - l2.x * l1.y; double discriminant = Math.pow(radius, 2) * Math.pow(d_r, 2) - Math.pow(determinant, 2); List<Point> offsets = new LinkedList<>(); if (MathUtil.approxEquals(discriminant, 0)) { offsets.add(new Point(0, 0)); } else if (discriminant > 0) { double x_determinant = sgn(d_y) * d_x * Math.sqrt(discriminant); double y_determinant = Math.abs(d_y) * Math.sqrt(discriminant); offsets.add(new Point(x_determinant, y_determinant)); offsets.add(new Point(-x_determinant, -y_determinant)); } List<Point> intersections = new LinkedList<>(); for (Point offset : offsets) { intersections.add(new Point ( (determinant * d_y + offset.x) / Math.pow(d_r, 2) + o.x, (-determinant * d_x + offset.y) / Math.pow(d_r, 2) + o.y )); } if (intersections.size() == 2 && (intersections.get(0).distance(ul2) > intersections.get(1).distance(ul2))) { intersections.add(intersections.remove(0)); } if (intersections.size() > 0) { return intersections.get(0); } else { return null; } }  }
MathUtil { public static Point lineSegmentCircleIntersection(Point ul1, Point ul2, Point o, double radius) { Point l1 = new Point(ul1.x - o.x, ul1.y - o.y); Point l2 = new Point(ul2.x - o.x, ul2.y - o.y); double d_x = l2.x - l1.x; double d_y = l2.y - l1.y; double d_r = Math.hypot(d_x, d_y); double determinant = l1.x * l2.y - l2.x * l1.y; double discriminant = Math.pow(radius, 2) * Math.pow(d_r, 2) - Math.pow(determinant, 2); List<Point> offsets = new LinkedList<>(); if (MathUtil.approxEquals(discriminant, 0)) { offsets.add(new Point(0, 0)); } else if (discriminant > 0) { double x_determinant = sgn(d_y) * d_x * Math.sqrt(discriminant); double y_determinant = Math.abs(d_y) * Math.sqrt(discriminant); offsets.add(new Point(x_determinant, y_determinant)); offsets.add(new Point(-x_determinant, -y_determinant)); } List<Point> intersections = new LinkedList<>(); for (Point offset : offsets) { intersections.add(new Point ( (determinant * d_y + offset.x) / Math.pow(d_r, 2) + o.x, (-determinant * d_x + offset.y) / Math.pow(d_r, 2) + o.y )); } if (intersections.size() == 2 && (intersections.get(0).distance(ul2) > intersections.get(1).distance(ul2))) { intersections.add(intersections.remove(0)); } if (intersections.size() > 0) { return intersections.get(0); } else { return null; } }  static double angleWrap(double angle); static double clamp(double d); static double clampAbove(double d, double threshold); static double cutOffBelow(double d, double threshold); static double powRetainingSign(double d, double power); static double deadZone(double d, double thresh); static Pose relativeOdometryUpdate(Pose fieldPose, Pose robotPoseDelta); static boolean approxEquals(double d1, double d2); static Point lineSegmentCircleIntersection(Point ul1, Point ul2, Point o, double radius); static boolean between(double r1, double r2, double val, double threshold); }
MathUtil { public static Point lineSegmentCircleIntersection(Point ul1, Point ul2, Point o, double radius) { Point l1 = new Point(ul1.x - o.x, ul1.y - o.y); Point l2 = new Point(ul2.x - o.x, ul2.y - o.y); double d_x = l2.x - l1.x; double d_y = l2.y - l1.y; double d_r = Math.hypot(d_x, d_y); double determinant = l1.x * l2.y - l2.x * l1.y; double discriminant = Math.pow(radius, 2) * Math.pow(d_r, 2) - Math.pow(determinant, 2); List<Point> offsets = new LinkedList<>(); if (MathUtil.approxEquals(discriminant, 0)) { offsets.add(new Point(0, 0)); } else if (discriminant > 0) { double x_determinant = sgn(d_y) * d_x * Math.sqrt(discriminant); double y_determinant = Math.abs(d_y) * Math.sqrt(discriminant); offsets.add(new Point(x_determinant, y_determinant)); offsets.add(new Point(-x_determinant, -y_determinant)); } List<Point> intersections = new LinkedList<>(); for (Point offset : offsets) { intersections.add(new Point ( (determinant * d_y + offset.x) / Math.pow(d_r, 2) + o.x, (-determinant * d_x + offset.y) / Math.pow(d_r, 2) + o.y )); } if (intersections.size() == 2 && (intersections.get(0).distance(ul2) > intersections.get(1).distance(ul2))) { intersections.add(intersections.remove(0)); } if (intersections.size() > 0) { return intersections.get(0); } else { return null; } }  static double angleWrap(double angle); static double clamp(double d); static double clampAbove(double d, double threshold); static double cutOffBelow(double d, double threshold); static double powRetainingSign(double d, double power); static double deadZone(double d, double thresh); static Pose relativeOdometryUpdate(Pose fieldPose, Pose robotPoseDelta); static boolean approxEquals(double d1, double d2); static Point lineSegmentCircleIntersection(Point ul1, Point ul2, Point o, double radius); static boolean between(double r1, double r2, double val, double threshold); static final double EPSILON; }