% R�dergelenkgest�nge: Beispiel 1
% 
% Date: 25.05.09
% Autor: Andre Meier, Noah Heynen
%

clear all;

% Anfangswinkel definieren
a10 = pi/4;
a20 = -pi/4;
% Radien definieren
R1 = 1;
R2 = 1.03;
% Position der Gelenke definieren
k1 = 0.5;
k2 = 0.75;
r1 = k1 * R1;
r2 = k2 * R2;
% Laenge der Gestaenge definieren 
%(Achtung: L�nge der Gest�nge muessen je nach Radien und Distanz der Raeder angepasst werden)
l1 = 2.5;
l2 = 3;
% Mittelpunkte der Raeder definieren
M1 = [0;0];
M2 = M1 + [R1+R2;0];
% "Lauf"-Winkel definieren
a = 0:pi/100:R2*R1*300*pi;
% Aktueller Winkel der Raeder 
a1 = a10 + a ;
a2 = a20 - R1/R2 .* a;

% Laufbahn der Gelenke auf den Raedern definieren
P1x = R1 * k1 * cos(a1) + M1(1);
P1y = R1 * k1 * sin(a1) + M1(2);
P1=[P1x;P1y];
P2x = R2 * k2 * cos(a2) + M2(1);
P2y = R2 * k2 * sin(a2) + M2(2);
P2=[P2x;P2y];


