Finding rotation of 3 given lines in 3D until intersection with 3 other given lines

Question

I already posted a similar question. but since the problem is still unanswered and the details are a bit changed, I'm posting it again.

I'm trying to solve the following mathematical problem:

Given 2 sets of 3 lines each in 3D space (where each line is given by a $3D$ point and a $3D$ vector): $$ set1=\{ [(px1,py1,pz1), (vx1,vy1,vz1)], [(px2,py2,pz2),(vx2,vy2,vz2)], [(px3,py3,pz3), (vx3,vy3,vz3)] \} $$ $$ set2=\{ [(qx1,qy1,qz1), (ux1,uy1,uz1)], [(qx2,qy2,qz2),(ux2,uy2,uz2)], [(qx3,qy3,qz3), (ux3,uy3,uz3)] \} $$

find the angles of rotation $x,y,z$ (in radians) such that:

$$R=rotate\_x*rotate\_y*rotate\_z$$ $$ {rotate\_x} = \left( {\matrix{ {1} & {0} & {0} \cr {0} & {cos(x)} & {-sin(x)} \cr {0} & {sin(x)} & {cos(x)} \cr } } \right) $$ $$ {rotate\_y} = \left( {\matrix{ {cos(y)} & {0} & {sin(y)} \cr {0} & {1} & {0} \cr {-sin(y)} & {0} & {cos(y)} \cr } } \right) $$ $$ {rotate\_z} = \left( {\matrix{ {cos(z)} & {-sin(z)} & {0} \cr {sin(z)} & {cos(z)} & {0} \cr {0} & {0} & {1} \cr } } \right) $$

and if we rotate $set1$ by $R$ then all the lines in $set1$ will intersect their corresponding lines in $set2$.

Meaning, let's define $set1\_r$ as $set1$ after the above rotation.

Then line $i$ $(i=1,2,3)$ in $set1\_r$ will intersect line $i$ in $set2$.

I already built the 3 equations with 3 variables that needed to be solved. but they seem to be unsolvable.

Here are the equations (only the angles $x,y,z$ are unknown and the rest are known):

