Symbolic solution for intersection points between a torus and a circle. How to make it concise? (for rewriting to C++).
14 ビュー (過去 30 日間)
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:
% 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 ... )
- What should I do to make the solution more concise and tractable?
- 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.
- Could the use of complex numbers be good here for checking the viability of the solutions, or for making things simpler?
- What is the meaning of 'z' variable in the root?
- 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.
回答 (1 件)
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.
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)
C = @(psi) [Crad*cos(psi), Crad*(sin(psi))]*Ctrans.' + C0;
t = linspace(0,2*pi)';
Cxyz = C(t);
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])
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])
But vpasolve found a solution. It lives at
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.