Line segment to circle collision algorithm

Question

I've written a function (in Python 3) which computes if a line segment (constraint) and a circle (body) collide, and returns the point of intersection (closest point to the centre of the circle):

def constraintCollide(self, constraint):

    p1 = constraint.point1 # Sets the first point object of the constraint to p1
    p2 = constraint.point2 # Sets the second point object of the constraint to p2

    if self.p.distance_to(p1.p) - p1.p.distance_to(p2.p) > self.r \
    or self.p.distance_to(p2.p) - p1.p.distance_to(p2.p) > self.r:
        return False 
    ''' Checks if difference in distance from the radius to a point and the length of the line segment is greater than the radius of the circle.
        If this is so, then the line segment should be nowhere near intercepting and the function should return false'''

    #Calculating the gradient m of the line segment:
    if p2.p.x - p1.p.x == 0: # In case of no difference in x coordinates,
        m = float('inf') # Gradient is infinity
    elif p2.p.y - p1.p.y == 0: # In as of no difference in y coordinates,
        m = 0 # Gradient is zero
    else: # Otherwise,
        m = (p2.p.y - p1.p.y)/(p2.p.x - p1.p.x) # Normal gradient calculation

    # Calculating the point of interception (if any):
    if p1.p.distance_to(self.p) <= self.r: # Checks if relative distance from point 1 is less that the radius,
        i = p1.p # In with case, 'intercection' is at the edge of the line segment,
    elif  p2.p.distance_to(self.p) <= self.r: # And likewise for point 2
        i = p2.p # "
    else: # Otherwise, solve quadratic equation for the point of intersection 
        a = self.p.x; b = self.p.y; c = p1.p.y -m*p1.p.x; r = self.r;
        # Defines centre of circle as (a, b), y-intercept of 'line' as c, and the radius as r
        x1 = (-(-a**2*m**2+2*a*b*m-2*a*c*m-b**2+2*b*c-c**2+m**2*r**2+r**2)**0.5 +a+b*m-c*m)/(m**2+1)
        x2 = ((-a**2*m**2+2*a*b*m-2*a*c*m-b**2+2*b*c-c**2+m**2*r**2+r**2)**0.5 +a+b*m-c*m)/(m**2+1)
        y1 = (-m*(-a**2*m**2+2*a*b*m-2*a*c*m-b**2+2*b*c-c**2+m**2*r**2+r**2)**0.5+a*m+b*m**2+c)/(m**2+1)
        y2 = (m*(-a**2*m**2+2*a*b*m-2*a*c*m-b**2+2*b*c-c**2+m**2*r**2+r**2)**0.5+a*m+b*m**2+c)/(m**2+1)
        ''' As this is a quadratic, there is a possibility of two real roots (points of interception), which 
            is why the x and y are computed twice to accommodate the (+/-) sign in a quadratic'''

        x = (x1.real+x2.real)/2 # Average x coordinate of the real solutions (As python 3 returns **.5 as complex number)
        y = (y1.real+y2.real)/2 # Average y coordinates of the real solutions ''
        i = pygame.math.Vector2(x1.real, y1.real) # Stores 'the point of intersection' as a pygame vector object 
        # In actuality, i is not  the point of intersection, but the 'closest point to the centre' (I hope)

    if i.distance_to(self.p) > self.r: # Checks if the closest point to the circle is larger than the circle radius,
        return False, # In which case the function returns false

    # Checks if the combined distance from the closest point i to each point is longer than the line segment,
    if (i-p1.p).length() + (i-p2.p).length() > p1.p.distance_to(p2.p): 
        return False # In which case return false

    # Line segment has passed all tests so, 
    return i # Returns i

The function surprisingly works as expected, with a success rate of, I'd say, 99%. The problem is that it seems very expensive for its purpose. My guess for the cause would be the solving of the quadratic with the lengthy formula:

$$(x-a)^2 + (y-b)^2 = r^2,\qquad y=mx+c$$

for which the solutions are:

$$\begin{align} x &= \frac{-\sqrt{-a^2m^2 + 2abm - 2acm - b^2 + 2bc - c^2 + m^2r^2 + r^2} + a + bm - cm}{m^2 + 1} \\ y &= \frac{-m\sqrt{-a^2m^2 + 2abm - 2acm - b^2 + 2bc - c^2 + m^2r^2 + r^2} + am + bm^2 + c}{m^2 + 1} \end{align}$$

and

$$\begin{align} x &= \frac{a^2 + b^2 - 2bc + c^2 -r^2}{2(a + mb - mc)} \\ y &= \frac{-a^2 + 2iac - b^2 + c^2 + r^2}{2(ma - b + c)} \end{align}$$

