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?