1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140
| function M=Transfo_vecA_to_vecB(vecA,vecB)
% vecB2=M*[vecA;1]; vecB=vecB2(1:3);
%
%
% calcul de la matrice de rotation 3D à partir de deux vecteurs normalisés
% ou non.
% Ces vecteurs ont la même origine. Il n'y a donc pas de calcul de
% translation.
% M est une matrice homogène de dimension 4x4.
% On projette les deux vecteurs dans chacun des plans. L'ordre des
% plans est indiqué dans la variable "plan" ci-dessous :
plan={'yz','zx','xy'};
vecA=[ 0, vecA(1), vecA(1);...
vecA(2), 0, vecA(2);...
vecA(3), vecA(3), 0];
vecB=[ 0, vecB(1), vecB(1);...
vecB(2), 0, vecB(2);...
vecB(3), vecB(3), 0];
% La variable "valid" va nous permettre de savoir si une matrice de
% rotation dans au moins un des plans a été calculé. Elle sert donc à
% calculer le produit des matrices de rotation de chacun des plans ou à
% retourner une matrice vide si aucune matrice de rotation dans chacun des
% plans n'a pu être calculé
valid=0;
% On fait une boucle sur chacun des plans
for a=1:numel(plan)
% on calcul la norme de la projection de chacun des vecteurs dans le
% plan
nA=norm(vecA(:,a));
nB=norm(vecB(:,a));
if nA~=0 && nB~=0
% si les deux normes sont non nulles alors on peut tester la
% colinéarité des deux vecteurs dans le plan
c = cross(vecA(:,a),vecB(:,a));
if sum(c)==0
% si le produit vectoriel est nul alors ils sont collinéaires,
% l'angle est égale à "pi" (ou 180°);
disp(['colinéaires dans le plan : ' plan(a)]);
% la matrice de rotation dans le plan peut être calculée, la
% variable "valid" est donc incrémentée
valid=valid+1;
% On calcul ensuite le coefficient de corrélation
cos_angle = dot(vecA(:,a),vecB(:,a))/(nA*nB);
% Si le coefficient de corrélation est égale à -1 on est en
% sens opposé, l'angle est donc égale à 180°. A l'inverse, si
% le coefficient de corrélation est égale à 1, les vecteurs ont
% la même direction, l'angle est donc nul.
angle = (cos_angle<0)*pi + (cos_angle>0)*0;
if a==1
rot_x_angle_yz = [1, 0, 0, 0;...
0, cos(angle), -sin(angle), 0;...
0, sin(angle), cos(angle), 0;...
0, 0, 0, 1];
angle
c
elseif a==2
rot_y_angle_zx = [cos(angle), 0, sin(angle), 0;...
0, 1, 0, 0;...
-sin(angle), 0, cos(angle), 0;...
0, 0, 0, 1];
angle
c
elseif a==3
rot_z_angle_xy = [cos(angle), -sin(angle), 0, 0;...
sin(angle), cos(angle), 0, 0;...
0, 0, 1, 0;...
0, 0, 0, 1];
angle
c
end
else
% si le produit vectoriel est non nul alors on peut calculer le
% codinus de l'angle entre les deux projections dans le plan
% par le calcul du produit scalaire.
cos_angle = dot(vecA(:,a),vecB(:,a))/(nA*nB);
% Le signe de la composante perperdiculaire au plan, nous
% permet de connaître le signe de l'angle afin de calculer le
% sinus de l'angle entre les deux projections.
angle = sign(c(a))*acos(cos_angle);
% la matrice de rotation dans le plan peut être calculée, la
% variable "valid" est donc incrémentée
valid=valid+1;
if a==1
rot_x_angle_yz = [1, 0, 0, 0;...
0, cos(angle), -sin(angle), 0;...
0, sin(angle), cos(angle), 0;...
0, 0, 0, 1];
angle
c
elseif a==2
rot_y_angle_zx = [cos(angle), 0, sin(angle), 0;...
0, 1, 0, 0;...
-sin(angle), 0, cos(angle), 0;...
0, 0, 0, 1];
angle
c
elseif a==3
rot_z_angle_xy = [cos(angle), -sin(angle), 0, 0;...
sin(angle), cos(angle), 0, 0;...
0, 0, 1, 0;...
0, 0, 0, 1];
angle
c
end
end
else
% si la norme d'au moins un des deux vecteurs est nulle, alors on
% ne peut calculer la matrice de rotation entre un vecteur et un
% point ou un point avec un autre point.
disp(['erreur de norme sur le plan : ' plan(a)])
% la matrice de rotation dans le plan ne peut pas être calculée, la
% variable "valid" n'est donc pas incrémentée
valid=valid+0;
% pour les cas, où la matrice de rotation M va être calculée, la
% matrice ne pouvant être caculée ici est égale à la matrice
% identité qui ne change rien à un produit de matrices.
if a==1
rot_x_angle_yz = eye(4);
elseif a==2
rot_y_angle_zx = eye(4);
elseif a==3
rot_z_angle_xy = eye(4);
end
end
end
% Si on a pu calculé au moins une matrice de rotation dans un des trois
% plans, la variable "valid" est supérieure à "0"
% Sinon, on retourne une matrice vide.
if valid>0
M = rot_x_angle_yz * rot_y_angle_zx * rot_z_angle_xy;
else
M=[];
end |
Partager