I need to solve a nonlinear system of equations in Python using Sympy.
For this, I wrote the code below. However, when I run this code the Python remains busy without returning any error message and, additionally, does not return the solution.
For comparison, I did the same work in Matlab and within a few seconds, the program returns two solutions for this system.
How, using Sympy, I can solve the system?
Regards.
import sympy as sym
import numpy as np
# Variables of the system
S, V, E, A, I, R = sym.symbols('S, V, E, A, I, R')
# Parameters of the system
N = sym.Symbol("N", positive = True)
mi = sym.Symbol("mi", positive = True)
v = sym.Symbol("v", positive = True)
epsilon = sym.Symbol("epsilon", positive = True)
alpha = sym.Symbol("alpha", positive = True)
gamma_as = sym.Symbol("gamma_as", positive = True)
gamma_s = sym.Symbol("gamma_s", positive = True)
gamma_a = sym.Symbol("gamma_a", positive = True)
lamb = sym.Symbol("lamb", positive = True)
tau = sym.Symbol("tau", positive = True)
beta = sym.Symbol("beta", positive = True)
x = sym.Symbol("x")
# Declaration of the system equations
system = [mi*N - v*S + R - (beta*(A+I)/N)*S - mi*S,\
v*S - (1-epsilon)*(beta*(A+I)/N)*V - mi*V,\
(beta*(A+I)/N)*S + (1-epsilon)*(beta*(A+I)/N)*V - sym.exp(-mi*tau)*(beta*(A+I)/N)*S - mi*E,\
alpha*sym.exp(-mi*tau)*(beta*(A+I)/N)*S - (gamma_as + gamma_a + mi)*A,\
(1-alpha)*sym.exp(-mi*tau)*(beta*(A+I)/N)*S + gamma_as*A - (gamma_s + mi)*I,\
gamma_a*A + gamma_s*I - (1+mi)*R]
# Solution
solution_set = sym.nonlinsolve(system, [S, V, E, A, I, R])
pyS, pyV, pyE, pyA, pyI, pyR = solution_set[0]
````
SymPy generally solves a system of polynomial equations like this using Groebner bases. To compute the Groebner basis SymPy needs to identify each of the equations as a polynomial in the given unknowns with coefficients in a computable field (a "domain"). Your coefficients involve both mi and exp(-mi*tau) which SymPy's construct_domain doesn't like so it gives up constructing a computable domain and uses the "EX" domain instead which is very slow.
The solution then is to replace exp(mi*tau) with another symbol (I'll just use tau) and then compute the Groebner basis explicitly yourself:
In [103]: rep = {exp(-mi*tau):tau}
In [104]: system2 = [eq.subs(rep) for eq in system]
In [105]: for eq in system2: pprint(eq)
S⋅β⋅(A + I)
N⋅mi + R - S⋅mi - S⋅v - ───────────
N
V⋅β⋅(1 - ε)⋅(A + I)
S⋅v - V⋅mi - ───────────────────
N
S⋅β⋅τ⋅(A + I) S⋅β⋅(A + I) V⋅β⋅(1 - ε)⋅(A + I)
-E⋅mi - ───────────── + ─────────── + ───────────────────
N N N
S⋅α⋅β⋅τ⋅(A + I)
-A⋅(γₐ + γₐₛ + mi) + ───────────────
N
S⋅β⋅τ⋅(1 - α)⋅(A + I)
A⋅γₐₛ - I⋅(γₛ + mi) + ─────────────────────
N
A⋅γₐ + I⋅γₛ - R⋅(mi + 1)
Now we could use solve or nonlinsolve but it's faster to compute and solve the Groebner basis ourselves:
In [106]: %time gb = groebner(system2, [S, V, E, A, I, R])
CPU times: user 3min 1s, sys: 100 ms, total: 3min 1s
Wall time: 3min 1s
The Groebner basis puts the system of equations into an almost solved form known as a rational univariate representation (RUR). In this case it looks like
S - a*R
V - b*R
E - c*R
A - d*R
I - e*R
R**2 + f*R + g
where the coefficients a, b, c, d, e, f, g are complicated rational functions of the symbolic parameters in the equations (alpha, beta etc). From here we can follow these steps to solve the Groebner basis:
Solve the first 5 linear equations for S, V, E, A and I in terms of R.
Solve the final quadratic equation for R giving two solutions R1 and R2.
Substitute the the solutions for R1 and R2 into the solutions for S, V, E, A and I.
Put it all together as two solution tuples.
That is:
In [115]: syms = [S, V, E, A, I, R]
In [116]: [lsol] = linsolve(gb[:-1], syms[:-1])
In [117]: R1, R2 = roots(gb[-1], R)
In [118]: sol1 = lsol.subs(R, R1) + (R1,)
In [119]: sol2 = lsol.subs(R, R2) + (R2,)
Now we have the two solution tuples in the form that would have been returned by nonlinsolve. Unfortunately the solutions are quite complicated so I won't show them in full. You can get some idea of the complexity by seeing the length of their string representations:
In [122]: print(len(str(sol1)))
794100
In [123]: print(len(str(sol2)))
27850
Now at this point it's worth considering what you actually wanted these solutions for. Maybe it's just that you wanted to substitute some explicit numeric values in for the symbolic parameters. It's worth noting here that potentially it would have been more efficient in the first place to substitute those values into the equations before attempting to solve them symbolically. If you want to see how your solutions depend on some particular parameters say just mi then you can substitute values for everything else and obtain a simpler form of the solution involving only that parameter more quickly:
In [136]: rep = {alpha:1, beta:2, epsilon:3, gamma_as:4, gamma_s:5, gamma_a:6, exp(-mi*tau):7, N:8, v
...: :9}
In [137]: system2 = [eq.subs(rep) for eq in system]
In [138]: %time solve(system2, syms)
CPU times: user 3.92 s, sys: 4 ms, total: 3.92 s
Wall time: 3.92 s
Out[138]:
⎡ ⎛ ⎛ 2
⎢⎛ 8⋅mi 72 ⎞ ⎜4⋅(mi + 5)⋅(mi + 10) 36⋅(mi + 5)⋅(mi + 10)⋅(mi + 12)⋅⎝mi + 4⋅mi
⎢⎜──────, ──────, 0, 0, 0, 0⎟, ⎜────────────────────, ─────────────────────────────────────────────
⎢⎝mi + 9 mi + 9 ⎠ ⎜ 7⋅(mi + 9) ⎛ 4 3 2
⎣ ⎝ 7⋅(mi + 9)⋅⎝3⋅mi + 38⋅mi + 161⋅mi + 718⋅mi
⎞ ⎛ 2 ⎞ ⎛ 3 2 ⎞
- 25⎠ 24⋅(mi + 1)⋅(mi + 5)⋅(mi + 10)⋅⎝mi + mi + 50⎠⋅⎝3⋅mi + 41⋅mi + 209⋅mi + 787⎠ -4⋅(mi + 1
───────, ──────────────────────────────────────────────────────────────────────────────, ──────────
⎞ ⎛ 2 ⎞ ⎛ 4 3 2 ⎞
+ 900⎠ 7⋅(mi + 12)⋅⎝mi + 4⋅mi - 25⎠⋅⎝3⋅mi + 38⋅mi + 161⋅mi + 718⋅mi + 900⎠ (mi +
⎛ 2 ⎞ ⎛ 2 ⎞ ⎛ 2 ⎞ ⎞⎤
)⋅(mi + 5)⋅⎝mi + mi + 50⎠ -16⋅(mi + 1)⋅⎝mi + mi + 50⎠ -8⋅(3⋅mi + 25)⋅⎝mi + mi + 50⎠ ⎟⎥
───────────────────────────, ─────────────────────────────, ───────────────────────────────⎟⎥
⎛ 2 ⎞ ⎛ 2 ⎞ ⎛ 2 ⎞ ⎟⎥
12)⋅⎝mi + 4⋅mi - 25⎠ (mi + 12)⋅⎝mi + 4⋅mi - 25⎠ (mi + 12)⋅⎝mi + 4⋅mi - 25⎠ ⎠⎦
If you substitute values for all parameters then it's a lot faster:
In [139]: rep = {alpha:1, beta:2, epsilon:3, gamma_as:4, gamma_s:5, gamma_a:6, exp(-mi*tau):7, N:8, v
...: :9, mi:10}
In [140]: system2 = [eq.subs(rep) for eq in system]
In [141]: %time solve(system2, syms)
CPU times: user 112 ms, sys: 0 ns, total: 112 ms
Wall time: 111 ms
Out[141]:
⎡⎛1200 124200 5224320 -960 -256 -640 ⎞ ⎛80 72 ⎞⎤
⎢⎜────, ──────, ───────, ─────, ─────, ─────⎟, ⎜──, ──, 0, 0, 0, 0⎟⎥
⎣⎝133 55727 67459 23 23 23 ⎠ ⎝19 19 ⎠⎦
If you look at your system you will see that the 4th and 5th equations have two solutions since solving the 4th for A and substituting into the 5th gives an expression that factors as I*f(S) -- giving, for the value of A, I = 0 and S such that f(S) = 0. Judicious selection of which equation(s) to solve next and taking time to lump constants together so you don't bog down the solver gives both solutions in about 10 seconds with relatively small operation counts (relative to the results of nonlinsolve above -- 10 and 5192 operations). The process gives the same solutions for the representative values above:
def condense(eq, *x, reps=None):
"""collapse additive/multiplicative constants into single
variables, returning condensed expression and replacement
values.
Note: use of the replacement dictionary may require topological sorting
if values depend on the keys.
"""
from sympy.core.traversal import bottom_up
from sympy.simplify.radsimp import collect
from sympy.utilities.iterables import numbered_symbols
if reps is None:
reps = {}
else:
reps = {v:k for k,v in reps.items()}
con = numbered_symbols('c')
free = eq.free_symbols
def c():
while True:
rv = next(con)
if rv not in free:
return rv
def do(e):
if not e.args:
return e
e = e.func(*[do(i) for i in e.args])
isAdd=e.is_Add
if not (isAdd or e.is_Mul):
return e
if isAdd:
ee = collect(e, x, exact=None)
if ee != e:
e = do(ee)
co, id = e.as_coeff_Add() if isAdd else e.as_coeff_Mul()
i, d = id.as_independent(*x, as_Add=isAdd)
if not i.args:
return e
return e.func(co, reps.get(i, reps.setdefault(i, c())), d)
rv = do(bottom_up(eq, do))
return rv, {v: k for k, v in reps.items()}
def repsort(*replace):
"""Return sorted replacement tuples `(o, n)` such that `(o_i, n_i)`
will appear before `(o_j, n_j)` if `o_j` appears in `n_i`. An error
will be raised if `o_j` appears in `n_i` and `o_i` appears in `n_k`
if `k >= i`.
Examples
========
>>> from sympy.abc import x, y, z, a
>>> repsort((x, y + 1), (z, x + 2))
[(z, x + 2), (x, y + 1)]
>>> repsort((x, y + 1), (z, x**2))
[(z, x**2), (x, y + 1)]
>>> repsort(*Tuple((x,y+z),(y,a),(z,1/y)))
[(x, y + z), (z, 1/y), (y, a)]
Any two of the following 3 tuples will not raise an error,
but together they contain a cycle that raises an error:
>>> repsort((x, y), (y, z), (z, x))
Traceback (most recent call last):
...
raise ValueError("cycle detected")
"""
from itertools import permutations
from sympy import default_sort_key, topological_sort
free = {i for i,_ in replace}
defs, replace = sift(replace,
lambda x: x[1].is_number or not x[1].has_free(*free),
binary=True)
edges = [(i, j) for i, j in permutations(replace, 2) if
i[1].has(j[0]) and (not j[0].is_Symbol or j[0] in i[1].free_symbols)]
rv = topological_sort([replace, edges], default_sort_key)
rv.extend(ordered(defs))
return rv
def dupdate(d, s):
"""update values in d with values from s and return the combined dictionaries"""
rv = {k: v.xreplace(s) for k,v in d.items()}
rv.update(s)
return rv
# Variables of the system
syms=S, V, E, A, I, R = symbols('S, V, E, A, I, R')
# Parameters of the system
const = var('a:j k')
system = [
-A*S*c/a - I*S*c/a + R + S*(-h - j) + a*h,
A*(V*c*d/a - V*c/a) + I*(V*c*d/a - V*c/a) + S*j - V*h,
A*(-S*c*k/a + S*c/a - V*c*d/a + V*c/a) - E*h +
I*(-S*c*k/a + S*c/a - V*c*d/a + V*c/a),
A*(S*b*c*k/a - e - f - h) + I*S*b*c*k/a,
A*(-S*b*c*k/a + S*c*k/a + f) + I*(-S*b*c*k/a + S*c*k/a - g - h),
A*e + I*g + R*(-h - 1)
]
import sympy as sym
# Variables of the system
syms = S, V, E, A, I, R = sym.symbols('S, V, E, A, I, R')
# Parameters of the system
N = sym.Symbol("N", positive = True)
mi = sym.Symbol("mi", positive = True)
v = sym.Symbol("v", positive = True)
epsilon = sym.Symbol("epsilon", positive = True)
alpha = sym.Symbol("alpha", positive = True)
gamma_as = sym.Symbol("gamma_as", positive = True)
gamma_s = sym.Symbol("gamma_s", positive = True)
gamma_a = sym.Symbol("gamma_a", positive = True)
lamb = sym.Symbol("lamb", positive = True)
tau = sym.Symbol("tau", positive = True)
beta = sym.Symbol("beta", positive = True)
# Declaration of the system equations
system = [
mi*N - v*S + R - (beta*(A+I)/N)*S - mi*S,
v*S - (1-epsilon)*(beta*(A+I)/N)*V - mi*V,
(beta*(A+I)/N)*S + (1-epsilon)*(beta*(A+I)/N)*V -
sym.exp(-mi*tau)*(beta*(A+I)/N)*S - mi*E,
alpha*sym.exp(-mi*tau)*(beta*(A+I)/N)*S - (gamma_as + gamma_a + mi)*A,
(1-alpha)*sym.exp(-mi*tau)*(beta*(A+I)/N)*S + gamma_as*A - (gamma_s + mi)*I,
gamma_a*A + gamma_s*I - (1+mi)*R]
system, srep = condense(Tuple(*system), *syms)
asol = solve(system[3], A, dict=True)[0]
aeq=Tuple(*[i.xreplace(asol) for i in system])
si = solve(aeq[4], *syms, dict=True)
sol1 = dupdate(asol, si[0])
sol1 = dupdate(sol1, solve(Tuple(*system).xreplace(sol1),syms,dict=1)[0]); sol1
aeqs4 = Tuple(*[i.xreplace(si[1]) for i in aeq])
ceq, crep = condense(Tuple(*aeqs4),*syms,reps=srep)
ir = solve([ceq[0], ceq[-1]], I, R, dict=1)[0]
ve = solve([i.simplify() for i in Tuple(*ceq).xreplace(ir)], syms, dict=True)[0] # if we don't simplify to make first element 0 we get no solution -- bug?
sol2 = dupdate(asol, si[1])
sol2 = dupdate(sol2, ir)
sol2 = dupdate(sol2, ve)
crep = repsort(*crep.items())
sol1 = Dict({k:v.subs(crep) for k,v in sol1.items()}) # 10 ops
sol2 = Dict({k:v.subs(crep) for k,v in sol2.items()}) # 5192 ops
Test for specific values (as above):
>>> rep = {alpha: 1, beta: 2, epsilon: 3, gamma_as: 4, gamma_s: 5,
... gamma_a: 6, exp(-mi*tau): 7, N: 8, v: 9, mi: 10}
...
>>> sol1.xreplace(rep)
{A: 0, E: 0, I: 0, R: 0, S: 80/19, V: 72/19}
>>> sol2.xreplace(rep)
{A: -960/23, E: 89280/851, I: -256/23,
R: -640/23, S: 1200/133, V: -124200/4921}
Of course, it took time to find this path to the solution. But if the solver could make better selections of what to solve (rather than trying to get the Groebner basis of the whole system) the time for obtaining a solution from SymPy could be greatly reduced.
I have an equation of the form:
𝑎𝑒^(𝑖𝜃2)+𝑏𝑒^(𝑖𝜃3)+𝑐𝑒^(𝑖𝜃4)+𝑑𝑒^(𝑖𝜃1)=0
In sympy, using the Euler identity I want to split it into two equations, one with real numbers and one imaginary.
𝑎cos(𝜃2)+𝑏cos(𝜃3)+𝑐cos(𝜃4)+𝑑cos(𝜃1)=0
𝑎𝑖sin(𝜃2)+𝑏𝑖sin(𝜃3)+𝑐𝑖sin(𝜃4)+𝑑𝑖sin(𝜃1)=0
So far I have tried:
a, b, c, d = symbols("a b c d")
theta1, theta2, theta3, theta4 = symbols("theta1 theta2 theta3 theta4")
eq1 = Eq(a*exp(I*theta2) + b*exp(I*theta3) + c*exp(I*theta4) +d*exp(I*theta1), 0)
eq1 = eq1.subs([(a,40), (b,120), (c,80), (d,100), (theta1,0), (theta2, 40 * (pi.evalf()/180))])
lhs_real, lhs_img = eq1.lhs.as_real_imag()
rhs_real, rhs_img = eq1.rhs.as_real_imag()
eq2 = Eq(lhs_real, rhs_real)
eq3 = Eq(lhs_img, rhs_img)
However my eq2 seems to contain real and imaginary terms.
−120cos(re(𝜃3))sinh(im(𝜃3))+120cos(re(𝜃3))cosh(im(𝜃3))−80cos(re(𝜃4))sinh(im(𝜃4))+80cos(re(𝜃4))cosh(im(𝜃4))+130.641777724759=0
How can I achieve this with Sympy?
I have looked at this question complex numbers in sympy: resolving Euler's Identity but the sympy.re and sympy.im will not work with Sympy equations.
First, welcome to SO!
If you want to separate expression into real and imaginary parts, you can only work with real variables (real=True). You're example could be rewritten as:
import sympy as sy
sy.init_printing()
a_r, b_r, c_r, d_r = sy.symbols('a_r, b_r, c_r, d_r', real=True)
a_i, b_i, c_i, d_i = sy.symbols('a_i, b_i, c_i, d_i', real=True)
th1, th2, th3, th4 = sy.symbols('theta_1, theta_2, theta_3, theta_4 ', real=True)
# complex numbers:
a, b = (a_r + sy.I*a_i), (a_r + sy.I*a_i)
c, d = (c_r + sy.I*c_i), (d_r + sy.I*d_i)
xpr0 = (a * sy.exp(sy.I*th2) + b * sy.exp(sy.I*th3) +
c * sy.exp(sy.I*th4) + d * sy.exp(sy.I*th1))
eq1 = sy.Eq(sy.re(xpr0), 0)
# Gives: -aᵢ⋅sin(θ₂) + aᵣ⋅cos(θ₂) - bᵢ⋅sin(θ₃) + bᵣ⋅cos(θ₃) - cᵢ⋅sin(θ₄) + cᵣ⋅cos(θ₄) - dᵢ⋅sin(θ₁) + dᵣ⋅cos(θ₁) = 0
eq2 = sy.Eq(sy.im(xpr0), 0)
# Gives aᵢ⋅cos(θ₂) + aᵣ⋅sin(θ₂) + bᵢ⋅cos(θ₃) + bᵣ⋅sin(θ₃) + cᵢ⋅cos(θ₄) + cᵣ⋅sin(θ₄) + dᵢ⋅cos(θ₁) + dᵣ⋅sin(θ₁) = 0
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);
}