eq1 = qx1*(sin(x)*sin(z) - cos(x)*cos(z)*sin(y)) + qy1*(cos(z)*sin(x) + cos(x)*sin(y)*sin(z)) + ((ux1*(sin(x)*sin(z) - cos(x)*cos(z)*sin(y)) + uy1*(cos(z)*sin(x) + cos(x)*sin(y)*sin(z)) + uz1*cos(x)*cos(y))*(px1 - qz1*sin(y) - qx1*cos(y)*cos(z) + qy1*cos(y)*sin(z) + (vx1*(qx1*(cos(x)*sin(z) + cos(z)*sin(x)*sin(y)) - py1 + qy1*(cos(x)*cos(z) - sin(x)*sin(y)*sin(z)) - qz1*cos(y)*sin(x) + ((ux1*(cos(x)*sin(z) + cos(z)*sin(x)*sin(y)) + uy1*(cos(x)*cos(z) - sin(x)*sin(y)*sin(z)) - uz1*cos(y)*sin(x))*(px1 - qz1*sin(y) - qx1*cos(y)*cos(z) + qy1*cos(y)*sin(z)))/(uz1*sin(y) + ux1*cos(y)*cos(z) - uy1*cos(y)*sin(z))))/(vy1 - (vx1*(ux1*(cos(x)*sin(z) + cos(z)*sin(x)*sin(y)) + uy1*(cos(x)*cos(z) - sin(x)*sin(y)*sin(z)) - uz1*cos(y)*sin(x)))/(uz1*sin(y) + ux1*cos(y)*cos(z) - uy1*cos(y)*sin(z)))))/(uz1*sin(y) + ux1*cos(y)*cos(z) - uy1*cos(y)*sin(z)) + qz1*cos(x)*cos(y) == pz1 + (vz1*(qx1*(cos(x)*sin(z) + cos(z)*sin(x)*sin(y)) - py1 + qy1*(cos(x)*cos(z) - sin(x)*sin(y)*sin(z)) - qz1*cos(y)*sin(x) + ((ux1*(cos(x)*sin(z) + cos(z)*sin(x)*sin(y)) + uy1*(cos(x)*cos(z) - sin(x)*sin(y)*sin(z)) - uz1*cos(y)*sin(x))*(px1 - qz1*sin(y) - qx1*cos(y)*cos(z) + qy1*cos(y)*sin(z)))/(uz1*sin(y) + ux1*cos(y)*cos(z) - uy1*cos(y)*sin(z))))/(vy1 - (vx1*(ux1*(cos(x)*sin(z) + cos(z)*sin(x)*sin(y)) + uy1*(cos(x)*cos(z) - sin(x)*sin(y)*sin(z)) - uz1*cos(y)*sin(x)))/(uz1*sin(y) + ux1*cos(y)*cos(z) - uy1*cos(y)*sin(z)))
eq2 = qx2*(sin(x)*sin(z) - cos(x)*cos(z)*sin(y)) + qy2*(cos(z)*sin(x) + cos(x)*sin(y)*sin(z)) + ((ux2*(sin(x)*sin(z) - cos(x)*cos(z)*sin(y)) + uy2*(cos(z)*sin(x) + cos(x)*sin(y)*sin(z)) + uz2*cos(x)*cos(y))*(px2 - qz2*sin(y) - qx2*cos(y)*cos(z) + qy2*cos(y)*sin(z) + (vx2*(qx2*(cos(x)*sin(z) + cos(z)*sin(x)*sin(y)) - py2 + qy2*(cos(x)*cos(z) - sin(x)*sin(y)*sin(z)) - qz2*cos(y)*sin(x) + ((ux2*(cos(x)*sin(z) + cos(z)*sin(x)*sin(y)) + uy2*(cos(x)*cos(z) - sin(x)*sin(y)*sin(z)) - uz2*cos(y)*sin(x))*(px2 - qz2*sin(y) - qx2*cos(y)*cos(z) + qy2*cos(y)*sin(z)))/(uz2*sin(y) + ux2*cos(y)*cos(z) - uy2*cos(y)*sin(z))))/(vy2 - (vx2*(ux2*(cos(x)*sin(z) + cos(z)*sin(x)*sin(y)) + uy2*(cos(x)*cos(z) - sin(x)*sin(y)*sin(z)) - uz2*cos(y)*sin(x)))/(uz2*sin(y) + ux2*cos(y)*cos(z) - uy2*cos(y)*sin(z)))))/(uz2*sin(y) + ux2*cos(y)*cos(z) - uy2*cos(y)*sin(z)) + qz2*cos(x)*cos(y) == pz2 + (vz2*(qx2*(cos(x)*sin(z) + cos(z)*sin(x)*sin(y)) - py2 + qy2*(cos(x)*cos(z) - sin(x)*sin(y)*sin(z)) - qz2*cos(y)*sin(x) + ((ux2*(cos(x)*sin(z) + cos(z)*sin(x)*sin(y)) + uy2*(cos(x)*cos(z) - sin(x)*sin(y)*sin(z)) - uz2*cos(y)*sin(x))*(px2 - qz2*sin(y) - qx2*cos(y)*cos(z) + qy2*cos(y)*sin(z)))/(uz2*sin(y) + ux2*cos(y)*cos(z) - uy2*cos(y)*sin(z))))/(vy2 - (vx2*(ux2*(cos(x)*sin(z) + cos(z)*sin(x)*sin(y)) + uy2*(cos(x)*cos(z) - sin(x)*sin(y)*sin(z)) - uz2*cos(y)*sin(x)))/(uz2*sin(y) + ux2*cos(y)*cos(z) - uy2*cos(y)*sin(z)))
eq3 = qx3*(sin(x)*sin(z) - cos(x)*cos(z)*sin(y)) + qy3*(cos(z)*sin(x) + cos(x)*sin(y)*sin(z)) + ((ux3*(sin(x)*sin(z) - cos(x)*cos(z)*sin(y)) + uy3*(cos(z)*sin(x) + cos(x)*sin(y)*sin(z)) + uz3*cos(x)*cos(y))*(px3 - qz3*sin(y) - qx3*cos(y)*cos(z) + qy3*cos(y)*sin(z) + (vx3*(qx3*(cos(x)*sin(z) + cos(z)*sin(x)*sin(y)) - py3 + qy3*(cos(x)*cos(z) - sin(x)*sin(y)*sin(z)) - qz3*cos(y)*sin(x) + ((ux3*(cos(x)*sin(z) + cos(z)*sin(x)*sin(y)) + uy3*(cos(x)*cos(z) - sin(x)*sin(y)*sin(z)) - uz3*cos(y)*sin(x))*(px3 - qz3*sin(y) - qx3*cos(y)*cos(z) + qy3*cos(y)*sin(z)))/(uz3*sin(y) + ux3*cos(y)*cos(z) - uy3*cos(y)*sin(z))))/(vy3 - (vx3*(ux3*(cos(x)*sin(z) + cos(z)*sin(x)*sin(y)) + uy3*(cos(x)*cos(z) - sin(x)*sin(y)*sin(z)) - uz3*cos(y)*sin(x)))/(uz3*sin(y) + ux3*cos(y)*cos(z) - uy3*cos(y)*sin(z)))))/(uz3*sin(y) + ux3*cos(y)*cos(z) - uy3*cos(y)*sin(z)) + qz3*cos(x)*cos(y) == pz3 + (vz3*(qx3*(cos(x)*sin(z) + cos(z)*sin(x)*sin(y)) - py3 + qy3*(cos(x)*cos(z) - sin(x)*sin(y)*sin(z)) - qz3*cos(y)*sin(x) + ((ux3*(cos(x)*sin(z) + cos(z)*sin(x)*sin(y)) + uy3*(cos(x)*cos(z) - sin(x)*sin(y)*sin(z)) - uz3*cos(y)*sin(x))*(px3 - qz3*sin(y) - qx3*cos(y)*cos(z) + qy3*cos(y)*sin(z)))/(uz3*sin(y) + ux3*cos(y)*cos(z) - uy3*cos(y)*sin(z))))/(vy3 - (vx3*(ux3*(cos(x)*sin(z) + cos(z)*sin(x)*sin(y)) + uy3*(cos(x)*cos(z) - sin(x)*sin(y)*sin(z)) - uz3*cos(y)*sin(x)))/(uz3*sin(y) + ux3*cos(y)*cos(z) - uy3*cos(y)*sin(z)))

