perm filename ARITH1.2[EAL,HE] blob
sn#701195 filedate 1983-03-24 generic text, type C, neo UTF8
COMMENT ⊗ VALID 00006 PAGES
C REC PAGE DESCRIPTION
C00001 00001
C00002 00002 (*$NOMAIN Arithmetic routines used by AL *)
C00003 00003 (* external routines *)
C00005 00004 (* trig & scalar functions: sind, cosd, tand, vdot, vmagn *)
C00007 00005 (* vector functions: svmul, vsdiv, vadd, vsub, unitv, vcross, tvmul *)
C00011 00006 (* trans functions: vsaxwr, construct, vmkfrc *)
C00015 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 construct;
var t: transp; x,y,z: vectorp; i: 1..3;
begin
org↑.refcnt := org↑.refcnt + 1; (* so we don't release org too soon *)
x := unitv(vsub(vx,org)); (* ok to reclaim vx now *)
vxy := vsub(vxy,org); (* ok to reclaim vxy now *)
z := unitv(vcross(x,vxy));
y := vcross(z,x);
t := newTrans;
with t↑ do
for i := 1 to 3 do
begin
val[i,1] := x↑.val[i];
val[i,2] := y↑.val[i];
val[i,3] := z↑.val[i];
val[i,4] := org↑.val[i];
end;
org↑.refcnt := org↑.refcnt - 1;
if org↑.refcnt <= 0 then relVector(org); (* can reclaim org now *)
relVector(x); relVector(y); relVector(z); (* done with these too *)
construct := t;
end;
function vmkfrc(v: vectorp): transp; external;
function vmkfrc;
var t: transp; x,y,z,a,b: real; i: integer;
begin (* takes force vector and makes a trans with the x-axis along vector *)
v := unitv(v); (* make force vec into unit vector *)
with v↑ do
begin x := val[1]; y := val[2]; z := val[3]; end;
relVector(v); (* done with v now *)
t := newTrans;
with t↑ do
begin
val[1,1] := x;
val[1,2] := y;
val[1,3] := z;
if (x = 0) and (y = 0) then
begin
for i := 1 to 3 do begin val[2,i] := 0; val[3,i] := 0 end;
val[2,2] := z;
val[3,1] := -z;
end
else
begin
b := sqrt(x*x + y*y);
a := - y / b;
b := x / b;
val[2,1] := a;
val[2,2] := b;
val[2,3] := 0;
val[3,1] := - b * z;
val[3,2] := a * z;
val[3,3] := b * x - a * y;
end;
for i := 1 to 3 do val[i,4] := 0;
end;
vmkfrc := t;
end;