-- BuildsplineImpl.mesa
-- Mesa 6 version
-- Last changed by J Warnock, July 29, 1980 10:13 AM

DIRECTORY
BuildSpline: FROM "BuildSpline",
RealFns: FROM "RealFns",
Cubic: FROM "Cubic",
Vector:FROM "Vector",
SystemDefs: FROM "SystemDefs";

BuildSplineImpl: PROGRAM
IMPORTS Cubic,Vector,RealFns,SystemDefs
EXPORTS BuildSpline =
BEGIN OPEN Vector, BuildSpline;

KnotHead:TYPE = POINTER TO Knot;
KnotArray: TYPE = DESCRIPTOR FOR ARRAY OF Knot;
Free:PROCEDURE[p:POINTER]={acnt←acnt-1; SystemDefs.FreeHeapNode[p];};
Alloc:PROCEDURE[nwords:CARDINAL]RETURNS [POINTER]={acnt←acnt+1; RETURN[SystemDefs.AllocateHeapNode[nwords]];};
acnt:INTEGER←0;
knothd:KnotHead←NIL;
knottail:KnotHead←NIL;
knotcnt:INTEGER←0;

StartSpline: PUBLIC PROCEDURE =
BEGIN
FreeChain[];
END;

EnterKnot:PUBLIC PROCEDURE [x,y:REAL] =
BEGIN
k:POINTER TO Knot←LinkKnot[];
k.p.x←x; k.p.y←y;
k.slopeflg←FALSE;
k.velflg←FALSE;
END;

EnterKnotSlope:PUBLIC PROCEDURE [x,y,sx,sy:REAL] =
BEGIN
k:POINTER TO Knot←LinkKnot[];
k.p.x←x; k.p.y←y;
k.s←Norm[Vec[sx,sy]];
k.slopeflg←TRUE;
k.velflg←FALSE;
END;

LinkKnot:PROCEDURE RETURNS [k:POINTER TO Knot]=
BEGIN
k←Alloc[SIZE[Knot]];
k.flink←knothd;
k.blink←NIL;
IF knothd=NIL THEN knottail←k ELSE knothd.blink←k;
knothd←k;
knotcnt←knotcnt+1;
END;

FreeChain:PROCEDURE=
BEGIN
tknot:POINTER TO Knot;
UNTIL knotcnt = 0 DO
tknot←knothd;
Free[knothd];
knothd←tknot.flink;
knotcnt←knotcnt-1;
ENDLOOP;
knothd←NIL;
END;