It can be seen that it's almost impossible (hopefully not impossible) to extract $x,y,z$ from the equations since there are many multiplications between $sin$ and $cos$ of the different angles. I couldn't solve it. and I also tried it in matlab using the 'solve' function but it didn't work.

Can anyone suggest a solution for finding the angles $x,y,z$? or any other way of finding the matrix $R$?

If it helps, here's a specific example of the 2 sets of lines in which I know the desired angles $x,y,z$ (there are probably many more triplets of angles that also solve the problem).

%set1
px1=0;
py1=0;
pz1=-30;

px2=0;
py2=0;
pz2=-30;

px3=0; 
py3=0;
pz3=-30;

vx1 = -0.083717247687439; 
vy1 = -0.107930827800543; 
vz1 = 0.990627255252918;

vx2 = 0.076364294519742;
vy2 = 0.060269029165473; 
vz2 = 0.995256820446840;

vx3 = -0.081460429387834; 
vy3 = 0.105021268850622; 
vz3 = 0.991128009660183; 

%set2
qx1=0; 
qy1=0;
qz1=-30;

qx2=0; 
qy2=0;
qz2=-30;

qx3=0; 
qy3=0; 
qz3=-30;

ux1 = -0.079382581863774;
uy1 = -0.095259098236529; 
uz1 = 0.992282273297173;

ux2 = 0.079382581863774; 
uy2 = 0.095259098236529; 
uz2 = 0.992282273297173;

ux3 = -0.086165283952334; 
uy3 = 0.103398340742801; 
uz3 = 0.990900765451843;

