File [Indigo]<Griffin>splines>localspline.mesa!1

--compiler localspline
--m.stone September 26, 1980  5:09 PM
DIRECTORY
	SplineMemoryDefs: FROM "SplineMemoryDefs",
	SplineDefs: FROM "SplineDefs";

LocalSpline: PROGRAM IMPORTS SplineMemoryDefs EXPORTS SplineDefs = 
BEGIN OPEN SplineDefs;

--here are some REAL constants used for assigning real constants.
ONE: REAL ← 1; TWO: REAL ← 2; 
E1: REAL ← 10; E2: REAL ← 100; E3: REAL ← 1000; E4: REAL ← 10000;
TooFewKnots: PUBLIC SIGNAL[numknots: INTEGER] = CODE;
UnknownSpline: PUBLIC SIGNAL[splineType: SplineType] = CODE;

RABS: PROCEDURE[r: REAL] RETURNS[REAL] = INLINE
	BEGIN RETURN[IF r<0 THEN -r ELSE r] END;

MakeSpline:  PUBLIC PROCEDURE
	[knots: DESCRIPTOR FOR ARRAY OF FPCoords, splineType: SplineType]
	RETURNS [DESCRIPTOR FOR ARRAY OF Coeffs] =

BEGIN
allCoeffs: DESCRIPTOR FOR ARRAY OF Coeffs;
numknots: INTEGER ← LENGTH[knots];
IF numknots <= 0 THEN SIGNAL TooFewKnots[numknots];
--special case for 1 and 2 points
IF numknots < 3 THEN BEGIN	
	allCoeffs ← DESCRIPTOR[SplineMemoryDefs.Allocate[SIZE[Coeffs]], 1];
	allCoeffs[0].t0 ← knots[0];
	allCoeffs[0].t1 ← [0,0];
	allCoeffs[0].t2 ← [0,0];
	allCoeffs[0].t3 ← [0,0];
	IF numknots=2 THEN BEGIN
		allCoeffs[0].t1[1] ← knots[1][1]-knots[0][1];
		allCoeffs[0].t1[2] ← knots[1][2]-knots[0][2];
		END;
	RETURN[allCoeffs];
	END;	
SELECT splineType FROM

	bsplineInterp => BEGIN
		interpKnots: DESCRIPTOR FOR ARRAY OF FPCoords ← KnotsToCPoints[knots];
		allCoeffs ← MakeLocalSpline[interpKnots,bspline];
		SplineMemoryDefs.Free[BASE[interpKnots]];	--free the array space
		END;

	bezier =>
		BEGIN
		kindex,cindex,i: INTEGER;
		fourKnots: ARRAY [0..3] OF FPCoords;
--allocate for allCoeffs
		IF numknots>=4 THEN i ← (numknots-1)/3 ELSE i ←0;	
		IF (numknots-1) MOD 3#0 THEN i ← i+1;	--corresponds to knots hanging over
		allCoeffs ← DESCRIPTOR[SplineMemoryDefs.Allocate[i*SIZE[Coeffs]],i];
		cindex ← 0;
--set up first 4 knots, duplicating if necessary
		FOR kindex IN [0..3] DO
			IF kindex >numknots-1 THEN EXIT;
			fourKnots[kindex] ← knots[kindex];
			ENDLOOP;
--only case where duplication is necessary since check above for numknots<3
		IF numknots=3 THEN fourKnots[3] ← fourKnots[2];
		kindex ← 4;
--need to do this at least once
		DO
			allCoeffs[cindex] ← Bezier[DESCRIPTOR[fourKnots]];
			cindex ← cindex + 1;
--Take 4th knot + next 3 knots.  Special case if less than 3 left
			fourKnots[0] ← fourKnots[3];
			SELECT numknots-kindex FROM
				<=0 =>	EXIT;

				=1  =>
					BEGIN
					fourKnots[1] ← fourKnots[0];
					fourKnots[2] ← knots[kindex];
					kindex ← kindex+1;
					fourKnots[3] ← fourKnots[2];
					END;
				=2  =>
					BEGIN
					FOR i IN [1..2] DO
						fourKnots[i] ← knots[kindex];
						kindex ← kindex+1;
						ENDLOOP;
					fourKnots[3] ← fourKnots[2];
					END;

				>=3 =>
					FOR i IN [1..3] DO
						fourKnots[i] ← knots[kindex];
						kindex ← kindex+1;
						ENDLOOP;

				ENDCASE;

			ENDLOOP;
		END;

	IN [bspline..crspline] => allCoeffs ← MakeLocalSpline[knots,splineType];

	ENDCASE => SIGNAL UnknownSpline[splineType];

