Symbolic solution for intersection points between a torus and a circle. How to make it concise? (for rewriting to C++).

14 ビュー (過去 30 日間)
Hi,
I would like to find symbolic equations for intersection points between two geometries in the 3D space: a torus and a circle. I tried 2 approaches, in 2D and 3D domains, but I probably don't use the toolbox correctly. You can skip this introduction and jump straight to the equation picture.
This problem is for an open source robot arm simulation (analytical inverse kinematics solution). Your help will be very meaningful. I plan to write a C++ solution based on the matlab symbolic solution.
I consider 0, 1, 2, 3, or 4 intersection points between a torus and an ordinary circle in the 3D space. A circle that can be tilted with respect to torus axis and freely positioned. However, the case with infinite intersection points will never occur. All the variables describing circle and torus are known and are precomputed in each siulation step. A circle and its plane is described with respect to a coordinate frame of a torus.
The problem can be approached in several ways:
Approach 1 (2D space):
1. Since the circle is described on the 3D plane, first find the equation that defines the intersection curve of this plane with a torus. This equation is described in a T,W Cartesian plane domain. Terms a, b, c, d, e are come from variables that describe torus and plane, I can precompute them. A paper that describes the equation: https://arxiv.org/pdf/1708.00803.pdf (pages 13 and 14)
2. Use the T,W domain to describe a circle on the plane.
3. Solve for the intersection points (t_n, w_n) given these two equations with two unknowns.
Equations and expected intersection points results:
Code:
% in both equations, to be solved
syms t w
% known precomputed variables for tours-plane intersection
syms a b c d
% known precomputed variables for circle on plane equation
syms tc wc % coordinates of center point
syms rc positive % radius
torus_plane_intersect = (t^2 + w^2)^2 + a*t^2 + b*w^2 + c*w + d == 0;
circle_eq = [(t-tc)^2 + (w-wc)^2 == rc^2];
[sol_t, sol_w] = solve([torus_plane_intersect, circle_eq], [t, w])
% This attempt commented below could not succeed
% S = solve([torus_plane_intersect, circle_eq], [t, w],'ReturnConditions',true)
The output for a sample root has around 150 terms, where a sample term looks like: 32*rc^2*wc^3
The beginning of one of the sol_w roots:
root(z^4 - (z^3*(- 32*rc^2*wc^3 + 64*tc^2*wc^3
+ 2*a*c - 2*b*c + 8*c*tc^2 + 4*a^2*wc - 24*a*wc^3 + 8*b*wc^3 - 8*c*wc^2
+ 32*tc^4*wc + 32 * wc^5 + 8*a*rc^2*wc - 8*b*rc^2*wc + 8*a*tc^2*wc ... )
QUESTIONS:
  1. What should I do to make the solution more concise and tractable?
  2. How to tell matlab that I am looking for root solutions for two variables that are related to each other? For example, for a point (t_n, w_n) solution t_1 is tied to w_1, t_2 tied to w_2, and so on. Basically to indicate that a combination of e.g. w_1 with t_2 does not make sense.
  3. Could the use of complex numbers be good here for checking the viability of the solutions, or for making things simpler?
  4. What is the meaning of 'z' variable in the root?
  5. How could I force matlab to ignore solutions that have infinite intersection points between circle and torus? For instance, when the Circle lies on the edge of the torus surface, this can happen when circle center is on the torus axis, and circle vector is co-axial with torus axis. I know that this will never happen in my computations).
For my C++ implementation, I need to keep the known variables, because they will be later substituted in the C++ code. I could possibly substitute R and r in matlab only (R and r are constant robot links length), but this will force me to recompute expressions in matlab and put back to C++ every time my robot link length design changes.
Here is another approach I tried. (You might skip this.)
Approach 2 (3D space):
1. Describe a torus in the X,Y,Z Cartesian space with respect to its own coordinate frame.
2. Describe a circle plane in the X,Y,Z Cartesian space with respect to the torus.
3. Describe a sphere in the X,Y,Z Cartesian space with respect to the torus. This sphere has its center in the circle center and radius equal to circle radius. Once this sphere is intersected with the plane, a 3D circle equation can be obtained.
4. Solve for the intersection points (x_n, y_n z_n) given these three equations with three unknowns.
syms x y z real % unknowns
syms x0 y0 z0 real %circle(sphere) center point
syms rc positive % circle(sphere) radius
syms a b c % circle plane normal vector
syms r R % torus radii
sphere_eq = (x-x0)^2 + (y-y0)^2 + (z-z0)^2 == rc^2;
plane_eq = a*(x-x0) + b*(y-y0) + c*(z-z0) == 0;
torus_eq = (x^2 + y^2 + z^2 + R^2 - r^2)^2 - 4*R^2 *(x^2 + y^2) == 0;
[sol_x, sol_y, sol_z] = solve([sphere_eq, plane_eq, torus_eq], [x y z]);
The solution for each root in this approach have a couple of thousands terms and takes a few minutes to compute. I am more enthusiastic about the approach 1.
It would be great to learn what commands or constraints I should use with the Symbolic Toolbox or other Matlab product to succeed. I would appreciate your help and support in this matter.
If the analytical solution does not go well, I will proceed with the numerical approach (that uses previously computed intersection point and searches for a new solution in the neighborhood. However, I am not sure about the stability and time performance of the numerical approach. Thank you again for your time.
Best regards,
Jakub

回答 (1 件)

John D'Errico
John D'Errico 2022 年 12 月 3 日
編集済み: John D'Errico 2022 年 12 月 3 日
How would I do it? Simple enoiucgh. The equation of a torus is easy to write in a parametric form, so as a function of two angles, and the constants that define the two different radii.
So, assuming your torus is centered at the origin, and there are two radii, a major radius and a minor one. Then we can define a parametric form for the torus. I'll just pick a couple of radii.
(If your goal is to solve this for unknown radii, then I will not be surprised if it is far more symbolically complex.)
Rmaj = 10;
Rmin = 3;
x = @(phi,theta) (Rmaj + Rmin*cos(phi)).*cos(theta);
y = @(phi,theta) (Rmaj + Rmin*cos(phi)).*sin(theta);
z = @(phi,theta) Rmin*sin(phi);
So Rmaj is the radius of the Torus from the origin to the centerline of the toroidal ring. Rmin is the radius of the toroidal ring. The result is the surface shown here, a 2-manifold.
fsurf(x,y,z)
axis equal
Now you have some general circle floating in space. A circle is easily defined in a parametric form. We need one parameter to define that curve, since a circle is just a simple path through space, a 1-manifold, embedded in R^3.
For example, we can do this:
C0 = [3 4 1]; % center coordinates
Cnormal = [1 -1 -1]; % normal vector to the plane of the circle
Crad = 5; % radius of the circle
That normal vector defines the plane the circle lives in. Now we can plot the circle too. We need a transformation matrix to put the circle into the R^3 space of the torus.
Ctrans = null(Cnormal)
Ctrans = 3×2
0.5774 0.5774 0.7887 -0.2113 -0.2113 0.7887
C = @(psi) [Crad*cos(psi), Crad*(sin(psi))]*Ctrans.' + C0;
t = linspace(0,2*pi)';
Cxyz = C(t);
hold on
plot3(Cxyz(:,1),Cxyz(:,2),Cxyz(:,3),'r-')
box on
grid on
Now, can we solve for the intersection points? There will be often be two points of intersection or zero. Bur there may also be 1, 3, or 4 points of intersection in some cases. First, can solve do the work?
syms PSI PHI THETA
solve(C(PSI) == [x(PHI,THETA);y(PHI,THETA);z(PHI,THETA)],[PSI,PHI,THETA])
ans = struct with fields:
PSI: [0×1 sym] PHI: [0×1 sym] THETA: [0×1 sym]
It looks like solve failed. My expectation is there might be no algebraic solution. So if you hope to find some closed form expression, you probably won't want to bother.
sol = vpasolve(C(PSI) == [x(PHI,THETA),y(PHI,THETA),z(PHI,THETA)],[PSI,PHI,THETA])
sol = struct with fields:
PSI: 0.75543752807667537570534280254977 PHI: 1.7802814065932110224695717314719 THETA: 0.7148702653629053198759572503334
But vpasolve found a solution. It lives at
vpa(C(sol.PSI))
ans = 
There will be another solution of course. You would find it by a different choice of starting values from the default that vpasolve uses.
Can you put this into C++? For that you would need to use a rootfinding solver, perhaps fsolve, or something similar.
  2 件のコメント
John D'Errico
John D'Errico 2022 年 12 月 4 日
編集済み: John D'Errico 2022 年 12 月 4 日
fsolve is easy enough. Set it up as I did, but leave it in a function form.
Rmaj = 10;
Rmin = 3;
x = @(phi,theta) (Rmaj + Rmin*cos(phi)).*cos(theta);
y = @(phi,theta) (Rmaj + Rmin*cos(phi)).*sin(theta);
z = @(phi,theta) Rmin*sin(phi);
C0 = [3 4 1]; % center coordinates
Cnormal = [1 -1 -1]; % normal vector to the plane of the circle
Crad = 5; % radius of the circle
Ctrans = null(Cnormal);
C = @(psi) [Crad*cos(psi), Crad*(sin(psi))]*Ctrans.' + C0;
fsolveobj = @(psi_phi_theta) C(psi_phi_theta(1)) - [x(psi_phi_theta(2),psi_phi_theta(3)),y(psi_phi_theta(2),psi_phi_theta(3)),z(psi_phi_theta(2),psi_phi_theta(3))];
See that I set it up to look for a zero vector, so I had to move one of those terms to the other side of the equality.
You need starting values for fsolve, as you do for any numerical solver. vpasolve just picks its own, but the solution would be dependent on that choice.
guess = [2 2 2]; % just an arbitrary guess
[PSI_PHI_THETA,fval,exitflag] = fsolve(fsolveobj,guess)
Equation solved. fsolve completed because the vector of function values is near zero as measured by the value of the function tolerance, and the problem appears regular as measured by the gradient.
PSI_PHI_THETA = 1×3
5.5417 4.0929 1.1756
fval = 1×3
1.0e-10 * -0.0537 -0.3442 -0.4732
exitflag = 1
And fsolve found one solution. It lies at the point
C(PSI_PHI_THETA(1))
ans = 1×3
3.1792 7.6217 -2.4425
Which should be the same as if I computed the corresponding point on the surface of the torus.
[x(PSI_PHI_THETA(2),PSI_PHI_THETA(3)),y(PSI_PHI_THETA(2),PSI_PHI_THETA(3)),z(PSI_PHI_THETA(2),PSI_PHI_THETA(3))]
ans = 1×3
3.1792 7.6217 -2.4425
As you can see, the point found is on the bottom on the torus, since z is negative. If I choose a different initial guess for the solver, we may find the other solution.
[PSI_PHI_THETA,fval,exitflag] = fsolve(fsolveobj,[5 5 5])
Equation solved. fsolve completed because the vector of function values is near zero as measured by the value of the function tolerance, and the problem appears regular as measured by the gradient.
PSI_PHI_THETA = 1×3
0.7554 1.7803 6.9981
fval = 1×3
1.0e-07 * -0.1801 0.0804 -0.0519
exitflag = 1
Remember, those are just angles. In (x,y,z) coordinates, we have:
C(PSI_PHI_THETA(1))
ans = 1×3
7.0807 6.1462 2.9344
And this point found was the solution that cuts through the top of the torus, since this time z is positive.
Other rootfinding solvers could work as easily, but fsolve is the one in MATLAB.
If you needed to find ALL solutions, then I would use a multi-start method. That is, generate multiple random starting vectors, perhaps using
ng = 10;
guesses = rand(ng,3)*2*pi
guesses = 10×3
5.9267 2.7315 0.9353 5.6292 3.6764 5.4151 1.3253 3.0233 2.9572 3.7811 0.9813 1.2186 1.2792 4.5448 0.9105 5.3185 0.8622 5.6596 0.5442 2.4663 5.3302 1.5698 4.5470 4.8984 1.5225 1.3004 0.6329 5.7612 1.7039 2.1402
Start fsolve from each row of that array, saving the solution found each time.
xyzsol = zeros(ng,3);
for i = 1:ng
[PSI_PHI_THETA(i,:),fval(i,:),exitflag(i)] = fsolve(fsolveobj,guesses(i,:));
xyzsol(i,:) = C(PSI_PHI_THETA(i,1));
end
Equation solved. fsolve completed because the vector of function values is near zero as measured by the value of the function tolerance, and the problem appears regular as measured by the gradient. Equation solved. fsolve completed because the vector of function values is near zero as measured by the value of the function tolerance, and the problem appears regular as measured by the gradient. Equation solved. fsolve completed because the vector of function values is near zero as measured by the value of the function tolerance, and the problem appears regular as measured by the gradient. Equation solved. fsolve completed because the vector of function values is near zero as measured by the value of the function tolerance, and the problem appears regular as measured by the gradient. Equation solved. fsolve completed because the vector of function values is near zero as measured by the value of the function tolerance, and the problem appears regular as measured by the gradient. Equation solved. fsolve completed because the vector of function values is near zero as measured by the value of the function tolerance, and the problem appears regular as measured by the gradient. Equation solved. fsolve completed because the vector of function values is near zero as measured by the value of the function tolerance, and the problem appears regular as measured by the gradient. Equation solved. fsolve completed because the vector of function values is near zero as measured by the value of the function tolerance, and the problem appears regular as measured by the gradient. Equation solved. fsolve completed because the vector of function values is near zero as measured by the value of the function tolerance, and the problem appears regular as measured by the gradient. Equation solved. fsolve completed because the vector of function values is near zero as measured by the value of the function tolerance, and the problem appears regular as measured by the gradient.
sortrows(xyzsol)
ans = 10×3
3.1792 7.6217 -2.4425 3.1792 7.6217 -2.4425 3.1792 7.6217 -2.4425 3.1792 7.6217 -2.4425 3.1792 7.6217 -2.4425 7.0807 6.1462 2.9344 7.0807 6.1462 2.9344 7.0807 6.1462 2.9344 7.0807 6.1462 2.9344 7.0807 6.1462 2.9344
As you can see, I found both solutions easily. I could now use a clustering tool to clean that up. Perhaps kmeans is a good choice.

サインインしてコメントする。

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by