For the above example, for $x=0.5236$ ($x$ is 30 degrees), $y=0, z=0$, meaning, $$ {R} = \left( {\matrix{ {1} & {0} & {0} \cr {0} & {0.866} & {-0.5} \cr {0} & {0.5} & {0.866} \cr } } \right) $$

$line$ $i$ ($i=1,2,3$) in $set1$ will intersect $line$ $i$ in $set2$.

Also, $x=0, y=0, z=0$ ($R$ is the identity matrix) is also a solution since all the $6$ lines already intersect at the point $(0,0,-30)$.

And I'm sure there are many more solutions.

But, of course I need the general solution.

Hope anyone can solve it since I've been trying to solve it for weeks :(.

Thanks

Answer 1

Writing the paramtric equations of the lines, with respective parameters $r_i$ and $s_j$, you have

$$R\,\,(\mathbb p_i+r_i\mathbb v_i)=\mathbb q_j+s_j\mathbb u_j$$ where $i,j$ are some (possibly distinct) permutations of $\{0,1,2\}$ (so that you will have to solve the problem six times).

Rewriting

$$R\,\mathbb p_i+r_iR\,\mathbb v_i=q_j+s_j\mathbb u_j$$ and taking the cross product with $(R\,\mathbb v_i)\times\mathbb u_j$, you can get rid of the parameters with

$$R\,\mathbb p_i\times((R\,\mathbb v_i)\times\mathbb u_j)=q_j\times((R\,\mathbb v_i)\times\mathbb u_j).$$

Then using the expression of $R$,

$$R=R_x\cdot R_y\cdot R_z,$$ you get a nasty system of nine nonlinear (trigonometric) equations in three unknows.

The equations can also be cast in terms of the matrix elements, which you constrain by expressing the orthonormality of the matrix,

$$R^TR=I.$$

Hence you get a monstrous system of fifteen quadratic equations in nine unknowns, which can have up to $512$ distinct solutions, if I am right, and will be subject to compatibility conditions.

Answer 2

It may be helpful to use a little more geometric intuition to guide the analysis of the equations.

Consider how we can identify the intersection of a line from set 1 with a line from set 2. Each line has a point closest to the origin. Divide the line into two half-lines at this point. To intersect one pair of lines is relatively straightforward: pick one half-line of the line from set 1, then choose a distance $t$ along the line from set 2. This selects a point $P$ on the selected line from set 2.

Now rotate set 1 to cause the selected half-line of set 1 to pass through the selected point $P,$ if possible. In order to do this, the half-line from set 1 must have a point $Q$ at the same distance from the origin as the already-selected point $P.$ If it does, there is a rotation that takes $Q$ to $P.$ In fact there are infinitely many such rotations, one for each line in the plane through the origin perpendicular to the line $PQ,$ but right now we only need to consider one such rotation; call it rotation $R_1.$

Given such a rotation $R_1,$ all the other rotations that take $Q$ to $P$ can be generated by first performing the rotation $R_1$ and then doing a rotation around the line $OP$ through the origin $O$ and the selected point $P.$

Now consider one of the other pairs of lines. One of these lines, call it $\ell_1,$ is in set 1. The other line is in set 2; call it $\ell_2.$ The rotation $R_1$ takes $\ell_1$ line to a new line, $R_1(\ell_1),$ the image of $\ell_1$ under $R_1.$ The line $R_1(\ell_1),$ under all the rotations around the axis $OP,$ sweeps out a cone (if $R_1(\ell_1)$ passes through the origin) or a hyperboloid. In either case the swept surface has a quadratic equation and intersects the line $\ell_2$ in at most two places. One of those two places will be further toward the "positive" end of $\ell_2$ (in the direction of its vector).

Once we choose one of the two intersections, the rotation of set 1 is completely determined: it's the composition $R_2\circ R_1$ of the rotation $R_1$ with whatever rotation $R_2$ rotates $R_1(\ell_1)$ onto the selected intersection point on $\ell_2.$ No further rotation is possible without undoing one of the two intersection points already selected.

