[From Bill Powers (991215.1525 MDT)]
look at how a model can be coded in Turbo Pascal 7.0.
Appended is the source code for Avoid3.pas. I will try to work up a diagram
and some hints about how to play around with the source code (be sure to
save a master copy that you don't change). This program has one person
start from the left of a field of 200 obstacles and move to a destination
circle on the right, avoiding collisions most of the time. I invite others
to improve the avoidance control systems. Anyone who has forgotten how to
extract and save the program part of a post can contact me direct for
reminders.
The actual code starts below the ============ line and ends just before the
second one.
Best,
Bill P.
···
To: All who are compiling Crowdv3 routines, and anyone else who just wants to
program Avoid3;
{This program is built on "seek3.pas". We now add a definition of
AvoidDirCont and AvoidProxCont to the list of control systems in
the active person. The avoidance systems change the direction and
proximity reference signals for the goal seeking systems.
}
uses dos,crt,graph,setSVGA,mouse;
const dt = 0.01; {real time for one iteration}
{$i \tp\crowdv3\specchar.inc} {include special character codes}
numobstacles = 200;
circlesize = 5;
type ControlSystemDataType = record
r,p,e,g,o,lim: double;
end;
activetype = record
{THESE ARE CONTROL SYSTEM DATA RECORDS}
SpeedCont, DirCont, AvoidDirCont,AvoidProxCont,
GoalProxCont, GoalDirCont: ControlSystemDataType;
{THESE HAVE TO DO WITH A PERSON'S MOVEMENTS}
FwdAccel, FwdVel, SideAccel,
Dir, TurnRate,
Xp, Yp, OldXp, OldYp,
Ax, Ay, Vx, Vy,
GoalX, GoalY,
GoalProx, RightProx, LeftProx,
dx, dy: double;
mass,drag: double;
end;
obstacletype = record
xloc,yloc: integer;
end;
{FOR DRAWING CIRCLES ERASABLE WITH XOR}
polycircletype = array[1..16] of record dx,dy: integer; end;
var ch : char;
i0,d0: double;
i: integer;
numstr: string;
iter: longint;
{NOTE: the following record will be expanded into an
array of all active persons when there is more than one}
person: activetype;
obstacles: array[1..numobstacles] of obstacletype;
polycircle: polycircletype;
personradius,range: double;
trace: boolean;
procedure initprogram;
begin
with person do
begin
Xp := -hsize div 2 + 50; {INITIAL POSITION OF PERSON}
Yp := 0; {meters}
Dir := pi; {radians} {INITIAL DIRECTION OF MOTION}
FwdVel := 30.0; {m/sec} {INITIAL FORWARD VELOCITY}
mass := 75; {kg} {MASS OF PERSON}
drag := 0.1; {DRAG TO LIMIT FORWARD SPEED}
SpeedCont.g := 100.0; {SPEED CONTROL GAIN}
DirCont.g := 100.0; {DIRECTION CONTROL GAIN}
GoalDirCont.r := 0.0; {GOAL TO BE KEPT STRAIGHT AHEAD}
GoalDirCont.g := 100.0; {RELATIVE DIRECTION CONTROL GAIN}
GoalProxCont.r := 4.0; {NATURAL LOG, DESIRED GOAL PROXIMITY}
GoalProxCont.g := 8.0; {GOAL PROXIMITY CONTROL GAIN}
AvoidProxCont.r := 0.0 ; {AVOIDANCE PROXIMITY CONTROL REF}
AvoidProxCont.g := 0.005; {AVOIDANCE PROXIMITY CONTROL GAIN}
AvoidDirCont.r := 0.0 ; {AVOIDANCE DIRECTION CONTROL REF}
AvoidDirCont.g := 0.0; {AVOIDANCE DIRECTION CONTROL GAIN}
GoalX := 0.45*Hsize; {X AND Y LOCATION OF GOAL CIRCLE}
GoalY := 0.0;
setcolor(lightgreen);
circle(Hsize div 2 + round(GoalX),
Vsize div 2 - round(GoalY),15); {DRAW GOAL}
end; {SCOPE OF "WITH PERSON"}
i0 := 100.0;
d0 := circlesize*circlesize;
personradius := hsize/200;
mousex := 0;
mousey := 0;
setfillstyle(0,0);
iter := 0;
range := 5.0;
end;
{NEEDED BECAUSE XOR NOT POSSIBLE WITH REGULAR CIRCLE COMMAND}
procedure makecircle(radius: double);
var i: integer;
a: double;
begin
for i := 1 to 16 do
begin
a := 2.0*pi/16.0*i;
polycircle[i].dx := round(radius*cos(a));
polycircle[i].dy := round(radius*sin(a));
end;
end;
procedure drawcircle(Xp,Yp: double;var c: polycircletype);
var i: integer;
c1: polycircletype;
begin
for i := 1 to 16 do
begin
c1[i].dx := round(Xp) + c[i].dx;
c1[i].dy := round(Yp) + c[i].dy;
end;
drawpoly(16,c1);
end;
{SHOW A LABELED FLOATING POINT NUMBER ON SCREEN AT X,Y}
procedure shownum(s: string; n:double; Xp,Yp: integer );
begin
str(n:7:3,numstr);
numstr := s + numstr;
bar(Xp,Yp,Xp+8*length(numstr),Yp+10);
outtextxy(Xp,Yp,numstr);
end;
{ADD TWO ANGLES, MAKE SURE RESULT IS IN RANGE -PI TO +PI}
function addangle(a1,a2: double): double;
var a: double;
begin
a := a1 + a2;
if a > pi then addangle := a - 2.0*pi
else
if a < -pi then addangle := a + 2.0*pi
else addangle := a;
end;
{FIND ANGLE FOR POINTS SEPARATED BY DELTA Y AND DELTA X}
function invtan(Dx,Dy: double): double;
var xx,yy,theta: double;
begin
xx := abs(Dx); yy := abs(Dy);
if xx = 0 then invtan := pi/2.0 {Take care of zero arguments}
else
if yy = 0 then invtan := 0.0
else
begin
if xx > yy then theta := arctan(yy/xx) {Reduce range to 0..45 degrees}
else theta := pi/2.0 - arctan(xx/yy); {for accuracy}
if Dx >= 0.0 then {First and fourth quadrants}
begin
if Dy >= 0.0 then invtan := theta
else invtan := -theta;
end
else {Second and third quadrants}
begin
if Dy >= 0.0 then invtan := pi - theta
else
invtan := - (pi - theta);
end;
end;
end;
procedure makeobstacles;
var i,j,Xp,Yp: integer;
begin
setcolor(green);
for i := 1 to numobstacles do
with obstacles[i] do
begin
xloc := random(3*hsize div 4) - 3*hsize div 8;
yloc := random(3*vsize div 4) - 3*vsize div 8;
circle(hsize div 2 + xloc,vsize div 2 - yloc,circlesize);
end;
end;
{
}
procedure avoidobstacle;
var i: integer;
angle,a,prox,rsq: double;
begin
with person do
begin
leftprox := 0.0; rightprox := 0.0;
for i := 1 to numobstacles do
with obstacles[i] do
begin
a := invtan(xloc - Xp,yloc - Yp);
{Compute relative angle to obstacle}
angle := addangle(-Dir, a);
{get square of distance to obstacle}
rsq := sqr(xloc - Xp) + sqr(yloc - Yp);
{limit proximity to max value}
if rsq < sqr(circlesize) then rsq := sqr(circlesize);
{The divisor "range", set to 5 makes the influence extend 5 times
farther away from the obstacle, so turns begin sooner}
rsq := rsq/range;
{Compute proximity as inverse-square function of distance
weighted to discount rearward proximities}
prox := (i0 * d0/rsq)*cos(0.5*angle);
{accumulate left and right proximities}
if angle > 0.0 then
rightprox := rightprox + prox
else
leftprox := leftprox + prox;
end;
{THE ABOVE COMPUTES OBJECTIVE PROXIMITIES.
NOW WE HAVE THE CONTROL SYSTEMS. }
{We use only the perceptual signal from the direction control system}
with AvoidDirCont do
begin
p := rightprox - leftprox;
end;
{Convert proximity error into a change of goal direction}
with AvoidProxCont do
begin
p := rightprox + leftprox;
e := r - p;
if e > 0 then e := 0.0; {one-way control}
e := e + 40.0*(random - 0.5); { dither to avoid impasse}
if AvoidDirCont.p > 0 then
o := g * e {output signal will bias goal-seeking ref signal}
else
o := -g * e;
end;
end;
end;
procedure SeekGoal; {Second level of control systems}
begin
with person do
begin
{GOAL SEEKING DIRECTION CONTROL}
With GoalDirCont do
begin
{Reference signal r is set by collision avoidance control system}
{Perceptual signal is direction to goal relative to direction
of travel, computed by invtan function}
r := AvoidProxCont.o;
p := addangle(Dir, -invtan(GoalX - Xp,GoalY - Yp));
o := g*(r - p); {Output goes to direction control reference signal}
end;
{Use a logarithmic proximity perception based on inverse square law}
GoalProx := ln(1.386 * i0 * d0/(sqr(GoalX - Xp) + sqr(GoalY-Yp)));
{GOAL SEEKING PROXIMITY CONTROL}
with GoalProxCont do
begin
p := GoalProx; { reference goal prox is fixed by InitProgram}
e := r - p;
o := g*e; {output goes to speed control reference signal}
end;
end;
end;
procedure MovePerson; {First level of control systems}
begin
with Person do
begin
with SpeedCont do
begin
r := GoalProxCont.o; {reference forward velocity}
e := r - p;
o := g*e/Mass; {Output is forward acceleration}
end;
with DirCont do
begin
r := GoalDirCont.o; {reference goal direction}
e := r - p;
o := g*e*dt; {output is turn rate}
end;
end;
end;
procedure Environment;
var u,v: double;
begin
with person do
begin
TurnRate := DirCont.o;
SideAccel := FwdVel*TurnRate;
FwdAccel := SpeedCont.o;
FwdVel := FwdVel +
(FwdAccel - drag*FwdVel*abs(FwdVel))*dt; {Completes speed control}
SpeedCont.p := FwdVel;
{Linear Xp and Yp accelerations for computing position in lab space}
u := cos(Dir);
v := sin(Dir);
Ax := (FwdAccel*u - SideAccel*v);
Ay := (FwdAccel*v + SideAccel*u);
{Amount of movement in Xp and Yp in lab space}
dx := (Vx + 0.5*Ax*dt)*dt;
dy := (Vy + 0.5*Ay*dt)*dt;
Vx := FwdVel*u; {u = Cos(Dir)}
Vy := FwdVel*v; {v = Sin(Dir)}
Xp := Xp + dx;
Yp := Yp + dy;
Dir := addangle(Dir,Turnrate*dt); {keep within -pi to pi}
end;
end;
procedure showperson;
const h: integer = 400;
v: integer = 300;
begin
if iter = 0 then
begin
h := hsize div 2;
v := vsize div 2;
end;
{ERASE OLD PERSON LOCATION, SHOW NEW ONE}
with person do
begin
setcolor(white);
if iter > 0 then
drawcircle(h + OldXp, v - OldYp, polycircle);
OldXp := Xp; OldYp := Yp;
drawcircle(h + OldXp, v - OldYp, polycircle);
if trace then putpixel(round(h + OldXp), round(v - OldYp),white);
end;
end;
begin
clrscr;
initmouse; {initialize mouse}
initSVGA(4); {Initialize screen to 800 X 600}
randomize;
ch := chr(0);
setcolor(white);
trace := true; {when true, leaves a track behind moving person}
repeat
clearviewport;
initprogram;
makeobstacles;
outtextxy(hsize div 3,vsize - 20,'SPACE FOR ANOTHER RUN, ESC OR q TO QUIT');
setwritemode(XORput);
makecircle(personradius); {create the circle that will be the person}
{Each iteration of the following loop represents an elapsed time of
"dt", set for now to 0.01 sec.}
while not keypressed do
begin
Readmouse;
SeekGoal;
MovePerson;
AvoidObstacle;
Environment;
showperson;