plotrob.m

来自「多智能体工具包」· M 代码 · 共 46 行

function [FirstArray,SecondArray,Fill1Array,Fill2Array]=plotrob(ObjectPosition,CurrentRobot,RobotSize,ObjectSize)
% PLOTROB	Draw robot.

%     Copyright (c) 1998-2000 Jiming Liu and Jianbing Wu

FirstArray=[];
SecondArray=[];
Fill1Array=[];
Fill2Array=[];

TheDirection=atan2(CurrentRobot(2)-ObjectPosition(2),CurrentRobot(1)-ObjectPosition(1));
FirstAngle=TheDirection+pi+pi/2;
ForthAngle=TheDirection+pi-pi/2;

Temp1X=CurrentRobot(1)+RobotSize(1)/2*cos(FirstAngle);
Temp1Y=CurrentRobot(2)+RobotSize(1)/2*sin(FirstAngle);
FirstArray=[FirstArray Temp1X];
SecondArray=[SecondArray Temp1Y];
Fill1Array=[Fill1Array Temp1X];
Fill2Array=[Fill2Array Temp1Y];

AnotherCenter=[CurrentRobot(1)+RobotSize(2)*cos(TheDirection) CurrentRobot(2)+RobotSize(2)*sin(TheDirection)];
Temp2X=AnotherCenter(1)+RobotSize(1)/2*cos(FirstAngle);
Temp2Y=AnotherCenter(2)+RobotSize(1)/2*sin(FirstAngle);
FirstArray=[FirstArray Temp2X];
SecondArray=[SecondArray Temp2Y];
Fill1Array=[Fill1Array (Temp1X+Temp2X)/2];
Fill2Array=[Fill2Array (Temp1Y+Temp2Y)/2];

Temp3X=AnotherCenter(1)+RobotSize(1)/2*cos(ForthAngle);
Temp3Y=AnotherCenter(2)+RobotSize(1)/2*sin(ForthAngle);
FirstArray=[FirstArray Temp3X];
SecondArray=[SecondArray Temp3Y];

Temp4X=CurrentRobot(1)+RobotSize(1)/2*cos(ForthAngle);
Temp4Y=CurrentRobot(2)+RobotSize(1)/2*sin(ForthAngle);
FirstArray=[FirstArray Temp4X];
SecondArray=[SecondArray Temp4Y];
Fill1Array=[Fill1Array (Temp3X+Temp4X)/2];
Fill2Array=[Fill2Array (Temp3Y+Temp4Y)/2];
Fill1Array=[Fill1Array Temp4X];
Fill2Array=[Fill2Array Temp4Y];

FirstArray=[FirstArray Temp1X];
SecondArray=[SecondArray Temp1Y];