Develop Reference

I have 2 points (x1,y1) & (x2,y2) and a circle, suppose there is a line between the 2 points I need to check if there is collision with the circle

As I mentioned in the title, suppose I have a line segment from point 1 to point 2 and there is a circle with a center and radius I need to check if there is going to be a collision with the circle using code. This is how far I got.
However, there is an issue with closestX and closestY since I need to check if they are on the line segment from point 1 to point 2 because if they are not on the line segment then there will be No collision. Sadly though Im stuck here and I cannot figure out a way to check if they are on the line segment or not. Please help thank you.
import math
p=2
obsHeight=200
DroneHeight=150
cx=3
cy=3
r=1
x1=1
y1=1
x2=1.5
y2=1.5
if DroneHeight<=obsHeight:
distX= x1 - x2
distY= y1 - y2
length=math.sqrt((distX*distX) + (distY*distY ))
dot= (((cx-x1)*(x2-x1)) + ((cy-y1)*(y2-y1)) )/(math.pow(length,p))
closestX=x1+( dot * (x2-x1))
closestY=y1+( dot * (y2-y1))
print(" Closest x: ",closestX)
print(" Closest y: ",closestY)
distX=closestX-cx
distY= closestY-cy
distance= math.sqrt((distX*distX) + (distY*distY ))
print("The distance is: ", distance)
print("The length is: ", length)
if (r==distance):
print("Touching")
elif (distance<r):
print("COLLIDING")
else:
print("Will not collide")
else:
print(" Will not collide, the drone is higher than the obstacle")

Ignoring the specificity of your code, let's say that you have a line segment, a center and a radius. Let's write a general solution to whether a line segment in N-dimensions intersects a hyper-sphere in N-dimensions. This will give us the correct solution for your problem in the special case of 2D.
Your function signature would look like this:
def intersects(p1, p2, c, r):
p1 and p2 are vectors of length N. In your case, p1 = np.array([1, 1]), and p2 = np.array([1.5, 1.5]). c is a vector of the same length (c = np.array([3, 3])), and r is a scalar radius (r = 1). I strongly recommend using numpy arrays for your math because it is much faster if you use it right, and you can apply element-wise operations to arrays (e.g. p2 - p1) without using a loop.
A line passing through p1 and p2 can be parametrized as p = p1 + t * (p2 - p1). Every point on the line p corresponds some value of the parameter t. Specifically, t == 0 corresponds to p = p1 and t == 1 corresponds to p = p2. That means that you can know if a point is on the line segment by checking if its parameter is in the range [0, 1].
The problem then becomes finding the value of t such that p is closest to c. If t < 0 or t > 1, then you know that the extrema for the line segment are at the endpoints. Otherwise, you need to compare the distances of both the endpoints and the p you found.
There are a couple of different ways of coming up with the solution. The geometric approach uses the fact that the nearest approach happens at the perpendicular from c to the line. The differential approach finds where the derivative of the length is zero. I will show the former here.
Looking at the diagram, you have the following equation:
(c - p).dot(p2 - p1) == 0
(c - p1 - t * (p2 - p1)).dot(p2 - p1) == 0
(c - p1).dot(p2 - p1) - t * (p2 - p1).dot(p2 - p1) == 0
t == (c - p1).dot(p2 - p1) / (p2 - p1).dot(p2 - p1)
You can now write your function like this:
def intersects(p1, p2, c, r):
c1 = np.subtract(c, p1)
c2 = np.subtract(c, p2)
dist1 = np.linalg.norm(c1)
dist2 = np.linalg.norm(c2)
# If point are on opposite sides of circle, intersects
if (r - dist1) * (r - dist2) < 0:
return True
# If both on inside, does not intersect
if r > dist1:
return False
dp = np.subtract(p2, p1)
t = dp.dot(c1) / dp.dot(dp)
# If closest approach is outside segment, does not intersect
# convince yourself of this (use symmetry about the line c-p)
if t < 0 or t > 1:
return False
cp = np.subtract(p1 + t * dp, c)
distp = np.linalg.norm(cp)
# only other possibility of approach is when closest point is inside radius
return distp <= r
The problem of finding the distance between a point and a line has cropped up a number of times on Stack Overflow, and in my applications as well, so I recently added it to a library of utilities that I maintain, haggis. You can build a solution using haggis.math.segment_distance with very similar logic. I specifically made the function operate in line segment or full-line mode for this purpose:
def intersects(p1, p2, c, r):
dist1 = np.linalg.norm(c1 := np.subtract(c, p1))
dist2 = np.linalg.norm(c2 := np.subtract(c, p2))
if (r - dist1) * (r - dist2) < 0: # Opposite sides of circle
return True
if r > dist1: # Both inside circle
return False
d = segment_distance(c, p1, p2)
return d < r
You could rewrite the last two lines as follows:
d, t = segment_distance(c, p1, p2, segment=False, return_t=True)
return d < r and 0 <= t <= 1

