<<>>
<<G3dCurveImpl.mesa>>
<<Copyright Ó 1984, 1992 by Xerox Corporation.  All rights reserved.>>
<<Bloomenthal, October 21, 1992 5:01 pm PDT>>

DIRECTORY Convert, G3dBasic, G3dCurve, G3dMatrix, G3dSpline, IO, Real, Rope;

G3dCurveImpl: CEDAR PROGRAM
IMPORTS Convert, G3dMatrix, G3dSpline, IO, Real, Rope 
EXPORTS G3dCurve

~ BEGIN
<<Errors>>
Error:                PUBLIC SIGNAL [code: ATOM, reason: ROPE Ź NIL] = CODE;
<<Definitions>>
Triple:                TYPE ~ G3dBasic.Triple;                -- RECORD [x, y, z: REAL]
RealSequence:        TYPE ~ G3dBasic.RealSequence;        -- sequence of reals
Matrix:                TYPE ~ G3dMatrix.Matrix;                -- a cubic spline
Spline:                TYPE ~ G3dSpline.Spline;                -- a cubic spline
SplineSequence:        TYPE ~ G3dSpline.SplineSequence;    -- a sequence of cubic splines
ROPE:                    TYPE ~ Rope.ROPE;
STREAM:                TYPE ~ IO.STREAM;

Curve:                TYPE ~ G3dCurve.Curve;
CurveRep:            TYPE ~ G3dCurve.CurveRep;
Section:                 TYPE ~ G3dCurve.Section;
Fragment:            TYPE ~ G3dCurve.Fragment;
Fragments:            TYPE ~ G3dCurve.Fragments;
Spot:                    TYPE ~ G3dCurve.Spot;
CurveSequence:        TYPE ~ G3dCurve.CurveSequence;
CurveSequenceRep:    TYPE ~ G3dCurve.CurveSequenceRep;

FSpot:                    TYPE ~ RECORD [f: Fragment, t: REAL];
<<Creation>>
CurveFromSplines: PUBLIC PROC [splines: SplineSequence, res: NAT Ź 100] RETURNS [c: Curve]
~ {
IF splines # NIL THEN {
tmp: Spline Ź G3dMatrix.ObtainMatrix[];
nSegs: NAT Ź MAX[1, res];
lenSegs: NAT Ź MAX[1, nSegs/2];
c Ź NEW[CurveRep[splines.length]];
c.length Ź splines.length;
FOR s: NAT IN [0..splines.length) DO
t0: REAL Ź 0.0;
spline: Spline Ź splines[s];
fragments: Fragments Ź NEW[G3dCurve.FragmentsRep[nSegs]];
c[s] Ź [spline: spline, accumLength: c.totalLength, fragments: fragments];
FOR n: NAT IN [1..nSegs] DO
t1: REAL Ź REAL[n]/REAL[nSegs];
len: REAL Ź G3dSpline.Length[G3dSpline.Reparameterize[spline, t0, t1, tmp], lenSegs];
fragments[n-1] Ź [t0, t1, t1-t0, len, c.totalLength];
c.totalLength Ź c.totalLength+len;
t0 Ź t1;
ENDLOOP;
c[s].length Ź c.totalLength-c[s].accumLength;
ENDLOOP;
G3dMatrix.ReleaseMatrix[tmp];
};
};
<<Tranformation>>
Transform: PUBLIC PROC [curve: Curve, matrix: Matrix] ~ {
IF curve # NIL THEN FOR n: NAT IN [0..curve.length) DO
curve[n].spline Ź G3dMatrix.Mul[curve[n].spline, matrix];
ENDLOOP;
};
<<Support>>
FSpotFromT: PROC [c: Curve, t: REAL] RETURNS [FSpot] ~ {
n: NAT Ź Real.Floor[t];                                        -- index to curve section
tt: REAL Ź t-n;                                                -- tt = parameter into section
fragments: Fragments Ź c[MIN[n, c.length-1]].fragments;
nf: NAT Ź Real.Round[tt*fragments.length];                -- guess at fragment
f: Fragment Ź fragments[MIN[nf, fragments.length-1]];
WHILE tt NOT IN [f.t0..f.t1] DO                                -- ensure correct fragment    
f Ź fragments[nf Ź nf+(IF tt < f.t0 THEN -1 ELSE 1)]; 
ENDLOOP;
RETURN[[f, tt]];
};
<<Conversions Between Arc-length and t>>
SpotFromT: PUBLIC PROC [c: Curve, t: REAL] RETURNS [Spot] ~ {
n: NAT Ź Real.Floor[t];
RETURN[[c[n].spline, t-n]];
};