RETURN[allCoeffs];
END;

--take knots and Spline procedure name and draw the curve
MakeLocalSpline:   PROCEDURE
	[knots: DESCRIPTOR FOR ARRAY OF FPCoords, splineType: SplineType]
	RETURNS [DESCRIPTOR FOR ARRAY OF Coeffs] =
BEGIN
fourKnots: ARRAY [0..3] OF FPCoords;
kindex,cindex,i: INTEGER;
numknots: INTEGER ← LENGTH[knots];
lastknot: INTEGER ← numknots-1;
allCoeffs: DESCRIPTOR FOR ARRAY OF Coeffs; 

IF numknots >= 4 THEN i ← numknots-3 ELSE i ← 1;
allCoeffs ← DESCRIPTOR[SplineMemoryDefs.Allocate[i*SIZE[Coeffs]],i];
cindex ← 0;

--set up first 4 knots
FOR kindex IN [0..3] DO
	IF kindex >lastknot THEN EXIT;
	fourKnots[kindex] ← knots[kindex];
	ENDLOOP;
IF numknots=3 THEN fourKnots[3] ← fourKnots[2];
kindex ← 4;

--need to do this at least once
DO
	allCoeffs[cindex] ← SELECT splineType FROM
		bspline => BSpline[DESCRIPTOR[fourKnots]],
		crspline => CRSpline[DESCRIPTOR[fourKnots]],
		ENDCASE => ZeroSpline[DESCRIPTOR[fourKnots]];
	cindex ← cindex+1;
	IF kindex > lastknot THEN EXIT;

--shift the segment over
	fourKnots[0] ← fourKnots[1];
	fourKnots[1] ← fourKnots[2];
	fourKnots[2] ← fourKnots[3];
	fourKnots[3] ← knots[kindex];
	kindex ← kindex+1;
	ENDLOOP;

RETURN[allCoeffs];
END;

--Preform Inversion routine to get control points from drawn points
--From Yamaguci's paper
KnotsToCPoints: PROCEDURE[fpKnots: DESCRIPTOR FOR ARRAY OF FPCoords] RETURNS[fpCPoints: DESCRIPTOR FOR ARRAY OF FPCoords] = 
BEGIN
delta: DESCRIPTOR FOR ARRAY OF REAL;
deltaMax,tolerence: REAL;
xyzw,i,loopCount: INTEGER;  numknots: INTEGER ← LENGTH[fpKnots]; 
fpCPoints ← 
	DESCRIPTOR[SplineMemoryDefs.Allocate[(numknots+2)*SIZE[FPCoords]],numknots+2];
delta ← DESCRIPTOR[SplineMemoryDefs.Allocate[numknots*SIZE[REAL]],numknots];
--inits
	FOR i IN [1..numknots] DO fpCPoints[i]  ← fpKnots[i-1]; ENDLOOP;
--set the first and last control points, and the tolerence
	fpCPoints[0] ← fpCPoints[1];
	fpCPoints[numknots+1] ← fpCPoints[numknots];
	tolerence ← ONE/3;

--Main Loop
loopCount ← 0;
DO
	deltaMax ← 0;
--iterate and collect maximum delta
	FOR xyzw IN [1..NDIM]  DO
		FOR i IN [1..numknots] DO
			delta[i-1] ← fpKnots[i-1][xyzw] -fpCPoints[i][xyzw]
					+fpKnots[i-1][xyzw]/2-(fpCPoints[i-1][xyzw]+fpCPoints[i+1][xyzw])/4;
			IF RABS[delta[i-1]] >= deltaMax THEN deltaMax ← RABS[delta[i-1]];
			ENDLOOP;
		FOR i IN [1..numknots] DO
			fpCPoints[i][xyzw] ← delta[i-1]+fpCPoints[i][xyzw]; ENDLOOP;
		ENDLOOP;