You can calculate the squared distance of the center of the circle to the line by
d2 = ((y1-y2)*(cx-x1)+(x2-x1)*(cy-y1))**2/((x2-x1)**2+(y2-y1)**2)
Now just compare that value to the squared radius. If d2<r**2 then the line cuts the circle.
Edit:
I think you already are pretty close to a correct solution. In the line where you calculate the dot product, you should divide by the length of the line segment and not by its squared length. Just putting a (...)**0.5 around the math.pow expression should give the correct value.
Note:
Phython has a builtin power operator called **, so no need to use math.pow

Python: Intersection of spheres

I am extremely new to programming but I decided to take on an interesting project as I recently learnt how to represent a sphere in parametric form. When intersecting three spheres, there are two points of intersections that are distinct unless they only overlap at a singular point.
Parametric representation of a sphere:
The code I have is modified from the answer from Python/matplotlib : plotting a 3d cube, a sphere and a vector?, adding the ability to dictate the x, y and z origin and the radius of the sphere. Many similar questions were written in C++, Java, and C#, which I cannot understand at all (I barely know what I am doing so go easy on me).
My Code:
import numpy as np
def make_sphere_x(x, radius):
u, v = np.mgrid[0:2 * np.pi:5000j, 0:np.pi:2500j]
x += radius * np.cos(u) * np.sin(v)
return x
def make_sphere_y(y, radius):
u, v = np.mgrid[0:2 * np.pi:5000j, 0:np.pi:2500j]
y += radius * np.sin(u) * np.sin(v)
return y
def make_sphere_z(z, radius):
u, v = np.mgrid[0:2 * np.pi:5000j, 0:np.pi:2500j]
z += radius * np.cos(v)
return z
#x values
sphere_1_x = make_sphere_x(0, 2)
sphere_2_x = make_sphere_x(1, 3)
sphere_3_x = make_sphere_x(-1, 4)
#y values
sphere_1_y = make_sphere_y(0, 2)
sphere_2_y = make_sphere_y(1, 3)
sphere_3_y = make_sphere_y(0, 4)
#z values
sphere_1_z = make_sphere_z(0, 2)
sphere_2_z = make_sphere_z(1, 3)
sphere_3_z = make_sphere_z(-2, 4)
#intercept of x-values
intercept_x = list(filter(lambda x: x in sphere_1_x, sphere_2_x))
intercept_x = list(filter(lambda x: x in intercept_x, sphere_3_x))
print(intercept_x)
Problems:
Clearly there must be a better way of finding the intercepts. Right now, the code generates points at equal intervals, with the number of intervals I specify under the imaginary number in np.mgrid. If this is increased, the chances of an intersection should increase (I think) but when I try to increase it to 10000j or above, it just spits a memory error.
There are obvious gaps in the array and this method would most likely be erroneous even if I have access to a super computer and can crank up the value to an obscene value. Right now the code results in a null set.
The code is extremely inefficient, not that this is a priority but people like things in threes right?
Feel free to flame me for rookie mistakes in coding or asking questions on Stack Overflow. Your help is greatly valued.

