COMMENT ⊗   VALID 00006 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	(*$NOMAIN	Arithmetic routines used by AL *)
C00004 00003	(* external routines *)
C00006 00004	(* trig & scalar functions: sind, cosd, tand, vdot, vmagn *)
C00009 00005	(* vector functions: svmul, vsdiv, vadd, vsub, unitv, vcross, tvmul *)
C00013 00006	(* trans functions: vsaxwr, construct, vmkfrc *)
C00026 ENDMK
C⊗;
(*$NOMAIN	Arithmetic routines used by AL *)

program arith;

const pi = 3.1415926535; rad = 0.0174532925;

type scalar = real;
     ascii = char;
     vectorval = array [1..3] of real;
     vector = record  refcnt: integer; val: vectorval end;
     vectorp = ↑vector;
     transval = array [1..3,1..4] of real;
     trans = record  refcnt: integer; val: transval end;
     transp = ↑trans;

cstring = packed array [1..10] of ascii;
c5str = packed array [1..5] of ascii;
c20str = packed array [1..20] of ascii;

(* external routines *)

  	(* From ALLOC *)
function newVector: vectorp; 				external;
procedure relVector(v: vectorp); 			external;
function newTrans: transp; 				external;
procedure relTrans(t: transp); 				external;

	(* From DISP *)
procedure ppLine; 					external;				
procedure ppOutNow; 					external;
procedure ppChar(ch: ascii); 				external;
procedure pp5(ch: c5str; length: integer); 		external;
procedure pp10(ch: cstring; length: integer); 		external;
procedure pp10L(ch: cstring; length: integer);		external;
procedure pp20(ch: c20str; length: integer); 		external;
procedure pp20L(ch: c20str; length: integer); 		external;
procedure ppReal(r: real); 				external;

(* trig & scalar functions: sind, cosd, tand, vdot, vmagn *)

function sind(d: real): real;  external;
function sind;
begin sind := sin(rad*d) end;

function cosd(d: real): real;  external;
function cosd;
begin cosd := cos(rad*d) end;

function tand(d: real): real;  external;
function tand;
begin tand := sin(rad*d)/cos(rad*d) end;

function vdot (u,v: vectorp): scalar; external;
function vdot ;
begin
 vdot := u↑.val[1]*v↑.val[1] + u↑.val[2]*v↑.val[2] + u↑.val[3]*v↑.val[3];
 if u↑.refcnt <= 0 then relVector(u);
 if v↑.refcnt <= 0 then relVector(v);
end;

function vmagn (v: vectorp): scalar; external;
function vmagn ;
begin
 with v↑ do vmagn := sqrt(val[1]*val[1] + val[2]*val[2] + val[3]*val[3]);
 if v↑.refcnt <= 0 then relVector(v);
end;

(* vector functions: svmul, vsdiv, vadd, vsub, unitv, vcross, tvmul *)

function svmul (s: scalar; v: vectorp): vectorp; external;
function svmul ;
var u: vectorp; i: 1..3;
begin
 if v↑.refcnt <= 0 then u := v else u := newVector;
 with v↑ do for i:= 1 to 3 do u↑.val[i] := s * val[i];
 svmul := u;
end;

function vsdiv (v: vectorp; s: scalar): vectorp; external;
function vsdiv ;
var u: vectorp; i: 1..3;
begin
 if v↑.refcnt <= 0 then u := v else u := newVector;
 if s = 0 then
   begin
   pp20L('VSDIV: Attempt to di',20); pp20('vide by zero!       ',13); ppLine;
   end;
 with v↑ do for i:= 1 to 3 do u↑.val[i] := val[i] / s;
 vsdiv := u;
end;

function vadd (u,v: vectorp): vectorp; external;
function vadd ;
var w: vectorp; i: 1..3;
begin
 w := newVector;
 with w↑ do for i:= 1 to 3 do val[i] := u↑.val[i] + v↑.val[i];
 if u↑.refcnt <= 0 then relVector(u);
 if v↑.refcnt <= 0 then relVector(v);
 vadd := w;
end;

function vsub (u,v: vectorp): vectorp; external;
function vsub ;
var w: vectorp; i: 1..3;
begin
 w := newVector;
 with w↑ do for i:= 1 to 3 do val[i] := u↑.val[i] - v↑.val[i];
 if u↑.refcnt <= 0 then relVector(u);
 if v↑.refcnt <= 0 then relVector(v);
 vsub := w;
end;

function unitv (v: vectorp): vectorp; external;
function unitv ;
begin
 unitv := vsdiv(v,vmagn(v));
end;

function vcross (u,v: vectorp): vectorp; external;
function vcross ;
var w: vectorp; i: 1..3;
begin
 w := newVector;
 with w↑ do
  begin
  val[1] := u↑.val[2]*v↑.val[3] - u↑.val[3]*v↑.val[2];
  val[2] := u↑.val[3]*v↑.val[1] - u↑.val[1]*v↑.val[3];
  val[3] := u↑.val[1]*v↑.val[2] - u↑.val[2]*v↑.val[1];
  end;
 if u↑.refcnt <= 0 then relVector(u);
 if v↑.refcnt <= 0 then relVector(v);
 vcross := w;
end;

function tvmul (t: transp; v: vectorp): vectorp; external;
function tvmul ;
var u: vectorp; i,j: 1..3;
begin
 u := newVector;
 with u↑ do
  for i:= 1 to 3 do
   begin
    val[i] := t↑.val[i,4];
    for j := 1 to 3 do val[i] := val[i] + t↑.val[i,j] * v↑.val[j];
   end;
 if t↑.refcnt <= 0 then relTrans(t);
 if v↑.refcnt <= 0 then relVector(v);
 tvmul := u;