Would there be a way to optimize this algorithm, or a different approach altogether with the ability to solve for two points of interception of a circle of known radius and a line segment of known endpoints?

Answer 1

In computer geometry, always use vectors if possible! Code gets more complicated if you try to work with Cartesian co-ordinates \$ (x, y) \$ or with line equations \$ y = mx + b \$. Here, for example, you have special cases for horizontal lines, \$ m = 0 \$, and vertical lines, \$ m = \infty \$.

So let's try to program this, sticking to vectors throughout.

First, let's review the problem. We have a line segment from p1.p to p2.p and we want to find the points of intersection with a circle centred at self.p and radius self.r. I'm going to write these as \$ p_1 \$, \$ p_2 \$, \$ q \$, and \$ r \$ respectively:

Any point on the line segment can be written \$ p_1 + t(p_2 - p_1) \$ for a scalar parameter \$ t \$ between 0 and 1. We'll be using \$ p_2 - p_1 \$ often, so let's write \$ v = p_2 - p_1 \$.

Let's set this up in Python. I'm assuming that all the points are pygame.math.Vector2 objects, so that we can add them and take dot products and so on. I'm also assuming that we're using Python 3, so that division returns a float. If you're using Python 2, then you'll need:

from __future__ import division

I'm going to use capital letters for vectors and lower case for scalars:

Q = self.p                  # Centre of circle
r = self.r                  # Radius of circle
P1 = constraint.point1      # Start of line segment
V = constraint.point2 - P1  # Vector along line segment

Now, a point \$ x \$ is on the circle if its distance from the centre of the circle is equal to the circle's radius, that is, if $$ \lvert x - q \rvert = r. $$ So the line intersects the circle when $$ \lvert p_1 + tv - q \rvert = r. $$ Squaring both sides gives $$ \lvert p_1 + tv - q \rvert^2 = r^2, $$ and now we can use a property of the dot product (namely \$ \lvert A \rvert^2 = A·A \$ for any vector \$ A \$) to get $$ (p_1 + tv - q)·(p_1 + tv - q) = r^2. $$ Expanding the dot product and collecting powers of \$ t \$ gives $$ t^2(v·v)+2t(v·(p_1 − q)) + (p_1·p_1 + q·q − 2p_1·q − r^2) = 0 $$ which is a quadratic equation in \$ t \$ with coefficients

$$\begin{array}{rl} a &= v·v \\ b &= 2(v·(p_1 − q)) \\ c &= p_1·p_1 + q·q − 2p_1·q − r^2\end{array}$$

and solutions $$ t = { −b ± \sqrt{b^2 − 4ac} \over 2a }. $$ Let's compute the coefficients in Python:

a = V.dot(V)
b = 2 * V.dot(P1 - Q)
c = P1.dot(P1) + Q.dot(Q) - 2 * P1.dot(Q) - r**2

The value \$ b^2 − 4ac \$ inside the square root is known as the discriminant. If this is negative, then there are no real solutions to the quadratic equation; that means that the line misses the circle entirely.

disc = b**2 - 4 * a * c
if disc < 0:
    return False, None

Otherwise, let's call the two solutions \$ t_1 \$ and \$ t_2 \$.

sqrt_disc = math.sqrt(disc)
t1 = (-b + sqrt_disc) / (2 * a)
t2 = (-b - sqrt_disc) / (2 * a)

If neither of these is between 0 and 1, then the line segment misses the circle (but would hit it if extended):

if not (0 <= t1 <= 1 or 0 <= t2 <= 1):
    return False, None

It's not clear to me from your code exactly what you want to return in the case where there is an intersection, but it looks as if you want the closest point on the line segment to the centre of the circle. (Can you explain the geometric significance of this?)

Now, the closest point on the extended line to the centre of the circle is \$ p_1 + tv \$ where $$ t = { (q - p_1)·v \over \lvert v \rvert^2 } = { -b \over 2a }. $$ See Wikipedia for an explanation. But we want to ensure that the point is on the line segment, so we must clamp \$ t \$ to lie between 0 and 1.

t = max(0, min(1, - b / (2 * a)))
return True, P1 + t * V

Notes

1. I've changed the return statements so that instead of sometimes returning False and sometimes returning a point of intersection, the function always returns a tuple whose first element is a Boolean indicating whether there was an intersection, and whose second element is the point of intersection. When a function always returns the same kind of data, it's less likely that the caller will make a mistake in handling it.

2. I haven't tested any of the code in this answer (since I don't have a version of PyGame that supports pygame.math.Vector2). So there might be a bug or two. But here's a JavaScript demo I wrote using the technique described here.