%约束空间生成
if((sqrt((x0-xd+r)^2+(y0-yd)^2)-3*r)>0)
fprintf('距离过远，最小半径泊车方法无法泊车\n');
return;
end
if((t1-t2)+(pi/8)<0 | (t1-t2)-(pi/8)>0)
fprintf('终点车身偏角过大，最小半径泊车方法无法泊车\n');
return;
end
%第一阶段轨迹
x1=x0+r;
y1=y0;
fai1=fai;
for i=pi:ds:(pi+t1)
x=x1+r*cos(i);
y=y1+r*sin(i);
theta=i-(pi/2);%theta0为起始点车身偏角
jiao1=atan((width/2)/(long-houxuan));
jiao2=jiao1;
jiao3=atan((width/2)/houxuan);
jiao4=jiao3;
jiao1=theta-jiao1;
jiao2=theta+jiao2;
jiao3=theta+pi-jiao3;
jiao4=theta+pi+jiao4;
r1=sqrt((width/2)^2+(long-houxuan)^2);%以下描述车身的四个端点
r2=sqrt((width/2)^2+houxuan^2);
youqianx=x+r1*cos(jiao1);
youqiany=y+r1*sin(jiao1);
zuoqianx=x+r1*cos(jiao2);
zuoqiany=y+r1*sin(jiao2);
zuohoux=x+r2*cos(jiao3);
zuohouy=y+r2*sin(jiao3);
youhoux=x+r2*cos(jiao4);
youhouy=y+r2*sin(jiao4);
h1=plot([youqianx,zuoqianx],[youqiany,zuoqiany],'-r');
h2=plot([zuoqianx,zuohoux],[zuoqiany,zuohouy],'-r');
h3=plot([zuohoux,youhoux],[zuohouy,youhouy],'-r');
h4=plot([youhoux,youqianx],[youhouy,youqiany],'-r');
jiao5=atan((width/2)/(long-qianxuan-houxuan));%以下描写车的四个轮子
jiao6=jiao5;
jiao5=theta-jiao5;
jiao6=theta+jiao6;
jiao7=theta+(pi/2);
jiao8=theta-(pi/2);
jiao9=theta+fai1;
jiaoa=theta;
r3=sqrt((width/2)^2+(long-qianxuan-houxuan)^2);
r4=width/2;
yqianlunzx=x+r3*cos(jiao5);
yqianlunzy=y+r3*sin(jiao5);
yqianlunqx=yqianlunzx+lunjin*cos(jiao9);
yqianlunqy=yqianlunzy+lunjin*sin(jiao9);
yqianlunhx=yqianlunzx-lunjin*cos(jiao9);
yqianlunhy=yqianlunzy-lunjin*sin(jiao9);
h5=plot([yqianlunqx,yqianlunhx],[yqianlunqy,yqianlunhy],'-k');
zqianlunzx=x+r3*cos(jiao6);
zqianlunzy=y+r3*sin(jiao6);
zqianlunqx=zqianlunzx+lunjin*cos(jiao9);
zqianlunqy=zqianlunzy+lunjin*sin(jiao9);
zqianlunhx=zqianlunzx-lunjin*cos(jiao9);
zqianlunhy=zqianlunzy-lunjin*sin(jiao9);
h6=plot([zqianlunqx,zqianlunhx],[zqianlunqy,zqianlunhy],'-k');
zhoulunzx=x+r4*cos(jiao7);
zhoulunzy=y+r4*sin(jiao7);
zhoulunqx=zhoulunzx+lunjin*cos(jiaoa);
zhoulunqy=zhoulunzy+lunjin*sin(jiaoa);
zhoulunhx=zhoulunzx-lunjin*cos(jiaoa);
zhoulunhy=zhoulunzy-lunjin*sin(jiaoa);
h7=plot([zhoulunqx,zhoulunhx],[zhoulunqy,zhoulunhy],'-k');
yhoulunzx=x+r4*cos(jiao8);
yhoulunzy=y+r4*sin(jiao8);
yhoulunqx=yhoulunzx+lunjin*cos(jiaoa);
yhoulunqy=yhoulunzy+lunjin*sin(jiaoa);
yhoulunhx=yhoulunzx-lunjin*cos(jiaoa);
yhoulunhy=yhoulunzy-lunjin*sin(jiaoa);
h8=plot([yhoulunqx,yhoulunhx],[yhoulunqy,yhoulunhy],'-k');
%在此添加判决函数，判断是否第一阶段会碰到障碍物
if(yhoulunzx-x6>=0 & yhoulunzy-y6>=0)
fprintf('第一阶段碰障，最小半径泊车方法无法泊车\n');
delete(h1);
delete(h2);
delete(h3);
delete(h4);
delete(h5);
delete(h6);
delete(h7);
delete(h8);
return;
end
pause(0.01);
delete(h1);
delete(h2);
delete(h3);
delete(h4);
delete(h5);
delete(h6);
delete(h7);
delete(h8);
end