Measure the distance between the third pair of lines after the rotation $R_2\circ R_1.$ If it's zero, you have your desired rotation.

Of course it is unlikely that we you would find a solution by making arbitrary choices as described above. Let's be clear about this: the rotation $R_2\circ R_1$ found by the procedure above will generally not be the solution. The point of the procedure is not to lead directly to a solution.

The point of the procedure is that you can first decide which half-lines to intersect for the first step of the procedure, which pair of lines $\ell_1$ and $\ell_2$ you will intersect next, and whether you will rotate $R_1(\ell_1)$ onto the "more positive" or "less positive" point of $\ell_2.$ After deciding those things, then you just have one more choice to make: the distance $t$ at the beginning of the procedure. Every other step of the procedure is then completely determined.

We can therefore consider the procedure as defining a function from a single parameter $t$ to a single output value $f(t),$ representing the amount by which the procedure "missed" the third intersection. This turns the problem into a minimization problem over just one variable, namely, the problem of minimizing $f(t)$ as a function of the single parameter $t.$

Conceptually, $f(t)$ could be the distance between the third pair of lines after the final rotation, though it's probably better to use the square of that distance instead. It might be even better to use some other function. In order to help solve the minimization problem, we would like $f'(t)$ to exist and to equal zero when $f(t)$ is minimized.

Now if the original problem is solvable at all, there is a reasonable chance that by making the various choices of half-lines and so forth and then minimizing $f(t),$ the minimum might occur when the third pair of lines intersects. If it doesn't work for these particular choices, however, you can try each of the variations in turn until either you find a solution or exhaust all possibilities and conclude there is no solution.

This is just an outline of a solution, not including the formulas to use. But I think it reduces the complexity of the problem by reducing it to just a few possible discrete choices and one real variable. While the equations would not be trivial to work out, I think they will be simpler than the ones you have and will likely be tractable for an equation solver. And if you can get a rotation in any format, there are methods to convert it to the three-axis format you asked for.

Answer 3

Let us change somehow you notation and define, for the first set of lines $$ \begin{array}{l} {\bf P}_{\,{\bf 1}} = \left( {\begin{array}{*{20}c} {p_{\,x,\,1} } & {p_{\,x,\,2} } & {p_{\,x,\,3} } \\ {p_{\,y,\,1} } & {p_{\,y,\,2} } & {p_{\,y,\,3} } \\ {p_{\,z,\,1} } & {p_{\,z,\,2} } & {p_{\,z,\,3} } \\ \end{array}} \right)\quad \quad {\bf V}_{\,{\bf 1}} = \left( {\begin{array}{*{20}c} {v_{\,x,\,1} } & {v_{\,x,\,2} } & {v_{\,x,\,3} } \\ {v_{\,y,\,1} } & {v_{\,y,\,2} } & {v_{\,y,\,3} } \\ {v_{\,z,\,1} } & {v_{\,z,\,2} } & {v_{\,z,\,3} } \\ \end{array}} \right) \\ {\bf \Lambda }_{\,{\bf 1}} = \left( {\begin{array}{*{20}c} {\lambda _{\,1,1} } & 0 & 0 \\ 0 & {\lambda _{\,1,2} } & 0 \\ 0 & 0 & {\lambda _{\,1,3} } \\ \end{array}} \right) \\ \end{array} $$ while using the index $2$ for the second set.

Given the values of each parameter $\lambda_{1,k}$, we are defining one point on each of the three lines of the first set, which constitutes a triangle in 3D.
Same for the second set of lines. $$ {\bf T}_{\,{\bf 1}} = {\bf P}_{\,{\bf 1}} + {\bf V}_{\,{\bf 1}} \,{\bf \Lambda }_{\,{\bf 1}} \quad {\bf T}_{\,{\bf 2}} = {\bf P}_{\,{\bf 2}} + {\bf V}_{\,{\bf 2}} \,{\bf \Lambda }_{\,\,{\bf 2}} $$

If I interpretated your question correctly, we are to
find the values of $\lambda_{1,k}$ and $\lambda_{2,k}$ such that the two triangles defined in each set might be made to coincide upon a rigid rotation around the origin, and find the parameters of the rotation realizing that.