Using scipy.optimize.fsolve you can find the root of a given function, given an initial guess that is somewhere in the range of your solution. I used this approach to solve your problem and it seems to work for me. The only downside is that it only provides you one intersection. To find the second one you would have to tinker with the initial conditions until fsolve finds the second root.
First we define our spheres by defining (arbitrary) radii and centers for each sphere:
a1 = np.array([0,0,0])
r1 = .4
a2 = np.array([.3,0,0])
r2 = .5
a3 = np.array([0,.3,0])
r3 = .5
We then define how to transform back into cartesian coordinates, given angles u,v
def position(a,r,u,v):
return a + r*np.array([np.cos(u)*np.sin(v),np.sin(u)*np.sin(v),np.cos(v)])
Now we think about what equation we need to find the root of. For any intersection point, it holds that for perfect u1,v1,u2,v2,u3,v3 the positions position(a1,r1,u1,v1) = position(a2,r2,u2,v2) = position(a3,r3,u3,v3) are equal. We thus find three equations which must be zeros, namely the differences of two position vectors. In fact, as every vector has 3 components, we have 9 equations which is more than enough to determine our 6 variables.
We find the function to minimize as:
def f(args):
u1,v1,u2,v2,u3,v3,_,_,_ = args
pos1 = position(a1,r1,u1,v1)
pos2 = position(a2,r2,u2,v2)
pos3 = position(a3,r3,u3,v3)
return np.array([pos1 - pos2, pos1 - pos3, pos2 - pos3]).flatten()
fsolve needs the same amount of input and output arguments. As we have 9 equations but only 6 variables I simply used 3 dummy variables so the dimensions match. Flattening the array in the last line is necessary as fsolve only accepts 1D-Arrays.
Now the intersection can be found using fsolve and a (pretty random) guess:
guess = np.array([np.pi/4,np.pi/4,np.pi/4,np.pi/4,np.pi/4,np.pi/4,0,0,0])
x0 = fsolve(f,guess)
u1,v1,u2,v2,u3,v3,_,_,_ = x0
You can check that the result is correct by plugging the angles you received into the position function.

The problem would be better tackled using trigonometry.
Reducing the problem into 2D circles, we could do:
import math
import numpy
class Circle():
def __init__(self, cx, cy, r):
"""initialise Circle and set main properties"""
self.centre = numpy.array([cx, cy])
self.radius = r
def find_intercept(self, c2):
"""find the intercepts between the current Circle and a second c2"""
#Find the distance between the circles
s = c2.centre - self.centre
self.dx, self.dy = s
self.d = math.sqrt(numpy.sum(s**2))
#Test if there is an overlap. Note: this won't detect if one circle completly surrounds the other.
if self.d > (self.radius + c2.radius):
print("no interaction")
else:
#trigonometry
self.theta = math.atan2(self.dy,self.dx)
#cosine rule
self.cosA = (c2.radius**2 - self.radius**2 + self.d**2)/(2*c2.radius*self.d)
self.A = math.acos(self.cosA)
self.Ia = c2.centre - [math.cos(self.A+self.theta)*c2.radius, math.sin(self.A+self.theta)*c2.radius]
self.Ib = c2.centre - [math.cos(self.A-self.theta)*c2.radius,-math.sin(self.A-self.theta)*c2.radius]
print("Interaction points are : ", self.Ia, " and: ", self.Ib)
#define two arbitrary circles
c1 = Circle(2,5,5)
c2 = Circle(1,6,4)
#find the intercepts
c1.find_intercept(c2)
#test results by reversing the operation
c2.find_intercept(c1)

Recover angles after transformation

This seems like an easy enough task but I've failed to find a solution and I've run out of ideas.
I have two angles which I employ to define some transformation coefficients. Now, I don't actually have the values for those angles in my real data, I have the coefficients and I need to recover the angles.
I thought the arctan2 function would take care of this, but there are cases where it fails to recover the proper a1 angle and instead returns its 180 complement, which later affects the recovery of the a2 angle.
What am I doing wrong and how can I recover the a1, a2 angles properly?
import numpy as np
# Repeat 100 times
for _ in range(100):
# Define two random angles in the range [-pi, pi]. I do not have these
# angles in my actual data, I have the A,B,C coefficients shown below.
a1, a2 = np.random.uniform(-180., 180., (2,))
# Transformation coefficients using the above angles.
# This is the data I actually have.
a1_rad, a2_rad = np.deg2rad(a1), np.deg2rad(a2) # to radians
A = - np.sin(a1_rad) * np.sin(a2_rad)
B = np.cos(a1_rad) * np.sin(a2_rad)
C = np.cos(a2_rad)
# Recover a1 using 'arctan2' (returns angle in the range [-pi, pi])
a1_recover = np.arctan2(-A / B, 1.)
# Now obtain sin(a2), used below to obtain 'a2'
sin_a2 = -A / np.sin(a1_recover)
# Recover a2 using 'arctan2', where: C = cos(a2)
a2_recover = np.arctan2(sin_a2, C)
# Print differences.
a1_recover = np.rad2deg(a1_recover)
print("a1: {:.2f} = {} - {}".format(a1 - a1_recover, a1, a1_recover))
a2_recover = np.rad2deg(a2_recover)
print("a2: {:.2f} = {} - {}\n".format(a2 - a2_recover, a2, a2_recover))

