I looked at this code:
import numpy as np
from matplotlib import pyplot as plt
def dipole(m, r, r0):
"""
Calculation of field B in point r. B is created by a dipole moment m located in r0.
"""
# R = r - r0 - subtraction of elements of vectors r and r0, transposition of array
R = np.subtract(np.transpose(r), r0).T
# Spatial components of r are the outermost axis
norm_R = np.sqrt(np.einsum("i...,i...", R, R)) # einsum - Einsteinova sumace
# Dot product of R and m
m_dot_R = np.tensordot(m, R, axes=1)
# Computation of B
B = 3 * m_dot_R * R / norm_R**5 - np.tensordot(m, 1 / norm_R**3, axes=0)
B *= 1e-7 # abbreviation for B = B * 1e-7, multiplication B of 1e-7, permeability of vacuum: 4\pi * 10^(-7)
# The result is the magnetic field B
return B
X = np.linspace(-1, 1)
Y = np.linspace(-1, 1)
Bx, By = dipole(m=[0, 1], r=np.meshgrid(X, Y), r0=[-0.2,0.8])
plt.figure(figsize=(8, 8))
plt.streamplot(X, Y, Bx, By)
plt.margins(0, 0)
plt.show()
It shows the following figure:
Is it possible to get coordinates of one line of force? I don't understand how it is plotted.
The streamplot returns a container object 'StreamplotSet' with two parts:
lines: a LineCollection of the streamlines
arrows: a PatchCollection containing FancyArrowPatch objects (these are the triangular arrows)
c.lines.get_paths() gives all the segments. Iterating through these segments, their vertices can be examined. When a segment starts where the previous ended, both belong to the same curve. Note that each segment is a short straight line; many segments are used together to form a streamline curve.
The code below demonstrates iterating through the segments. To show what's happening, each segment is converted to an array of 2D points suitable for plt.plot. Default, plt.plot colors each curve with a new color (repeating every 10). The dots show where each of the short straight segments are located.
To find one particular curve, you could hover with the mouse over the starting point, and note the x coordinate of that point. And then test for that coordinate in the code. As an example, the curve that starts near x=0.48 is drawn in a special way.
import numpy as np
from matplotlib import pyplot as plt
from matplotlib import patches
def dipole(m, r, r0):
R = np.subtract(np.transpose(r), r0).T
norm_R = np.sqrt(np.einsum("i...,i...", R, R))
m_dot_R = np.tensordot(m, R, axes=1)
B = 3 * m_dot_R * R / norm_R**5 - np.tensordot(m, 1 / norm_R**3, axes=0)
B *= 1e-7
return B
X = np.linspace(-1, 1)
Y = np.linspace(-1, 1)
Bx, By = dipole(m=[0, 1], r=np.meshgrid(X, Y), r0=[-0.2,0.8])
plt.figure(figsize=(8, 8))
c = plt.streamplot(X, Y, Bx, By)
c.lines.set_visible(False)
paths = c.lines.get_paths()
prev_end = None
start_indices = []
for index, segment in enumerate(paths):
if not np.array_equal(prev_end, segment.vertices[0]): # new segment
start_indices.append(index)
prev_end = segment.vertices[-1]
for i0, i1 in zip(start_indices, start_indices[1:] + [len(paths)]):
# get all the points of the curve that starts at index i0
curve = np.array([paths[i].vertices[0] for i in range(i0, i1)] + [paths[i1 - 1].vertices[-1]])
special_x_coord = 0.48
for i0, i1 in zip(start_indices, start_indices[1:] + [len(paths)]):
# get all the points of the curve that starts at index i0
curve = np.array([paths[i].vertices[0] for i in range(i0, i1)] + [paths[i1 - 1].vertices[-1]])
if abs(curve[0,0] - special_x_coord) < 0.01: # draw one curve in a special way
plt.plot(curve[:, 0], curve[:, 1], '-', lw=10, alpha=0.3)
else:
plt.plot(curve[:, 0], curve[:, 1], '.', ls='-')
plt.margins(0, 0)
plt.show()
Related
For a problem, I implemented the Nagumo equation via Crank-Nicolson's scheme. Now the problem is that I plotted the truncation error vs dt but the graph should give me a line with a slope of about 62° while the other graph should give a line with a negative slope (angle of 117°).
With the code below I get the following graphs:
# -*- coding: utf-8 -*-
"""
Created on Sun Feb 5 13:21:29 2023
#author: theha
"""
import numpy as np
#import itertools
import numpy.linalg as l
import matplotlib.pyplot as plt
def generateMatrix(N, sigma):
""" Computes the matrix for the Nagumo's equation with Crank-Nicolson
Dirichlet condition at i=0 and at i=1
Parameters:
----------
N: int
Number of discretization points
sigma: float
dt/(2*dx^2)
Returns:
-------
A: 2D numpy array of float
Matrix for Nagumo's equation
"""
# Setup the diagonal
d = np.diag(np.ones(N+1)*(2*r+1))
# Setup upper diagonal
ud = np.diag(np.ones(N)*-r, 1)
# Setup lower diagonal
ld = np.diag(np.ones(N)*-r, -1)
A = d + ud + ld
return A
def generateRHS(u1, sigma,alpha):
""" Computes right-hand side of linear system for Nagumo's equation
with Crank-Nicolson scheme
Parameters:
----------
T: array of float
Nagumo's solution at current time step
sigma: float
dt/(2*dx^2)
Returns:
-------
b: array of float
Right-hand side of Nagumo's equation
with Crank-Nicolson scheme
"""
N=len(u1)
F=((1-2*sigma)*u1)+sigma*((np.append(0,u1[0:N-1]))+np.append(u1[1:N+1],1))+ dt* ((u1)*(1-u1)*(u1-alpha))
F[0]=0
F[-1]=1
return F
def CrankNicolson(T, A, nt, sigma,alpha):
""" Nagumo's equation in time with Crank-Nicolson
Parameters:
----------
T: array of float
initial Nagumo profile
A: 2D array of float
Matrix with discretized Nagumo equation
nt: int
number of time steps
sigma: float
dt/(2*(dx^2))
Returns:
-------
T: array of floats
Nagumo profile after nt time steps
"""
for t in range(nt):
Tn = T.copy()
b = generateRHS(Tn, sigma,alpha)
# Use numpy.linalg.solve
T_interior = np.linalg.solve(A,b)
T = T_interior
return T
#Domain x in [-L,L]
L = 100
nx = 400 # Partition in x
alpha = 0.25
dx = 2*(L/nx) # Step size
dt= .00001 #Time step
r = dt/(2*(dx**2))
sigma = r
nt = 5 # Partition in time
x = np.linspace(-L,L,nx+1)
u0=np.zeros(nx)
t=0
gamma1 = np.sqrt(2)/2*x + (0.5-alpha)*t
gamma2 = (np.sqrt(2)/2*alpha*x) + alpha*(alpha-2)*t/2
u0=(np.exp(gamma1)+alpha*np.exp(gamma2))/(np.exp(gamma1)+np.exp(gamma2)+1) #Initial condition u(x,0)
#initial time step
Ti = u0;
A = generateMatrix(nx, sigma) # A matrix
T = CrankNicolson(Ti.copy(), A, nt, sigma, alpha) #Solution of system Ax=b
def T_analytical(x, t,n_max, alpha):
"""Computes the exact solution for Nagumo's equation
Paramters:
---------
x : array of float
Spatial position
t : float
Evaluation time
n_max: int
Number of terms to evaluate expression
alpha: float
r coefficient of A matrix associated
L : float
Size of rod
Returns:
-------
T : array of float
u(x,t) at each location x
"""
#T = 100
for n in range(1,n_max+1):
gamma1=(np.sqrt(2)*(x))/(2) + (0.5- alpha)*(t)
gamma2=(np.sqrt(2)*(alpha*x))/2 + (alpha*(alpha-2))*((t)/2)
ue=(np.exp(gamma1)+(alpha*np.exp(gamma2)))/(np.exp(gamma1)+np.exp(gamma2)+1)
T=ue
return T
"Graph of approximate solution and exact solution"
T_exact = T_analytical(x, dt*nt, 100, alpha)
fig=plt.figure()
plt.plot(x,T,'x-', label='Aproximada',color='#003366')
plt.plot(x,T_exact,'c|-' ,label='Solucion exacta',color='red')
plt.xlabel('x (espacio)', fontsize = 12)
plt.ylabel('u', fontsize = 12)
plt.xticks(fontsize = 12)
plt.yticks(fontsize = 12)
plt.axis([-L,L,0,1])
plt.grid(True)
plt.legend()
plt.show()
def L2_error(T, T_exact):
"""Computes L2 norm of error
Parameters:
----------
T : array of float
array with numerical solution
T_exact: array of float
array with exact solution
Returns:
-------
e: L2 norm of error
"""
e = l.norm(T_exact-T)
return e
"Calculation of the error in time"
nx = 5
t_final = 1
t_initial = 0
dt_values = np.asanyarray([2.0000e-01,4.0000e-01,6.0000e-01,8.0000e-01,1.0000]) #Values of dt
error = np.zeros(len(dt_values)) #error's array
x = np.linspace(-L,L,nx+1) #Discretization in space
Ti = T_analytical(x, t_initial, 100, alpha) #Simulation of Initial condition, u(x,0) = u0(x)
T_exact = T_analytical(x, t_final, 100, alpha) #Simulation of analytical solution
"Loop for the error calculation in time"
for i,dt in enumerate(dt_values):
#print(i, dt)
sigma = dt/(2*(dx**2))
nt = int((t_final-t_initial)/dt)
A = generateMatrix(nx, sigma)
T = CrankNicolson(Ti.copy(), A, nt, sigma,alpha)
error[i] = L2_error(T,T_exact)
"Plot of error vs dt on logarithmic scale"
plt.figure(figsize=(8,8))
plt.xlabel(r'$\Delta t$', fontsize=18)
plt.ylabel(r'norma $L_2$ del error', fontsize=18)
plt.axis('equal')
plt.loglog(dt_values, error, color='k', ls='--', lw=2, marker='o')
plt.xticks(dt_values,dt_values)
plt.grid(True)
plt.legend(['Crank-Nicolson']);
#loop=nx
#rate =np.log(error[1:loop-1]/error[2:loop])/np.log(dt_values[1:loop-1]/dt_values[2:loop])
"Loop for the error calculation in space"
dx_values = np.asanyarray([0.5,0.25,0.2,0.125,0.0625]) #Values of dx
#dx_values = np.asanyarray([0.0625,0.125,0.2,0.25,0.5]) #Values of dx
error_x = np.zeros(len(dx_values)) #error's array
nt = 5
dt = 0.01
for i,dx in enumerate(dx_values):
sigma = dt/(2*(dx_values[i]**2))
nx = int((2*L)/dx_values[i])
#int((t_final-t_initial)/dt)
x =np.linspace(-L,L,nx+1)
Ti = T_analytical(x, t_initial, nx+1, alpha) #Simulation of Initial condition, u(x,0) = u0(x)
T_exact = T_analytical(x, t_final, nx+1, alpha) #Simulation of analytical solution
A = generateMatrix(nx, sigma)
T = CrankNicolson(Ti.copy(), A, nt, sigma,alpha)
error_x[i] = round(l.norm(T_exact - T), 2)
error_x
"Plot of error vs dx on logarithmic scale"
plt.figure(figsize=(8,8))
plt.xlabel(r'$\Delta x$', fontsize=18)
plt.ylabel(r'norma $L_2$ del error', fontsize=18)
plt.axis('equal')
plt.loglog(dx_values,error_x , color='k', ls='--', lw=2, marker='o')
plt.xticks(np.round(dx_values,2),np.round(dx_values,2))
plt.grid(True)
plt.legend(['Crank-Nicolson']);
The graphs that I got
I would like someone to tell me what the error is in the propagation or if indeed the results I obtained are correct.
I have one hermitian matrix (specifically, a Hamiltonian). Though phase of a singe eigenvector can be arbitrary, the quantities I am calculating is physical (I reduced the code a bit keeping just the reproducible part). eig and eigh are giving very different results.
import numpy as np
import numpy.linalg as nlg
import matplotlib.pyplot as plt
def Ham(Ny, Nx, t, phi):
h = np.zeros((Ny,Ny), dtype=complex)
for ii in range(Ny-1):
h[ii+1,ii] = t
h[Ny-1,0] = t
h=h+np.transpose(np.conj(h))
u = np.zeros((Ny,Ny), dtype=complex)
for ii in range(Ny):
u[ii,ii] = -t*np.exp(-2*np.pi*1j*phi*ii)
u = u + 1e-10*np.eye(Ny)
H = np.kron(np.eye(Nx,dtype=int),h) + np.kron(np.diag(np.ones(Nx-1), 1),u) + np.kron(np.diag(np.ones(Nx-1), -1),np.transpose(np.conj(u)))
H[0:Ny,Ny*(Nx-1):Ny*Nx] = np.transpose(np.conj(u))
H[Ny*(Nx-1):Ny*Nx,0:Ny] = u
x=[]; y=[];
for jj in range (1,Nx+1):
for ii in range (1,Ny+1):
x.append(jj); y.append(ii)
x = np.asarray(x)
y = np.asarray(y)
return H, x, y
def C_num(Nx, Ny, E, t, phi):
H, x, y = Ham(Ny, Nx, t, phi)
ifhermitian = np.allclose(H, np.transpose(np.conj(H)), rtol=1e-5, atol=1e-8)
assert ifhermitian == True
Hp = H
V,wf = nlg.eigh(Hp) ##Check. eig gives different result
idx = np.argsort(np.real(V))
wf = wf[:, idx]
normmat = wf*np.conj(wf)
norm = np.sqrt(np.sum(normmat, axis=0))
wf = wf/(norm*np.sqrt(len(H)))
wf = wf[:, V<=E] ##Chose a subset of eigenvectors
V01 = wf*np.exp(1j*x)[:,None]; V12 = wf*np.exp(1j*y)[:,None]
V23 = wf*np.exp(1j*x)[:,None]; V30 = wf*np.exp(1j*y)[:,None]
wff = np.transpose(np.conj(wf))
C01 = np.dot(wff,V01); C12 = np.dot(wff,V12); C23 = np.dot(wff,V23); C30 = np.dot(wff,V30)
F = nlg.multi_dot([C01,C12,C23,C30])
ifhermitian = np.allclose(F, np.transpose(np.conj(F)), rtol=1e-5, atol=1e-8)
assert ifhermitian == True
evals, efuns = nlg.eig(F) ##Check eig gives different result
C = (1/(2*np.pi))*np.sum(np.angle(evals));
return C
C = C_num(16, 16, 0, 1, 1/8)
print(C)
Changing both nlg.eigh to nlg.eig, or even changing only the last one, giving very different results.
As I mentioned elsewhere, the eigenvalue and eigenvector are not unique.
The only thing that is true is that for each eigenvalue $A v = lambda v$, the two matrices returned by eig and eigh describe those solutions, it is natural that eig inexact but approximate results.
You can see that both the solutions will triangularize your matrix in different ways
H, x, y = Ham(16, 16, 1, 1./8)
D, V = nlg.eig(H)
Dh, Vh = nlg.eigh(H)
Then
import matplotlib.pyplot as plt
plt.figure(figsize=(14, 7))
plt.subplot(121);
plt.imshow(abs(np.conj(Vh.T) # H # Vh))
plt.title('diagonalized with eigh')
plt.subplot(122);
plt.imshow(abs(np.conj(V.T) # H # V))
plt.title('diagonalized with eig')
Plots this
That both diagonalizations were successfull, but the eigenvalues are indifferent order.
If you sort the eigenvalues you see they match
plt.plot(np.diag(np.real(np.conj(Vh.T) # H # Vh)))
plt.plot(np.diag(np.imag(np.conj(Vh.T) # H # Vh)))
plt.plot(np.sort(np.diag(np.real(np.conj(V.T) # H # V))))
plt.title('eigenvalues')
plt.legend(['real eigh', 'imag eigh', 'sorted real eig'], loc='upper left')
Since many eigenvalues are repeated, the eigenvector associated with a given eigenvalue is not unique as well, the only thing we can guarantee is that the eigenvectors for a given eigenvalue must span the same subspace.
The diagonalization test is the best in my opinion.
Is eigh always better than eig?
If you search for the eigenvalues in the lapack routines you will have many options. So it is I cannot discuss each possible implementation here. The common sense says that we can expect that the symmetric/hermitian routines to perform better, otherwise ther would be no reason to add one more routine that is more limited. But I never tested carefully the behavior of eig vs eigh.
To have an intuition compare the equation for tridiagonalization for symmetric matrices, and the equation for reduction of a general matrix to its Heisenberg form found here.
I have two sets of data. One is nominal form. The other is actual form. The problem is that when I wish to calculate the form error alone. It's a big problem when the two sets of data isn't "on top of each other". That gives errors that also include positional error.
Both curves are read from a series of data. The nominal shape (black) is made up from many different size radius that are tangent to each other. Its the leading edge of an airfoil profile.
I have tried various methods of "Best-Fit" I've found both here and on where ever google took me. But the problem is that they all smooth my "actual" data. So it get modified and is not keeping it's actual form.
Is there any function in scipy or any other python lib that "simply" can fit my two curves together without altering the actual shape?
I wish for the green curve with red dots to lie as much as possible on top of the black.
Might it be possible to calculate the center of gravity of both curves and then move the actual curve in x and y depending on the value difference from the center point? It might not be the ultimate solution, but it would get closer?
Here is a solution assuming that the nominal form can be described as a conic, i.a as solution of the equation ax^2 + by^2 + cxy + dx + ey = 1. Then, a least square fit can be applied to find the coefficients (a, b, c, d, e).
import numpy as np
import matplotlib.pylab as plt
# Generate example data
t = np.linspace(-2, 2.5, 25)
e, theta = 0.5, 0.3 # ratio minor axis/major & orientation angle major axis
c, s = np.cos(theta), np.sin(theta)
x = c*np.cos(t) - s*e*np.sin(t)
y = s*np.cos(t) + c*e*np.sin(t)
# add noise:
xy = 4*np.vstack((x, y))
xy += .08 *np.random.randn(*xy.shape) + np.random.randn(2, 1)
# Least square fit by a generic conic equation
# a*x^2 + b*y^2 + c*x*y + d*x + e*y = 1
x, y = xy
x = x - x.mean()
y = y - y.mean()
M = np.vstack([x**2, y**2, x*y, x, y]).T
b = np.ones_like(x)
# solve M*w = b
w, res, rank, s = np.linalg.lstsq(M, b, rcond=None)
a, b, c, d, e = w
# Get x, y coordinates for the fitted ellipse:
# using polar coordinates
# x = r*cos(theta), y = r*sin(theta)
# for a given theta, the radius is obtained with the 2nd order eq.:
# (a*ct^2 + b*st^2 + c*cs*st)*r^2 + (d*ct + e*st)*r - 1 = 0
# with ct = cos(theta) and st = sin(theta)
theta = np.linspace(-np.pi, np.pi, 97)
ct, st = np.cos(theta), np.sin(theta)
A = a*ct**2 + b*st**2 + c*ct*st
B = d*ct + e*st
D = B**2 + 4*A
radius = (-B + np.sqrt(D))/2/A
# Graph
plt.plot(radius*ct, radius*st, '-k', label='fitted ellipse');
plt.plot(x, y, 'or', label='measured points');
plt.axis('equal'); plt.legend();
plt.xlabel('x'); plt.ylabel('y');
I use binocular camera to reconstruct points in 3d from 2d picture,I took many pictures by binocular camera and reconstructed points(feature points have been found already),but I found that the 3d models I reconstructed are not in a same coordinate.
I don't know the extrinsic params(by the way,I wonder how to get this params,because I got the intrinsic matrix from calibration already)
so, I compute the E matrix(8 points algorithm) and assume project matrix P1 of camera1 is P[I|0] and calculate P2 by P1 and E
the last step is to calculate the points in 3d by triangulation.
Code:
def compute_normalized_image_to_image_matrix(p1, p2, compute_essential=False):
""" Computes the fundamental or essential matrix from corresponding points
using the normalized 8 point algorithm.
:input p1, p2: corresponding points with shape 3 x n
:returns: fundamental or essential matrix with shape 3 x 3
"""
n = p1.shape[1]
if p2.shape[1] != n:
raise ValueError('Number of points do not match.')
# preprocess image coordinates
p1n, T1 = scale_and_translate_points(p1)
p2n, T2 = scale_and_translate_points(p2)
# compute F or E with the coordinates
F = compute_image_to_image_matrix(p1n, p2n, compute_essential)
# reverse preprocessing of coordinates
# We know that P1' E P2 = 0
F = np.dot(T1.T, np.dot(F, T2))
return F / F[2, 2]
def compute_fundamental_normalized(p1, p2):
return compute_normalized_image_to_image_matrix(p1, p2)
def compute_essential_normalized(p1, p2):
return compute_normalized_image_to_image_matrix(p1, p2, compute_essential=True)
def scale_and_translate_points(points):
""" Scale and translate image points so that centroid of the points
are at the origin and avg distance to the origin is equal to sqrt(2).
:param points: array of homogenous point (3 x n)
:returns: array of same input shape and its normalization matrix
"""
x = points[0]
y = points[1]
center = points.mean(axis=1) # mean of each row
cx = x - center[0] # center the points
cy = y - center[1]
dist = np.sqrt(np.power(cx, 2) + np.power(cy, 2))
scale = np.sqrt(2) / dist.mean()
norm3d = np.array([
[scale, 0, -scale * center[0]],
[0, scale, -scale * center[1]],
[0, 0, 1]
])
return np.dot(norm3d, points), norm3d
def compute_P_from_fundamental(F):
""" Compute the second camera matrix (assuming P1 = [I 0])
from a fundamental matrix.
"""
e = compute_epipole(F.T) # left epipole
Te = skew(e)
return np.vstack((np.dot(Te, F.T).T, e)).T
def compute_P_from_essential(E):
""" Compute the second camera matrix (assuming P1 = [I 0])
from an essential matrix. E = [t]R
:returns: list of 4 possible camera matrices.
"""
U, S, V = np.linalg.svd(E)
# Ensure rotation matrix are right-handed with positive determinant
if np.linalg.det(np.dot(U, V)) < 0:
V = -V
# create 4 possible camera matrices (Hartley p 258)
W = np.array([[0, -1, 0], [1, 0, 0], [0, 0, 1]])
P2s = [np.vstack((np.dot(U, np.dot(W, V)).T, U[:, 2])).T,
np.vstack((np.dot(U, np.dot(W, V)).T, -U[:, 2])).T,
np.vstack((np.dot(U, np.dot(W.T, V)).T, U[:, 2])).T,
np.vstack((np.dot(U, np.dot(W.T, V)).T, -U[:, 2])).T]
return P2s
def linear_triangulation(p1, p2, m1, m2):
"""
Linear triangulation (Hartley ch 12.2 pg 312) to find the 3D point X
where p1 = m1 * X and p2 = m2 * X. Solve AX = 0.
:param p1, p2: 2D points in homo. or catesian coordinates. Shape (2 x n)
:param m1, m2: Camera matrices associated with p1 and p2. Shape (3 x 4)
:returns: 4 x n homogenous 3d triangulated points
"""
num_points = p1.shape[1]
res = np.ones((4, num_points))
for i in range(num_points):
A = np.asarray([
(p1[0, i] * m1[2, :] - m1[0, :]),
(p1[1, i] * m1[2, :] - m1[1, :]),
(p2[0, i] * m2[2, :] - m2[0, :]),
(p2[1, i] * m2[2, :] - m2[1, :])
])
_, _, V = np.linalg.svd(A)
X = V[-1, :]
res[:, i] = X / X[3]
return res
so how can I solve this? I want all my reconstructed points to be in a same coordinate system,could you please tell me?thank you very much!
Given a line segment, that is two points (x1,y1) and (x2,y2), one point P(x,y) and an angle theta. How do we find if this line segment and the line ray that emanates from P at an angle theta from horizontal intersects or not? If they do intersect, how to find the point of intersection?
Let's label the points q = (x1, y1) and q + s = (x2, y2). Hence s = (x2 − x1, y2 − y1). Then the problem looks like this:
Let r = (cos θ, sin θ). Then any point on the ray through p is representable as p + t r (for a scalar parameter 0 ≤ t) and any point on the line segment is representable as q + u s (for a scalar parameter 0 ≤ u ≤ 1).
The two lines intersect if we can find t and u such that p + t r = q + u s:
See this answer for how to find this point (or determine that there is no such point).
Then your line segment intersects the ray if 0 ≤ t and 0 ≤ u ≤ 1.
Here is a C# code for the algorithm given in other answers:
/// <summary>
/// Returns the distance from the ray origin to the intersection point or null if there is no intersection.
/// </summary>
public double? GetRayToLineSegmentIntersection(Point rayOrigin, Vector rayDirection, Point point1, Point point2)
{
var v1 = rayOrigin - point1;
var v2 = point2 - point1;
var v3 = new Vector(-rayDirection.Y, rayDirection.X);
var dot = v2 * v3;
if (Math.Abs(dot) < 0.000001)
return null;
var t1 = Vector.CrossProduct(v2, v1) / dot;
var t2 = (v1 * v3) / dot;
if (t1 >= 0.0 && (t2 >= 0.0 && t2 <= 1.0))
return t1;
return null;
}
Thanks Gareth for a great answer. Here is the solution implemented in Python. Feel free to remove the tests and just copy paste the actual function. I have followed the write-up of the methods that appeared here, https://rootllama.wordpress.com/2014/06/20/ray-line-segment-intersection-test-in-2d/.
import numpy as np
def magnitude(vector):
return np.sqrt(np.dot(np.array(vector),np.array(vector)))
def norm(vector):
return np.array(vector)/magnitude(np.array(vector))
def lineRayIntersectionPoint(rayOrigin, rayDirection, point1, point2):
"""
>>> # Line segment
>>> z1 = (0,0)
>>> z2 = (10, 10)
>>>
>>> # Test ray 1 -- intersecting ray
>>> r = (0, 5)
>>> d = norm((1,0))
>>> len(lineRayIntersectionPoint(r,d,z1,z2)) == 1
True
>>> # Test ray 2 -- intersecting ray
>>> r = (5, 0)
>>> d = norm((0,1))
>>> len(lineRayIntersectionPoint(r,d,z1,z2)) == 1
True
>>> # Test ray 3 -- intersecting perpendicular ray
>>> r0 = (0,10)
>>> r1 = (10,0)
>>> d = norm(np.array(r1)-np.array(r0))
>>> len(lineRayIntersectionPoint(r0,d,z1,z2)) == 1
True
>>> # Test ray 4 -- intersecting perpendicular ray
>>> r0 = (0, 10)
>>> r1 = (10, 0)
>>> d = norm(np.array(r0)-np.array(r1))
>>> len(lineRayIntersectionPoint(r1,d,z1,z2)) == 1
True
>>> # Test ray 5 -- non intersecting anti-parallel ray
>>> r = (-2, 0)
>>> d = norm(np.array(z1)-np.array(z2))
>>> len(lineRayIntersectionPoint(r,d,z1,z2)) == 0
True
>>> # Test ray 6 --intersecting perpendicular ray
>>> r = (-2, 0)
>>> d = norm(np.array(z1)-np.array(z2))
>>> len(lineRayIntersectionPoint(r,d,z1,z2)) == 0
True
"""
# Convert to numpy arrays
rayOrigin = np.array(rayOrigin, dtype=np.float)
rayDirection = np.array(norm(rayDirection), dtype=np.float)
point1 = np.array(point1, dtype=np.float)
point2 = np.array(point2, dtype=np.float)
# Ray-Line Segment Intersection Test in 2D
# http://bit.ly/1CoxdrG
v1 = rayOrigin - point1
v2 = point2 - point1
v3 = np.array([-rayDirection[1], rayDirection[0]])
t1 = np.cross(v2, v1) / np.dot(v2, v3)
t2 = np.dot(v1, v3) / np.dot(v2, v3)
if t1 >= 0.0 and t2 >= 0.0 and t2 <= 1.0:
return [rayOrigin + t1 * rayDirection]
return []
if __name__ == "__main__":
import doctest
doctest.testmod()
Note: this solution works without making vector classes or defining vector multiplication/division, but is longer to implement. It also avoids division by zero errors. If you just want a block of code and don’t care about the derivation, scroll to the bottom of the post.
Let’s say we have a ray defined by x, y, and theta, and a line defined by x1, y1, x2, and y2.
First, let’s draw two rays that point from the ray’s origin to the ends of the line segment. In pseudocode, that’s
theta1 = atan2(y1-y, x1-x);
theta2 = atan2(y2-y, x2-x);
Next we check whether the ray is inside these two new rays. They all have the same origin, so we only have to check the angles.
To make this easier, let’s shift all the angles so theta1 is on the x axis, then put everything back into a range of -pi to pi. In pseudocode that’s
dtheta = theta2-theta1; //this is where theta2 ends up after shifting
ntheta = theta-theta1; //this is where the ray ends up after shifting
dtheta = atan2(sin(dtheta), cos(dtheta))
ntheta = atan2(sin(ntheta), cos(ntheta))
(Note: Taking the atan2 of the sin and cos of the angle just resets the range of the angle to within -pi and pi without changing the angle.)
Now imagine drawing a line from theta2’s new location (dtheta) to theta1’s new location (0 radians). That’s where the line segment ended up.
The only time where the ray intersects the line segment is when theta is between theta1 and theta2, which is the same as when ntheta is between dtheta and 0 radians. Here is the corresponding pseudocode:
sign(ntheta)==sign(dtheta)&&
abs(ntheta)<=abs(dtheta)
This will tell you if the two lines intersect. Here it is in one pseudocode block:
theta1=atan2(y1-y, x1-x);
theta2=atan2(y2-y, x2-x);
dtheta=theta2-theta1;
ntheta=theta-theta1;
dtheta=atan2(sin(dtheta), cos(dtheta))
ntheta=atan2(sin(ntheta), cos(ntheta))
return (sign(ntheta)==sign(dtheta)&&
abs(ntheta)<=abs(dtheta));
Now that we know the points intersect, we need to find the point of intersection. We’ll be working from a completely clean slate here, so we can ignore any code up to this part. To find the point of intersection, you can use the following system of equations and solve for xp and yp, where lb and rb are the y-intercepts of the line segment and the ray, respectively.
y1=(y2-y1)/(x2-x1)*x1+lb
yp=(y2-y1)/(x2-x1)*xp+lb
y=sin(theta)/cos(theta)*x+rb
yp=sin(theta)/cos(theta)*x+rb
This yields the following formulas for xp and yp:
xp=(y1-(y2-y1)/(x2-x1)*x1-y+sin(theta)/cos(theta)*x)/(sin(theta)/cos(theta)-(y2-y1)/(x2-x1));
yp=sin(theta)/cos(theta)*xp+y-sin(theta)/cos(theta)*x
Which can be shortened by using lm=(y2-y1)/(x2-x1) and rm=sin(theta)/cos(theta)
xp=(y1-lm*x1-y+rm*x)/(rm-lm);
yp=rm*xp+y-rm*x;
You can avoid division by zero errors by checking if either the line or the ray is vertical then replacing every x and y with each other. Here’s the corresponding pseudocode:
if(x2-x1==0||cos(theta)==0){
let rm=cos(theta)/sin(theta);
let lm=(x2-x1)/(y2-y1);
yp=(x1-lm*y1-x+rm*y)/(rm-lm);
xp=rm*yp+x-rm*y;
}else{
let rm=sin(theta)/cos(theta);
let lm=(y2-y1)/(x2-x1);
xp=(y1-lm*x1-y+rm*x)/(rm-lm);
yp=rm*xp+y-rm*x;
}
TL;DR:
bool intersects(x1, y1, x2, y2, x, y, theta){
theta1=atan2(y1-y, x1-x);
theta2=atan2(y2-y, x2-x);
dtheta=theta2-theta1;
ntheta=theta-theta1;
dtheta=atan2(sin(dtheta), cos(dtheta))
ntheta=atan2(sin(ntheta), cos(ntheta))
return (sign(ntheta)==sign(dtheta)&&abs(ntheta)<=abs(dtheta));
}
point intersection(x1, y1, x2, y2, x, y, theta){
let xp, yp;
if(x2-x1==0||cos(theta)==0){
let rm=cos(theta)/sin(theta);
let lm=(x2-x1)/(y2-y1);
yp=(x1-lm*y1-x+rm*y)/(rm-lm);
xp=rm*yp+x-rm*y;
}else{
let rm=sin(theta)/cos(theta);
let lm=(y2-y1)/(x2-x1);
xp=(y1-lm*x1-y+rm*x)/(rm-lm);
yp=rm*xp+y-rm*x;
}
return (xp, yp);
}