-- GrowPatches.mesa
--last edited by Maureen Stone July 20, 1982 9:31 am
--last edited by Michael Plass August 30, 1982 1:42 pm

DIRECTORY Complex, Cubic, Curve, JaMFnsDefs, LSPiece, Seq, Vector, Real, RealFns, FitDisplayUtilities;

GrowPatches: PROGRAM IMPORTS Cubic, Curve, JaMFnsDefs, LSPiece, Vector, Real, RealFns, FitDisplayUtilities =
BEGIN OPEN JaMFnsDefs;

myEps: REAL ← .0001; --something very small. We're using maximum deviation as test
MyEps: PROC = {myEps ← GetReal[]};

FitPatches: PROC = {
 z: Seq.ComplexSequence ← Curve.CurrentSamples[Curve.defaultHandle];
 n: NAT ← z.length;
 t: Seq.RealSequence ← NEW[Seq.RealSequenceRec[n]];
 node,cusp: Seq.NatSequence;
 tangent,ctangents: Seq.ComplexSequence;
 maxit: NAT;
 b: Cubic.Bezier;
 err,maxd: REAL;
 iterations: NAT;
 nextCusp,cIndex: INT ← 0;
 [node,tangent] ← Curve.CurrentNodes[Curve.defaultHandle];
 [cusp,ctangents] ← Curve.CurrentCusps[Curve.defaultHandle];
 maxit ← PopInteger[];
 maxd ← GetReal[];
 Curve.ResetLinks[Curve.defaultHandle];
 nextCusp ← IF cusp.length=0 THEN -1 ELSE cusp[0];
FOR i:NAT IN [1..node.length) DO
  t1,t2: Complex.Vec;
  IF node[i-1]=nextCusp THEN {
   t1 ← ctangents[2*cIndex+1];
   cIndex ← cIndex+1;
   IF cIndex<cusp.length THEN nextCusp ← cusp[cIndex]; --stop on end cusp
   }
  ELSE t1 ← tangent[i-1];
  IF node[i]=nextCusp THEN t2 ← ctangents[2*cIndex]
  ELSE t2 ← tangent[i];
  IF GetJaMBreak[] THEN EXIT;
  [b,err,iterations] ←
   FitPiece[z: z, t: t,
   from: node[i-1], thru: node[i],
   eps: myEps, maxd: maxd, maxit: maxit,
   initFree: FALSE, finalFree: FALSE,
   initTangent: t1, finalTangent: t2];
  Curve.AddLink[Curve.defaultHandle, b];
  ENDLOOP;
 };

GrowPatches: PROC = {
 pstart,pend: NAT;
 z: Seq.ComplexSequence ← Curve.CurrentSamples[Curve.defaultHandle];
 n: NAT ← z.length;
 t: Seq.RealSequence ← NEW[Seq.RealSequenceRec[n]];
 node,cusp: Seq.NatSequence;
 tangent,ctangents: Seq.ComplexSequence;
 maxit: NAT;
 b,prevB: Cubic.Bezier;
 err,maxd: REAL;
 iterations,npieces: NAT;
 maxdev: REAL ← 0;
 nextCusp,cIndex: INT ← 0;
 t1,t2: Complex.Vec;
 [node,tangent] ← Curve.CurrentNodes[Curve.defaultHandle];
 [cusp,ctangents] ← Curve.CurrentCusps[Curve.defaultHandle];
 maxit ← PopInteger[];
 maxd ← GetReal[];
 pstart ← npieces ← 0;
 Curve.ResetLinks[Curve.defaultHandle];
 nextCusp ← IF cusp.length=0 THEN -1 ELSE cusp[0];
WHILE pstart < node.length-1 DO
  IF GetJaMBreak[] THEN EXIT;
  IF pstart=nextCusp THEN {
   t1 ← ctangents[2*cIndex+1];
   cIndex ← cIndex+1;
   IF cIndex<cusp.length THEN nextCusp ← cusp[cIndex]; --stop on end cusp
   }
  ELSE t1 ← tangent[pstart];
  pend ← pstart+1;
  WHILE pend < node.length DO
   IF pend=nextCusp THEN t2 ← ctangents[2*cIndex]
   ELSE t2 ← tangent[pend];
   [b,err,iterations,maxdev] ← FitPiece[z: z, t: t,
     from: node[pstart], thru: node[pend],
     eps: myEps, maxd: maxd, maxit: maxit,
     initFree: FALSE, finalFree: FALSE,
     initTangent: t1, finalTangent: t2];
   IF pend=nextCusp THEN EXIT;
   IF maxdev>maxd THEN {
    IF pend-pstart>1 THEN {pend ← pend - 1; b ← prevB};
    EXIT};
   prevB ← b;
   pend ← pend+1;
   ENDLOOP;
  npieces ← npieces+1;
  Curve.AddLink[Curve.defaultHandle, b];
  pstart ← pend;
  ENDLOOP;
 PushInteger[npieces];
 };