% Positionen fuer den Punkt Q berechen
for n=1:length(P1x)
    x1=P1x(n);
    x2=P2x(n);
    y1=P1y(n);
    y2=P2y(n);
    
    % (x-x1)^2+(y-y1)^2-l1^2=0
    % (x-x1)^2+(y-y2)^2-l2^2=0
    % Schnittpunkte der beiden Kreise ergeben die Positionen fuer Punkt Q
    PQy(n) = 1/2*(-y2^2*y1+y1*x2^2-2*y2*x1*x2-2*y1*x1*x2+l2^2*y1+x2^2*y2-y2*y1^2+y1*x1^2+y2*l1^2-l1^2*y1-y2*l2^2+y2*x1^2+y2^3+y1^3+(12*x2^2*l2^2*x1^2+2*x2^2*l2^2*l1^2-8*x1^3*x2*l1^2+2*x1*x2*l2^4+2*x1*x2*l1^4-8*x1^3*x2*l2^2-6*y2^2*y1^2*x2^2-2*y1^2*x2^4-2*x2^4*y2^2-x2^2*y2^4-2*y1^2*x1^4-y1^4*x1^2-2*y2^2*x1^4-y2^4*x1^2+8*y2*x1*x2*l2^2*y1+8*y2*x1*x2*l1^2*y1+2*x2^2*y2^2*l1^2+2*x2^2*y2^2*l2^2+4*y2*y1^3*x1^2+2*y1^2*x1^2*l1^2+4*y1*x1^4*y2+2*y2^2*l1^2*x1^2+4*y2^3*y1*x2^2-6*y2^2*y1^2*x1^2-x2^6+4*y2^3*y1*x1^2+8*y1^2*x2^3*x1+2*y1^2*x2^2*l2^2+4*y1*x2^4*y2+4*y1^3*x2^2*y2-12*y1^2*x2^2*x1^2+2*y1^2*x2^2*l1^2-12*y2^2*x1^2*x2^2+8*y2^2*x1*x2^3+8*y2^2*x1^3*x2+2*y2^4*x1*x2+8*y1^2*x1^3*x2+2*y1^4*x1*x2+2*l2^2*y1^2*x1^2-y1^4*x2^2-8*y2^3*y1*x1*x2+12*y2^2*y1^2*x1*x2-16*y1*x2^3*y2*x1-4*y1*x2^2*y2*l1^2-4*y1*x2^2*y2*l2^2+24*y1*x2^2*y2*x1^2-16*y2*x1^3*x2*y1-4*y2^2*x1*x2*l1^2-4*y2^2*x1*x2*l2^2-8*y2*x1*x2*y1^3-4*y1^2*x1*x2*l2^2-4*y1^2*x1*x2*l1^2-4*l2^2*y1*y2*x1^2+2*x2^4*l2^2-x2^2*l2^4-15*x2^2*x1^4-x2^2*l1^4+2*x2^4*l1^2+20*x2^3*x1^3-15*x2^4*x1^2+6*x2^5*x1+6*x1^5*x2+2*x1^4*l1^2-x1^2*l2^4-x1^2*l1^4+2*x1^4*l2^2+2*y2^2*l2^2*x1^2-8*x2^3*l2^2*x1-8*x2^3*l1^2*x1+12*x2^2*l1^2*x1^2-4*y1*x1^2*y2*l1^2-4*x1*x2*l2^2*l1^2+2*x1^2*l2^2*l1^2-x1^6)^(1/2))/(x2^2-2*x1*x2-2*y2*y1+x1^2+y2^2+y1^2);
    PQx(n) = 1/2*(-x2*l2^2+x1^3-l1^2*x1+l2^2*x1-x2^2*x1+l1^2*x2+x2^3-2*y1*y2*x1+x2*y2^2-x2*x1^2-2*x2*y2*y1+y1^2*x1+y2^2*x1+x2*y1^2+(4*x2^3*x1*y2^2-6*x2^2*x1^2*y1^2-6*x2^2*x1^2*y2^2+4*x2^3*x1*y1^2+2*l1^2*x2^2*y2^2+2*l1^2*x2^2*y1^2+2*x2^4*y2*y1-12*y1^2*y2^2*x1^2+8*y1^3*y2*x1^2+8*y1*y2^3*x1^2+8*x2^2*y2^3*y1+4*x2*y2^4*x1-12*x2^2*y2^2*y1^2+8*x2^2*y2*y1^3+12*y2^2*y1^2*l1^2+12*y2^2*y1^2*l2^2-8*y2*y1^3*l1^2+2*y2*y1*l2^4+2*y2*y1*l1^4-8*y2^3*y1*l2^2-8*y2^3*y1*l1^2-8*y2*y1^3*l2^2-y2^2*l2^4-y2^2*l1^4+2*y2^4*l2^2+2*y2^4*l1^2+2*y1^4*l1^2-y1^2*l2^4-y1^2*l1^4+2*y1^4*l2^2-x1^4*y1^2-x1^4*y2^2-y2^6-y1^6-4*y2*y1*l2^2*l1^2+8*x2*l2^2*y1*y2*x1+2*x2^2*l2^2*y2^2+2*x2^2*l2^2*y1^2+2*x1^4*y1*y2+4*x1^3*x2*y2^2+4*x1^3*x2*y1^2+2*l1^2*x1^2*y1^2+2*l1^2*x1^2*y2^2+2*l2^2*x1^2*y1^2+2*l2^2*x1^2*y2^2-4*x2^2*l2^2*y2*y1-4*x2*l2^2*y1^2*x1-4*x2*l2^2*y2^2*x1-8*x1^3*x2*y2*y1-4*l1^2*x1^2*y1*y2-4*l1^2*x1*x2*y2^2-4*l1^2*x1*x2*y1^2-4*l2^2*x1^2*y1*y2+12*x2^2*x1^2*y1*y2-8*x2^3*x1*y2*y1-4*l1^2*x2^2*y2*y1-16*y1*y2^3*x1*x2+24*y1^2*y2^2*x1*x2-16*y1^3*y2*x1*x2+2*y2^2*l2^2*l1^2+2*y1^2*l2^2*l1^2+4*y1^4*x1*x2+8*l1^2*x1*x2*y2*y1-x2^4*y2^2-x2^4*y1^2-2*x2^2*y2^4-2*y1^4*x1^2-2*y2^4*x1^2-2*x2^2*y1^4-15*y2^2*y1^4-15*y2^4*y1^2+6*y2^5*y1+6*y2*y1^5+20*y2^3*y1^3)^(1/2))/(x2^2-2*x1*x2-2*y2*y1+x1^2+y2^2+y1^2);
end

% Berechnete Kurven plotten
hold on;
axis equal;
grid on;
plot(R1*cos(a1)+M1(1),R1*sin(a1)+M1(2))
plot(R2*cos(a2)+M2(1),R2*sin(a2)+M2(2))
plot(P1x,P1y,'r')
plot(P2x,P2y,'r')
plot(PQx,PQy,'g')