BuildSpline: PUBLIC PROCEDURE[proc:PROCEDURE[cb:Cubic.Coeffs]] =
BEGIN OPEN Cubic;
i:INTEGER;
bz:Bezier;
cb:Coeffs;
tknot:POINTER TO Knot;
SELECT knotcnt FROM
< 1 => RETURN;
< 2 => {proc[Coeffs[Vec[knothd.p.x,knothd.p.y],Vec[0,0],Vec[0,0],Vec[0,0]]];
FreeChain[];
RETURN};
< 3 => {t1x,t0x,t1y,t0y:REAL;
tknot←knothd;
t0x←tknot.p.x; t0y←tknot.p.y;
tknot←tknot.flink;
t1x←tknot.p.x-t0x; t1y←tknot.p.y-t0y;
proc[Coeffs[Vec[t0x,t0y],Vec[t1x,t1y],Vec[0,0],Vec[0,0]]];
FreeChain[];
};
ENDCASE =>
{i←knotcnt;
tknot←knothd;
UNTIL i = 0
DO
i←i-1;
IF NOT tknot.slopeflg THEN
{IF tknot.blink=NIL THEN
{tknot.s←Norm[Sub[tknot.p,tknot.flink.p]];
tknot.slopeflg←TRUE;};
IF tknot.flink=NIL THEN
{tknot.s←Norm[Sub[tknot.blink.p,tknot.p]];
tknot.slopeflg←TRUE;};
IF tknot.flink#NIL AND tknot.blink#NIL THEN
{o:Vec;
val:BOOLEAN;
[o,val]←FindCircleCenter[tknot.blink.p,tknot.p,tknot.flink.p];
IF NOT val THEN
{tknot.s←Norm[Sub[tknot.p,tknot.flink.p]];
tknot.slopeflg←TRUE;} ELSE
{vf,vc:Vec;
vc←Sub[tknot.p,o];
vf←Sub[tknot.flink.p,o];
IF Cross[vf,vc] <= 0 THEN
{tknot.s.x←vc.y;tknot.s.y←-vc.x;}
ELSE
{tknot.s.x←-vc.y;tknot.s.y←vc.x;};
tknot.s←Norm[tknot.s];
tknot.slopeflg←TRUE;};
};
};
--velocity stuff goes here--
IF NOT tknot.velflg THEN
{q,nq:Vec;
mq,r,st,ct:REAL;
eps:REAL←0.001;
eps1:REAL←0.00001;
IF tknot.flink # NIL THEN
{q←Sub[tknot.p,tknot.flink.p];
nq←Norm[q];
mq←Mag[q];
IF mq < eps
THEN {tknot.vin←0;}
ELSE
{ct←Dot[nq,tknot.s];
st←RealFns.SqRt[1-(ct*ct)];
IF ABS[st] < eps1 THEN {tknot.vin← mq/2;}
ELSE
{r←mq/(2*st);
tknot.vin←ABS[(4*r*(1-ct))/(3*st)];
};
};
};
IF tknot.blink # NIL THEN
{q←Sub[tknot.blink.p,tknot.p];
nq←Norm[q];
mq←Mag[q];
IF mq < eps
THEN {tknot.vout←0;}
ELSE
{ct←Dot[nq,tknot.s];
st←RealFns.SqRt[1-(ct*ct)];
IF ABS[st] < eps1 THEN {tknot.vout← mq/2;}
ELSE
{r←mq/(2*st);
tknot.vout←ABS[(4*r*(1-ct))/(3*st)];
};
};
};
tknot.velflg←TRUE;};
tknot←tknot.flink;
ENDLOOP;
i←knotcnt;
tknot←knothd;
UNTIL i = 0
DO
i←i-1;
bz.b0←tknot.p;
bz.b1←Sub[tknot.p,Mul[tknot.s,tknot.vin]];
bz.b2←Add[tknot.flink.p,Mul[tknot.flink.s,tknot.flink.vout]];
bz.b3←tknot.flink.p;
cb←BezierToCoeffs[bz];
proc[cb];
tknot←tknot.flink;
IF tknot.flink=NIL THEN EXIT;
ENDLOOP;
};
FreeChain[];
END;

FindCircleCenter:PUBLIC PROCEDURE[p0,p1,p2:Vec] RETURNS [o:Vec,valid:BOOLEAN]=
{d1,d2:Vec;
d20,d21,det:REAL;
x20,y20,x21,y21,x22,y22:REAL;
eps:REAL←0.001;
d1←Mul[Sub[p0,p1],2];
d2←Mul[Sub[p1,p2],2];
x20←p0.x*p0.x; y20←p0.y*p0.y;
x21←p1.x*p1.x; y21←p1.y*p1.y;
x22←p2.x*p2.x; y22←p2.y*p2.y;
d20←x20-x21+y20-y21;
d21←x21-x22+y21-y22;
det←Det[Matrix[d1.x,d1.y,d2.x,d2.y]];
IF ABS [det] < eps THEN
{RETURN[Vec[0,0],FALSE]} ELSE
{o.x←(d20*d2.y-d21*d1.y)/det; o.y←(d1.x*d21-d2.x*d20)/det;
RETURN[o,TRUE];};
};

Build2P2SPBezier: PUBLIC PROCEDURE[b0,b3,s0,s3,p:Vec]RETURNS[b1,b2:Vec]=
{k0,k1,tx,ty,c:REAL;
c←s3.x*s0.y-s3.y*s0.x;
tx←2*p.x-b0.x-b3.x;
ty←2*p.y-b0.y-b3.y;
k0←4.0*(s3.y*tx-s3.x*ty)/(3.0*c);
k1←4.0*(s0.y*tx-s0.x*ty)/(3.0*c);
b1←Vec[b0.x+k0*s0.x,b0.y+k0*s0.y];
b2←Vec[b3.x+k1*s3.x,b3.y+k1*s3.y];
};

BuildCyclicSpline: PUBLIC PROCEDURE
[proc:PROCEDURE[cb:Cubic.Coeffs]]=
BEGIN
knothd.blink←knottail;
knottail.flink←knothd;
BuildSpline[proc];
END;

Mag:PROCEDURE[v:Vec] RETURNS [m:REAL]=
BEGIN
m←RealFns.SqRt[v.x*v.x+v.y*v.y];
END;

Norm:PROCEDURE[v:Vec] RETURNS [nv:Vec]=
BEGIN
m:REAL;
IF (m←Mag[v]) = 0 THEN RETURN[Vec[0,0]];
nv.x←v.x/m; nv.y←v.y/m;
END;


END.