FitPiece: PROCEDURE
 [z: Seq.ComplexSequence, t: Seq.RealSequence ← NIL,
 from, thru: NAT, -- fit points in range [from..thru] modulo z.length
 eps: REAL ← .00001, -- stop iterating if the sum of squares gets below this amount
maxd: REAL ← .00001, --stop iterating if the maximum deviation gets below this amount
 maxit: NAT ← 500, -- limit on number of iterations
 deltat: REAL ← 0.000005, -- stop when the t values each change by less than this
 initFree, finalFree: BOOLEANFALSE,
 initTangent, finalTangent: Complex.Vec ← [0,0],
 useOldTValues: BOOLEANFALSE,
 unitDeviation: REAL ← 1,
 exponent: NAT ← 2
 ]
RETURNS [b: Cubic.Bezier, err: REAL, iterations: NAT, maxDev: REAL, powerError: REAL] = {

--this makes sure there are enough points to give a good fit, then calls LSPiece.FitPiece
 npoints: NATIF thru>from THEN thru-from+1 ELSE thru-from+z.length+1;
 minPoints: NAT ← 6;
IF npoints>=minPoints THEN {
  [b,err,iterations, maxDev] ← LSPiece.FitPiece[z: z, t: t, from: from, thru: thru,
   eps: eps, maxd: maxd, maxit: maxit, deltat: deltat,
   initFree: initFree, finalFree: finalFree,
   initTangent: initTangent,
   finalTangent: finalTangent, useOldTValues: useOldTValues];
  powerError ← Error[b, z, t, from, thru, unitDeviation, exponent];
  }
ELSE { --else interpolate and then call
  newZ: Seq.ComplexSequence ← NEW[Seq.ComplexSequenceRec[minPoints]];
  newT: Seq.RealSequence ← NEW[Seq.RealSequenceRec[minPoints]];
  ninterp: INT ← minPoints-npoints;
  zi: NAT ← 0;
  modZ: PROC[index: NAT] RETURNS [NAT] = {RETURN[index MOD z.length]};
  interpolate: PROC[p0,p1: Vector.Vec, npts: NAT] = {
   dv: Vector.Vec ← Vector.Mul[Vector.Sub[p1,p0],1.0/(npts+1)];
   FOR j: NAT IN [0..npts) DO
    newZ[zi] ← Vector.Add[newZ[zi-1],dv];
    zi ← zi+1;
    ENDLOOP;
   };
  newZ[0] ← z[from];
  IF npoints=2 THEN {zi ← 1; interpolate[p0: z[from], p1: z[thru], npts: ninterp]}
  ELSE {
   d: Seq.RealSequence ← NEW[Seq.RealSequenceRec[npoints-1]];
   minD: REAL ← 100000000;
   maxD: REAL ← 0;
   totalD: REAL ← 0;
   np: NAT ← 0;
   i,n: NAT ← 0;
   adjust: BOOLEANFALSE;
   FOR i ← from, modZ[i+1] UNTIL i=thru DO
    d[n] ← Vector.Mag[Vector.Sub[z[i],z[modZ[i+1]]]];
    totalD ← totalD+d[n];
    IF d[n]<minD THEN minD ← d[n];
    IF d[n]>maxD THEN maxD ← d[n];
    n ← n+1;
    ENDLOOP;
   FOR i IN [0..d.length) DO np ← np+Real.RoundI[(d[i]/totalD)*ninterp]; ENDLOOP;
   IF np#ninterp THEN adjust ← TRUE;
   FOR i IN [0..d.length) DO
    p: INTEGER ← Real.RoundI[(d[i]/totalD)*ninterp];
    newZ[zi] ← z[modZ[from+i]]; zi ← zi+1;
    IF adjust THEN{
     IF np>ninterp AND d[i]=minD THEN {p←p-1; np ← np-1};
     IF np<ninterp AND d[i]=maxD THEN {p←p+1; np ← np+1};
     IF np=ninterp THEN adjust ← FALSE}; --should only be off by one
    IF p>0 THEN interpolate[p0: z[modZ[from+i]], p1: z[modZ[from+i+1]], npts: p];
    ENDLOOP;
   };
  newZ[newZ.length-1] ← z[thru];
  [b,err,iterations, maxDev] ← LSPiece.FitPiece[z: newZ, t: newT, from: 0, thru: newZ.length-1,
   eps: eps, maxd: maxd, maxit: maxit, deltat: deltat,
   initFree: initFree, finalFree: finalFree,
   initTangent: initTangent,
   finalTangent: finalTangent, useOldTValues: useOldTValues];
  powerError ← Error[b, newZ, newT, 0, newZ.length-1, unitDeviation, exponent];
  };
RETURN[b,err,iterations, maxDev, powerError];
 };

