Is there a way to get a solution to three spheres intersection (trilateration) with SymPy? sympy.geometry doesn't have a sphere object, so a direct approach is not feasible. Can SymPy solve a system of non-linear equations as shown at Trilateration and the Intersection of Three Spheres?
Yes. There are different ways but e.g.:
In [21]: x, y, z = symbols('x, y, z', real=True)
In [22]: eq1 = (x-1)**2 + (y-2)**2 + (z-3)**2 - 1
In [23]: eq2 = (x-1)**2 + (y-S(5)/2)**2 + (z-3)**2 - 1
In [24]: eq3 = (x-S.Half)**2 + (y-S(5)/2)**2 + (z-3)**2 - 1
In [25]: solve([eq1, eq2, eq3], [x, y, z])
Out[25]:
⎡⎛ √14⎞ ⎛ √14 ⎞⎤
⎢⎜3/4, 9/4, 3 - ───⎟, ⎜3/4, 9/4, ─── + 3⎟⎥
⎣⎝ 4 ⎠ ⎝ 4 ⎠⎦
This post is in case someone benefits from what I have posted below.
One can actually solve this problem somewhat more geometrically, rather than solving systems of quadratic equations. The approach below can handle three spheres given as centers and radii or as spheres represented by three quadratic equations. Even if the three spheres are given as three quadratic equations of three variables, one can extract the coefficients and obtain the radii and the coordinates of the centers of the three spheres. Then, the two intersection points can be found geometrically, by applying the law of cosine a few times.
def norm_2(v):
return v.dot(v)
def norm(v):
return np.sqrt(norm_2(v))
def extract_center_and_radius(sphere_coefficients):
'''
sphere_equation should be
x^2 + y^2 + z^2 + a1*x + a2*y + a3*z + a4 = 0
so sphere equation should be parametrized
as a list of 4 coefficients
[a1, a2, a3, a4]
'''
a = np.array(sphere_coefficients)
center = - a[0:3] / 2
radius = np.sqrt(center.dot(center) - a[3])
return center, radius
def intersecton_3_spheres(sphere1, sphere2, sphere3):
'''
spherei = (center, radius) is a tuple or a list,
with first entry an np.array representing
the coordinates of the center,
the second being the radius
'''
A, rA = sphere1
B, rB = sphere2
C, rC = sphere3
AB = B-A
AC = C-A
l_AB = norm(AB)
l_AC = norm(AC)
l_BC = norm(C-B)
x2 = (l_AB**2 + l_AC**2 - l_BC**2) / (2 * l_AB)
y2 = np.sqrt(l_AC**2 - x2**2)
x3 = (l_AB**2 + rA**2 - rB**2) / (2*l_AB)
y3 = (l_AC**2 + rA**2 - rC**2) / 2
y3 = (y3 - x2*x3) / y2
z3 = np.sqrt(rA**2 - x3**2 - y3**2)
n = np.cross(AB, AC)
n = n / norm(n)
AB = AB / l_AB
b = np.cross(n, AB)
return A + x3*AB + y3*b + z3*n, A + x3*AB + y3*b - z3*n
def intersecton_of_3_spheres(sphere1, sphere2, sphere3):
'''
the input is three lists of 4 coefficients each
e.g. [a1, a2, a3, a4] which describe the coefficients
of three sphere equations
x^2 + y^2 + z^2 + a1*x + a2*y + a3*z + a4 = 0
'''
center_radius_1 = extract_center_and_radius(sphere1)
center_radius_2 = extract_center_and_radius(sphere2)
center_radius_3 = extract_center_and_radius(sphere3)
return intersecton_3_spheres(center_radius_1, center_radius_2, center_radius_3)
Related
I am trying to find a common tangent to two curves using python but I am not able to solve it.
The equations to the two curves are complicated that involve logarithms.
Is there a way in python to compute the x coordinates of a tangent that is common to both the curves in general. If I have 2 curves f(x) and g(x), I want to find the x-coordinates x1 and x2 on a common tangent where x1 lies on f(x) and x2 on g(x). I am trying f'(x1) = g'(x2) and f'(x1) = f(x1) - f(x2) / (x1 - x2) to get x1 and x2 but I am not able to get values using nonlinsolve as the equations are too complicated.
I want to just find x-coordinates of the common tangent
Can anyone suggest a better way?
import numpy as np
import sympy
from sympy import *
from matplotlib import pyplot as plt
x = symbols('x')
a, b, c, d, e, f = -99322.50019502985, -86864.87072433547, -96876.05627516498, -89703.35055202093, -3390.863799999999, -20942.518
def func(x):
y1_1 = a - a*x + b*x
y1_2 = c - c*x + d*x
c1 = (1 - x) ** (1 - x)
c2 = (x ** x)
y2 = 12471 * (sympy.log((c1*c2)))
y3 = 2*f*x**3 - x**2*(e + 3*f) + x*(e + f)
eqn1 = y1_1 + y2 + y3
eqn2 = y1_2 + y2 + y3
return eqn1, eqn2
val = np.linspace(0, 1)
f1 = sympy.lambdify(x, func(x)[0])(val)
f2 = sympy.lambdify(x, func(x)[1])(val)
plt.plot(val, f1)
plt.plot(val, f2)
plt.show()
I am trying this
x1, x2 = sympy.symbols('x1 x2')
fun1 = func(x1)[0]
fun2 = func(x2)[0]
diff1 = diff(fun1,x1)
diff2 = diff(fun2,x2)
eq1 = diff1 - diff2
eq2 = diff1 - ((fun1 - fun2) / (x1 - x2))
sol = nonlinsolve([eq1, eq2], [x1, x2])
the first thing that needs to be done is to reduce the formulas
for example the first formula is actually this:
formula = x*(1 - x)*(17551.6542 - 41885.036*x) + x*(1 - x)*(41885.036*x - 24333.3818) + 12457.6294706944*x + log((x/(1 - x))**(12000*x)*(1 - x)**12000) - 99322.5001950298
formula = (x-x^2)*(17551.6542 - 41885.036*x) + (x-x^2)*(41885.036*x - 24333.3818) + 12457.6294706944*x + log((x/(1 - x))**(12000*x)*(1 - x)**12000) - 99322.5001950298
# constants
a = 41885.036
b = 17551.6542
c = 24333.3818
d = 12457.6294706944
e = 99322.5001950298
f = 12000
formula = (x-x^2)*(b - a*x) + (x-x^2)*(a*x - c) + d*x + log((x/(1 - x))**(f*x)*(1 - x)**f) - e
formula = (ax^3 -bx^2 + bx - ax^2) + (x-x^2)*(a*x - c) + d*x + log((x/(1 - x))**(f*x)*(1 - x)**f) - e
formula = ax^3 -bx^2 + bx - ax^2 -ax^3 + ax^2 + cx^2 -cx + d*x + log((x/(1 - x))**(f*x)*(1 - x)**f) - e
# collect x terms by power (note how the x^3 tern drops out, so its easier).
formula = (c-b)*x^2 + (b-c+d)*x + log((x/(1 - x))**(f*x)*(1 - x)**f) - e
which is much cleaner and is a quadratic with a log term.
i expect that you can do some work on the log term too, but this is an excercise for the original poster.
likewise the second formula can be reduced in the same way, which is again an excercise for the original poster.
From this, both equations need to be differentiated with respect to x to find the tangent. Then set both formulas to be equal to each other (for a common tangent).
This would completely solve the question.
I actually wonder if this is a python question at all or actually a pure maths question.....
The important point to note is that, since the derivatives are monotonic, for any value of derivative of fun1, there is a solution for fun2. This can be easily seen if you plot both derivatives.
Thus, we want a function that, given an x1, returns an x2 that matches it. I'll use numerical solution because the system is too cumbersome for numerical solution.
import scipy.optimize
def find_equal_value(f1, f2, x, x1):
goal = f1.subs(x, x1)
to_solve = sympy.lambdify(x, (f2 - goal)**2) # Quadratic functions tend to be better behaved, and the result is the same
sol = scipy.optimize.fmin(func=to_solve, x0=x1, ftol=1e-8, disp=False) # The value for f1 is a good starting guess
return sol[0]
I used fmin as the solver above because it worked and I knew how to use it by heart. Maybe root_scalar can give better results.
Using the function above, let's get some pairs (x1, x2) where the derivatives are equal:
df1 = sympy.diff(func(x)[0])
df2 = sympy.diff(func(x)[1])
x1 = 0.25236537 # Close to the zero derivative
x2 = find_equal_value(df1, df2, x, x1)
print(f'Derivative of f1 in x1: {df1.subs(x, x1)}')
print(f'Derivative of f2 in x2: {df2.subs(x, x2)}')
print(f'Error: {df1.subs(x, x1) - df2.subs(x, x2)}')
This results is:
Derivative of f1 in x1: 0.0000768765858083498
Derivative of f2 in x2: 0.0000681969431752805
Error: 0.00000867964263306931
If you want a x2 for several x1s (beware that in some cases the solver hits a value where the logs are invalid. Always check your result for validity):
x1s = np.linspace(0.2, 0.8, 50)
x2s = [find_equal_value(df1, df2, x, x1) for x1 in x1s]
plt.plot(x1s, x2s); plt.grid(); plt.show()
I am working on a python script and I am sure there is an easier way to approach this problem then my solution so far.
Give two coordinates, say (0,0) and (40,40) and a set distance to travel, say 5, how would I find a new coordinate pair for the point that is 5 units from (0,0) heading along the line that connects (0,0) and (40,40)?
I am doing the following given the distance_to_travel and points p0, p1:
ang = math.atan2(p1.y - p0.y, p1.x - p0.x)
xx = node0.x + (distance_to_travel * math.cos(ang))
yy = node0.y + (distance_to_travel * math.sin(ang))
Following the method outlined in my comment:
# find the vector a-b
ab.x, ab.y = p1.x - p0.x, p1.y - p0.y
# find the length of this vector
len_ab = (ab.x * ab.x + ab.y * ab.y) ** 0.5
# scale to length 5
ab5.x, ab5.y = ab.x *5 / len_ab, ab.y *5 / len_ab
# add to a (== p0)
fin.x, fin.y = p0.x + ab5.x, p0.y + ab5.y
You should find the slope of the line between a and b |ab|.
Then you can use 2D spherical (polar) coordinate system to get r mount of distance with a given slope (The slope you found earlier).
Now you can convert from spherical to Cartesian to find x, y coordinates of new point. Then you add this values to values of point a:
import math
def slope(p1, p2):
return math.atan2(
(p2[1] - p1[1]), (p2[0] - p1[0])
)
def spherical_to_cartesian(r, theta):
x = r * math.cos(theta)
y = r * math.sin(theta)
return x, y
def add_points(p1, p2):
return p1[0] + p2[0], p1[1] + p2[1]
if __name__ == '__main__':
a = [0, 0]
b = [40, 40]
slp = slope(a, b)
r = 5
new_p = spherical_to_cartesian(r, slp)
res = add_points(a, new_p)
print(res)
I need algorithm, that solve systems like this:
Example 1:
5x - 6y = 0 <--- line
(10- x)**2 + (10- y)**2 = 2 <--- circle
Solution:
find y:
(10- 6/5*y)**2 + (10- y)**2 = 2
100 - 24y + 1.44y**2 + 100 - 20y + y**2 = 2
2.44y**2 - 44y + 198 = 0
D = b**2 - 4ac
D = 44*44 - 4*2.44*198 = 3.52
y[1,2] = (-b+-sqrt(D))/2a
y[1,2] = (44+-1.8761)/4.88 = 9.4008 , 8.6319
find x:
(10- x)**2 + (10- 5/6y)**2 = 2
100 - 20x + y**2 + 100 - 5/6*20y + (5/6*y)**2 = 2
1.6944x**2 - 36.6666x + 198 = 0
D = b**2 - 4ac
D = 36.6666*36.6666 - 4*1.6944*198 = 2.4747
x[1,2] = (-b+-sqrt(D))/2a
x[1,2] = (36.6666+-1.5731)/3.3888 = 11.2841 , 10.3557
my skills are not enough to write this algorithm please help
and another algorithm that solve this system.
5x - 6y = 0 <--- line
|-10 - x| + |-10 - y| = 2 <--- rhomb
as answer here i need two x and two y.
You can use sympy, Python's symbolic math library.
Solutions for fixed parameters
from sympy import symbols, Eq, solve
x, y = symbols('x y', real=True)
eq1 = Eq(5 * x - 6 * y, 0)
eq2 = Eq((10 - x) ** 2 + (10 - y) ** 2, 2)
solutions = solve([eq1, eq2], (x, y))
print(solutions)
for x, y in solutions:
print(f'{x.evalf()}, {y.evalf()}')
This leads to two solutions:
[(660/61 - 6*sqrt(22)/61, 550/61 - 5*sqrt(22)/61),
(6*sqrt(22)/61 + 660/61, 5*sqrt(22)/61 + 550/61)]
10.3583197613288, 8.63193313444070
11.2810245009662, 9.40085375080520
The other equations work very similar:
eq1 = Eq(5 * x - 6 * y, 0)
eq2 = Eq(Abs(-10 - x) + Abs(-10 - y), 2)
leading to :
[(-12, -10),
(-108/11, -90/11)]
-12.0000000000000, -10.0000000000000
-9.81818181818182, -8.18181818181818
Dealing with arbitrary parameters
For your new question, how to deal with arbitrary parameters, sympy can help to find formulas, at least when the structure of the equations is fixed:
from sympy import symbols, Eq, Abs, solve
x, y = symbols('x y', real=True)
a, b, xc, yc = symbols('a b xc yc', real=True)
r = symbols('r', real=True, positive=True)
eq1 = Eq(a * x - b * y, 0)
eq2 = Eq((xc - x) ** 2 + (yc - y) ** 2, r ** 2)
solutions = solve([eq1, eq2], (x, y))
Studying the generated solutions, some complicated expressions are repeated. Those could be substituted by auxiliary variables. Note that this step isn't necessary, but helps a lot in making sense of the solutions. Also note that substitution in sympy often only considers quite literal replacements. That's by the introduction of c below is done in two steps:
c, d = symbols('c d', real=True)
for xi, yi in solutions:
print(xi.subs(a ** 2 + b ** 2, c)
.subs(r ** 2 * a ** 2 + r ** 2 * b ** 2, c * r ** 2)
.subs(-a ** 2 * xc ** 2 + 2 * a * b * xc * yc - b ** 2 * yc ** 2 + c * r ** 2, d)
.simplify())
print(yi.subs(a ** 2 + b ** 2, c)
.subs(r ** 2 * a ** 2 + r ** 2 * b ** 2, c * r ** 2)
.subs(-a ** 2 * xc ** 2 + 2 * a * b * xc * yc - b ** 2 * yc ** 2 + c * r ** 2, d)
.simplify())
Which gives the formulas:
x1 = b*(a*yc + b*xc - sqrt(d))/c
y1 = a*(a*yc + b*xc - sqrt(d))/c
x2 = b*(a*yc + b*xc + sqrt(d))/c
y2 = a*(a*yc + b*xc + sqrt(d))/c
These formulas then can be converted to regular Python code without the need of sympy. That code will only work for an arbitrary line and circle. Some tests need to be added around, such as c == 0 (meaning the line is just a dot), and d either be zero, positive or negative.
The stand-alone code could look like:
import math
def give_solutions(a, b, xc, yc, r):
# intersection between a line a*x-b*y==0 and a circle with center (xc, yc) and radius r
c =a ** 2 + b ** 2
if c == 0:
print("degenerate line equation given")
else:
d = -a**2 * xc**2 + 2*a*b * xc*yc - b**2 * yc**2 + c * r**2
if d < 0:
print("no solutions")
elif d == 0:
print("1 solution:")
print(f" x1 = {b*(a*yc + b*xc)/c}")
print(f" y1 = {a*(a*yc + b*xc)/c}")
else: # d > 0
print("2 solutions:")
sqrt_d = math.sqrt(d)
print(f" x1 = {b*(a*yc + b*xc - sqrt_d)/c}")
print(f" y1 = {a*(a*yc + b*xc - sqrt_d)/c}")
print(f" x2 = {b*(a*yc + b*xc + sqrt_d)/c}")
print(f" y2 = {a*(a*yc + b*xc + sqrt_d)/c}")
For the rhombus, sympy doesn't seem to be able to work well with abs in the equations. However, you could use equations for the 4 sides, and test whether the obtained intersections are inside the range of the rhombus. (The four sides would be obtained by replacing abs with either + or -, giving four combinations.)
Working this out further, is far beyond the reach of a typical stackoverflow answer, especially as you seem to ask for an even more general solution.
I am trying to find the intersect between a straight line and a quadratic curve, however the result I am getting appears to be imaginary although I don't see how this can be the case as I can see them intersect on real axes:
Import numpy
#quadratic coefficients
a,b,c = (-3.09363812e-04, 1.52138019e+03, -1.87044961e+09)
# y = ax^2 + bx + c
#line coefficients
m,d = (1.06446434e-03, -2.61660911e+03)
#y = mx + d
intersect = (-(b-m)+((b-m)**2 - 4*a*(c-d))**0.5)/(2*a)
print(intersect)
The output of this is 2458883.4674943495-107.95731226786134j
I am trying to find the intersect between the yellow curve over the blue points and the black dotted line
The graphed curves you presented vs. your equations are not the same, and your equations do not intersect.
I rewrote some example code for you. numpy isn't needed, and an exact solution is possible.
import math
import collections
def calculateIntersection(p, l):
b = p.B - l.slope
c = p.C - l.yInt
discriminant = b**2 - (4 * p.A * c)
if discriminant > 0.0:
# 2 points of intersection
x1 = (-b + math.sqrt(discriminant)) / (2.0 * p.A)
x2 = (-b - math.sqrt(discriminant)) / (2.0 * p.A)
return discriminant, [(x1, l.slope * x1 + l.yInt), (x2, l.slope * x2 + l.yInt)]
elif discriminant == 0.0:
# 1 point of intersection
x1 = -b / (2.0 * p.A)
return discriminant, [(x1, slope * x1 + l.yInt)]
else:
# no points of intersection
return discriminant, []
Line = collections.namedtuple('Line', 'slope yInt')
Poly = collections.namedtuple('Poly', 'A B C')
p = Poly(A=-3.09363812e-04, B=1.52138019e+03, C=-1.87044961e+09)
print(p)
l = Line(slope=1.06446434e-03, yInt=-2.61660911e+03)
print(l)
(discriminant, points) = calculateIntersection(p, l)
if (len(points) > 0):
print("Intersection: {}".format(points))
else:
print("No intersection: {}".format(discriminant))
Given 3 points in space (3D): A = (x1, y1, z1), B = (x2, y2, z2) C = (x3, y3, z3); then how to find the center and radius of the circle (arc) that passes through these three points, i.e. find circle equation? Using Python and Numpy here is my initial code
import numpy as np
A = np.array([x1, y1, z1])
B = np.array([x2, y2, z2])
C = np.array([x3, y3, z3])
#Find vectors connecting the three points and the length of each vector
AB = B - A
BC = C - B
AC = C - A
# Triangle Lengths
a = np.linalg.norm(AB)
b = np.linalg.norm(BC)
c = np.linalg.norm(AC)
From the Circumradius definition, the radius can be found using:
R = (a * b * c) / np.sqrt(2.0 * a**2 * b**2 +
2.0 * b**2 * c**2 +
2.0 * c**2 * a**2 -
a**4 - b**4 - c**4)
However, I am having problems finding the Cartesian coordinates of the center. One possible solution is to use the "Barycentric Coordinates" of the triangle points to find the trilinear coordinates of the circumcenter (Circumcenter).
First (using this source) we find the circumcenter barcyntric coordinates:
#barcyntric coordinates of center
b1 = a**2 * (b**2 + c**2 - a**2)
b2 = b**2 * (c**2 + a**2 - b**2)
b3 = c**2 * (a**2 + b**2 - c**2)
Then the Cartesian coordinates of the center (P) would be:
Px = (b1 * A[0]) + (b2 * B[0]) + (b3 * C[0])
Py = (b1 * A[1]) + (b2 * B[1]) + (b3 * C[1])
Pz = (b1 * A[2]) + (b2 * B[2]) + (b3 * C[2])
However, the barcyntric coordinates values above do not seem to be correct. When solved with an example of known values, the radius is correct, but the coordinates of the center are not.
Example: For these three points:
A = np.array([2.0, 1.5, 0.0])
B = np.array([6.0, 4.5, 0.0])
C = np.array([11.75, 6.25, 0.0])
The radius and center coordinates are:
R = 15.899002930062595
P = [13.4207317073, -9.56097560967, 0]
Any ideas on how to find the center coordinates?
There are two issues with your code.
The first is in the naming convention. For all the formulas you are using to hold, the side of length a has to be the one opposite the point A, and similarly for b and B and c and C. You can solve that by computing them as:
a = np.linalg.norm(C - B)
b = np.linalg.norm(C - A)
c = np.linalg.norm(B - A)
The second has to do with the note in your source for the barycentric coordinates of the circumcenter: not necessarily homogeneous. That is, they need not be normalized in any way, and the formula you are using to compute the Cartesian coordinates from the barycentric ones is only valid when they add up to one.
Fortunately, you only need to divide the resulting Cartesian coordinates by b1 + b2 + b3 to get the result you are after. Streamlining a little bit your code for efficiency, I get the results you expect:
>>> A = np.array([2.0, 1.5, 0.0])
>>> B = np.array([6.0, 4.5, 0.0])
>>> C = np.array([11.75, 6.25, 0.0])
>>> a = np.linalg.norm(C - B)
>>> b = np.linalg.norm(C - A)
>>> c = np.linalg.norm(B - A)
>>> s = (a + b + c) / 2
>>> R = a*b*c / 4 / np.sqrt(s * (s - a) * (s - b) * (s - c))
>>> b1 = a*a * (b*b + c*c - a*a)
>>> b2 = b*b * (a*a + c*c - b*b)
>>> b3 = c*c * (a*a + b*b - c*c)
>>> P = np.column_stack((A, B, C)).dot(np.hstack((b1, b2, b3)))
>>> P /= b1 + b2 + b3
>>> R
15.899002930062531
>>> P
array([ 13.42073171, -9.56097561, 0. ])
As an extension to the original problem: Now suppose we have an arc of known length (e.g. 1 unit) extending from point A as defined above (away from point B). How to find the 3D coordinates of the end point (N)? The new point N lies on the same circle passing through points A, B & C.
This can be solved by first finding the angle between the two vectors PA & PN:
# L = Arc Length
theta = L / R
Now all we need to do is rotate the vector PA (Radius) by this angle theta in the correct direction. In order to do that, we need the 3D rotation matrix. For that we use the Euler–Rodrigues formula:
def rotation_matrix_3d(axis, theta):
axis = axis / np.linalg.norm(axis)
a = np.cos(theta / 2.0)
b, c, d = axis * np.sin(theta / 2.0)
rot = np.array([[a*a+b*b-c*c-d*d, 2*(b*c-a*d), 2*(b*d+a*c)],
[ 2*(b*c+a*d), a*a+c*c-b*b-d*d, 2*(c*d-a*b)],
[ 2*(b*d-a*c), 2*(c*d+a*b), a*a+d*d-b*b-c*c]])
return rot
Next, we need to find the rotation axis to rotate PA around. This can be achieved by finding the axis normal to the plane of the circle passing through A, B & C. The coordinates of point N can then be found from the center of the circle.
PA = P - A
PB = P - B
xx = np.cross(PB, PA)
r3d = rotation_matrix_3d(xx, theta)
PN = np.dot(r3d, PA)
N = P - PN
as an example, we want to find coordinates of point N that are 3 degrees away from point A, the coordinates would be
N = (1.43676498, 0.8871264, 0.)
Which is correct! (manually verified using CAD program)