Related
I have a set of data ([x[0],x[1]],y), many points in 3D space
and use scikit-learn to fit a learn model.
How I can calculate the distance between all the points to the fitting plane?
Does sklearn provide such function? I mean perpendicular distance.
My code works but too manually.
I am looking for an existing quick function in a package like sklearn.
Thanks.
def Linfit3D(x,y):
# x is a 2D array, they should be location of each bump, x_loc and y_loc
# y is the CTV or BTV that need to be fit to the least square plane
# three value will be returned, a,b, and c, which indicate a + b*x1 + c*x2 =y
model = sklearn.linear_model.LinearRegression()
model.fit(x, y)
coefs = model.coef_
intercept = model.intercept_
print("Equation: y = {:.5f} + {:.5f}*x1 + {:.5f}*x2".format(intercept, coefs[0],coefs[1]))
a=coefs[0]
b=coefs[1]
c=-1
d=intercept
return a,b,c,d
def point_to_plane_dist(x,y, a, b, c, d):
# the plane equation is: a*x + b*y + c*z + d = 0, and typically c=-1
# so the plane equation typicall is z = a*x + b*y + d
# and output has concerned the positive/negtive of point on top/bottom of the plane
f = abs((a * x[0] + b * x[1] + c * y + d))
e = (math.sqrt(a * a + b * b + c * c))
zp=a*x[0]+b*x[1]+d
# print('y = %2f, zp = %2f' %(y,zp))
if y>=zp:
return f/e
elif y<zp:
return (f/e)*(-1)
I have a code that works perfectly well but I wish to speed up the time it takes to converge. A snippet of the code is shown below:
def myfunction(x, i):
y = x + (min(0, target[i] - data[i, :]x))*data[i]/(norm(data[i])**2))
return y
rows, columns = data.shape
start = time.time()
iterate = 0
iterate_count = []
norm_count = []
res = 5
x_not = np.ones(columns)
norm_count.append(norm(x_not))
iterate_count.append(0)
while res > 1e-8:
for row in range(rows):
y = myfunction(x_not, row)
x_not = y
iterate += 1
iterate_count.append(iterate)
norm_count.append(norm(x_not))
res = abs(norm_count[-1] - norm_count[-2])
print('Converge at {} iterations'.format(iterate))
print('Duration: {:.4f} seconds'.format(time.time() - start))
I am relatively new in Python. I will appreciate any hint/assistance.
Ax=b is the problem we wish to solve. Here, 'A' is the 'data' and 'b' is the 'target'
Ugh! After spending a while on this I don't think it can be done the way you've set up your problem. In each iteration over the row, you modify x_not and then pass the updated result to get the solution for the next row. This kind of setup can't be vectorized easily. You can learn the thought process of vectorization from the failed attempt, so I'm including it in the answer. I'm also including a different iterative method to solve linear systems of equations. I've included a vectorized version -- where the solution is updated using matrix multiplication and vector addition, and a loopy version -- where the solution is updated using a for loop to demonstrate what you can expect to gain.
1. The failed attempt
Let's take a look at what you're doing here.
def myfunction(x, i):
y = x + (min(0, target[i] - data[i, :] # x)) * (data[i] / (norm(data[i])**2))
return y
You subtract
the dot product of (the ith row of data and x_not)
from the ith row of target,
limited at zero.
You multiply this result with the ith row of data divided my the norm of that row squared. Let's call this part2
Then you add this to the ith element of x_not
Now let's look at the shapes of the matrices.
data is (M, N).
target is (M, ).
x_not is (N, )
Instead of doing these operations rowwise, you can operate on the entire matrix!
1.1. Simplifying the dot product.
Instead of doing data[i, :] # x, you can do data # x_not and this gives an array with the ith element giving the dot product of the ith row with x_not. So now we have data # x_not with shape (M, )
Then, you can subtract this from the entire target array, so target - (data # x_not) has shape (M, ).
So far, we have
part1 = target - (data # x_not)
Next, if anything is greater than zero, set it to zero.
part1[part1 > 0] = 0
1.2. Finding rowwise norms.
Finally, you want to multiply this by the row of data, and divide by the square of the L2-norm of that row. To get the norm of each row of a matrix, you do
rownorms = np.linalg.norm(data, axis=1)
This is a (M, ) array, so we need to convert it to a (M, 1) array so we can divide each row. rownorms[:, None] does this. Then divide data by this.
part2 = data / (rownorms[:, None]**2)
1.3. Add to x_not
Finally, we're adding each row of part1 * part2 to the original x_not and returning the result
result = x_not + (part1 * part2).sum(axis=0)
Here's where we get stuck. In your approach, each call to myfunction() gives a value of part1 that depends on target[i], which was changed in the last call to myfunction().
2. Why vectorize?
Using numpy's inbuilt methods instead of looping allows it to offload the calculation to its C backend, so it runs faster. If your numpy is linked to a BLAS backend, you can extract even more speed by using your processor's SIMD registers
The conjugate gradient method is a simple iterative method to solve certain systems of equations. There are other more complex algorithms that can solve general systems well, but this should do for the purposes of our demo. Again, the purpose is not to have an iterative algorithm that will perfectly solve any linear system of equations, but to show what kind of speedup you can expect if you vectorize your code.
Given your system
data # x_not = target
Let's define some variables:
A = data.T # data
b = data.T # target
And we'll solve the system A # x = b
x = np.zeros((columns,)) # Initial guess. Can be anything
resid = b - A # x
p = resid
while (np.abs(resid) > tolerance).any():
Ap = A # p
alpha = (resid.T # resid) / (p.T # Ap)
x = x + alpha * p
resid_new = resid - alpha * Ap
beta = (resid_new.T # resid_new) / (resid.T # resid)
p = resid_new + beta * p
resid = resid_new + 0
To contrast the fully vectorized approach with one that uses iterations to update the rows of x and resid_new, let's define another implementation of the CG solver that does this.
def solve_loopy(data, target, itermax = 100, tolerance = 1e-8):
A = data.T # data
b = data.T # target
rows, columns = data.shape
x = np.zeros((columns,)) # Initial guess. Can be anything
resid = b - A # x
resid_new = b - A # x
p = resid
niter = 0
while (np.abs(resid) > tolerance).any() and niter < itermax:
Ap = A # p
alpha = (resid.T # resid) / (p.T # Ap)
for i in range(len(x)):
x[i] = x[i] + alpha * p[i]
resid_new[i] = resid[i] - alpha * Ap[i]
# resid_new = resid - alpha * A # p
beta = (resid_new.T # resid_new) / (resid.T # resid)
p = resid_new + beta * p
resid = resid_new + 0
niter += 1
return x
And our original vector method:
def solve_vect(data, target, itermax = 100, tolerance = 1e-8):
A = data.T # data
b = data.T # target
rows, columns = data.shape
x = np.zeros((columns,)) # Initial guess. Can be anything
resid = b - A # x
resid_new = b - A # x
p = resid
niter = 0
while (np.abs(resid) > tolerance).any() and niter < itermax:
Ap = A # p
alpha = (resid.T # resid) / (p.T # Ap)
x = x + alpha * p
resid_new = resid - alpha * Ap
beta = (resid_new.T # resid_new) / (resid.T # resid)
p = resid_new + beta * p
resid = resid_new + 0
niter += 1
return x
Let's solve a simple system to see if this works first:
2x1 + x2 = -5
−x1 + x2 = -2
should give a solution of [-1, -3]
data = np.array([[ 2, 1],
[-1, 1]])
target = np.array([-5, -2])
print(solve_loopy(data, target))
print(solve_vect(data, target))
Both give the correct solution [-1, -3], yay! Now on to bigger things:
data = np.random.random((100, 100))
target = np.random.random((100, ))
Let's ensure the solution is still correct:
sol1 = solve_loopy(data, target)
np.allclose(data # sol1, target)
# Output: False
sol2 = solve_vect(data, target)
np.allclose(data # sol2, target)
# Output: False
Hmm, looks like the CG method doesn't work for badly conditioned random matrices we created. Well, at least both give the same result.
np.allclose(sol1, sol2)
# Output: True
But let's not get discouraged! We don't really care if it works perfectly, the point of this is to demonstrate how amazing vectorization is. So let's time this:
import timeit
timeit.timeit('solve_loopy(data, target)', number=10, setup='from __main__ import solve_loopy, data, target')
# Output: 0.25586539999994784
timeit.timeit('solve_vect(data, target)', number=10, setup='from __main__ import solve_vect, data, target')
# Output: 0.12008900000000722
Nice! A ~2x speedup simply by avoiding a loop while updating our solution!
For larger systems, this will be even better.
for N in [10, 50, 100, 500, 1000]:
data = np.random.random((N, N))
target = np.random.random((N, ))
t_loopy = timeit.timeit('solve_loopy(data, target)', number=10, setup='from __main__ import solve_loopy, data, target')
t_vect = timeit.timeit('solve_vect(data, target)', number=10, setup='from __main__ import solve_vect, data, target')
print(N, t_loopy, t_vect, t_loopy/t_vect)
This gives us:
N t_loopy t_vect speedup
00010 0.002823 0.002099 1.345390
00050 0.051209 0.014486 3.535048
00100 0.260348 0.114601 2.271773
00500 0.980453 0.240151 4.082644
01000 1.769959 0.508197 3.482822
I have two functions that compute the same metric. One ends up using a list comprehension to cycle through a calculation, the other uses only numpy tensor operations. The functions take in a (N, 3) array, where N is the number of points in 3D space. When N <~ 3000 the tensor function is faster, when N >~ 3000 the list comprehension is faster. Both seem to have linear time complexity in terms of N i.e two time-N lines cross at N=~3000.
def approximate_area_loop(section, num_area_divisions):
n_a_d = num_area_divisions
interp_vectors = get_section_interp_(section)
a1 = section[:-1]
b1 = section[1:]
a2 = interp_vectors[:-1]
b2 = interp_vectors[1:]
c = lambda u: (1 - u) * a1 + u * a2
d = lambda u: (1 - u) * b1 + u * b2
x = lambda u, v: (1 - v) * c(u) + v * d(u)
area = np.sum([np.linalg.norm(np.cross((x((i + 1)/n_a_d, j/n_a_d) - x(i/n_a_d, j/n_a_d)),\
(x(i/n_a_d, (j +1)/n_a_d) - x(i/n_a_d, j/n_a_d))), axis = 1)\
for i in range(n_a_d) for j in range(n_a_d)])
Dt = section[-1, 0] - section[0, 0]
return area, Dt
def approximate_area_tensor(section, num_area_divisions):
divisors = np.linspace(0, 1, num_area_divisions + 1)
interp_vectors = get_section_interp_(section)
a1 = section[:-1]
b1 = section[1:]
a2 = interp_vectors[:-1]
b2 = interp_vectors[1:]
c = np.multiply.outer(a1, (1 - divisors)) + np.multiply.outer(a2, divisors) # c_areas_vecs_divs
d = np.multiply.outer(b1, (1 - divisors)) + np.multiply.outer(b2, divisors) # d_areas_vecs_divs
x = np.multiply.outer(c, (1 - divisors)) + np.multiply.outer(d, divisors) # x_areas_vecs_Divs_divs
u = x[:, :, 1:, :-1] - x[:, :, :-1, :-1] # u_areas_vecs_Divs_divs
v = x[:, :, :-1, 1:] - x[:, :, :-1, :-1] # v_areas_vecs_Divs_divs
sub_area_norm_vecs = np.cross(u, v, axis = 1) # areas_crosses_Divs_divs
sub_areas = np.linalg.norm(sub_area_norm_vecs, axis = 1) # areas_Divs_divs (values are now sub areas)
area = np.sum(sub_areas)
Dt = section[-1, 0] - section[0, 0]
return area, Dt
Why does the list comprehension version work faster at large N? Surely the tensor version should be faster? I'm wondering if it's something to do with the size of the calculations meaning it's too big to be done in cache? Please ask if I haven't included enough information, I'd really like to get to the bottom of this.
The bottleneck in the fully vectorized function was indeed in the np.linalg.norm as #hpauljs comment suggested.
Norm was used only to get the magnitude of all the vectors contained in axis 1. A much simpler and faster method was to just:
sub_areas = np.sqrt((sub_area_norm_vecs*sub_area_norm_vecs).sum(axis = 1))
This gives exactly the same results and sped up the code by up to 25 times faster than the loop implementation (even when the loop doesn't use linalg.norm either).
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);
}