G2dSplineImpl.mesa
Copyright Ó 1988, 1992 by Xerox Corporation. All rights reserved.
Bloomenthal, July 1, 1992 7:08 pm PDT
DIRECTORY G2dBasic, G2dSpline, G2dVector, Rope, Vector2;
G2dSplineImpl: CEDAR PROGRAM
IMPORTS G2dVector, Vector2
EXPORTS G2dSpline
~ BEGIN
Type Declarations
RealSequence:   TYPE ~ G2dBasic.RealSequence;
RealSequenceRep:  TYPE ~ G2dBasic.RealSequenceRep;
Pair:      TYPE ~ G2dBasic.Pair;
PairSequence:   TYPE ~ G2dBasic.PairSequence;
PairSequenceRep:  TYPE ~ G2dBasic.PairSequenceRep;
Spline1d:     TYPE ~ G2dSpline.Spline1d;
Spline1dRep:    TYPE ~ G2dSpline.Spline1dRep;
Spline1dSequence:  TYPE ~ G2dSpline.Spline1dSequence;
Spline1dSequenceRep: TYPE ~ G2dSpline.Spline1dSequenceRep;
Spline2d:     TYPE ~ G2dSpline.Spline2d;
Spline2dRep:    TYPE ~ G2dSpline.Spline2dRep;
Spline2dSequence:  TYPE ~ G2dSpline.Spline2dSequence;
Spline2dSequenceRep: TYPE ~ G2dSpline.Spline2dSequenceRep;
Error: PUBLIC ERROR [reason: Rope.ROPE] = CODE;
1D Interpolation
Interpolate1d: PUBLIC PROC [
knots: RealSequence,
spline: Spline1dSequence ¬ NIL,
noOvershoot: BOOL ¬ FALSE]
RETURNS [Spline1dSequence]
~ {
SELECT knots.length FROM
0 => RETURN[spline];
1, 2 => {
p0: REAL ~ knots[0];
p1: REAL ~ IF knots.length = 2 THEN knots[1] ELSE knots[0];
dp: REAL ¬ p0-p1;
IF spline = NIL OR spline.maxLength < 1 THEN spline ¬ NEW[Spline1dSequenceRep[1]];
spline.length ¬ 1;
spline[0] ¬ NEW[Spline1dRep ¬ [2.0*dp, -3.0*dp, 0.0, p0]]; -- see G3dSpline.SlowInOut
RETURN[spline];
};
ENDCASE => {
chords: RealSequence ¬ ComputeChords1d[knots];
tangents: RealSequence ¬ ComputeTangents1d[knots, chords];
IF noOvershoot THEN ClampTangents1d[knots, tangents];
RETURN[ComputeSpline1d[knots, tangents, chords, spline]];
};
};
ClampTangents1d: PROC [knots, tangents: RealSequence] ~ {
FOR n: NAT IN [0..knots.length) DO
Clamp: PROC [knot0, knot1: REAL] ~ {
s: REAL ¬ 3.0*(knot1-knot0);
tangents[n] ¬ IF s < 0.0
THEN MIN[0.0, MAX[s, tangents[n]]]
ELSE MAX[0.0, MIN[s, tangents[n]]];
};
IF n > 0 THEN Clamp[knots[n-1], knots[n]];
IF n < knots.length-1 THEN Clamp[knots[n], knots[n+1]];
ENDLOOP;
};
ComputeChords1d: PROC [knots: RealSequence] RETURNS [chords: RealSequence] ~ {
max: INTEGER ~ knots.length-1;
chords ¬ NEW[RealSequenceRep[knots.length]];
FOR n: NAT IN [0..max) DO
IF (chords[n] ¬ ABS[knots[n+1]-knots[n]]) = 0.0 THEN chords[n] ¬ 1.0;
ENDLOOP;
IF (chords[max] ¬ ABS[knots[0]-knots[max]]) = 0.0 THEN chords[max] ¬ 1.0;
};
ComputeTangents1d: PROC [knots: RealSequence, chords: RealSequence]
RETURNS [tangents: RealSequence]
~ {
z: NAT ¬ knots.length-1;
Na: RealSequence ¬ NEW[RealSequenceRep[knots.length]];  -- nonzero elements of M
Nb: RealSequence ¬ NEW[RealSequenceRep[knots.length]];
Nc: RealSequence ¬ NEW[RealSequenceRep[knots.length]];
B: RealSequence ¬ NEW[RealSequenceRep[knots.length]];  -- a control matrix
tangents ¬ NEW[RealSequenceRep[knots.length]];
Na[0] ¬ 0.0; Nb[0] ¬ 1.0; Nc[0] ¬ 0.5;
B[0] ¬ (1.5/chords[0])*(knots[1]-knots[0]);
Na[z] ¬ 2.0; Nb[z] ¬ 4.0; Nc[z] ¬ 0.0;
B[z] ¬ (6.0/chords[z-1])*(knots[z]-knots[z-1]);
GaussianEliminate1d[knots, B, tangents, Na, Nb, Nc, chords];
};
GaussianEliminate1d: PROC [knots, B, tangents, Na, Nb, Nc, chords: RealSequence] ~ {
nPts: NAT ¬ knots.length;
FOR n: NAT IN[1..nPts-1) DO
l0: REAL ¬ chords[n-1];
l1: REAL ¬ chords[n];
Na[n] ¬ l1; Nb[n] ¬ l0+l0+l1+l1; Nc[n] ¬ l0;
B[n] ¬ (3.0/(l0*l1))*(l0*l0*(knots[n+1]-knots[n])+l1*l1*(knots[n]-knots[n-1]));
ENDLOOP;
FOR n: NAT IN[1..nPts) DO           -- gaussian elimination
d, q: REAL;
IF Na[n] = 0.0 THEN LOOP;
d ¬ Nb[n-1]/Na[n];
Na[n] ¬ d*Na[n]-Na[n-1]; Nb[n] ¬ d*Nb[n]-Nb[n-1]; Nc[n] ¬ d*Nc[n]-Nc[n-1];
B[n] ¬ d*B[n]-B[n-1];
q ¬ 1.0/Nb[n];
Na[n] ¬ q*Na[n]; Nb[n] ¬ q*Nb[n]; Nc[n] ¬ q*Nc[n];
B[n] ¬ q*B[n];
ENDLOOP;
tangents[nPts-1] ¬ B[nPts-1]/Nb[nPts-1];  -- back substitution
FOR n: NAT IN [2..nPts] DO
tangents[nPts-n] ¬ (B[nPts-n]-tangents[nPts-n+1]*Nc[nPts-n])/Nb[nPts-n];
ENDLOOP;
};
ComputeSpline1d: PROC [
knots, tangents: RealSequence,
chords: RealSequence,
in: Spline1dSequence ¬ NIL]
RETURNS [Spline1dSequence]
~ {
nSpline: NAT ¬ knots.length-1;
IF in = NIL OR in.maxLength < nSpline THEN in ¬ NEW[Spline1dSequenceRep[nSpline]];
in.length ¬ nSpline;
FOR m: NAT IN [0..nSpline) DO
l: REAL ¬ chords[m];
dknots: REAL ¬ knots[m+1]-knots[m];
a: REAL ¬ tangents[m]*l;
b: REAL ¬ tangents[m+1]*l+a;
c: REAL ¬ -2.0*dknots+b;
d: REAL ¬ 3.0*dknots-b-a;
IF in[m] = NIL THEN in[m] ¬ NEW[Spline1dRep];
in[m]­ ¬ [c, d, a, knots[m]];
ENDLOOP;
RETURN[in];
};
2D Interpolation
Interpolate2d: PUBLIC PROC [
knots: PairSequence,
spline: Spline2dSequence ¬ NIL]
RETURNS [Spline2dSequence]
~ {
SELECT knots.length FROM
0 => RETURN[spline];
1, 2 => {
p0: Pair ~ knots[0];
p1: Pair ~ IF knots.length = 2 THEN knots[1] ELSE knots[0];
dp: Pair ¬ Vector2.Sub[p0, p1];
IF spline = NIL OR spline.maxLength < 1 THEN spline ¬ NEW[Spline2dSequenceRep[1]];
spline.length ¬ 1;
spline[0] ¬ NEW[Spline2dRep ¬[Vector2.Mul[dp, 2.0], Vector2.Mul[dp, -3.0], [0, 0], p0]];
RETURN[spline];
};
ENDCASE => {
chords: RealSequence ¬ ComputeChords2d[knots];
tangents: PairSequence ¬ ComputeTangents2d[knots, chords];
RETURN[ComputeSpline2d[knots, tangents, chords, spline]];
};
};
ComputeChords2d: PROC [knots: PairSequence] RETURNS [chords: RealSequence] ~ {
max: INTEGER ~ knots.length-1;
chords ¬ NEW[RealSequenceRep[knots.length]];
FOR n: NAT IN [0..max) DO
IF (chords[n] ¬ G2dVector.Distance[knots[n+1], knots[n]]) = 0.0 THEN chords[n] ¬ 1.0;
ENDLOOP;
IF (chords[max] ¬ G2dVector.Distance[knots[0], knots[max]]) = 0.0 THEN chords[max] ¬ 1.0;
RETURN[chords];
};
ComputeTangents2d: PROC [knots: PairSequence, chords: RealSequence]
RETURNS [tangents: PairSequence]
~ {
z: NAT ¬ knots.length-1;
Na: RealSequence ¬ NEW[RealSequenceRep[knots.length]];  -- nonzero elements of M
Nb: RealSequence ¬ NEW[RealSequenceRep[knots.length]];
Nc: RealSequence ¬ NEW[RealSequenceRep[knots.length]];
B: PairSequence ¬ NEW[PairSequenceRep[knots.length]];  -- a control matrix
tangents ¬ NEW[PairSequenceRep[knots.length]];
Na[0] ¬ 0.0; Nb[0] ¬ 1.0; Nc[0] ¬ 0.5;
B[0] ¬ Vector2.Mul[Vector2.Sub[knots[1], knots[0]], 1.5/chords[0]];
Na[z] ¬ 2.0; Nb[z] ¬ 4.0; Nc[z] ¬ 0.0;
B[z] ¬ Vector2.Mul[Vector2.Sub[knots[z], knots[z-1]], 6.0/chords[z-1]];
GaussianEliminate2d[knots, B, tangents, Na, Nb, Nc, chords];
};
GaussianEliminate2d: PROC [
knots, B, tangents: PairSequence,
Na, Nb, Nc, chords: RealSequence]
~ {
nPts: NAT ¬ knots.length;
FOR n: NAT IN[1..nPts-1) DO
l0: REAL ¬ chords[n-1];
l1: REAL ¬ chords[n];
Na[n] ¬ l1; Nb[n] ¬ l0+l0+l1+l1; Nc[n] ¬ l0;
B[n] ¬ Vector2.Mul[
Vector2.Add[
Vector2.Mul[Vector2.Sub[knots[n+1], knots[n]], l0*l0],
Vector2.Mul[Vector2.Sub[knots[n], knots[n-1]], l1*l1]],
3.0/(l0*l1)];
ENDLOOP;
FOR n: NAT IN[1..nPts) DO           -- gaussian elimination
d, q: REAL;
IF Na[n] = 0.0 THEN LOOP;
d ¬ Nb[n-1]/Na[n];
Na[n] ¬ d*Na[n]-Na[n-1]; Nb[n] ¬ d*Nb[n]-Nb[n-1]; Nc[n] ¬ d*Nc[n]-Nc[n-1];
B[n] ¬ Vector2.Sub[Vector2.Mul[B[n], d], B[n-1]];
q ¬ 1.0/Nb[n];
Na[n] ¬ q*Na[n]; Nb[n] ¬ q*Nb[n]; Nc[n] ¬ q*Nc[n];
B[n] ¬ Vector2.Mul[B[n], q];
ENDLOOP;
tangents[nPts-1] ¬ Vector2.Div[B[nPts-1], Nb[nPts-1]];  -- back substitution
FOR n: NAT IN[2..nPts] DO
tangents[nPts-n] ¬
Vector2.Div[
Vector2.Sub[
B[nPts-n],
Vector2.Mul[tangents[nPts-n+1], Nc[nPts-n]]],
Nb[nPts-n]];
ENDLOOP;
};
ComputeSpline2d: PROC [
knots, tangents: PairSequence,
chords: RealSequence,
in: Spline2dSequence ¬ NIL]
RETURNS [Spline2dSequence]
~ {
nSpline: NAT ¬ knots.length-1;
IF in = NIL OR in.maxLength < nSpline THEN in ¬ NEW[Spline2dSequenceRep[nSpline]];
in.length ¬ nSpline;
FOR m: NAT IN[0..nSpline) DO
l: REAL ¬ chords[m];
dknots: Pair ¬ Vector2.Sub[knots[m+1], knots[m]];
a: Pair ¬ Vector2.Mul[tangents[m], l];
b: Pair ¬ Vector2.Add[Vector2.Mul[tangents[m+1], l], a];
c: Pair ¬ Vector2.Add[Vector2.Mul[dknots, -2.0], b];
d: Pair ¬ Vector2.Sub[Vector2.Sub[Vector2.Mul[dknots, 3.0], b], a];
IF in[m] = NIL THEN in[m] ¬ NEW[Spline2dRep];
in[m]­ ¬ [c, d, a, knots[m]];
ENDLOOP;
RETURN[in];
};
1d Evaluation
Position1d: PUBLIC PROC [spline: Spline1d, t: REAL] RETURNS [REAL] ~ {
t2: REAL;
IF t = 0.0 THEN RETURN[spline[3]];
IF t = 1.0 THEN RETURN[spline[0]+spline[1]+spline[2]+spline[3]];
t2 ¬ t*t;
RETURN[t*t2*spline[0]+t2*spline[1]+t*spline[2]+spline[3]];
};
Velocity1d: PUBLIC PROC [spline: Spline1d, t: REAL] RETURNS [REAL] ~ {
IF t = 0.0 THEN RETURN[spline[2]];
IF t = 1.0 THEN RETURN[3.0*spline[0]+2.0*spline[1]+spline[2]];
RETURN[3.0*t*t*spline[0]+2.0*t*spline[1]+spline[2]];
};
Acceleration1d: PUBLIC PROC [spline: Spline1d, t: REAL] RETURNS [REAL] ~ {
RETURN[6.0*t*spline[0]+spline[1]+spline[1]];
};
2d Evaluation
Position2d: PUBLIC PROC [spline: Spline2d, t: REAL] RETURNS [Pair] ~ {
t2, t3: REAL;
IF t = 0.0 THEN RETURN[spline[3]];
IF t = 1.0 THEN RETURN[[
spline[0].x+spline[1].x+spline[2].x+spline[3].x,
spline[0].y+spline[1].y+spline[2].y+spline[3].y]];
IF t = 1.0 THEN RETURN[[
spline[0].x+spline[1].x+spline[2].x+spline[3].x,
spline[0].y+spline[1].y+spline[2].y+spline[3].y]];
t2 ¬ t*t;
t3 ¬ t2*t;
RETURN[[
t3*spline[0].x+t2*spline[1].x+t*spline[2].x+spline[3].x,
t3*spline[0].y+t2*spline[1].y+t*spline[2].y+spline[3].y]];
};
Velocity2d: PUBLIC PROC [spline: Spline2d, t: REAL] RETURNS [Pair] ~ {
t2: REAL;
IF t = 0.0 THEN RETURN[spline[2]];
IF t = 1.0 THEN RETURN[[
3.0*spline[0].x+2.0*spline[1].x+spline[2].x,
3.0*spline[0].y+2.0*spline[1].y+spline[2].y]];
t2 ¬ 3.0*t*t;
t ¬ t+t;
RETURN[[
t2*spline[0].x+t*spline[1].x+spline[2].x,
t2*spline[0].y+t*spline[1].y+spline[2].y]];
};
Acceleration2d: PUBLIC PROC [spline: Spline2d, t: REAL] RETURNS [Pair] ~ {
t6: REAL ¬ 6.0*t;
RETURN[[t6*spline[0].x+spline[1].x+spline[1].x, t6*spline[0].y+spline[1].y+spline[1].y]];
};
END.