Avoid3.pas

[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;

  if (iter mod 10) = 0 then
  with Person do
  begin
   shownum('GoalProx = ',GoalProx,0,0);
   shownum('Dir = ',Dir,0,12);
   shownum('SpeedCont.p = ',Speedcont.p,0,24);
   shownum('SpeedCont.r = ',SpeedCont.r,0,36);
   shownum('TurnRate = ',TurnRate,0,48);
   shownum('FwdAccel = ',FwdAccel,0,60);
   shownum('RightProx = ',rightprox,0,72);
   shownum('LeftProx = ',leftprox,0,84);
   shownum('GoalDirCont.r = ',GoalDircont.r,0,96);
  end;
  inc(iter);
end;
if keypressed then ch := readkey;
until (ch = 'q') or (ch = chr(Esc));
Closegraph;
end.