CODEWITHSUNDEEP
pythonnumpyvector
Matthie456
Matthie456

Reputation: 81

Line-Line intersection in Python with numpy

I have a relatively simple question, I know the answer but I can't seem to find the right implementation using Python and Numpy. The idea is, I have two lines and I need to find the virtual intersection point (I used the example from https://www.youtube.com/watch?v=kCyoaidiXAU&t=313s).

Both lines have the form of r = r0 + t*V, with r0 as position vector (a point that the line passes through), t a variable and V, the direction vector. Direction vector V can be found simply by finding the vector through both points of the line, e.g. V = A - B.

With that, it's possible to formulate the line as:
L1 = r0 (known point) + t (unknown variable) * V (direction vector)

Now, I can easily find t manually, but I have no clue how to tell Python.. I tried numpy.linalg.solve, but that gives me a matrix, while I need a single value.

For example:

# Line A and Line B that intersect somewhere
A = LineString([(4., 0.), (4., -3.)])
B = LineString([(6., 2.), (10., 2.)])

# direction vectors for line A and B
v1 = (A[0].x - A[1].x, A[0].y, A[1].y) # --> [0,3]
v2 = (B[0].x - B[1].x, B[0].y, B[1].y) # --> [-4,0]

L1 = A[1] + x * v1
L2 = B[1] + y * v2

Now, I would solve this manually by solving L1 for x:

L1 = [4, 0] + x * [0, 3] = [4, 3x]
L2 = [6, 2] + y * [-4, 0] = [6-4y, 2]

# finding intersection point by solving L1 = L2
4 = 6-4y  &  3x = 2
y = 1/2   &   x = 2/3

But I have no idea how to tell numpy/python how to solve for x and y.

Any help or guide in the right direction would be much appreciated.

Upvotes: 5

Views: 11504

Answers (3)

xmduhan
xmduhan

Reputation: 1025

import numpy as np

data = np.array([
    #  segment1               segment2
    # [[x1, y1], [x2, y2]],  [[x1, y1], [x2, y2]]
    [(4., 0.), (4., -3.), (6., 2.), (10., 2.)],
])

def intersect(data):
    L = len(data)
    x1, y1, x2, y2 = data.reshape(L * 2, -1).T
    R = np.full([L, 2], np.nan)
    X = np.concatenate([
        (y2 - y1).reshape(L * 2, -1), 
        (x1 - x2).reshape(L * 2, -1)], 
        axis=1
    ).reshape(L, 2, 2)
    B = (x1 * y2 - x2 * y1).reshape(L, 2)
    I = np.isfinite(np.linalg.cond(X))
    R[I] = np.matmul(np.linalg.inv(X[I]), B[I][:,:,None]).squeeze(-1)
    return R

intersect(data)

array([[4., 2.]])

Upvotes: 0

john k
john k

Reputation: 6615

Here is a function I wrote to find the closest point between two 3d lines

import scipy.optimize
#takes in two lines, the line formed by pt1 and pt2, and the line formed by pt3 and pt4, and finds their intersection or closest point
def fourptsMeetat(pt1,pt2,pt3,pt4):
    #least squares method
    def errFunc(estimates):
        s, t = estimates
        x = pt1 + s * (pt2 - pt1) - (pt3 + t * (pt4 - pt3))
        return x

    estimates = [1, 1]

    sols = scipy.optimize.least_squares(errFunc, estimates)
    s,t = sols.x

    x1 =  pt1[0] + s * (pt2[0] - pt1[0])
    x2 =  pt3[0] + t * (pt4[0] - pt3[0])
    y1 =  pt1[1] + s * (pt2[1] - pt1[1])
    y2 =  pt3[1] + t * (pt4[1] - pt3[1])
    z1 =  pt1[2] + s * (pt2[2] - pt1[2])
    z2 = pt3[2] + t * (pt4[2] - pt3[2])

    x = (x1 + x2) / 2  #halfway point if they don't match
    y = (y1 + y2) / 2  # halfway point if they don't match
    z = (z1 + z2) / 2  # halfway point if they don't match

    return (x,y,z)

Upvotes: 0

user6655984
user6655984

Reputation:

The line through A0 and A1 has parametric equation (1-t)*A0 + t*A1, where t is the parameter. The line through B0 and B1 has parametric equation (1-s)*A0 + s*A1, where s is the parameter. Setting these equal, we get the system (A1-A0)t + (B0-B1)s == B0-A0. So, the right hand side is B0-A0 and the matrix has columns A1-A0 and B0-B1. The system can be solved with np.linalg.solve. Complete example:

A = np.array([[4, 0], [4, -3]])
B = np.array([[6, 2], [10, 2]])
t, s = np.linalg.solve(np.array([A[1]-A[0], B[0]-B[1]]).T, B[0]-A[0])
print((1-t)*A[0] + t*A[1])
print((1-s)*B[0] + s*B[1])

Both print commands output [4., 2.] confirming the correctness. (The second print it really redundant.)

Upvotes: 8

Related Questions