% パフィアン制約の安定化
% 自動車の運動方程式を数値的に解く

global mass;
global inertia;
global damping;
global angulardamping;
global gamma;

mass = 1000;
inertia = 1500;
damping = 200;
angulardamping = 500;
gamma = 2000;

stepsize = 0.1;
interval = 0:stepsize:100; % 固定ステップ
qinit = [0; 0; pi/6; 0; 0; 0]; % 初期値
[time, q] = ode45(@car, interval, qinit);

set(0,'defaultAxesFontSize',16);
set(0,'defaultTextFontSize',16);

plot(time,q(:,1),time,q(:,2),'--');
xlabel('time');
ylabel('position');
legend({'x', 'y'}, 'Location', 'best');
grid on;
saveas(gcf, 'car-position.eps', 'eps');
fprintf("t-x t-y グラフ\n");
fprintf("一時停止：何かのキーを押してください\n");
pause;

plot(time,q(:,3));
xlabel('time');
ylabel('orientation');
legend({'\theta'}, 'Location', 'southeast');
grid on;
saveas(gcf, 'car-orientation.eps', 'eps');
fprintf("t-theta グラフ\n");
fprintf("一時停止：何かのキーを押してください\n");
pause;

constraint = q(:,4).*sin(q(:,3)) - q(:,5).*cos(q(:,3));
plot(time,constraint);
ylim([-1,1]);
xlabel('time');
ylabel('constraint');
legend('Q');
grid on;
saveas(gcf, 'car-computed-constraint.eps', 'eps');
fprintf("t-制約 グラフ\n");
fprintf("一時停止：何かのキーを押してください\n");
pause;

plot(q(:,1),q(:,2));
hold on;
xlim([-50,600]);
ylim([-50,600]);
xticks([0:100:600]);
yticks([0:100:600]);
xlabel('x');
ylabel('y');
set(gca,'DataAspectRatio',[1 1 1]);
grid on;
a = 12; b = 6;
for k = 1:5/stepsize:size(time)
    x = q(k,1); y = q(k,2); theta = q(k,3);
    C = cos(theta); S = sin(theta);
    fl = [x;y] + [C,-S; S,C]*[ a; b];
    bl = [x;y] + [C,-S; S,C]*[-a; b];
    br = [x;y] + [C,-S; S,C]*[-a;-b];
    fr = [x;y] + [C,-S; S,C]*[ a;-b];
    plot([fl(1);bl(1);br(1);fr(1);fl(1)], ...
         [fl(2);bl(2);br(2);fr(2);fl(2)], 'k');
end
hold off;
saveas(gcf, 'car-path.eps', 'eps');
fprintf("自動車の軌跡\n");
fprintf("一時停止：何かのキーを押してください\n");
pause;