When a2_rad equals 0, (A, B, C) equals (0, 0, 1) no matter what a1_rad equals. So the transformation is not 1-to-1. Therefore there is no well-defined inverse.
def ABC(a1, a2):
a1_rad, a2_rad = np.deg2rad(a1), np.deg2rad(a2) # to radians
A = - np.sin(a1_rad) * np.sin(a2_rad)
B = np.cos(a1_rad) * np.sin(a2_rad)
C = np.cos(a2_rad)
return A, B, C
print(ABC(0, 0))
# (-0.0, 0.0, 1.0)
print(90, 0)
# (-0.0, 0.0, 1.0)
print(-90, 0)
# (-0.0, 0.0, 1.0)
A similar problem happens at the opposite (South) pole. Within the limits of floating point accuracy, all these values (of the form ABC(a1, 180)) are essentially equal too:
ABC(1, 180)
# (-2.1373033680837913e-18, 1.2244602795081332e-16, -1.0)
ABC(0, 180)
# (-0.0, 1.2246467991473532e-16, -1.0)
ABC(90, 180)
# (-1.2246467991473532e-16, 7.498798913309288e-33, -1.0)
You can think of a1, a2 as coordinates on a unit sphere where a1
represents the angle away from the x-axis (more often called theta) and a2
represents the angle away from the z-axis (often called phi).
A,B,C represents the same point on the unit sphere in Cartesian coordinates.
Usually spherical coordinates restrict a1 to the range [0, 2*pi) and a2 to the range [0, pi].
Even with this restriction, the North and South poles have more than one (actually infinite number of) valid representation.

You cannot restore angle sign information because it was loosed in A,B calculation (formation).
8 possible combinations of sin/cos signs give only 4 results of A/B signs (and sign of cos(a2) cannot help here).
Note that for spherical coordinates inclination range is only 0..Pi

