Metoda Jacobiego

clear;
clc;
close all;

A = [2 2 2
     2 5 7
     2 7 9]; %Macierz z polecenia zadania
X = [1 0 0
     0 1 0
     0 0 1]; %Macierz jednostkowa
 k=12; %Ilosc kroków
 for g=1:k

     [i_end,j_end]=size(A); %Dostaję wymiar macierzy
     max_A=0;
     p=1;
     q=1;
     for i=1:i_end          %Pętle sterujące po macierzy A
     for j=1:j_end
         if(i~=j && abs(A(i,j))>max_A)
             p=i;
             q=j;
             max_A=abs(A(i,j));
         end
     end
     end


         u=(A(p,p)-A(q,q))/2;
         v=sqrt(A(p,q)*A(p,q)+u^2);

         cos_th=sqrt((abs(u)+v)/(2*v));
         sin_th=-1*A(p,q)*sign(u)/(2*v*cos_th);

         for i=1:i_end
             if(i~=p&&i~=q)
                 x=A(i,p)*cos_th-A(i,q)*sin_th
                 y=A(i,p)*sin_th+A(i,q)*cos_th
                 A(i,p)=x;
                 A(i,q)=y;
             end
         end

         for j=1:j_end
            if(j~=p&&j~=q)
                x=A(p,j)*cos_th-A(q,j)*sin_th;
                y=A(p,j)*sin_th+A(q,j)*cos_th;
                A(p,j)=x;
                A(q,j)=y;
            end
         end

         x=A(p,p)*cos_th*cos_th+A(q,q)*sin_th*sin_th-2*A(p,q)*sin_th*cos_th;
         y=A(p,p)*sin_th*sin_th+A(q,q)*cos_th*cos_th+2*A(p,q)*sin_th*cos_th;
         z=(A(p,p)-A(q,q))*sin_th*cos_th+A(p,q)*(cos_th*cos_th-sin_th*sin_th);
         A(p,p)=x;
         A(q,q)=y;
         A(q,p)=z;
         A(p,q)=z;

         for i=1:i_end
            x = X(i,p)*cos_th-X(i,q)*sin_th;
            y=X(i,p)*sin_th+X(i,q)*cos_th;
            X(i,p)=x;
            X(i,q)=y;
         end


 end