%   Rechturner mit Koordinatenvorgabe


x = [0.00  0.15  0.13  0.19  0.29  0.28];
y = [0.00 -0.19 -0.46 -0.60 -0.71 -0.78];

ReckT=[x;y]

plot(x, y,'r');
hold on;
plot(x, y,'or');


[Koordneu]=Drehung(ReckT,60)


xneu=Koordneu(1,:);
yneu=Koordneu(2,:);

plot(xneu,yneu,'b');
plot(xneu,yneu,'ob');



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Drehung um 1 Gelenk
% Stange, Schulter,Hüfte, Knie, Sprunggelenk, Zeh
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Gelenk=3;
GelenksWinkel=60;




Folge_Gelenk=Gelenk+1;

% Verelgung des Koordinatenurpsrungs in das zu drehende Gelenk
x_Ursprung=xneu+abs(xneu(1,Gelenk));
y_Ursprung=yneu+abs(yneu(1,Gelenk));
%Zusammenfassung zu einer Matrix
XY_Gelenk=[x_Ursprung;y_Ursprung];

%Damit die vorherigen Variablen nicht überschrieben werden.
xneu_Gelenk=xneu;
yneu_Gelenk=yneu;

Dreh_Gelenk=Drehung(XY_Gelenk(:,Gelenk:end),GelenksWinkel);

%Löscht die ertse Spalte "Koordinaten-Ursprung" aus der Matrix
Dreh_Gelenk(:,1)=[];

%Bringt die neuen Gelenkskoordinaten in das "alte Modell" ein
xneu_Gelenk(1,Folge_Gelenk:end)=Dreh_Gelenk(1,:)+xneu(1,Gelenk);
yneu_Gelenk(1,Folge_Gelenk:end)=Dreh_Gelenk(2,:)+yneu(1,Gelenk);

plot(xneu_Gelenk,yneu_Gelenk,'b');
plot(xneu_Gelenk,yneu_Gelenk,'ob');