function [] = momentanpol_sol()

a = 30; % "Breite" der Ellipse
b = 50; % "Höhe"   der Ellipse
l = 15; % Abstand von Punkt P zu C

n = 100;     % Anzahl Zeitschritte
T = 1;       % Endzeit
t = 0:T/n:T; % Zeitpunkte

C = zeros(n+1,2); % Vektor fuer Koordinaten Punkt C
P = zeros(n+1,2); % Vektor fuer Koordinaten Punkt P
M = zeros(n+1,2); % Vektor fuer Koordinaten Momentanpol M 

figure;

for i=1:n+1
    [Cneu Pneu VC VP] = coordinates(t(i),a,b,l);
    C(i,:) = Cneu;
    P(i,:) = Pneu;
    
    A = [VC; VP];
    d(1,1) = Cneu*VC';
    d(2,1) = Pneu*VP';
    
    M(i,:) = (A\d)';
    
    % Plot animiert
    hold on;
    axis([-60 60 -60 60])
    plot(C(1:i,1), C(1:i,2), 'Color','black');
    plot(P(1:i,1), P(1:i,2), 'Color','red');
    plot(M(1:i,1), M(1:i,2), 'Color','green');
    hold off;
    pause(0.05);
end

% Plot komplett
figure;
hold on;
axis([-60 60 -60 60])
plot(C(:,1), C(:,2), 'Color','black');
plot(P(:,1), P(:,2), 'Color','red');
plot(M(:,1), M(:,2), 'Color','green');
hold off;


end


function [C P VC VP] = coordinates(t,a,b,l)
% Gibt Koordinaten und Geschwindigkeitskomponenten von C und P zurueck

Phi  = @(t) (2*pi*t);
dPhi = @(t) (2*pi);

C  = zeros(1,2);
P  = zeros(1,2);
VC = zeros(1,2);
VP = zeros(1,2);

C(1) = a * cos(Phi(t));
C(2) = b * sin(Phi(t));
term = (a/b)*(a/b)*C(2)*C(2) + (b/a)*(b/a)*C(1)*C(1);
P(1) = C(1) + l*(a/b)*C(2)/sqrt(term);
P(2) = C(2) - l*(b/a)*C(1)/sqrt(term);

VC(1) = - dPhi(t) * (a/b) * C(2);
VC(2) =   dPhi(t) * (b/a) * C(1);
VP(1) = VC(1) + l*dPhi(t)*C(1)/sqrt(term) ...
        - l*dPhi(t)*(a/b)*C(2)*(((a/b)-(b/a))*C(1)*C(2))/(term*sqrt(term));
VP(2) = VC(2) + l*dPhi(t)*C(2)/sqrt( term ) ...
        + l*dPhi(t)*(b/a)*C(1)*(((a/b)-(b/a))*C(1)*C(2))/(term*sqrt(term));
    
end