Since you wrote that
line $i$ in set 1 shall intersecate line $i$ in set 2
then the triangles shall also coincide
vertex $i$ to vertex $i$.

That means that the pyramid $OT_2$ shall rigidly rotate around the vertex in $O$ and come to coincide with the pyramid $OT_1$.
And being the rotation rigid, then all the dimensions of the pyramid (lengths, areas and volume) shall be preserved.

We can translate the above into the following conditions

• the distance from the origin of each vertex of $T_1$ shall equal the distance of the corresponding vertex of $T_2$, and
• the angle between each couple of edges from the origin to two vertices of $T_1$ shall equal the corresponding on $T_2$, and
• (to avoid reflection) the signed volume of the pyramid $OT_1$ shall equal that of $OT_2$, when the vertices are listed in the same order.

That means that it should be (indicating the transposed with an overline) $$ \bbox[lightyellow] { \overline {{\bf T}_{\,{\bf 1}} } \;{\bf T}_{\,{\bf 1}} = \overline {{\bf T}_{\,{\bf 2}} } \;{\bf T}_{\,{\bf 2}} } \tag{1} $$ which provides $6$ quadratic equations in the $6$ unknowns $\lambda_{1,k}$, $\lambda_{2,j}$, and where the sign of the solutions (if any) shall be chosen as to give that the determinant of ${\bf T}_{\,{\bf 1}} $ equals that of ${\bf T}_{\,{\bf 2}} $.

The diagonal equations provide a relation between $\lambda_{1,k}$ and $\lambda_{2,k}$ , with $k=1 \cdots 3$, of the type $$ \lambda _{\,1,\,\,k} = a_{\,k} \pm \sqrt {b_{\,k} \,\lambda _{\,2,\,\,k} ^{\,2} + c_{\,k} \,\lambda _{\,2,\,\,k} + d_{\,k} \,} $$ Replacing these values into the three off-diagonal equations, produces equations involving radicals whose solution is quite complicated.
But I cannot see at the moment a better way.

Once the $\lambda$s have been determined, an efficient way to find the rotation matrix could be that of determining
- first the rotation that brings the centroid of $T_2$ onto that of $T_1$ (around the axis normal to both)
- secondly, the rotation about the common central axis that brings one vertex of $T_2$ onto the corresponding vertex of $T_1$.

To recapitulate and try to make it more clear, the process is:

1. given the two sets of three lines each (however the lines be mutually placed within each set: skew, coincident, ..) we look for the position of a point in each line (the $\lambda$), such that the triangles resulting in each set ($T_1$ and $T_2$) be "congruent" under a rotation around the origin;
2. according to the question, as worded, congruent means that upon rotation around the origin they shall become coincident vertex $i$ over vertex $i$, excluding reflection;
3. a necessary and sufficient condition for that to occur is that, considering the pyramids $OT_1$ and $OT_2$, based on each triangle and with the vertex at the origin, the lengths of the corresponding edges from the origin be equal and the angles between corresponding pairs of those edges be equal, i.e. be equal their scalar product;
4. such condition is summarized in the matrix equation (1), where each side contains the scalar product of each edge with itself and with the others, therefore the matrices are symmetric and result in $6$ quadratic equations in the $6$ parameters;
5. note that, if only superposition of the triangles be required (allowing permutation), then in equation (1) we shall add the OR of the cases in which $T_2$ is permuted, but only with permutation matrices of det $+1$ if pure rotation without reflection is allowed;
6. once the parameters are assigned with the values (if existing) making the triangles congruent as per above, then the whole pyramids are congruent (equal edges, angles, areas and volume), and when the triangles are brought to coincide, the pyramids will also coincide, keeping firm their vertex at the origin, i.e. by a rigid rotation around the origin;
7. such a rotation can be found by separating it into a rotation that brings one edge (or the centroid axis) onto the corresponding one, followed by a rotation around such common edge/axis to get the coincidence of other two edges.

Concerning the solution approach to eq. (1) refer to the related post