I am in the early stages of creating a program to plot orbits using the Runge-Kutta method, and would like to plot the orbit in 2D, however, no matter what the initial conditions are, i get a straight line. I have seen a similar question but it didn't solve my problem. Why is this happening?
import numpy as np
import matplotlib.pyplot as mpl
def derX(vx):
return vx
def derY(vy):
return vy
def derVx(x,y):
return -(G*M*x)/((x**2 + y**2)**(3/2))
def timestep(x,k1,k2,k3,k4):
return x + (step/6)*(k1 + 2*k2 +2*k3 + k4)
G=6.67408E-11 #m^3/kg s^2
M=5.972E24 #kg, mass of Earth
step=100 #seconds
x=4596194 #initial conditions in m and m/s
y=4596194
vx=-6646
vy=6646
t=0
T=3600
bodyx = 444 #stationary body position metres
bodyy = 444
tarray=[]
xarray=[]
yarray=[]
vxarray=[]
vyarray=[]
while t<T:
k1 = np.zeros(4)
k2 = np.zeros(4)
k3 = np.zeros(4)
k4 = np.zeros(4)
tarray.append(t)
xarray.append(x)
yarray.append(y)
vxarray.append(vx)
vyarray.append(vy)
x = bodyx - x
y = bodyy - y
k1[0]=derX(vx)
k1[1]=derY(vy)
k1[2]=derVx(x,y)
k1[3]=derVx(y,x)
k2[0]=derX(vx+(step/2)*k1[2])
k2[1]=derY(vy+(step/2)*k1[3])
k2[2]=derVx(x+(step/2)*k1[0],y+(step/2)*k1[1])
k2[3]=derVx(y+(step/2)*k1[1],x+(step/2)*k1[0])
k3[0]=derX(vx+(step/2)*k2[2])
k3[1]=derY(vy+(step/2)*k2[3])
k3[2]=derVx(x+(step/2)*k2[0],y+(step/2)*k2[1])
k3[3]=derVx(y+(step/2)*k2[1],x+(step/2)*k2[0])
k4[0]=derX(vx+step*k3[2])
k4[1]=derY(vy+step*k3[3])
k4[2]=derVx(x+step*k3[0],y+step*k3[1])
k4[3]=derVx(y+step*k3[1],vx+step*k3[0])
t=t+step
x=timestep(x,k1[0],k2[0],k3[0],k4[0])
y=timestep(x,k1[1],k2[1],k3[1],k4[1])
vx=timestep(x,k1[2],k2[2],k3[2],k4[2])
vy=timestep(x,k1[3],k2[3],k3[3],k4[3])
mpl.plot(xarray, yarray)
There is a spurious v in the computation of k4[3].
The call of timestep has x as argument where it should be y, vx, vy.
And another error seems to be that in the difference computation
x = bodyx - x
y = bodyy - y
you also change the absolute position. Also the force direction becomes reversed.
Change that to something like
diffx = x - bodyx
diffy = y - bodyy
and use these relative positions in the force computation.
To compare, the built-in procedures produce scipy.integrate.odeint with data
G=6.67408E-11 #m^3/kg s^2
M=5.972E24 #kg, mass of Earth
bodyx = 444 #stationary body position metres
bodyy = 444
def system(u,t):
x,y,vx,vy = u
x -= bodyx
y -= bodyy
f = -(G*M)/((x**2 + y**2)**(1.5))
return [ vx, vy, f*x, f*y ]
x0=4596194 #initial conditions in m and m/s
y0=4596194
vx0=-6646
vy0=6646
u0 = [ x0, y0, vx0, vy0 ]
T= np.linspace(0,3600,36+1)
sol = odeint(system, u0, T)
mpl.plot(sol[:,0], sol[:,1]); mpl.show()
gives a nicely curved bow, about 1/4 of a full orbit.
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 am tring to solve the equation of motion of charged particle in planetary magnetic field to see the path of the particle using Forward Euler's and RK5 method in python (as an excercise in learning Numerical methods) I encounter two problems:
The 'for loop' in the RK4 method does not update the new values. It give the values of the first iteration for all iteration.
With the change of the sing of 'β = charge/mass' the path of particle which is expected does not change. It seems the path is unaffected by the nature(sign) of the particle. What does this mean physically or mathematically?
The codes are adapted from :
python two coupled second order ODEs Runge Kutta 4th order
and
Applying Forward Euler Method to a Three-Box Model System of ODEs
I would be immensely grateful if anyone explain to me what is wrong in the code.
thank you.
The Code are as under:
import numpy as np
import matplotlib.pyplot as plt
from math import sin, cos
from scipy.integrate import odeint
scales = np.array([1e7, 0.1, 1, 1e-5, 10, 1e-5])
def LzForce(t,p):
# assigning each ODE to a vector element
r,x,θ,y,ϕ,z = p*scales
# constants
R = 60268e3 # metre
g_20 = 1583e-9
Ω = 9.74e-3 # degree/second
B_θ = (R/r)**4*g_20*cos(θ)*sin(θ)
B_r = 2*(R/r)**4*g_20*(0.5*(3*cos(θ)**2-1))
β = +9.36e10
# defining the ODEs
drdt = x
dxdt = r*(y**2 +(z+Ω)**2*sin(θ)**2-β*z*sin(θ)*B_θ)
dθdt = y
dydt = (-2*x*y +r*(z+Ω)**2*sin(θ)*cos(θ)+β*r*z*sin(θ)*B_r)/r
dϕdt = z
dzdt = (-2*x*(z+Ω)*sin(θ)-2*r*y*(z+Ω)*cos(θ)+β*(x*B_θ-r*y*B_r))/(r*sin(θ))
return np.array([drdt,dxdt,dθdt,dydt,dϕdt,dzdt])/scales
def ForwardEuler(fun,t0,p0,tf,dt):
r0 = 6.6e+07
x0 = 0.
θ0 = 88.
y0 = 0.
ϕ0 = 0.
z0 = 22e-3
p0 = np.array([r0,x0,θ0,y0,ϕ0,z0])
t = np.arange(t0,tf+dt,dt)
p = np.zeros([len(t), len(p0)])
p[0] = p0
for i in range(len(t)-1):
p[i+1,:] = p[i,:] + fun(t[i],p[i,:]) * dt
return t, p
def rk4(fun,t0,p0,tf,dt):
# initial conditions
r0 = 6.6e+07
x0 = 0.
θ0 = 88.
y0 = 0.
ϕ0 = 0.
z0 = 22e-3
p0 = np.array([r0,x0,θ0,y0,ϕ0,z0])
t = np.arange(t0,tf+dt,dt)
p = np.zeros([len(t), len(p0)])
p[0] = p0
for i in range(len(t)-1):
k1 = dt * fun(t[i], p[i])
k2 = dt * fun(t[i] + 0.5*dt, p[i] + 0.5 * k1)
k3 = dt * fun(t[i] + 0.5*dt, p[i] + 0.5 * k2)
k4 = dt * fun(t[i] + dt, p[i] + k3)
p[i+1] = p[i] + (k1 + 2*(k2 + k3) + k4)/6
return t,p
dt = 0.5
tf = 1000
p0 = [6.6e+07,0.0,88.0,0.0,0.0,22e-3]
t0 = 0
#Solution with Forward Euler
t,p_Euler = ForwardEuler(LzForce,t0,p0,tf,dt)
#Solution with RK4
t ,p_RK4 = rk4(LzForce,t0, p0 ,tf,dt)
print(t,p_Euler)
print(t,p_RK4)
# Plot Solutions
r,x,θ,y,ϕ,z = p_Euler.T
fig,ax=plt.subplots(2,3,figsize=(8,4))
plt.xlabel('time in sec')
plt.ylabel('parameters')
for a,s in zip(ax.flatten(),[r,x,θ,y,ϕ,z]):
a.plot(t,s); a.grid()
plt.title("Forward Euler", loc='left')
plt.tight_layout(); plt.show()
r,x,θ,y,ϕ,z = p_RK4.T
fig,ax=plt.subplots(2,3,figsize=(8,4))
plt.xlabel('time in sec')
plt.ylabel('parameters')
for a,q in zip(ax.flatten(),[r,x,θ,y,ϕ,z]):
a.plot(t,q); a.grid()
plt.title("RK4", loc='left')
plt.tight_layout(); plt.show()
[RK4 solution plot][1]
[Euler's solution methods][2]
''''RK4 does not give iterated values.
The path is unaffected by the change of sign which is expected as it is under Lorentz force''''
[1]: https://i.stack.imgur.com/bZdIw.png
[2]: https://i.stack.imgur.com/tuNDp.png
You are not iterating more than once inside the for loop in rk4 because it returns after the first iteration.
for i in range(len(t)-1):
k1 = dt * fun(t[i], p[i])
k2 = dt * fun(t[i] + 0.5*dt, p[i] + 0.5 * k1)
k3 = dt * fun(t[i] + 0.5*dt, p[i] + 0.5 * k2)
k4 = dt * fun(t[i] + dt, p[i] + k3)
p[i+1] = p[i] + (k1 + 2*(k2 + k3) + k4)/6
# This is the problem line, the return was tabbed in, to be inside the for block, so the block executed once and returned.
return t,p
For physics questions please try a different forum.
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 am trying to create the animation in this video using Python. But I stuck on the very first step. Till now I've created a Circle and a point rotating around its circumference. My code is given below. Now I want to plot the y values corresponding to x=np.arange(0, I*np.pi, 0.01) along the x-axis (as shown in update() function in the code). For this I have to define another function to plot these x and y and pass that function inside a new animation.FuncAnimation().
Is there any way to plot everything using only the update() function?
Note I have found a code of this animation in here. But it is written in Java!
My Code
import matplotlib.pyplot as plt
from matplotlib import animation
import numpy as np
W = 6.5
H = 2
radius = 1
I = 2
T = 3
N = 2
plt.style.use(['ggplot', 'dark_background'])
def create_circle(x, y, r):
circle = plt.Circle((x, y), radius=r, fill=False, alpha=0.7, color='w')
return circle
def create_animation():
fig = plt.figure()
ax = plt.axes(xlim=(-2, W + 2), ylim=(-H, H))
circle = create_circle(0, 0, radius)
ax.add_patch(circle)
line1, = ax.plot(0, 1, marker='o', markersize=3, color='pink', alpha=0.7)
def update(theta):
x = radius * np.cos(theta)
y = radius * np.sin(theta)
line1.set_data([0, x], [0, y])
return line1,
anim = []
anim.append(animation.FuncAnimation(fig, update,
frames=np.arange(0, I * np.pi, 0.01),
interval=10, repeat=True))
# anim.append(animation.FuncAnimation(fig, update_line, len(x),
# fargs=[x, y, line, line1], interval=10))
plt.grid(False)
plt.gca().set_aspect('equal')
plt.gca().spines['left'].set_visible(False)
plt.gca().spines['top'].set_visible(False)
plt.gca().spines['right'].set_visible(False)
plt.gca().spines['bottom'].set_visible(False)
plt.gca().set_xticks([])
plt.gca().set_yticks([])
plt.show()
if __name__ == '__main__':
create_animation()
Edit. I've improved the task by defining a global variable pos and changing the update() function in the following manner ...The animation now looks better but still having bugs!
Improved Portion
plot, = ax.plot([], [], color='w', alpha=0.7)
level = np.arange(0, I * np.pi, 0.01)
num = []
frames = []
for key, v in enumerate(level):
num.append(key)
frames.append(v)
def update(theta):
global pos
x = radius * np.cos(theta)
y = radius * np.sin(theta)
wave.append(y)
plot.set_data(np.flip(level[:pos] + T), wave[:pos])
line1.set_data([0, x], [0, y])
pos += 1
return line1, plot,
Edit Till now I've done the following:
def update(theta):
global pos
x, y = 0, 0
for i in range(N):
prev_x = x
prev_y = y
n = 2 * i + 1
rad = radius * (4 / (n * np.pi))
x += rad * np.cos(n * theta)
y += rad * np.sin(n * theta)
wave.append(y)
circle = create_circle(prev_x, prev_y, rad)
ax.add_patch(circle)
plot.set_data(np.flip(level[:pos] + T), wave[:pos])
line2.set_data([x, T], [y, y])
line1.set_data([prev_x, x], [prev_y, y])
pos += 1
return line1, plot, line2,
Output
Please help to correct this animation. Or, is there any efficient way to do this animation?
Edit Well, now the animation is partially working. But there is a little issue: In my code (inside the definition of update()) I have to add circles centered at (prev_x, prev_y) of radius defined as rad for each frame. For this reason I try to use a for loop in the definition of update() but then all the circles remains in the figure (see the output below). But I want one circle in each frame with the centre and radius as mentioned above. Also the same problem is with the plot. I try to use ax.clear() inside the for loop but it didn't work.
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);
}