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

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

Answer 1

[John D'Errico]

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.

[Jakub Tomasz Kaminski]

Hi John,

Thank you for your amazing work and great effort you put to show the process step-by-step and the result. I learned a lot from this example. Can you think of an example with fsolve to show the algebraic solution? (One tractable for plugging it to the C++).

Your reach questadvice me if I should mark this question "Accepted".

A side note, I got the iterative numerical solution (mentioned in the 1st post) working with the robot simulation. I realized it by measuring the delta of elbow position error with respect to the delta of angular position of one of the joints that is controled in the nullspace and applying interpolated joint positions increments iteratively until the result is found. It goes beyond the scope of the original question with torus and circle only, though, as it requires knowledge of the entire kinematic chain to solve. For a tiny robot arm with links of around 40mm length, I got elbow position error in the order of e-12 mm after 4 iterations (non-accumulative error). It is sufficent for my needs. I will be happy to link the github repo here once it is released and I rewrite my matlab scripts to C++.

[John D'Errico]

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.