ChopPatches: PROC = {
 pstart,pend,lastbad,next: NAT;
 z: Seq.ComplexSequence ← Curve.CurrentSamples[Curve.defaultHandle];
 n: NAT ← z.length;
 t: Seq.RealSequence ← NEW[Seq.RealSequenceRec[n]];
 node: Seq.NatSequence;
 tangent: Seq.ComplexSequence;
 maxit: NAT;
 b: Cubic.Bezier;
 err,maxd,maxdev: REAL;
 iterations,npieces: NAT;
 Curve.DeleteNode[Curve.defaultHandle, n-2];
 Curve.DeleteNode[Curve.defaultHandle, n-3];
 [node,tangent] ← Curve.CurrentNodes[Curve.defaultHandle];
 maxit ← PopInteger[];
 maxd ← GetReal[];
 pstart ← npieces ← 0;
 Curve.ResetLinks[Curve.defaultHandle];
WHILE pstart < node.length-1 DO
  pend ← lastbad ← node.length-1;
IF GetJaMBreak[] THEN EXIT;
DO
  IF node[pend]-node[pstart]<3 THEN EXIT;
  [b,err,iterations,maxdev] ← FitPiece[z: z, t: t, from: node[pstart], thru: node[pend], eps: myEps, maxd: maxd,maxit: maxit, initFree: FALSE, finalFree: FALSE, initTangent: tangent[pstart], finalTangent: tangent[pend]];
  IF maxdev<=maxd
   THEN {
   next ← (pend+lastbad)/2;
   IF next=lastbad OR next=pend
    THEN EXIT --this is best solution as the next bigger one has already been marked bad
    ELSE pend ← next; --try a bigger one
   }
   ELSE {lastbad ← pend; pend ← (pend+pstart)/2}; --subdivide and try a smaller one.
  ENDLOOP;
WHILE node[pend]-node[pstart] < 3 DO pend ← pend + 1 ENDLOOP;
  [b,err,iterations,maxdev] ← FitPiece[z: z, t: t, from: node[pstart], thru: node[pend], eps: myEps, maxd: maxd, maxit: maxit, initFree: FALSE, finalFree: FALSE, initTangent: tangent[pstart], finalTangent: tangent[pend]];
  npieces ← npieces+1;
  Curve.AddLink[Curve.defaultHandle, b];
  pstart ← pend;
ENDLOOP;
 PushInteger[npieces];
 };

DynSpline: PROC =
BEGIN
 z: Seq.ComplexSequence ← Curve.CurrentSamples[Curve.defaultHandle];
 t: Seq.RealSequence ← NEW[Seq.RealSequenceRec[z.length]];
 maxit, exponent: NAT;
 bezier: Cubic.Bezier;
 unitDeviation: REAL;
 penalty: REAL;
 bestPrev: Seq.NatSequence;
 segCount: Seq.NatSequence;
 subtotalCost: Seq.RealSequence;
OutputPatchesUpto: PROC [m: NAT] =
  BEGIN
  IF m>0 THEN
   BEGIN
   OutputPatchesUpto[bestPrev[m]];
   [bezier] ← FitPiece[
    z: z, t: t,
    from: node[bestPrev[m]], thru: node[m],
    eps: myEps,
    maxd: unitDeviation/4,
    maxit: maxit*2,
    initFree: FALSE, finalFree: FALSE,
    initTangent: tangent[bestPrev[m]],
    finalTangent: tangent[m]
    ];
   Curve.AddLink[Curve.defaultHandle, bezier];
   END;
  END;
 n: NAT;
 node: Seq.NatSequence;
 tangent: Seq.ComplexSequence;
 oldbezier: Cubic.Bezier ← [[-1,-1],[-1,-1],[-1,-1],[-1,-1]];
 [node,tangent] ← Curve.CurrentNodes[Curve.defaultHandle];
 n ← node.length;
 exponent ← PopInteger[];
 maxit ← PopInteger[];
 unitDeviation ← GetReal[];
 penalty ← GetReal[];
 bestPrev ← NEW[Seq.NatSequenceRec[n]];
 segCount ← NEW[Seq.NatSequenceRec[n]];
 subtotalCost ← NEW[Seq.RealSequenceRec[n]];
 bestPrev[0] ← 0;
 segCount[0] ← 0;
 subtotalCost[0] ← 0.0;
