% rem  Test 
% https://gomatlab.de/bewegung-eines-starren-krpers-darstellen-t52022.html
% 2024-08-26 1. Version Busmaster, benutzt homogene Koordinaten erfordert Funktionen rot2d.m, tran2d_h.m

clear all
clf
hold off
figure(1)

% 1. Körper
SK1m = [-5 -3
        -5  3
	     5  3
		 5 -3];					% Abmessungen des 1. Koerpers 
SK10 = [0 10];					% Zentrum des 1. Koerpers
SK1  = SK1m + ones(4,1)*SK10;	% Koordinaten der Eckpunkte Koerper 1
P1   = mean(SK1);				% Schwerpunkt des 1. Koerpers

% 2. Koerper
SK2m = [-5 -3
        -5  3
	     5  3
		 5 -3];					% Abmessungen des 2. Koerpers 
SK20 = [0 0];					% Zentrum des 2. Koerpers
SK2  = SK2m + ones(4,1)*SK20;	% Koordinaten der Eckpunkte Koerper 2
P2   = mean(SK2);				% Schwerpunkt des 2. Koerpers

% Koordinaten des Drehpunktes
T = [-2 -1];

% Drehwinkel (in Grad für rot2d.m)
alpha = 45;

% Darstellung 1. Koerper
PH1 = patch(SK1(:,1), SK1(:,2),'r');
set(PH1,'FaceAlpha',.5,'FaceColor','r')

% Darstellung 2. Koerper
PH2 = patch(SK2(:,1), SK2(:,2),'g');
set(PH2,'FaceAlpha',.5,'FaceColor','g')
axis equal
grid

for alpha = 0:15:90   % Drehung Koerper 1 um Punkt T verschiedene Winkel, wartet mit Pause
   % Transformationsmatrix M berechnen
   % verschiebe Schwerpunkt in Koordinatenursprung:	tran2d_h(-T)
   % drehe um Winkel alpha:							rot2d(alpha)
   % verschiebe zurück:								tran2d_h(+T1)
   M = tran2d_h(T)*rot2d(alpha)*tran2d_h(-T);			% alle Transformationen in einer Matrix
   
   SK1r = (M*[SK1(:,:) ones(4,1)]')';					% Transformation ausführen 
   SK1r = SK1r(:,1:2);									% homogene Koordinate entfernen
   P1r  = mean(SK1r);									% Schwerpunkt des rotierten 1. Koerpers
   
   % Darstellung des gedrehten Koerpers
   PH1r = patch(SK1r(:,1), SK1r(:,2),'r');
   set(PH1r,'FaceAlpha',.75)
   
   hold on
   PH4 = plot([P1(1) T(1) P1r(1) ], [P1(2) T(2) P1r(2)],'k-');  % Linie vom Schwerpunkt vor Rotation --> Drehpunkt --> Schwerpunkt nach Rotation
   set(PH4,'MarkerEdgeColor','k', 'MarkerFaceColor','k', 'Marker','hexagram')
   axis equal
   pause   
   
end