SpotFromArcLength: PUBLIC PROC [c: Curve, arcLength: REAL] RETURNS [Spot Ź []] ~ {
SectionSpot: PROC [section: Section] RETURNS [spot: Spot] ~ {
FragmentSpot: PROC [f: Fragment] RETURNS [Spot] ~ {
RETURN[[section.spline, f.t0+f.dt*((arcLength-f.accumLength)/f.length)]];
};
f: Fragments Ź section.fragments;
spot.spline Ź section.spline;
FOR nf: NAT IN [1..f.length) DO
IF f[nf].accumLength > arcLength THEN RETURN[FragmentSpot[f[nf-1]]];
ENDLOOP;
RETURN[FragmentSpot[f[f.length-1]]];
};
IF arcLength < 0.0 THEN Error[$BadArcLength];
IF arcLength > c.totalLength THEN {
s: Section Ź c[c.length-1];
f: Fragment Ź s.fragments[s.fragments.length-1];
IF arcLength-c.totalLength > f.length*0.1     -- arbitrary
THEN Error[$BadArcLength]
ELSE RETURN[[s.spline, f.t1]];
};
FOR n: NAT IN [1..c.length) DO
IF c[n].accumLength > arcLength THEN RETURN[SectionSpot[c[n-1]]];
ENDLOOP;
RETURN[SectionSpot[c[c.length-1]]];
};

TFromArcLength: PUBLIC PROC [c: Curve, arcLength: REAL] RETURNS [t: REAL] ~ {
RETURN[SpotFromArcLength[c, arcLength].t];
};

ArcLengthFromT: PUBLIC PROC [c: Curve, t: REAL] RETURNS [REAL] ~ {
fs: FSpot Ź FSpotFromT[c, t];
RETURN[fs.f.accumLength+((fs.t-fs.f.t0)/fs.f.dt)*fs.f.length];
};

ArcLengthFromTs: PUBLIC PROC [c: Curve, t0, t1: REAL] RETURNS [REAL] ~ {
RETURN[ArcLengthFromT[c, t1]-ArcLengthFromT[c, t0]];
};
<<Evaluation Given Parameter t>>
PositionT: PUBLIC PROC [c: Curve, t: REAL] RETURNS [Triple] ~ {
s: Spot Ź SpotFromT[c, t];
RETURN[G3dSpline.Position[s.spline, s.t]];
};

VelocityT: PUBLIC PROC [c: Curve, t: REAL] RETURNS [Triple] ~ {
s: Spot Ź SpotFromT[c, t];
RETURN[G3dSpline.Position[s.spline, s.t]];
};

AccelerationT: PUBLIC PROC [c: Curve, t: REAL] RETURNS [Triple] ~ {
s: Spot Ź SpotFromT[c, t];
RETURN[G3dSpline.Acceleration[s.spline, s.t]];
};

CurvatureT: PUBLIC PROC [c: Curve, t: REAL] RETURNS [Triple] ~ {
s: Spot Ź SpotFromT[c, t];
RETURN[G3dSpline.Curvature[s.spline, s.t]];
};
<<>>
CurvatureMagT: PUBLIC PROC [c: Curve, t: REAL] RETURNS [REAL] ~ {
s: Spot Ź SpotFromT[c, t];
RETURN[G3dSpline.CurvatureMag[s.spline, s.t]];
};
<<Evaluation Given Arc Length>>
PositionA: PUBLIC PROC [c: Curve, arcLength: REAL] RETURNS [Triple] ~ {
s: Spot Ź SpotFromArcLength[c, arcLength];
RETURN[G3dSpline.Position[s.spline, s.t]];
};

VelocityA: PUBLIC PROC [c: Curve, arcLength: REAL] RETURNS [Triple] ~ {
s: Spot Ź SpotFromArcLength[c, arcLength];
RETURN[G3dSpline.Velocity[s.spline, s.t]];
};