--set the first and last control points
	fpCPoints[0] ← fpCPoints[1];
	fpCPoints[numknots+1] ← fpCPoints[numknots];
--End Check
	IF RABS[deltaMax] <= tolerence THEN EXIT;
	ENDLOOP;
SplineMemoryDefs.Free[BASE[delta]];	--free the deltas array
END;

--take 4 knots and make the BSpline Coeficient matrix
BSpline: PROCEDURE[fpKnots4: DESCRIPTOR FOR ARRAY OF FPCoords] RETURNS[coeffs: Coeffs] =
BEGIN
xyzw: INTEGER;
FOR xyzw IN [1..NDIM] DO
	coeffs.t3[xyzw] ← (-fpKnots4[0][xyzw] +3*fpKnots4[1][xyzw] -3*fpKnots4[2][xyzw] +fpKnots4[3][xyzw])/6;
	coeffs.t2[xyzw] ← (3*fpKnots4[0][xyzw]-6*fpKnots4[1][xyzw]+3*fpKnots4[2][xyzw])/6;
	coeffs.t1[xyzw] ← (-3*fpKnots4[0][xyzw]+3*fpKnots4[2][xyzw])/6;
	coeffs.t0[xyzw] ← (fpKnots4[0][xyzw]+4*fpKnots4[1][xyzw]+fpKnots4[2][xyzw])/6;
	ENDLOOP;
END;

--take 4 knots and make the Catmul-Rom Coeficient matrix
--tan = a(P2 - P1) where a is chosen to be .5
CRSpline: PROCEDURE[fpKnots4: DESCRIPTOR FOR ARRAY OF FPCoords] RETURNS[coeffs: Coeffs] =
BEGIN
xyzw: INTEGER;
a: REAL ←5/E1;
FOR xyzw IN [1..NDIM] DO
	coeffs.t3[xyzw] ← (-a*fpKnots4[0][xyzw] +(2-a)*fpKnots4[1][xyzw] +(a-2)*fpKnots4[2][xyzw] +a*fpKnots4[3][xyzw]);
	coeffs.t2[xyzw] ← (2*a*fpKnots4[0][xyzw]+(a-3)*fpKnots4[1][xyzw]+(3-2*a)*fpKnots4[2][xyzw]-a*fpKnots4[3][xyzw]);
	coeffs.t1[xyzw] ← (-a*fpKnots4[0][xyzw]+a*fpKnots4[2][xyzw]);
	coeffs.t0[xyzw] ← fpKnots4[1][xyzw];
	ENDLOOP;
END;

--take 4 knots and make the Bezier Coeficient matrix
Bezier: PROCEDURE[fpKnots4: DESCRIPTOR FOR ARRAY OF FPCoords] RETURNS[coeffs: Coeffs] =
BEGIN
xyzw: INTEGER;
FOR xyzw IN [1..NDIM] DO
	coeffs.t3[xyzw] ← -fpKnots4[0][xyzw] +3*fpKnots4[1][xyzw] -3*fpKnots4[2][xyzw] +fpKnots4[3][xyzw];
	coeffs.t2[xyzw] ← 3*fpKnots4[0][xyzw]-6*fpKnots4[1][xyzw]+3*fpKnots4[2][xyzw];
	coeffs.t1[xyzw] ← -3*fpKnots4[0][xyzw]+3*fpKnots4[1][xyzw];
	coeffs.t0[xyzw] ← fpKnots4[0][xyzw];
	ENDLOOP;
END;

--ZeroSpline fills the coeff matrix with zeros.  Mostly for a pesky SELECT expression, but
--might be generally useful
ZeroSpline: PROCEDURE[fpKnots4: DESCRIPTOR FOR ARRAY OF FPCoords] RETURNS[coeffs: Coeffs] =
BEGIN
xyzw: INTEGER;
FOR xyzw IN [1..NDIM] DO
	coeffs.t3[xyzw] ← 0;
	coeffs.t2[xyzw] ← 0;
	coeffs.t1[xyzw] ← 0;
	coeffs.t0[xyzw] ← 0;
	ENDLOOP;
END;

END.