I'm making a python script right now that is trying to find the length of an arc, where it given this information:
center of arc: x1, y1
start point of arc: x2, y2
end point of arc: x3, y3
direction, cw, ccw
so far I have been able to successfully calculate the radius, and I tried calculating the angle using the equation:
But for any arcs that have an angle greater than 1*pi or 180 degrees, it returns the incorrect (but correct) inside angle.
What is the correct equation knowing the radius and these three points that I can use to find the value of the angle of the arc from 0 rad/degrees to 360 degrees/2pi radians, going in either the clockwise or counterclockwise direction (it can be either or and I need to be able to calculate for both scenarios)
Code:
# code to find theta
aVector = np.array([x1 - x2, y1 - y2])
bVector = np.array([x1 - x3, y1 - y3])
aMag = np.linalg.norm(aVector)
bMag = np.linalg.norm(aVector)
theta = np.arcos(np.dot(aVector, bVector) / (aMag * bMag))
as you can see here, I'm using arccos which to my dismay only outputs 0-180 degrees
Solution/Working code:
# equation for angle using atan2
start = math.atan2(y2 - y1, x2 - x1)
end = math.atan2(y3 - y1, x3 - x1)
if gcodeAnalysis[tempLineNum][4] == "G3": # going CW
start, end = end, start
tau = 2.0 * math.pi
theta = math.fmod(math.fmod(end - start, tau) + tau, tau)
Working Values:
X1 = 0.00048399999999998444
Y1 = 0.0002720000000007161
X2 = 0.378484
Y2 = -14.694728
X3 = 3.376
Y3 = -14.307
Proper result/value
Theta = 6.077209477545957
Assume this arc was done CCW
As you noticed, the range of math.acos is [0, pi], making it rather useless for telling you the relative directions of the vectors. To get full circular information about a pair of angles, you can use math.atan2. While regular math.atan has a range of [-pi/2, pi/2], atan2 splits the inputs into two parts and returns an angle in the range (-pi, pi]. You can compute the angles relative to any reference, not necessarily relative to each other:
start = math.atan2(y2 - y1, x2 - x1)
end = math.atan2(y3 - y1, x3 - x1)
Now you can use some common formulae to find the difference between the angles in whatever direction you want. I've implemented some of these in a small utility library I made called haggis. The specific function you want is haggis.math.ang_diff_pos.
First, the "manual" computation:
if direction == 'cw':
start, end = end, start
tau = 2.0 * math.pi
angle = math.fmod(math.fmod(end - start, tau) + tau, tau)
If you want to use my function, you can do
if direction == 'cw':
start, end = end, start
angle = ang_diff_pos(start, end)
All of these operations can be easily vectorized using numpy if you find yourself dealing with many points all at once.
You can use the cross product of the two vector to determine if the two vector need to rotate clock or counter-clock wise.
See code below:
import numpy as np
from numpy import linalg as LA
x1 = 0
y1 = 0
x2 = 2
y2 = 0
x3 = 2
y3 = -2
dir = 'ccw' # or ccw
v1 = np.array([x2-x1,y2-y1])
v2 = np.array( [x3-x1,y3-y1])
# if the cross product is positive, then the two vector need to rotate counter clockwise
rot = np.cross(v1,v2)
vdir = 'ccw' if rot >0 else 'cw'
r = (v1[0]*v2[0]+v1[1]*v2[1])/(LA.norm(v1)*LA.norm(v2))
deg = np.arccos(r)/np.pi*180
if vdir != dir:
deg = 360 -deg
print(deg)
I have code to expand the polygon, it works by multiplying the xs and ys by a factor then re centering the resultant polyon at the center of the original.
I also have code to find the value for the expansion factor, given a point that the polygon needs to reach:
import numpy as np
import itertools as IT
import copy
from shapely.geometry import LineString, Point
def getPolyCenter(points):
"""
http://stackoverflow.com/a/14115494/190597 (mgamba)
"""
area = area_of_polygon(*zip(*points))
result_x = 0
result_y = 0
N = len(points)
points = IT.cycle(points)
x1, y1 = next(points)
for i in range(N):
x0, y0 = x1, y1
x1, y1 = next(points)
cross = (x0 * y1) - (x1 * y0)
result_x += (x0 + x1) * cross
result_y += (y0 + y1) * cross
result_x /= (area * 6.0)
result_y /= (area * 6.0)
return (result_x, result_y)
def expandPoly(points, factor):
points = np.array(points, dtype=np.float64)
expandedPoly = points*factor
expandedPoly -= getPolyCenter(expandedPoly)
expandedPoly += getPolyCenter(points)
return np.array(expandedPoly, dtype=np.int64)
def distanceLine2Point(points, point):
points = np.array(points, dtype=np.float64)
point = np.array(point, dtype=np.float64)
points = LineString(points)
point = Point(point)
return points.distance(point)
def distancePolygon2Point(points, point):
distances = []
for i in range(len(points)):
if i==len(points)-1:
j = 0
else:
j = i+1
line = [points[i], points[j]]
distances.append(distanceLine2Point(line, point))
minDistance = np.min(distances)
#index = np.where(distances==minDistance)[0][0]
return minDistance
"""
Returns the distance from a point to the nearest line of the polygon,
AND the distance from where the normal to the line (to reach the point)
intersets the line to the center of the polygon.
"""
def distancePolygon2PointAndCenter(points, point):
distances = []
for i in range(len(points)):
if i==len(points)-1:
j = 0
else:
j = i+1
line = [points[i], points[j]]
distances.append(distanceLine2Point(line, point))
minDistance = np.min(distances)
i = np.where(distances==minDistance)[0][0]
if i==len(points)-1:
j = 0
else:
j = i+1
line = copy.deepcopy([points[i], points[j]])
centerDistance = distanceLine2Point(line, getPolyCenter(points))
return minDistance, centerDistance
minDistance, centerDistance = distancePolygon2PointAndCenter(points, point)
expandedPoly = expandPoly(points, 1+minDistance/centerDistance)
This code only works when the point is directly opposing one of the polygons lines.
Modify your method distancePolygon2PointAndCenter to instead of
Returns the distance from a point to the nearest line of the polygon
To return the distance from a point to the segment intersected by a ray from the center to the point. This is the line that will intersect the point once the polygon is fully expanded. To get this segment, take both endpoints of each segment of your polygon, and plug them into the equation for the line parallel & intersecting the ray mentioned earlier. That is y = ((centerY-pointY)/(centerX-pointX)) * (x - centerX) + centerY. You want to want to find endpoints where either one of them intersect the line, or the two are on opposite sides of the line.
Then, the only thing left to do is make sure that we pick the segment intersecting the right "side" of the line. To do this, there are a few options. The fail-safe method would be to use the formula cos(theta) = sqrt((centerX**2 + centerY**2)*(pointX**2 + pointY**2)) / (centerX * pointX + centerY * pointY) however, you could use methods such as comparing x and y values, taking the arctan2(), and such to figure out which segment is on the correct "side" of center. You'll just have lots of edge cases to cover. After all this is said and done, your two (unless its not convex, in which case take the segment farthest from you center) endpoints makeup the segment to expand off of.
Determine what is "polygon center" as central point C of expanding. Perhaps it is centroid (or some point with another properties?).
Make a segment from your point P to C. Find intersection point I between PC and polygon edges. If polygon is concave and there are some intersection points, choose the closest one to P.
Calculate coefficient of expanding:
E = Length(PC) / Length(CI)
Calculate new vertex coordinates. For i-th vertex of polygon:
V'[i].X = C.X + (V[i].X - C.X) * E
V'[i].Y = C.Y + (V[i].Y - C.Y) * E
Decide which point you want to reach, then calculate how much % your polygon needs to expand to reach that point and use the shapely.affinity.scale function. For example, in my case I just needed to make the polygon 5% bigger:
region = shapely.affinity.scale(myPolygon,
xfact=1.05, yfact=1.05 )
I'm trying to find the angle required to move my camera so it's directly in front of an object. If my camera is looking at the object at a 30 degree angle from the left, then my script should return 30 degrees. I'm using cv2.decomposeHomographyMat to find a rotation matrix which works fine. There are 4 solutions returned from this function, so in my script I am outputting 4 angles. Of these angles, there are only two unique angles. My problem is I don't know which of these two angles is correct.
I know the decomposeHomographyMat returns four possible solutions, but shouldn't the angles be the same? I also found the coordinates of my points projected on a 2D plane, but I wasn't sure what to do with this information in regards to finding which angle is correct (here pts3D are the 2D points of the object taken from the camera image with a 0 added for the z column making it 3D pts):
for i in range(len(Ts)):
projectedPts = cv2.projectPoints(pts3D, Rs[i], Ts[i], CAM_MATRIX, DIST_COEFFS)[0][:,0,:]
Here is a snippet from my code. Maybe I am incorrectly determining the angles from the rotation matrix? In my example below, y1 and y2 will be the same angle, and y3 and y4 will be the same angle. Can someone help explain how I determine which angle is the correct angle, and why there are two different angles returned?
def rotationMatrixToEulerAngles(R):
sy = math.sqrt(Rs[0][0] * R[0][0] + R[1][0] * R[1][0])
singular = sy < 1e-6
if not singular :
x = math.atan2(R[2][1] , R[2][2])
y = math.atan2(-R[2][0], sy)
z = math.atan2(R[1][0], R[0][0])
else :
x = math.atan2(-R[1][2], R[1][1])
y = math.atan2(-R[2][0], sy)
z = 0
return np.rad2deg(y)
H, status = cv2.findHomography(corners, REFPOINTS)
output = cv2.warpPerspective(frame, H, (800,800))
# Rs returns 4 matricies, we use the first one
_, Rs, Ts, Ns = cv2.decomposeHomographyMat(H, CAM_MATRIX)
y1 = rotationMatrixToEulerAngles(Rs[0])
y2 = rotationMatrixToEulerAngles(Rs[1])
y3 = rotationMatrixToEulerAngles(Rs[2])
y4 = rotationMatrixToEulerAngles(Rs[3])
Thanks!
Let's say you have two arrays of data values from a calculation, that you can model with a continuos, differentiable function each. Both "lines" of data points intersect at (at least) one point and now the question is whether the functions behind these datasets are actually crossing or anticrossing.
The image below shows the situation, where I know (from the physics behind it) that at the upper two "contact points" the yellow and green lines actually should "switch color", whereas at the lower one both functions go out of each others way:
To give an easier "toy set" of data, take this code for example:
import matplotlib.pyplot as plt
import numpy as np
x=np.arange(-10,10,.5)
y1=[np.absolute(i**3)+100*np.absolute(i) for i in x]
y2=[-np.absolute(i**3)-100*np.absolute(i) for i in x][::-1]
plt.scatter(x,y1)
plt.scatter(x,y2,color='r')
plt.show()
Which should produce the following image:
Now how could I extrapolate whether the trend behind the data is crossing (so the data from the lower left continues to the upper right) or anti-crossing (as indicated with the colors above, the data from the lower left continues to the lower right)?
So far I was able to find the "contact point" between these to datasets by looking at the derivative of the Difference between them, roughly like this:
closePoints=np.where(np.diff(np.diff(array_A - array_B) > 0))[0] + 1
(which probably would be faster to evaluate with something like scipy's cKDTree).
Should I go on and (probably very inefficiently) check the derivative on both sides of the intersection? Or can I somehow check if the extrapolation of the data on the left side fits better to crossing or anticrossing?
I understood your problem as:
You have two sequences of points in a 2D plane.
The true curves can be approximated by straight lines between consecutive points of the sequences.
You want to know how often and where the two curves intersect (not only come into contact but really cross each other) (polygon intersection).
A potential solution is:
You look at each combination of a line segment of one curve with a line segment of another curve.
Combinations where the bounding boxes of the line segments have an overlap can potentially contain intersection points.
You solve a linear equation system to compute if and where an intersection between two lines occurs
In case of no solution to the equation system the lines are parallel but not overlapping, dismiss this case
In case of one solution check that it is truly within the segments, if so record this crossing point
In case of infinitely many intersections the lines are identical. This is also no real crossing and can be dismissed.
Do this for all combinations of line segments and eliminate twin cases, i.e. where the two curves intersect at a segment start or end
Let me give some details:
How to check if two bounding-boxes (rectangles) of the segments overlap so that the segments potentially can intersect?
The minimal x/y value of one rectangle must be smaller than the maximal x/y value of the other. This must hold for both.
If you have two segments how do you solve for intersection points?
Let's say segment A has two points (x1, y1) and (x2, y2) and segment B has two points (x2, y3) and (x4, y4).
Then you simply have two parametrized line equations which have to be set equal:
(x1, y1) + t * (x2 - x1, y2 - y1) = (x3, y3) + q * (x4 - x3, y4 - y3)
And you need to find all solutions where t or q in [0, 1). The corresponding linear equation system may be rank deficient or not solvable at all, best is to use a general solver (I chose numpy.linalg.lstsq) that does everything in one go.
Curves sharing a common point
Surprisingly difficult are cases where one point is common in the segmentation of both curves. The difficulty lies then in the correct decision of real intersection vs. contact points. The solution is to compute the angle of both adjacent segments of both curves (gives 4 angles) around the common point and look at the order of the angles. If both curves come alternating when going around the equal point then it's an intersection, otherwise it isn't.
And a code example based on your data:
import math
import matplotlib.pyplot as plt
import numpy as np
def intersect_curves(x1, y1, x2, y2):
"""
x1, y1 data vector for curve 1
x2, y2 data vector for curve 2
"""
# number of points in each curve, number of segments is one less, need at least one segment in each curve
N1 = x1.shape[0]
N2 = x2.shape[0]
# get segment presentation (xi, xi+1; xi+1, xi+2; ..)
xs1 = np.vstack((x1[:-1], x1[1:]))
ys1 = np.vstack((y1[:-1], y1[1:]))
xs2 = np.vstack((x2[:-1], x2[1:]))
ys2 = np.vstack((y2[:-1], y2[1:]))
# test if bounding-boxes of segments overlap
mix1 = np.tile(np.amin(xs1, axis=0), (N2-1,1))
max1 = np.tile(np.amax(xs1, axis=0), (N2-1,1))
miy1 = np.tile(np.amin(ys1, axis=0), (N2-1,1))
may1 = np.tile(np.amax(ys1, axis=0), (N2-1,1))
mix2 = np.transpose(np.tile(np.amin(xs2, axis=0), (N1-1,1)))
max2 = np.transpose(np.tile(np.amax(xs2, axis=0), (N1-1,1)))
miy2 = np.transpose(np.tile(np.amin(ys2, axis=0), (N1-1,1)))
may2 = np.transpose(np.tile(np.amax(ys2, axis=0), (N1-1,1)))
idx = np.where((mix2 <= max1) & (max2 >= mix1) & (miy2 <= may1) & (may2 >= miy1)) # overlapping segment combinations
# going through all the possible segments
x0 = []
y0 = []
for (i, j) in zip(idx[0], idx[1]):
# get segment coordinates
xa = xs1[:, j]
ya = ys1[:, j]
xb = xs2[:, i]
yb = ys2[:, i]
# ax=b, prepare matrices a and b
a = np.array([[xa[1] - xa[0], xb[0] - xb[1]], [ya[1] - ya[0], yb[0]- yb[1]]])
b = np.array([xb[0] - xa[0], yb[0] - ya[0]])
r, residuals, rank, s = np.linalg.lstsq(a, b)
# if this is not a
if rank == 2 and not residuals and r[0] >= 0 and r[0] < 1 and r[1] >= 0 and r[1] < 1:
if r[0] == 0 and r[1] == 0 and i > 0 and j > 0:
# super special case of one segment point (not the first) in common, need to differentiate between crossing or contact
angle_a1 = math.atan2(ya[1] - ya[0], xa[1] - xa[0])
angle_b1 = math.atan2(yb[1] - yb[0], xb[1] - xb[0])
# get previous segment
xa2 = xs1[:, j-1]
ya2 = ys1[:, j-1]
xb2 = xs2[:, i-1]
yb2 = ys2[:, i-1]
angle_a2 = math.atan2(ya2[0] - ya2[1], xa2[0] - xa2[1])
angle_b2 = math.atan2(yb2[0] - yb2[1], xb2[0] - xb2[1])
# determine in which order the 4 angle are
if angle_a2 < angle_a1:
h = angle_a1
angle_a1 = angle_a2
angle_a2 = h
if (angle_b1 > angle_a1 and angle_b1 < angle_a2 and (angle_b2 < angle_a1 or angle_b2 > angle_a2)) or\
((angle_b1 < angle_a1 or angle_b1 > angle_a2) and angle_b2 > angle_a1 and angle_b2 < angle_a2):
# both in or both out, just a contact point
x0.append(xa[0])
y0.append(ya[0])
else:
x0.append(xa[0] + r[0] * (xa[1] - xa[0]))
y0.append(ya[0] + r[0] * (ya[1] - ya[0]))
return (x0, y0)
# create data
def data_A():
# data from question (does not intersect)
x1 = np.arange(-10, 10, .5)
x2 = x1
y1 = [np.absolute(x**3)+100*np.absolute(x) for x in x1]
y2 = [-np.absolute(x**3)-100*np.absolute(x) for x in x2][::-1]
return (x1, y1, x2, y2)
def data_B():
# sine, cosine, should have some intersection points
x1 = np.arange(-10, 10, .5)
x2 = x1
y1 = np.sin(x1)
y2 = np.cos(x2)
return (x1, y1, x2, y2)
def data_C():
# a spiral and a diagonal line, showing the more general case
t = np.arange(0, 10, .2)
x1 = np.sin(t * 2) * t
y1 = np.cos(t * 2) * t
x2 = np.arange(-10, 10, .5)
y2 = x2
return (x1, y1, x2, y2)
def data_D():
# parallel and overlapping, should give no intersection point
x1 = np.array([0, 1])
y1 = np.array([0, 0])
x2 = np.array([-1, 3])
y2 = np.array([0, 0])
return (x1, y1, x2, y2)
def data_E():
# crossing at a segment point, should give exactly one intersection point
x1 = np.array([-1,0,1])
y1 = np.array([0,0,0])
x2 = np.array([0,0,0])
y2 = np.array([-1,0,1])
return (x1, y1, x2, y2)
def data_F():
# contacting at one segment point, should give no intersection point
x1 = np.array([-1,0,-1])
y1 = np.array([-1,0,1])
x2 = np.array([1,0,1])
y2 = np.array([-1,0,1])
return (x1, y1, x2, y2)
x1, y1, x2, y2 = data_F() # select the data you like here
# show example data
plt.plot(x1, y1, 'b-o')
plt.plot(x2, y2, 'r-o')
# call to intersection computation
x0, y0 = intersect_curves(x1, y1, x2, y2)
print('{} intersection points'.format(len(x0)))
# display intersection points in green
plt.plot(x0, y0, 'go')
plt.show() # zoom in to see that the algorithm is correct
I tested it extensively and should get most (all) border cases right (see data_A-F in code). Some examples:
Some Comments:
The assumption about the line approximation is crucial. Most true curves might only be to some extent be approximable to lines locally. Because of this places where the two curves come close but to not intersect with a distance in the order of the distance of consecutive sampling points of your curve - you may obtain false positives or false negatives. The solution is then to either use more points or to use additonal knowledge about the true curves. Splines might give a lower error rate but also require more computations, better sampling of the curves would be preferable then.
Self-intersection is trivially included when taking two times the same curve and let them intersect
This solution has the additional advantage that it isn't restricted to curves of the form y=f(x) but it's applicable to arbitrary curves in 2D.
You could use a spline interpolation for the difference function g(x) = y1(x) - y(2). Finding the minimum of the square g(x)**2 would be a contact or crossing point. Looking at the first and second derivative you could decide if it is a contact point( g(x) has minimum, g'(x)==0, g''(x) != 0) or a crossing point (g(x) is a stationary point, g'(x)==0, g''(x)==0).
The following code searches for a minimum of g(x)**2 in constrained interval and then plot the derivatives. The use of a constrained interval is to find multiple points successively by excluding intervals in which previous points were.
import matplotlib.pyplot as plt
import numpy as np
import scipy.optimize as sopt
import scipy.interpolate as sip
# test functions:
nocrossingTest = True
if nocrossingTest:
f1 = lambda x: +np.absolute(x**3)+100*np.absolute(x)
f2 = lambda x: -np.absolute(x**3)-100*np.absolute(x)
else:
f1 = lambda x: +np.absolute(x**3)+100*x
f2 = lambda x: -np.absolute(x**3)-100*x
xp = np.arange(-10,10,.5)
y1p, y2p = f1(xp), f2(xp) # test array
# Do Interpolation of y1-y2 to find crossing point:
g12 = sip.InterpolatedUnivariateSpline(xp, y1p - y2p) # Spline Interpolator of Difference
dg12 = g12.derivative() # spline derivative
ddg12 = dg12.derivative() # spline derivative
# Bounded least square fit to find minimal distance
gg = lambda x: g12(x)*g12(x)
rr = sopt.minimize_scalar(gg, bounds=[-1,1]) # search minium in Interval [-1,1]
x_c = rr['x'] # x value with minimum distance
print("Crossing point is at x = {} (Distance: {})".format(x_c, g12(x_c)))
fg = plt.figure(1)
fg.clf()
fg,ax = plt.subplots(1, 1,num=1)
ax.set_title("Function Values $y$")
ax.plot(xp, np.vstack([y1p,y2p]).T, 'x',)
xx = np.linspace(xp[0], xp[-1], 1000)
ax.plot(xx, np.vstack([f1(xx), f2(xx)]).T, '-', alpha=0.5)
ax.grid(True)
ax.legend(loc="best")
fg.canvas.draw()
fg = plt.figure(2)
fg.clf()
fg,axx = plt.subplots(3, 1,num=2)
axx[0].set_title("$g(x) = y_1(x) - y_2(x)$")
axx[1].set_title("$dg(x)/dx$")
axx[2].set_title("$d^2g(x)/dx^2$")
for ax,g in zip(axx, [g12, dg12, ddg12]):
ax.plot(xx, g(xx))
ax.plot(x_c, g(x_c), 'ro', alpha=.5)
ax.grid(True)
fg.tight_layout()
plt.show()
The difference function show that the difference is not smooth:
I have seen the great tutorial by Peter Colling Ridge on
http://www.petercollingridge.co.uk/pygame-physics-simulation/
and I am extending the PyParticles script
The code is available on the site(for free), I am using PyParticles4.py
Classes used in the tutorial
The Particle Class
Circular 2d objects with radius,mass,velocity,location
The Spring Class
A spring that binds 2 objects (Particles) and uses the Hooke's law (F = -kx) to determine the interaction between them
The Environment Class
The Environment where the Particles interact
I was wondering if I could to use 2 Particles and make a 'Rod' class (like the Spring class in the tutorial) that had a specific length and didn't allow the particles to come closer go further than that (specified) length.
Also,
Appling a force (when needed) to each Particle such that if one is pulled toward the left, so does the other, but Realistically..
Much like if a 2 different types of balls were joined(from the center) using a steel rod, but in 2-d..
And I don't want to use 3rd party modules
Thanks in advance..
EDIT/UPDATE:
Tried to apply constraint theorem (it failed)
Here's the code:
class Rod:
def __init__(self, p1, p2, length=50):
self.p1 = p1
self.p2 = p2
self.length = length
def update(self):
'Updates The Rod and Particles'
# Temp store of co-ords of Particles involved
x1 = self.p1.x
x2 = self.p2.x
###### Same for Y #######
y1 = self.p1.y
y2 = self.p2.y
# Calculation of d1,d2,d3 and final values (x2,y2)
# from currently known values(x1,y1)...
# From Constraint algorithm(see #HristoIliev's comment)
dx1 = x2 - x1
dy1 = y2 - y1
# the d1, d2, d3
d1 = math.hypot(dx1,dy1)
d2 = abs(d1)
d3 = (d2-self.length)/d2
x1 = x1 + 0.5*d1*d3
x2 = x2 - 0.5*d1*d3
y1 = y1 + 0.5*d1*d3
y2 = y1 - 0.5*d1*d3
# Reassign next positions
self.p1.x = x1
self.p2.x = x2
###### Same for Y #######
self.p1.y = y1
self.p2.y = y2
A rod in 2D has 3 degrees of freedom (2 velocities/positions + 1 rotation/angular freq).
I would represent the position of the center which is modified by forces in the usual way and calculate the position of the particles using the rotation (for simplicity, about the center of the system) variable.
The rotation is modified by forces by
ang_accel = F * r * sin (angle(F,r)) / (2*M * r^2)
Where
ang_accel is the angular acceleration
F is a force acting on a particular ball so there is 2 torques* that add up as there is two forces that add up (vector-wise) in order to update the position of the center.
r is half of the length
angle(F,r) is the angle between the force vector and the radius vector (from the center to the particle that suffers the force),
So that
F * r * sin (angle(F,r)) is the torque about the center, and
2*M * r^2 is the moment of inertia of the system of two points around the center.