You should use np.arctan2(-A , B) instead of np.arctan2(-A / B, 1.). With the latter you are losing information: A = -1 and B = 1 will give the same result as A - 1 and B = -1, hence the 180 mismatch sometimes.
If you restrict a2 to be in (0,180) then you can recover the angles. Note that with this restriction a2 can be recovered as acos(C). (I've tried this but since my program is in C it might not be helpful)

How to find a point (if any) on quadratic Bezier with a given tangent direction?

I'm currently working on a python library to extract pen strokes from TrueType fonts
- Here I'm defining a stroke as a midline running between a test point and its reflected point.
I'm using the term reflected point to refer to the closest point on the opposite side of the 'ink' region which under normal circumstances (apart from say at a serif stem) would also have a tangent in the opposite direction to the test point.
I'm working in python using fontTools and a bezier library that I rolled from the processing code described at http://pomax.github.io/bezierinfo/#extremities .
Where I'm stuck stuck at the moment is on how to find the point on a quadratic bezier curve that has a given tangent, my mathematics skills are pretty rudimentary on a good day with a clear head [which it is not rite now] so I was hoping someone with a sharper mind could point out a birds eye overview on how to achieve this.
At the moment the only thing I can think of is to approach it numerically with something similar to the Newton-Raphson root finding algorithm but evaluating the 1st derivative against the target direction values. I am hoping however there is a symbolic solution as this needs to be run on every other curve for each curve in the glyphs contours.

Using the notation given in http://pomax.github.io/bezierinfo/#extremities, a quadratic Bezier curve is given by:
B(t) = P1*(1-t)**2 + 2*P2*(1-t)*t + P3*t**2
Therefore, (by taking the derivative of B with respect to t) the tangent to the curve is given by
B'(t) = -2*P1*(1-t) + 2*P2*(1-2*t) + 2*P3*t
= 2*(P1 - 2*P2 + P3)*t + 2*(-P1 + P2)
Given some tangent direction V, solve
B'(t) = V
for t. If there is a solution, t = ts, then the point on the Bezier curve which has tangent direction V is given by B(ts).
We essentially want to know if two vectors (B'(t) and V) are parallel or anti-parallel. There is a trick to doing that.
Two vectors, X and Y, are perpendicular if their dot product is zero. If X = (a,b) and Y = (c,d) then the dot product of X and Y is given by
a*c + b*d
So, X and Y are parallel if X and Y_perp are perpendicular, where Y_perp is a vector perpendicular to Y.
In 2-dimensions, if in coordinates Y = (a,b) then Y_perp = (-b, a). (There are two perpendicular vectors possible, but this one will do.) Notice that -- using the formula above -- the dot product of Y and Y_perp is
a*(-b) + b*(a) = 0
So indeed, this jibes with the claim that perpendicular vectors have a dot product equal to 0.
Now, to solve our problem: Let
B'(t) = (a*t+b, c*t+d)
V = (e, f)
Then B'(t) is parallel (or anti-parallel) to V if or when
B'(t) is perpendicular to V_perp, which occurs when
dot product((a*t+b, c*t+d), (-f, e)) = 0
-(a*t+b)*f + (c*t+d)*e = 0
We know a, b, c, d, e and f. Solve for t. If t lies between 0 and 1, then B(t) is part of the Bezier curve segment between P1 and P3.

Thanks to #ubutbu for pointing out the solution for me, figured I would post a working implementation in case someone googles upon this question with a need in the future:
def findParallelT(P, V):
"""finds the t value along a quadratic Bezier such that its tangent (1st derivative) is parallel with the direction vector V.
P : a 3x2 matrix containing the control points of B(t).
V : a pair of values representing the direction of interest (magnitude is ignored).
returns 0.0 <= t <= 1.0 or None
Note the result may be in the same direction or flipped 180 degrees from V"""
#refer to answer given by 'unutbu' on the stackoverflow question:
#http://stackoverflow.com/questions/20825173/how-to-find-a-point-if-any-on-quadratic-bezier-with-a-given-tangent-direction
#for explanation.
# also the 'Rearrange It' app at http://www.wolframalpha.com/widgets/view.jsp?id=4be4308d0f9d17d1da68eea39de9b2ce was invaluable.
assert len(P)==3 and len(P[0])==2 and len(P[1])==2 and len(P[2])==2
assert len(V)==2
P1=P[0]
P2=P[1]
P3=P[2]
# B(t) = P1 * (1-t)**2 + 2*P2*(1-t)*t + P3*t**2
# B'(t) = 2 * (1-t) * (P2 - P1) + 2*t*(P3-P2)
# B'(t) = (a*t + b, c*t + d), V = (e, f)
a = -2 * (P2[0] - P1[0]) + 2 * (P3[0]-P2[0])
b = 2 * (P2[0] - P1[0])
c = -2 * (P2[1] - P1[1]) + 2 * (P3[1]-P2[1])
d = 2 * (P2[1] - P1[1])
e = V[0]
f = V[1]
# -(a*t+b)*f + (c*t+d)*e = 0 for parallel... t = (d*e - b*f) / (a*f - c*e)
num = float(d * e - b * f)
den = float(a * f - c * e)
if den == 0:
return None
else:
t = num / den
return t if 0.0 <= t <= 1.0 else None
if __name__ == "__main__":
assert findParallelT([[0.0,0.0], [0.0,25.0], [25.0,25.0]], [25.0,25.0]) == 0.5
assert findParallelT([[0.0,0.0], [0.0,25.0], [25.0,25.0]], [25.0,-25.0]) == None
assert findParallelT([[200.0,200.0], [10.0, 20.0], [300.0, 0.0]], [10.0,0.0]) == None
assert findParallelT([[407.5, 376.5],[321.0,463.0],[321.0,586.0]], [-246.0,0.0] ) == None
assert findParallelT([[617.0, 882.0],[740.0, 882.0], [826.5, 795.5]], [-245.0,0.0]) == 0.0

shadertoy.com has a search function and "bezier" gets you to analytic solutions for calculation (over 2 domains)
The shortest distance of any planar point to a planar quadratic-bezier, by calculating 2 roots of a cubic function
(quadratic Bezier will only have 2 real roots) (euclidean distance increases exponent of polynomial by +1).
3 points on the Bezier that equate to the 3 local extrema of the cubic function, one for each root of the quadratic.
so you just calculate which of the 3 points is closest.

Calculating point of intersection based on angle and speed

I have a vector consisting of a point, speed and direction. We will call this vector R. And another vector that only consists of a point and a speed. No direction. We will call this one T.
Now, what I am trying to do is to find the shortest intersection point of these two vectors. Since T has no direction, this is proving to be difficult. I was able to create a formula that works in CaRMetal but I can not get it working in python.
Can someone suggest a more efficient way to solve this problem? Or solve my existing formula for X?
Formula:
(source: bja888.com)
Key:
(source: bja888.com)
Where o or k is the speed difference between vectors. R.speed / T.speed

My math could be a bit rusty, but try this:
p and q are the position vectors, d and e are the direction vectors. After time t, you want them to be at the same place:
(1) p+t*d = q+t*e
Since you want the direction vector e, write it like this
(2) e = (p-q)/t + d
Now you don't need the time t, which you can calculate using your speed constraint s (otherwise you could just travel to the other point directly):
The direction vector e has to be of the length s, so
(3) e12 + e22 = s2
After some equation solving you end up with
(4)
I) a = sum(p-q)/(s2-sum(d2))
II) b = 2*sum(d*(p-q))/(s2-sum(d2))
III) c = -1
IV) a + b*t + c*t2 = 0
The sum goes over your vector components (2 in 2d, 3 in 3d)
The last one is a quadratic formula which you should be able to solve on your own ;-)

