Circle line-segment collision detection algorithm?

前端 未结 28 1328
被撕碎了的回忆
被撕碎了的回忆 2020-11-22 06:38

I have a line from A to B and a circle positioned at C with the radius R.

\"Image\"

What is a good alg

相关标签:
28条回答
  • 2020-11-22 07:04

    Here is a solution written in golang. The method is similar to some other answers posted here, but not quite the same. It is easy to implement, and has been tested. Here are the steps:

    1. Translate coordinates so that the circle is at the origin.
    2. Express the line segment as parametrized functions of t for both the x and y coordinates. If t is 0, the function's values are one end point of the segment, and if t is 1, the function's values are the other end point.
    3. Solve, if possible, the quadratic equation resulting from constraining values of t that produce x, y coordinates with distances from the origin equal to the circle's radius.
    4. Throw out solutions where t is < 0 or > 1 ( <= 0 or >= 1 for an open segment). Those points are not contained in the segment.
    5. Translate back to original coordinates.

    The values for A, B, and C for the quadratic are derived here, where (n-et) and (m-dt) are the equations for the line's x and y coordinates, respectively. r is the radius of the circle.

    (n-et)(n-et) + (m-dt)(m-dt) = rr
    nn - 2etn + etet + mm - 2mdt + dtdt = rr
    (ee+dd)tt - 2(en + dm)t + nn + mm - rr = 0
    

    Therefore A = ee+dd, B = - 2(en + dm), and C = nn + mm - rr.

    Here is the golang code for the function:

    package geom
    
    import (
        "math"
    )
    
    // SegmentCircleIntersection return points of intersection between a circle and
    // a line segment. The Boolean intersects returns true if one or
    // more solutions exist. If only one solution exists, 
    // x1 == x2 and y1 == y2.
    // s1x and s1y are coordinates for one end point of the segment, and
    // s2x and s2y are coordinates for the other end of the segment.
    // cx and cy are the coordinates of the center of the circle and
    // r is the radius of the circle.
    func SegmentCircleIntersection(s1x, s1y, s2x, s2y, cx, cy, r float64) (x1, y1, x2, y2 float64, intersects bool) {
        // (n-et) and (m-dt) are expressions for the x and y coordinates
        // of a parameterized line in coordinates whose origin is the
        // center of the circle.
        // When t = 0, (n-et) == s1x - cx and (m-dt) == s1y - cy
        // When t = 1, (n-et) == s2x - cx and (m-dt) == s2y - cy.
        n := s2x - cx
        m := s2y - cy
    
        e := s2x - s1x
        d := s2y - s1y
    
        // lineFunc checks if the  t parameter is in the segment and if so
        // calculates the line point in the unshifted coordinates (adds back
        // cx and cy.
        lineFunc := func(t float64) (x, y float64, inBounds bool) {
            inBounds = t >= 0 && t <= 1 // Check bounds on closed segment
            // To check bounds for an open segment use t > 0 && t < 1
            if inBounds { // Calc coords for point in segment
                x = n - e*t + cx
                y = m - d*t + cy
            }
            return
        }
    
        // Since we want the points on the line distance r from the origin,
        // (n-et)(n-et) + (m-dt)(m-dt) = rr.
        // Expanding and collecting terms yeilds the following quadratic equation:
        A, B, C := e*e+d*d, -2*(e*n+m*d), n*n+m*m-r*r
    
        D := B*B - 4*A*C // discriminant of quadratic
        if D < 0 {
            return // No solution
        }
        D = math.Sqrt(D)
    
        var p1In, p2In bool
        x1, y1, p1In = lineFunc((-B + D) / (2 * A)) // First root
        if D == 0.0 {
            intersects = p1In
            x2, y2 = x1, y1
            return // Only possible solution, quadratic has one root.
        }
    
        x2, y2, p2In = lineFunc((-B - D) / (2 * A)) // Second root
    
        intersects = p1In || p2In
        if p1In == false { // Only x2, y2 may be valid solutions
            x1, y1 = x2, y2
        } else if p2In == false { // Only x1, y1 are valid solutions
            x2, y2 = x1, y1
        }
        return
    }
    

    I tested it with this function, which confirms that solution points are within the line segment and on the circle. It makes a test segment and sweeps it around the given circle:

    package geom_test
    
    import (
        "testing"
    
        . "**put your package path here**"
    )
    
    func CheckEpsilon(t *testing.T, v, epsilon float64, message string) {
        if v > epsilon || v < -epsilon {
            t.Error(message, v, epsilon)
            t.FailNow()
        }
    }
    
    func TestSegmentCircleIntersection(t *testing.T) {
        epsilon := 1e-10      // Something smallish
        x1, y1 := 5.0, 2.0    // segment end point 1
        x2, y2 := 50.0, 30.0  // segment end point 2
        cx, cy := 100.0, 90.0 // center of circle
        r := 80.0
    
        segx, segy := x2-x1, y2-y1
    
        testCntr, solutionCntr := 0, 0
    
        for i := -100; i < 100; i++ {
            for j := -100; j < 100; j++ {
                testCntr++
                s1x, s2x := x1+float64(i), x2+float64(i)
                s1y, s2y := y1+float64(j), y2+float64(j)
    
                sc1x, sc1y := s1x-cx, s1y-cy
                seg1Inside := sc1x*sc1x+sc1y*sc1y < r*r
                sc2x, sc2y := s2x-cx, s2y-cy
                seg2Inside := sc2x*sc2x+sc2y*sc2y < r*r
    
                p1x, p1y, p2x, p2y, intersects := SegmentCircleIntersection(s1x, s1y, s2x, s2y, cx, cy, r)
    
                if intersects {
                    solutionCntr++
                    //Check if points are on circle
                    c1x, c1y := p1x-cx, p1y-cy
                    deltaLen1 := (c1x*c1x + c1y*c1y) - r*r
                    CheckEpsilon(t, deltaLen1, epsilon, "p1 not on circle")
    
                    c2x, c2y := p2x-cx, p2y-cy
                    deltaLen2 := (c2x*c2x + c2y*c2y) - r*r
                    CheckEpsilon(t, deltaLen2, epsilon, "p2 not on circle")
    
                    // Check if points are on the line through the line segment
                    // "cross product" of vector from a segment point to the point
                    // and the vector for the segment should be near zero
                    vp1x, vp1y := p1x-s1x, p1y-s1y
                    crossProd1 := vp1x*segy - vp1y*segx
                    CheckEpsilon(t, crossProd1, epsilon, "p1 not on line ")
    
                    vp2x, vp2y := p2x-s1x, p2y-s1y
                    crossProd2 := vp2x*segy - vp2y*segx
                    CheckEpsilon(t, crossProd2, epsilon, "p2 not on line ")
    
                    // Check if point is between points s1 and s2 on line
                    // This means the sign of the dot prod of the segment vector
                    // and point to segment end point vectors are opposite for
                    // either end.
                    wp1x, wp1y := p1x-s2x, p1y-s2y
                    dp1v := vp1x*segx + vp1y*segy
                    dp1w := wp1x*segx + wp1y*segy
                    if (dp1v < 0 && dp1w < 0) || (dp1v > 0 && dp1w > 0) {
                        t.Error("point not contained in segment ", dp1v, dp1w)
                        t.FailNow()
                    }
    
                    wp2x, wp2y := p2x-s2x, p2y-s2y
                    dp2v := vp2x*segx + vp2y*segy
                    dp2w := wp2x*segx + wp2y*segy
                    if (dp2v < 0 && dp2w < 0) || (dp2v > 0 && dp2w > 0) {
                        t.Error("point not contained in segment ", dp2v, dp2w)
                        t.FailNow()
                    }
    
                    if s1x == s2x && s2y == s1y { //Only one solution
                        // Test that one end of the segment is withing the radius of the circle
                        // and one is not
                        if seg1Inside && seg2Inside {
                            t.Error("Only one solution but both line segment ends inside")
                            t.FailNow()
                        }
                        if !seg1Inside && !seg2Inside {
                            t.Error("Only one solution but both line segment ends outside")
                            t.FailNow()
                        }
    
                    }
                } else { // No intersection, check if both points outside or inside
                    if (seg1Inside && !seg2Inside) || (!seg1Inside && seg2Inside) {
                        t.Error("No solution but only one point in radius of circle")
                        t.FailNow()
                    }
                }
            }
        }
        t.Log("Tested ", testCntr, " examples and found ", solutionCntr, " solutions.")
    }
    

    Here is the output of the test:

    === RUN   TestSegmentCircleIntersection
    --- PASS: TestSegmentCircleIntersection (0.00s)
        geom_test.go:105: Tested  40000  examples and found  7343  solutions.
    

    Finally, the method is easily extendable to the case of a ray starting at one point, going through the other and extending to infinity, by only testing if t > 0 or t < 1 but not both.

    0 讨论(0)
  • 2020-11-22 07:06

    Another one in c# (partial Circle class). Tested and works like a charm.

    public class Circle : IEquatable<Circle>
    {
        // ******************************************************************
        // The center of a circle
        private Point _center;
        // The radius of a circle
        private double _radius;
    
       // ******************************************************************
        /// <summary>
        /// Find all intersections (0, 1, 2) of the circle with a line defined by its 2 points.
        /// Using: http://math.stackexchange.com/questions/228841/how-do-i-calculate-the-intersections-of-a-straight-line-and-a-circle
        /// Note: p is the Center.X and q is Center.Y
        /// </summary>
        /// <param name="linePoint1"></param>
        /// <param name="linePoint2"></param>
        /// <returns></returns>
        public List<Point> GetIntersections(Point linePoint1, Point linePoint2)
        {
            List<Point> intersections = new List<Point>();
    
            double dx = linePoint2.X - linePoint1.X;
    
            if (dx.AboutEquals(0)) // Straight vertical line
            {
                if (linePoint1.X.AboutEquals(Center.X - Radius) || linePoint1.X.AboutEquals(Center.X + Radius))
                {
                    Point pt = new Point(linePoint1.X, Center.Y);
                    intersections.Add(pt);
                }
                else if (linePoint1.X > Center.X - Radius && linePoint1.X < Center.X + Radius)
                {
                    double x = linePoint1.X - Center.X;
    
                    Point pt = new Point(linePoint1.X, Center.Y + Math.Sqrt(Radius * Radius - (x * x)));
                    intersections.Add(pt);
    
                    pt = new Point(linePoint1.X, Center.Y - Math.Sqrt(Radius * Radius - (x * x)));
                    intersections.Add(pt);
                }
    
                return intersections;
            }
    
            // Line function (y = mx + b)
            double dy = linePoint2.Y - linePoint1.Y;
            double m = dy / dx;
            double b = linePoint1.Y - m * linePoint1.X;
    
            double A = m * m + 1;
            double B = 2 * (m * b - m * _center.Y - Center.X);
            double C = Center.X * Center.X + Center.Y * Center.Y - Radius * Radius - 2 * b * Center.Y + b * b;
    
            double discriminant = B * B - 4 * A * C;
    
            if (discriminant < 0)
            {
                return intersections; // there is no intersections
            }
    
            if (discriminant.AboutEquals(0)) // Tangeante (touch on 1 point only)
            {
                double x = -B / (2 * A);
                double y = m * x + b;
    
                intersections.Add(new Point(x, y));
            }
            else // Secant (touch on 2 points)
            {
                double x = (-B + Math.Sqrt(discriminant)) / (2 * A);
                double y = m * x + b;
                intersections.Add(new Point(x, y));
    
                x = (-B - Math.Sqrt(discriminant)) / (2 * A);
                y = m * x + b;
                intersections.Add(new Point(x, y));
            }
    
            return intersections;
        }
    
        // ******************************************************************
        // Get the center
        [XmlElement("Center")]
        public Point Center
        {
            get { return _center; }
            set
            {
                _center = value;
            }
        }
    
        // ******************************************************************
        // Get the radius
        [XmlElement]
        public double Radius
        {
            get { return _radius; }
            set { _radius = value; }
        }
    
        //// ******************************************************************
        //[XmlArrayItemAttribute("DoublePoint")]
        //public List<Point> Coordinates
        //{
        //    get { return _coordinates; }
        //}
    
        // ******************************************************************
        // Construct a circle without any specification
        public Circle()
        {
            _center.X = 0;
            _center.Y = 0;
            _radius = 0;
        }
    
        // ******************************************************************
        // Construct a circle without any specification
        public Circle(double radius)
        {
            _center.X = 0;
            _center.Y = 0;
            _radius = radius;
        }
    
        // ******************************************************************
        // Construct a circle with the specified circle
        public Circle(Circle circle)
        {
            _center = circle._center;
            _radius = circle._radius;
        }
    
        // ******************************************************************
        // Construct a circle with the specified center and radius
        public Circle(Point center, double radius)
        {
            _center = center;
            _radius = radius;
        }
    
        // ******************************************************************
        // Construct a circle based on one point
        public Circle(Point center)
        {
            _center = center;
            _radius = 0;
        }
    
        // ******************************************************************
        // Construct a circle based on two points
        public Circle(Point p1, Point p2)
        {
            Circle2Points(p1, p2);
        }
    

    Required:

    using System;
    
    namespace Mathematic
    {
        public static class DoubleExtension
        {
            // ******************************************************************
            // Base on Hans Passant Answer on:
            // http://stackoverflow.com/questions/2411392/double-epsilon-for-equality-greater-than-less-than-less-than-or-equal-to-gre
    
            /// <summary>
            /// Compare two double taking in account the double precision potential error.
            /// Take care: truncation errors accumulate on calculation. More you do, more you should increase the epsilon.
            public static bool AboutEquals(this double value1, double value2)
            {
                if (double.IsPositiveInfinity(value1))
                    return double.IsPositiveInfinity(value2);
    
                if (double.IsNegativeInfinity(value1))
                    return double.IsNegativeInfinity(value2);
    
                if (double.IsNaN(value1))
                    return double.IsNaN(value2);
    
                double epsilon = Math.Max(Math.Abs(value1), Math.Abs(value2)) * 1E-15;
                return Math.Abs(value1 - value2) <= epsilon;
            }
    
            // ******************************************************************
            // Base on Hans Passant Answer on:
            // http://stackoverflow.com/questions/2411392/double-epsilon-for-equality-greater-than-less-than-less-than-or-equal-to-gre
    
            /// <summary>
            /// Compare two double taking in account the double precision potential error.
            /// Take care: truncation errors accumulate on calculation. More you do, more you should increase the epsilon.
            /// You get really better performance when you can determine the contextual epsilon first.
            /// </summary>
            /// <param name="value1"></param>
            /// <param name="value2"></param>
            /// <param name="precalculatedContextualEpsilon"></param>
            /// <returns></returns>
            public static bool AboutEquals(this double value1, double value2, double precalculatedContextualEpsilon)
            {
                if (double.IsPositiveInfinity(value1))
                    return double.IsPositiveInfinity(value2);
    
                if (double.IsNegativeInfinity(value1))
                    return double.IsNegativeInfinity(value2);
    
                if (double.IsNaN(value1))
                    return double.IsNaN(value2);
    
                return Math.Abs(value1 - value2) <= precalculatedContextualEpsilon;
            }
    
            // ******************************************************************
            public static double GetContextualEpsilon(this double biggestPossibleContextualValue)
            {
                return biggestPossibleContextualValue * 1E-15;
            }
    
            // ******************************************************************
            /// <summary>
            /// Mathlab equivalent
            /// </summary>
            /// <param name="dividend"></param>
            /// <param name="divisor"></param>
            /// <returns></returns>
            public static double Mod(this double dividend, double divisor)
            {
                return dividend - System.Math.Floor(dividend / divisor) * divisor;
            }
    
            // ******************************************************************
        }
    }
    
    0 讨论(0)
  • 2020-11-22 07:10

    Maybe there is another way to solve this problem using rotation of coordinate system.

    Normally, if one segment is horizontal or vertical, which means parallel to x or y axis, it's quite easy to solve the intersection point since we already know one coordinate of the intersection, if any. The rest is obviously finding the other coordinate using circle's equation.

    Inspired by this idea, we could apply coordinates system rotation to make one axis's direction coincide with segment's direction.

    Let's take an example of circle x^2+y^2=1 and segment P1-P2 with P1(-1.5,0.5) and P2(-0.5,-0.5) in x-y system. And the following equations to remind you of the rotation principles, where theta is the angle anticlockwise, x'-y' is the system after rotation :

    x' = x * cos(theta) + y * sin(theta)

    y' = - x * sin(theta) + y * cos(theta)

    and inversely

    x = x' * cos(theta) - y' * sin(theta)

    y = x' * sin(theta) + y' * cos(theta)

    Considering the segment P1-P2 direction (45° in terms of -x), we could take theta=45°. Taking the second equations of rotation into circle's equation in x-y system : x^2+y^2=1 and after simple operations we get the 'same' equation in x'-y' system : x'^2+y'^2=1.

    Segment endpoints become in x'-y' system using the first equations of rotation => P1(-sqrt(2)/2, sqrt(2)), P2(-sqrt(2)/2, 0).

    Assuming the intersection as P. We have in x'-y' Px = -sqrt(2)/2. Using the new equation of circle, we get Py = +sqrt(2)/2. Converting P into original x-y system, we get finally P(-1,0).

    To implement this numerically, we could firstly have a look at segment's direction : horizontal, vertical or not. If it belongs to the two first cases, it's simple like I said. If the last case, apply the algorithms above.

    To juge if there is intersection, we could compare the solution with the endpoints coordinates, to see whether there is one root between them.

    I believe this method could be also applied to other curves as long as we have its equation. The only weakness is that we should solve the equation in x'-y' system for the other coordinate, which might be difficult.

    0 讨论(0)
  • 2020-11-22 07:13

    I know it's been a while since this thread was open. From the answer given by chmike and improved by Aqib Mumtaz. They give a good answer but only works for a infinite line as said Aqib. So I add some comparisons to know if the line segment touch the circle, I write it in Python.

    def LineIntersectCircle(c, r, p1, p2):
        #p1 is the first line point
        #p2 is the second line point
        #c is the circle's center
        #r is the circle's radius
    
        p3 = [p1[0]-c[0], p1[1]-c[1]]
        p4 = [p2[0]-c[0], p2[1]-c[1]]
    
        m = (p4[1] - p3[1]) / (p4[0] - p3[0])
        b = p3[1] - m * p3[0]
    
        underRadical = math.pow(r,2)*math.pow(m,2) + math.pow(r,2) - math.pow(b,2)
    
        if (underRadical < 0):
            print("NOT")
        else:
            t1 = (-2*m*b+2*math.sqrt(underRadical)) / (2 * math.pow(m,2) + 2)
            t2 = (-2*m*b-2*math.sqrt(underRadical)) / (2 * math.pow(m,2) + 2)
            i1 = [t1+c[0], m * t1 + b + c[1]]
            i2 = [t2+c[0], m * t2 + b + c[1]]
    
            if p1[0] > p2[0]:                                           #Si el punto 1 es mayor al 2 en X
                if (i1[0] < p1[0]) and (i1[0] > p2[0]):                 #Si el punto iX esta entre 2 y 1 en X
                    if p1[1] > p2[1]:                                   #Si el punto 1 es mayor al 2 en Y
                        if (i1[1] < p1[1]) and (i1[1] > p2[1]):         #Si el punto iy esta entre 2 y 1
                            print("Intersection")
                    if p1[1] < p2[1]:                                   #Si el punto 2 es mayo al 2 en Y
                        if (i1[1] > p1[1]) and (i1[1] < p2[1]):         #Si el punto iy esta entre 1 y 2
                            print("Intersection")
    
            if p1[0] < p2[0]:                                           #Si el punto 2 es mayor al 1 en X
                if (i1[0] > p1[0]) and (i1[0] < p2[0]):                 #Si el punto iX esta entre 1 y 2 en X
                    if p1[1] > p2[1]:                                   #Si el punto 1 es mayor al 2 en Y
                        if (i1[1] < p1[1]) and (i1[1] > p2[1]):         #Si el punto iy esta entre 2 y 1
                            print("Intersection")
                    if p1[1] < p2[1]:                                   #Si el punto 2 es mayo al 2 en Y
                        if (i1[1] > p1[1]) and (i1[1] < p2[1]):         #Si el punto iy esta entre 1 y 2
                            print("Intersection")
    
            if p1[0] > p2[0]:                                           #Si el punto 1 es mayor al 2 en X
                if (i2[0] < p1[0]) and (i2[0] > p2[0]):                 #Si el punto iX esta entre 2 y 1 en X
                    if p1[1] > p2[1]:                                   #Si el punto 1 es mayor al 2 en Y
                        if (i2[1] < p1[1]) and (i2[1] > p2[1]):         #Si el punto iy esta entre 2 y 1
                            print("Intersection")
                    if p1[1] < p2[1]:                                   #Si el punto 2 es mayo al 2 en Y
                        if (i2[1] > p1[1]) and (i2[1] < p2[1]):         #Si el punto iy esta entre 1 y 2
                            print("Intersection")
    
            if p1[0] < p2[0]:                                           #Si el punto 2 es mayor al 1 en X
                if (i2[0] > p1[0]) and (i2[0] < p2[0]):                 #Si el punto iX esta entre 1 y 2 en X
                    if p1[1] > p2[1]:                                   #Si el punto 1 es mayor al 2 en Y
                        if (i2[1] < p1[1]) and (i2[1] > p2[1]):         #Si el punto iy esta entre 2 y 1
                            print("Intersection")
                    if p1[1] < p2[1]:                                   #Si el punto 2 es mayo al 2 en Y
                        if (i2[1] > p1[1]) and (i2[1] < p2[1]):         #Si el punto iy esta entre 1 y 2
                            print("Intersection")
    
    0 讨论(0)
  • 2020-11-22 07:14

    No one seems to consider projection, am I completely off track here?

    Project the vector AC onto AB. The projected vector, AD, gives the new point D.
    If the distance between D and C is smaller than (or equal to) R we have an intersection.

    Like this:
    Image by SchoolBoy

    0 讨论(0)
  • 2020-11-22 07:15

    I just needed that, so I came up with this solution. The language is maxscript, but it should be easily translated to any other language. sideA, sideB and CircleRadius are scalars, the rest of the variables are points as [x,y,z]. I'm assuming z=0 to solve on the plane XY

    fn projectPoint p1 p2 p3 = --project  p1 perpendicular to the line p2-p3
    (
        local v= normalize (p3-p2)
        local p= (p1-p2)
        p2+((dot v p)*v)
    )
    fn findIntersectionLineCircle CircleCenter CircleRadius LineP1 LineP2=
    (
        pp=projectPoint CircleCenter LineP1 LineP2
        sideA=distance pp CircleCenter
        --use pythagoras to solve the third side
        sideB=sqrt(CircleRadius^2-sideA^2) -- this will return NaN if they don't intersect
        IntersectV=normalize (pp-CircleCenter)
        perpV=[IntersectV.y,-IntersectV.x,IntersectV.z]
        --project the point to both sides to find the solutions
        solution1=pp+(sideB*perpV)
        solution2=pp-(sideB*perpV)
        return #(solution1,solution2)
    )
    
    0 讨论(0)
提交回复
热议问题