Rodrigues - Grafika

syms wx wy wz real;
syms theta;
assume(theta >=0);
assumeAlso(theta <=2*pi);

r = sym('r', [1,3]);
w = sym('w', [1,3]);
assume(r, 'real');
assume(w, 'real');

sample_w = [1,0,0];
sample_r = [2,1,2];
sample_theta = pi/3;

% Mind a ketto ugyanyanazt fejezi ki, a masodik a matrix megalkotasahoz a
% masodik a matrix megalkotasat segiti, mert kihasznalom, hogy
% r - w*(dot(r,w)) = -cross(w,cross(w,r))
r_rot = r*cos(theta) + w*dot(r,w)*(1-cos(theta)) + cross(w,r)*sin(theta);
r_rot_simple = r + cross(w,cross(w,r))*(1-cos(theta)) + cross(w,r)*sin(theta);

% Behelyettesitem az ertekeket mind a ket egyenletbe
r_rot_value = subs(r_rot, [w, r, theta], [sample_w, sample_r, sample_theta]);
r_rot2_value = subs(r_rot_simple, [w, r, theta], [sample_w, sample_r, sample_theta]);

disp(r_rot_value);
disp(r_rot2_value);
% Megnezem, hogy valoban sikerulte az egyszerusites, tehat a ket megoldas
% egyezik
if (logical(r_rot_value == r_rot2_value))
    fprintf('Helyes egyszerusites, a két érték megegyezik!\n\n');
else
    fprintf('Elrontottad az egyszerusitest\n\n');
end

% Megjelenitem a megoldast a sajat egyenletemmel, es egy egyszeru kez
% szamolassal (lasd kozos doksi)
disp('Levezetett egyenletbe beirva:')
disp(double(r_rot2_value));
disp('Kezzel kiszamolva:')
kezzel = [2,0,0] + [0,1,2]*cos(pi/3) + [0,-2,1]*sin(pi/3);
disp(kezzel);

% Ha egyezik a ketto akkor minden fasza
if (double(r_rot2_value) >= kezzel - 0.001 & double(r_rot2_value) <= kezzel + 0.001)
    fprintf('A ket ertek megegyezik!\n\n');
else
    fprintf('Nem egyezik a ket ertek\n\n');
end

% K matrix szamolasa amire igaz lesz, hogy cross(w,r) = K*r
K = [0,-wz,wy;wz,0,-wx;-wy,wx,0];

% K^2 szamolasa - cross(w,cross(w,r)) = K^2*r
K_squared = K^2;
% Kihasznalom, hogy pl.: -wz^2 -wy^2 = wx^2 - 1 (sqrt(x^2+y^2+z^2) = 1 miatt)
K_squared_simple = [wx^2 - 1,wx*wy,wx*wz;wx*wy,wy^2-1,wy*wz;wx*wz,wy*wz,wz^2-1];
% R matrix (rodrigues) szamolasa, amire igaz hogy r_rot = R*rT
R = eye(3) + (1-cos(theta))*K_squared_simple + sin(theta)*K;
R = simplify(R);

fprintf('Szamolas matrixal:\nA matrix altalanosan\n');
disp(R);
fprintf('\nA matrix a megadott adatok alapjan\n');
R_value = subs(R, [wx, wy, wz, theta], [sample_w(1),sample_w(2),sample_w(3),sample_theta]);
disp(R_value);

fprintf('\nMegoldas (R*rT):\n');
disp(double(transpose(R_value*transpose(sample_r))));

if (transpose(R_value*transpose(sample_r)) == r_rot2_value)
    fprintf('A ket ertek megegyezik!\n\n');
else
    fprintf('Nem egyezik a ket ertek\n\n');
end