pythoncollision-detectionar.drone

I have 2 points (x1,y1) & (x2,y2) and a circle, suppose there is a line between the 2 points I need to check if there is collision with the circle


As I mentioned in the title, suppose I have a line segment from point 1 to point 2 and there is a circle with a center and radius I need to check if there is going to be a collision with the circle using code. This is how far I got. However, there is an issue with closestX and closestY since I need to check if they are on the line segment from point 1 to point 2 because if they are not on the line segment then there will be No collision. Sadly though Im stuck here and I cannot figure out a way to check if they are on the line segment or not. Please help thank you.

import math
p=2
obsHeight=200
DroneHeight=150
cx=3
cy=3
r=1
x1=1
y1=1
x2=1.5
y2=1.5


if DroneHeight<=obsHeight:
    distX= x1 - x2
    distY= y1 - y2
    length=math.sqrt((distX*distX) + (distY*distY ))
    dot= (((cx-x1)*(x2-x1)) + ((cy-y1)*(y2-y1)) )/(math.pow(length,p))
    closestX=x1+( dot * (x2-x1))
    closestY=y1+( dot * (y2-y1))
    print(" Closest x: ",closestX)
    print(" Closest y: ",closestY)
    distX=closestX-cx
    distY= closestY-cy
    distance= math.sqrt((distX*distX) + (distY*distY ))
    print("The distance is: ", distance)
    print("The length is: ", length)
    if (r==distance):
        print("Touching")
    elif (distance<r):
        print("COLLIDING")
    else:
        print("Will not collide")
else:
    print(" Will not collide, the drone is higher than the obstacle")



Solution

  • Ignoring the specificity of your code, let's say that you have a line segment, a center and a radius. Let's write a general solution to whether a line segment in N-dimensions intersects a hyper-sphere in N-dimensions. This will give us the correct solution for your problem in the special case of 2D.

    Your function signature would look like this:

    def intersects(p1, p2, c, r):
    

    p1 and p2 are vectors of length N. In your case, p1 = np.array([1, 1]), and p2 = np.array([1.5, 1.5]). c is a vector of the same length (c = np.array([3, 3])), and r is a scalar radius (r = 1). I strongly recommend using numpy arrays for your math because it is much faster if you use it right, and you can apply element-wise operations to arrays (e.g. p2 - p1) without using a loop.

    A line passing through p1 and p2 can be parametrized as p = p1 + t * (p2 - p1). Every point on the line p corresponds some value of the parameter t. Specifically, t == 0 corresponds to p = p1 and t == 1 corresponds to p = p2. That means that you can know if a point is on the line segment by checking if its parameter is in the range [0, 1].

    The problem then becomes finding the value of t such that p is closest to c. If t < 0 or t > 1, then you know that the extrema for the line segment are at the endpoints. Otherwise, you need to compare the distances of both the endpoints and the p you found.

    There are a couple of different ways of coming up with the solution. The geometric approach uses the fact that the nearest approach happens at the perpendicular from c to the line. The differential approach finds where the derivative of the length is zero. I will show the former here.

    enter image description here

    Looking at the diagram, you have the following equation:

    (c - p).dot(p2 - p1) == 0
    (c - p1 - t * (p2 - p1)).dot(p2 - p1) == 0
    (c - p1).dot(p2 - p1) - t * (p2 - p1).dot(p2 - p1) == 0
    t == (c - p1).dot(p2 - p1) / (p2 - p1).dot(p2 - p1)
    

    You can now write your function like this:

    def intersects(p1, p2, c, r):
        c1 = np.subtract(c, p1)
        c2 = np.subtract(c, p2)
    
        dist1 = np.linalg.norm(c1)
        dist2 = np.linalg.norm(c2)
    
        # If point are on opposite sides of circle, intersects
        if (r - dist1) * (r - dist2) < 0:
            return True
    
        # If both on inside, does not intersect
        if r > dist1:
            return False
    
        dp = np.subtract(p2, p1)
        t = dp.dot(c1) / dp.dot(dp)
    
        # If closest approach is outside segment, does not intersect
        # convince yourself of this (use symmetry about the line c-p)
        if t < 0 or t > 1:
            return False
    
        cp = np.subtract(p1 + t * dp, c)
        distp = np.linalg.norm(cp)
        # only other possibility of approach is when closest point is inside radius
        return distp <= r
    

    The problem of finding the distance between a point and a line has cropped up a number of times on Stack Overflow, and in my applications as well, so I recently added it to a library of utilities that I maintain, haggis. You can build a solution using haggis.math.segment_distance with very similar logic. I specifically made the function operate in line segment or full-line mode for this purpose:

    def intersects(p1, p2, c, r):
        dist1 = np.linalg.norm(c1 := np.subtract(c, p1))
        dist2 = np.linalg.norm(c2 := np.subtract(c, p2))
        if (r - dist1) * (r - dist2) < 0: # Opposite sides of circle
            return True
        if r > dist1:                     # Both inside circle
            return False
        d = segment_distance(c, p1, p2)
        return d < r
    

    You could rewrite the last two lines as follows:

        d, t = segment_distance(c, p1, p2, segment=False, return_t=True)
        return d < r and 0 <= t <= 1