AccelerationA: PUBLIC PROC [c: Curve, arcLength: REAL] RETURNS [Triple] ~ {
s: Spot Ź SpotFromArcLength[c, arcLength];
RETURN[G3dSpline.Acceleration[s.spline, s.t]];
};

CurvatureA: PUBLIC PROC [c: Curve, arcLength: REAL] RETURNS [Triple] ~ {
s: Spot Ź SpotFromArcLength[c, arcLength];
RETURN[G3dSpline.Curvature[s.spline, s.t]];
};
<<>>
CurvatureMagA: PUBLIC PROC [c: Curve, arcLength: REAL] RETURNS [REAL] ~ {
s: Spot Ź SpotFromArcLength[c, arcLength];
RETURN[G3dSpline.CurvatureMag[s.spline, s.t]];
};
<<IO>>
Write: PUBLIC PROC [out: STREAM, c: Curve, name: ROPE Ź NIL] ~ {
IF name # NIL THEN name Ź Rope.Concat[name, " "];
IO.PutF[out, "Curve%g, %g sections:\n", IO.rope[name], IO.int[c.length]];
FOR n: NAT IN [0..c.length) DO
spline: Spline Ź c[n].spline;
IO.PutRope[out, "\t"];
FOR i: NAT IN [0..4) DO
r: ARRAY [0..4) OF REAL Ź spline[i];
IO.PutFL[out, "%g %g %g %g ",
LIST[IO.real[r[0]], IO.real[r[1]], IO.real[r[2]], IO.real[r[3]]]];
ENDLOOP;
IO.PutRope[out, "\n"];
ENDLOOP;
};
<<>>
Read: PUBLIC PROC [in: STREAM] RETURNS [c: Curve] ~ {
ENABLE IO.Error, Convert.Error => GOTO Bad;
line: ROPE Ź IO.GetLineRope[in];
p1: INT Ź Rope.Find[line, "sections:"];
p0: INT Ź Rope.FindBackward[line, " ", p1-2];
nSections: INT Ź Convert.IntFromRope[Rope.Substr[line, p0, p1]];
splines: SplineSequence Ź NEW[G3dSpline.SplineSequenceRep[nSections]];
FOR n: NAT IN [0..splines.length Ź nSections) DO
splines[n] Ź NEW[G3dSpline.SplineRep];
FOR i: NAT IN [0..4) DO
splines[n][i] Ź [IO.GetReal[in], IO.GetReal[in], IO.GetReal[in], IO.GetReal[in]];
ENDLOOP;
ENDLOOP;
RETURN[CurveFromSplines[splines]];
EXITS Bad => Error[$BadInputFormat, IO.PutFR1["file position: %g", IO.int[IO.GetIndex[in]]]];
};
<<Sequences>>
CopyCurveSequence: PUBLIC PROC [curves: CurveSequence]
RETURNS [CurveSequence] ~ {
copy: CurveSequence Ź NIL;
IF curves # NIL THEN {
copy Ź NEW[CurveSequenceRep[curves.length]];
copy.length Ź curves.length;
FOR n: NAT IN [0..curves.length) DO copy[n] Ź curves[n]; ENDLOOP;
};
RETURN[copy];
};

AddToCurveSequence: PUBLIC PROC [curves: CurveSequence, curve: Curve]
RETURNS [CurveSequence]
~ {
IF curves = NIL THEN curves Ź NEW[CurveSequenceRep[1]];
IF curves.length = curves.maxLength THEN curves Ź LengthenCurveSequence[curves];
curves[curves.length] Ź curve;
curves.length Ź curves.length+1;
RETURN[curves];
};
<<>>
LengthenCurveSequence: PUBLIC PROC [curves: CurveSequence, amount: REAL Ź 1.3]
RETURNS [new: CurveSequence] ~ {
newLength: CARDINAL Ź Real.Ceiling[amount*curves.maxLength];
newLength Ź MAX[newLength, 3];
new Ź NEW[CurveSequenceRep[newLength]];
FOR i: NAT IN [0..curves.length) DO new[i] Ź curves[i]; ENDLOOP;
new.length Ź curves.length;
};


END.