FOR m:NAT IN [1..n) DO
  badness: REAL;
  lowbad:REALLAST[INT]/2;
  bestl:NAT ← 0;
  err, err2:REAL ← 0;
  FitDisplayUtilities.HiLight[z[node[m]].x, z[node[m]].y];
  FOR l:NAT DECREASING IN [0..m-1] WHILE err+penalty<lowbad DO
   IF GetJaMBreak[] THEN RETURN;
   IF node[m]-node[l] >= 1 THEN
    BEGIN
    FitDisplayUtilities.HiLight[z[node[l]].x, z[node[l]].y];
    [b: bezier, err: err2, powerError: err] ←
    FitPiece[z: z, t: t,
      from: node[l], thru: node[m],
      eps: myEps,
      maxd: unitDeviation/2,
      maxit: maxit,
      initFree: FALSE, finalFree: FALSE,
      initTangent: tangent[l],
      finalTangent: tangent[m],
      unitDeviation: unitDeviation,
      exponent: exponent
      ];
    badness ← err + subtotalCost[l] + penalty;
    IF badness < lowbad THEN
     BEGIN
     lowbad ← badness;
     bestl ← l;
     END;
    FitDisplayUtilities.HiLight[z[node[l]].x, z[node[l]].y];
    FitDisplayUtilities.ShowBezierInverted[oldbezier];
    FitDisplayUtilities.ShowBezierInverted[bezier];
    oldbezier ← bezier;
    END;
   ENDLOOP;
  subtotalCost[m] ← lowbad;
  segCount[m] ← segCount[bestl] + 1;
  bestPrev[m] ← bestl;
  FitDisplayUtilities.HiLight[z[node[m]].x, z[node[m]].y];
  ENDLOOP;
 FitDisplayUtilities.ShowBezierInverted[oldbezier];
 Curve.ResetLinks[Curve.defaultHandle];
 OutputPatchesUpto[n-1];
END;  

Error: PROC [
 bezier: Cubic.Bezier,
 z: Seq.ComplexSequence,
 t: Seq.RealSequence,
 from, thru: NAT,
 unitDeviation: REAL,
 exponent: NAT
 ] RETURNS [error: REAL] = {-- finds sum of powers of distances
Wrap: PROC[i: NAT] RETURNS [imod: NAT] = INLINE
  {imod ← IF i>=z.length THEN i-z.length ELSE i};
 c: Cubic.Coeffs ← Cubic.BezierToCoeffs[bezier];
 unitDevSqrRecip: REAL ← 1/(unitDeviation*unitDeviation);
 error ← 0;
FOR i: NAT ← from, Wrap[i+1] DO
  ti: REAL ← t[i];
  x: REAL ← ((c.c3.x*ti+c.c2.x)*ti+c.c1.x)*ti+c.c0.x;
  y: REAL ← ((c.c3.y*ti+c.c2.y)*ti+c.c1.y)*ti+c.c0.y;
  zi: Complex.Vec ← z[i];
  dSqr: REAL ← ((zi.x-x)*(zi.x-x) + (zi.y-y)*(zi.y-y))*unitDevSqrRecip;
  dPower: REAL ← 1;
  e: NAT ← exponent;
  WHILE e>1 DO dPower ← dPower*dSqr; e ← e-2 ENDLOOP;
  IF e=1 THEN dPower ← dPower*RealFns.SqRt[ABS[dSqr]];
  error ← error + dPower;
  IF i=thru THEN EXIT
  ENDLOOP;
 };

JaMFnsDefs.Register[".fit"L,FitPatches];  -- maxdev maxit => . Makes links between every pair of nodes
JaMFnsDefs.Register[".grow"L,GrowPatches];  -- maxdev maxit => . Makes links
JaMFnsDefs.Register[".chop"L,ChopPatches];  -- maxdev maxit => . Makes links
JaMFnsDefs.Register[".dynspline"L,DynSpline];  -- penalty unitdev maxit exponent => . Makes links using dynamic programming
JaMFnsDefs.Register[".myeps"L,MyEps];  -- myEps is sum of the distance squared

END.

Michael Plass August 19, 1982 9:04 am. Added QuickTangents.
Michael Plass August 19, 1982 2:15 pm. Enhanced dynspline error calculations.
Michael Plass August 26, 1982 12:56 pm. Moved OpenCurve, DynNodes, and QuickTangents into NodeImpl.
Michael Plass August 30, 1982 1:41 pm. Moved SetTangents to NodeImpl.