I have a matlab code with 3 equations and 3 variables that I'm trying to solve (and don't succeed). The equations are symbolic with three unknown variables x,y,z and the rest of the symbols are known. I need to find the values of x,y,z as expressions of the other known symbols. Here's the matlab code:
clear;
close all;
clc;
%unknown variables
syms x y z %The angles of rotations (in radians)
%known symbols
syms px1 py1 pz1 %point on line1 set1
syms px2 py2 pz2 %point on line2 set1
syms px3 py3 pz3 %point on line3 set1
syms vx1 vy1 vz1 %vector of line1 set1
syms vx2 vy2 vz2 %vector of line2 set1
syms vx3 vy3 vz3 %vector of line3 set1
syms qx1 qy1 qz1 %point on line1 set2
syms qx2 qy2 qz2 %point on line2 set2
syms qx3 qy3 qz3 %point on line3 set2
syms ux1 uy1 uz1 %vector of line1 set2
syms ux2 uy2 uz2 %vector of line2 set2
syms ux3 uy3 uz3 %vector of line3 set2
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)))
equations_system(1) = eq1;
equations_system(2) = eq2;
equations_system(3) = eq3;
fprintf('Trying to solve the equations (the function never finish)\n');
[x_solution, y_solution, z_solution] = solve(equations_system,[x, y, z]);
%The above equations are solving the following problem:
%Given 2 sets of 3 lines each (where each line is given by a point and a vector):
%set1={ [(px1,py1,pz1), (vx1,vy1,vz1)], [(px2,py2,pz2),(vx2,vy2,vz2)],[(px3,py3,pz3), (vx3,vy3,vz3)] } (the meaning of the symbols are written in their definition above).
%set2={ [(qx1,qy1,qz1), (ux1,uy1,uz1)], [(qx2,qy2,qz2),(ux2,uy2,uz2)],[(qx3,qy3,qz3), (ux3,uy3,uz3)] } (the meaning of the symbols are written in their definition above).
%find the angles of rotation x,y,z (in radians) such that:
%R=rotate_x*rotate_y*rotate_z
% rotate_x = [1 0 0; 0 cos(x) -sin(x) ; 0 sin(x) cos(x)];
% rotate_y = [cos(y) 0 sin(y); 0 1 0 ; -sin(y) 0 cos(y)];
% rotate_z = [cos(z) -sin(z) 0; sin(z) cos(z) 0 ; 0 0 1];
%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 all the following will occure:
% - the first line in set1_r intersects the first line in set2
% - the second line in set1_r intersects the second line in set2
% - the third line in set1_r intersects the third line in set2
%Here's an example of the two sets:
% 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;
%
% 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;
%
%example solution for the above values
%all zeros since all the lines pass through (0,0,-30)
% x=0;
% y=0;
% z=0;
%another solution for the above values
% x=deg2rad(30); %x=0.523598775598299
% y=0;
% z=0;
%There are probably more solutions.
The 'solve' function never finishes so I can't get the solution.
Can anyone suggest a way to solve the equations?
The equations in the matlab code are solving the following mathematical problem (it's also written as comments in matlab with a numeric example that I didn't write here):
Given 2 sets of 3 lines each in 3D space (where each line is given by a point and a 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 = [1 0 0; 0 cos(x) -sin(x) ; 0 sin(x) cos(x)];
rotate_y = [cos(y) 0 sin(y); 0 1 0 ; -sin(y) 0 cos(y)];
rotate_z = [cos(z) -sin(z) 0; sin(z) cos(z) 0 ; 0 0 1];
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.
As mentioned above, a numeric example is written in comments in matlab (including the desired angles that I want to get. or at least part of them, since I don't know how many solutions there really are). I didn't write the numeric example here since my post is already too long..
In case the equations in matlab are unsolvable (I hope not..), can anyone suggest a better way of solving the mathematical problem?