Let's assume that the first point,
A, has zero speed. In this case, it
should be very simple to find the
direction which will give the
fastest intersection.
Now, A does have a speed. We can force it to have zero speed by deducting it's speed vector from the vector of B. Now we can solve as we did in 1.
Just a rough idea that came to mind...
Some more thoughts:
If A is standing still, then the direction B need to travel in is directly towards A. This gives us the direction in the coordinate system in which A is standing still. Let's call it d.
Now we only need to convert the direction B needs to travel from the coordinate system in which A is still to the coordinate system in which A is moving at the given speed and direction, d2.
This is simply vector addition. d3 = d - d2
We can now find the direction of d3.
And a bit more formal:
A is stationary:
Sb = speed of B, known, scalar
alpha = atan2( a_y-b_y, a_x-b_x )
Vb_x = Sb * cos(alpha)
Vb_y = Sb * sin(alpha)
A moves at speed Sa, direction beta:
Vb_x' = Sb * cos(alpha) + Sa * cos(beta)
Vb_y' = Sb * sin(alpha) + Sa * sin(beta)
alpha' = atan2( Vb_y', Vb_x' )
Haven't tested the above, but it looks reasonable at first glance...

In nature hunters use the constant bearing decreasing range algorithm to catch prey.
I like the explanation of how bats do this link text
We need to define a few more terms.
Point A - the position associated with vector R.
Point B - the position associated with vector T.
Vector AB - the vector from point A to point B
Angle beta - the angle between vector R and vector AB.
Angle theta - the angle between vector T and vector AB
The formula is usually given as
theta = asin( |R| * sin(beta) / |T| )
where
beta = acos( AB.xR.x + AB.yR.y )
You don't want to use this directly, since asin and acos only return angles between -PI/2 to PI/2.
beta = atan2( R.y, R.x ) - atan2( AB.y, AB.x )
x = |R| * sin(beta) / |T|
y = 1 + sqrt( 1 - x*x )
theta = 2*atan2( y, x )
Of course if x > 1 R is too fast and intersection doesn't exist
EG

OK, if I understand you right, you have
R = [ xy0, v, r ]
T = [ xy1, v ]
If you are concerned about the shortest intersection point, this will be achieved when your positions are identical, and in an Euclidean space this also forces the direction of the second "thing" being perpendicular to the first. You can write down the equations for this and solve them easily.