end;

(* trans functions: vsaxwr, construct, vmkfrc *)

function vsaxwr(ax: vectorp; w: real): transp; external;
function vsaxwr;
var cx,cy,cz,sw,cw: real; r: transp; i: 1..3;
begin (* produces a rotn, given axis vector ax & magnitude w *)
 ax := unitv(ax);
 cx := ax↑.val[1];
 cy := ax↑.val[2];
 cz := ax↑.val[3];
 sw := sind(w); cw := cosd(w);
 r := newTrans;
 with r↑ do
  begin
  val[1,1] := cw + (1-cw) * cx * cx;
  val[1,2] := (1-cw) * cx * cy - cz * sw;
  val[1,3] := (1-cw) * cx * cz + cy * sw;
  val[2,1] := (1-cw) * cx * cy + cz * sw;
  val[2,2] := cw + (1-cw) * cy * cy;
  val[2,3] := (1-cw) * cy * cz - cx * sw;
  val[3,1] := (1-cw) * cx * cz - cy * sw;
  val[3,2] := (1-cw) * cy * cz + cx * sw;
  val[3,3] := cw + (1-cw) * cz * cz;
  for i := 1 to 3 do val[i,4] := 0;
  end;
 if ax↑.refcnt <= 0 then relVector(ax);
 vsaxwr := r;
end;

function construct(org,vx,vxy: vectorp): transp; external;
function constru`πPv~∃m¬dAht↓ieC]M`vAp1rYtt↓mKGi=e`vA$t@b\8fv~∃	KOS\4∀A←e≥<]eK→G]h@hzA←e≥<]eK→G]h@,@bv∩ TAg↑↓oJAI=\OhAIKYKCMJA←e≤Ai←↑↓g←←\TR~∀↓p@tz↓k]SiXQmgkλQmpY=eNRRl∩∩PT↓←VAi<AeKG1CSZAYpA]←\@TR~(AmqrtzAmMkDQmarY←e≤Rv∩∩$PTA←,Ai↑AIKGYC%ZAmqdA]←nTR~∀↓t@tz↓k]SiXQmGe=gfQp1mqrR$v~∀Ad@tzAYGe←gLQtYp$v~∀AP@tzA9Ko)e¬]fv~(AoSi Ai<A⊃↑~∀@↓M←dA$@tz@DAi↑@LAI↑~(@@AE∃OS\~(@@Am¬Y7RXE:@tz↓q<]m¬Y7S:l~∀@@↓mCY7$Xe:@hzAs<9mCY7%:v~∀@AmC17RXgt@tzAi<]mC17S:v4∀@@AYCY7R0i:@ttA←eOx]mCYmS:v~(@@AK9Hv~∀↓←eO<9eKMG9h@tz↓←eO<9eKMG9h@Z@Dv~∀A%LA←e≥<]eK→G]h@pz@`AQQK\AIKY-K
i←dQ=eNRv$PTAG¬\AeK
YCSZ↓←eNA9←n@T$~∀Ae∃Y-KGQ←dQp$vAeK1-KGi=dQrRlAeKYYKGi←HQtRv$PTAI=]JAo%iPAi!KgJAQ←↑@T$~∀AG=]gieUGh@ttAhv~)K]Hv4∀~∃MU]GiS=\Am[-MeFQXtAmK
i←e`$tAie¬]g`v↓KqiKI]CXv4∃Mk]
iS←\↓m[WMIFv~∀↓mCdAPtAie¬]g`v↓pYrYhYBYDhAeKC0vARt↓S]iK≥Kdv~(AEKO%\∩PT↓iCWKLAM←e
JAmK
i←dA¬]HA[¬WKfA∧AieC9fAoSQPAiQ∀Ap[CaSfAC1←]NAYKGi←H@TR~(Al@ttAk]SQlQlRl∩∩∩∩ TA[C-JAM←IGJAm∃FAS]Q↑Ak]%hAmK
i←d@(R~∀A]SiPAY<AI↑4∀@AE∃OS\A`@tzAYCY6ctvAr@hzAmC16e:v↓t@tz↓mCY6M:vAK9Hv~∀↓eKY-∃Gi←d!lRv∩$∩∩PT↓I←]J↓oSiP↓lA]←\@TR~(Ah@ttA]KoQeC]fl~∀Ao%iPAixAI↑~(@AEK≥S\~∀AmCYlbXc:tzApl~∀@AYCY6b0e:@ttArv~(@AmC16bXgt@tzAhv~∀@↓SL@Q`@z@`$AC]HQr@z`RAi!K\~∀@@AE∃OS\~(@@@A→←dARtz@b↓i↑@f↓I↑AE∃OS\AYCY6d1S:@tt@`vAYCY6f1S:@tt@`AK9Hv~∀@@Am¬Y6dXI:@tz↓tv~∀@@Am¬Y6fXE:@tz[tv~(@@@A∃]H~∀@AKYMJ~∀@@AEK≥S\~∀@@ADtzAgEehQp)p@VAdUrRv4∀@@@↓B@tzZAr@<ADv~(@@@Aλ@tzA`@↑ADl~∀@@AmCYldXc:tzABl~∀@@AmCYldXe:tzADl~∀@@AmCYldXg:tz@`l~∀@@AmCYlfXc:tz@Z↓D@TAhv~∀@@AmC16fXet@tzA∧@TAtl~∀@@AmCYlfXg:tzADTAp@4AB@T↓rv~∀@@AK9Hv~∀AM←d↓R@tzbAi↑fAI↑↓mCY7$Xi:@hz@`v4∀@AK9Hv~∀↓m[WMIF@tz↓hv~∀↓K]Hv4∀~∀