Matlab做一個剛體運動仿真，模擬小車避障

由於老師要求不用實現太複雜，自己也沒花太多時間，就簡單模擬小車（剛體）撞牆自動轉彎，效果如下：

核心代碼如下，還有幾個是前後左右轉動的子代碼。

clear all;
clc;
close all;
%構造剛體
x=[0 10 -10 0 0];
 y=[10 -10 -10 10 0];
 fill(x,y,'b')
 hold on;
 %畫布大小
 axis([-200,200,-200,200])
 g=[x;y];
 %規定運行區域
 center_x = -150;
 center_y = -150;
 center_width = 300;
 center_high = 300;
 rectangle('Position',[center_x,center_y,center_high,center_width],'Curvature', [0 0], 'edgecolor','r')
 right_flag = 0;
 left_flag = 0;
 go_up =1;
 go_flag = 1;
 go_down =0;
 
  for i = 0:1:500
      if(go_up == 1)
          g = go_ahead(g,0,1);
          pause(0.05);
          fill(g(1,:),g(2,:),'b');
      end;
      if(go_down == 1)
          g = go_ahead(g,0,-1);
          pause(0.05);
          fill(g(1,:),g(2,:),'b'); 
      end;
      if(left_flag == 1)
          g = go_ahead(g,-1,0);
          pause(0.05);
          fill(g(1,:),g(2,:),'b');
      end;
      if(right_flag == 1)
         
          g = go_ahead(g,1,0);
          pause(0.05);
          fill(g(1,:),g(2,:),'b'); 
      end;
      %向上走碰到邊界左轉
      if((go_up == 1) && (150 - g(2,5))<11)
          g = left(g);
          left_flag = 1;
          go_down =0;
          right_flag = 0;
          go_up = 0;
      end;
        %向左走碰到邊界左轉，向下走
      if((left_flag == 1) && (150-g(2,5))<11 && (g(1,5)+150)<11)
          g = left(g);
          left_flag = 0;
          go_down =1;
          right_flag = 0;
          go_up = 0;
      end;
      %向下走碰到邊界左轉，向右走
      if((go_down == 1) && (g(2,5)+150)<11 && (g(1,5)+150)<11)
           g = left(g);
          left_flag = 0;
          go_down =0;
          right_flag = 1;
          go_up = 0;
      end;
      %向右走碰到邊界左轉，向上走
       if((right_flag == 1) && (g(2,5)+150)<11 && (150-g(1,5))<11)
          g = left(g);
          left_flag = 0;
          go_down =0;
          right_flag = 0;
          go_up = 1;
      end;
  end;