Basic Types
Matrix: TYPE ~ Matrix3d.Matrix;
MatrixRep: TYPE ~ Matrix3d.MatrixRep;
Pair: TYPE ~ Matrix3d.Pair;
Quad: TYPE ~ Matrix3d.Quad;
Line: TYPE ~ Vector3d.Line;
Triple: TYPE ~ Spline3d.Triple;
TripleSequence: TYPE ~ Spline3d.TripleSequence;
TripleSequenceRec: TYPE ~ Spline3d.TripleSequenceRec;
RealSequence: TYPE ~ Spline3d.RealSequence;
RealSequenceRec: TYPE ~ Spline3d.RealSequenceRec;
KnotSequence: TYPE ~ Spline3d.KnotSequence;
KnotSequenceRep: TYPE ~ Spline3d.KnotSequenceRep;
Bezier: TYPE ~ Spline3d.Bezier;
Bspline: TYPE ~ Spline3d.Bspline;
Hermite: TYPE ~ Spline3d.Hermite;
Coeffs: TYPE ~ Spline3d.Coeffs;
CoeffsRep: TYPE ~ Spline3d.CoeffsRep;
CoeffsSequence: TYPE ~ Spline3d.CoeffsSequence;
CoeffsSequenceRec: TYPE ~ Spline3d.CoeffsSequenceRec;
TPosSequence: TYPE ~ Spline3d.TPosSequence;
TPosSequenceRec: TYPE ~ Spline3d.TPosSequenceRec;
Interpolation Procedures
InterpolateCyclic:
PUBLIC
PROC [knots: KnotSequence, tension:
REAL ← 1.0]
RETURNS [CoeffsSequence] ~ {
From /Cedar/CedarChest6.0/CubicSplinePackage/RegularALSplineImpl.mesa, in which
coeffs.t3, .t2, .t1, and .t0 referred to the 3rd, 2nd and 1st derivatives, and position of
the curve. In Spline3d, these correspond to coeffs[0], [1], [2], and [3].
coeffs: CoeffsSequence;
nCoeffs: NAT ← MAX[1, knots.length-1]; -- number of curve intervals
lastK: NAT ← nCoeffs-1; -- max for interval index
h, a, b, c, d, r, s: RealSequence;
IF knots.length < 1 THEN RETURN[NIL];
IF coeffs=NIL OR coeffs.maxLength<nCoeffs THEN coeffs ← NEW[CoeffsSequenceRec[nCoeffs]];
IF knots.length < 3
THEN {
[coeffs[0][3][0], coeffs[0][3][1], coeffs[0][3][2]] ← knots[0];
FOR c: NAT IN[0..2] DO FOR r: NAT IN[0..3] DO coeffs[0][c][r] ← 0.0; ENDLOOP; ENDLOOP;
IF knots.length = 2
THEN {
coeffs[0][2][0] ← knots[1].x-knots[0].x;
coeffs[0][2][1] ← knots[1].y-knots[0].y;
coeffs[0][2][2] ← knots[1].z-knots[0].z;
};
RETURN[coeffs];
};
h ← NEW[RealSequenceRec[nCoeffs]];
a ← NEW[RealSequenceRec[nCoeffs]];
b ← NEW[RealSequenceRec[nCoeffs]];
d ← NEW[RealSequenceRec[nCoeffs]];
c ← NEW[RealSequenceRec[nCoeffs]];
r ← NEW[RealSequenceRec[nCoeffs]];
s ← NEW[RealSequenceRec[nCoeffs]];
set cubic constants:
FOR i:
NAT
IN [0..nCoeffs)
DO
[coeffs[i][3][0], coeffs[i][3][1], coeffs[i][3][2]] ← knots[i];
ENDLOOP;
compute parametrization (chord length approximation):
h ← ComputeChords[knots, h];
cyclic end conditions: precompute coefficients a and c:
a[0] ← 2.0*(h[0]+h[lastK]);
FOR i: NAT IN [1..lastK) DO a[i] ← 2.0*(h[i-1]+h[i])-h[i-1]*h[i-1]/a[i-1]; ENDLOOP;
c[0] ← h[lastK];
FOR i: NAT IN [1..lastK) DO c[i] ← -h[i-1]*c[i-1]/a[i-1]; ENDLOOP;
compute each dimension separately:
FOR i:
NAT
IN [0..3)
DO
p2: REAL;
compute constant coefficient:
FOR k:
NAT
IN [0..lastK]
DO
d[k] ←
SELECT i
FROM
0 => (knots[k+1].x-knots[k].x)/h[k],
1 => (knots[k+1].y-knots[k].y)/h[k],
ENDCASE => (knots[k+1].z-knots[k].z)/h[k];
ENDLOOP;
cyclic end conditions: compute coefficients b, r, & s:
b[0] ← 6.0*(d[0]-d[lastK]);
FOR k: NAT IN [1..lastK) DO b[k] ← 6.0*(d[k]-d[k-1]); ENDLOOP;
FOR k: NAT IN [1..lastK) DO b[k] ← b[k]-h[k-1]*b[k-1]/a[k-1]; ENDLOOP;
r[lastK] ← 1.0;
s[lastK] ← 0.0;
FOR k:
NAT
DECREASING
IN [0..lastK)
DO
r[k] ← -(h[k]*r[k+1]+c[k])/a[k];
s[k] ← (b[k]-h[k]*s[k+1])/a[k];
ENDLOOP;
compute coefficient t2 (second derivative/2):
p2 ← 6.0*(d[lastK]-d[lastK-1])-h[lastK]*s[0]-h[lastK-1]*s[lastK-1];
p2 ← p2/(h[lastK]*r[0]+h[lastK-1]*r[lastK-1]+2.0*(h[lastK]+h[lastK-1]));
note: coeffs[lastK+1].t2.[i] = coeffs[0].t2[i]
coeffs[lastK][1][i] ← 0.5*p2;
FOR k: NAT IN [0..lastK) DO coeffs[k][1][i] ← 0.5*(r[k]*p2+s[k]); ENDLOOP;
compute coefficients t3 (third derivative/6) & and t1 (first derivative):
FOR k:
NAT
IN [0..lastK)
DO
coeffs[k][0][i] ← (coeffs[k+1][1][i]-coeffs[k][1][i])/3.0;
coeffs[k][2][i] ← d[k]-h[k]*(2.0*coeffs[k][1][i]+coeffs[k+1][1][i])/3.0;
ENDLOOP;
note again: coeffs[lastK+1].t2[i] = coeffs[0].t2[i] (0 for natural end conditions)
coeffs[lastK][0][i] ← (coeffs[0][1][i]-coeffs[lastK][1][i])/3.0;
coeffs[lastK][2][i] ← d[lastK]-h[lastK]*(2.0*coeffs[lastK][1][i]+coeffs[0][1][i])/3.0;
reparametrize coefficients to interval [0..1]:
FOR k:
NAT
IN [0..lastK]
DO
coeffs[k][0][i] ← h[k]*h[k]*coeffs[k][0][i];
coeffs[k][1][i] ← h[k]*h[k]*coeffs[k][1][i];
coeffs[k][2][i] ← h[k]*coeffs[k][2][i];
ENDLOOP;
RETURN[coeffs];
Interpolate:
PUBLIC PROC [knots: KnotSequence, tan0, tan1: Triple ← [0.0, 0.0, 0.0], tension:
REAL ← 1.0, coeffs: CoeffsSequence ←
NIL]
RETURNS [CoeffsSequence] ~ {
Mostly from Rogers and Adams, with tension added:
SELECT knots.length
FROM
0 => RETURN[coeffs];
1, 2 => {
len: REAL ← Vector3d.Distance[knots[0], knots[1]];
IF coeffs = NIL OR coeffs.maxLength < 1 THEN coeffs ← NEW[CoeffsSequenceRec[1]];
coeffs[0] ← CoeffsFromHermite[
[knots[0],
knots[1],
Vector3d.Mul[tan0, len],
Vector3d.Mul[tan1, len]],
coeffs[0]];
RETURN[coeffs];
};
ENDCASE => {
chords: RealSequence ← ComputeChords[knots, NIL];
tangents: TripleSequence ← ComputeTangents[knots, chords, NIL, tan0, tan1];
RETURN[ComputeCoeffs[knots, tangents, tension, chords, coeffs]];
};
};
ComputeChords:
PROC [knots: KnotSequence, chords: RealSequence ←
NIL]
RETURNS [RealSequence] ~ {
IF chords =
NIL
OR chords.maxLength < knots.length
THEN chords ← NEW[RealSequenceRec[knots.length]];
FOR n:
NAT
IN[0..knots.length-1)
DO
IF (chords[n] ← Vector3d.Distance[knots[n+1], knots[n]]) = 0.0 THEN chords[n] ← 1.0;
ENDLOOP;
RETURN[chords];
};
ComputeTangents:
PROC [knots: KnotSequence, chords: RealSequence, tangents: TripleSequence ←
NIL, tan0, tan1: Triple]
RETURNS [TripleSequence] ~ {
N: TripleSequence ← NEW[TripleSequenceRec[knots.length]]; -- nonzero elements of M
B: TripleSequence ← NEW[TripleSequenceRec[knots.length]]; -- a control matrix
IF tangents =
NIL
OR tangents.maxLength < knots.length
THEN tangents ← NEW[TripleSequenceRec[knots.length]];
SetEndConditions[knots, N, B, tangents, chords, tan0, tan1];
GaussianEliminate[knots, N, B, tangents, chords];
RETURN[tangents];
};
SetEndConditions:
PROC [knots: KnotSequence, N, B, tangents: TripleSequence, chords: RealSequence, tan0, tan1: Triple] ~ {
z: NAT ← knots.length-1;
IF Vector3d.Null[tan0]
THEN {
N[0] ← [0.0, 1.0, 0.5];
B[0] ← Vector3d.Mul[Vector3d.Sub[knots[1], knots[0]], 1.5/chords[0]];
}
ELSE {
tangents[0] ← B[0] ← tan0;
N[0] ← [0.0, 1.0, 0.0];
};
IF Vector3d.Null[tan1]
THEN {
N[z] ← [2.0, 4.0, 0.0];
B[z] ← Vector3d.Mul[Vector3d.Sub[knots[z], knots[z-1]], 6.0/chords[z-1]];
}
ELSE {
tangents[z] ← B[z] ← tan1;
N[z] ← [0.0, 1.0, 0.0];
};
};
GaussianEliminate:
PROC [knots: KnotSequence, N, B, tangents: TripleSequence, chords: RealSequence] ~ {
nPts: NAT ← knots.length;
FOR n:
NAT
IN[1..nPts-1)
DO
l0: REAL ← chords[n-1];
l1: REAL ← chords[n];
N[n] ← [l1, l0+l0+l1+l1, l0];
B[n] ← Vector3d.Mul[
Vector3d.Add[
Vector3d.Mul[Vector3d.Sub[knots[n+1], knots[n]], l0*l0],
Vector3d.Mul[Vector3d.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 N[n].x = 0.0 THEN LOOP;
d ← N[n-1].y/N[n].x;
N[n] ← Vector3d.Sub[Vector3d.Mul[N[n], d], N[n-1]];
B[n] ← Vector3d.Sub[Vector3d.Mul[B[n], d], B[n-1]];
q ← 1.0/N[n].y;
N[n] ← Vector3d.Mul[N[n], q];
B[n] ← Vector3d.Mul[B[n], q];
ENDLOOP;
tangents[nPts-1] ← Vector3d.Div[B[nPts-1], N[nPts-1].y]; -- back substitution
FOR n:
NAT
IN[2..nPts]
DO
tangents[nPts-n] ←
Vector3d.Div[
Vector3d.Sub[
B[nPts-n],
Vector3d.Mul[tangents[nPts-n+1], N[nPts-n].z]],
N[nPts-n].y];
ENDLOOP;
};
ComputeCoeffs:
PROC [knots: KnotSequence, tangents: TripleSequence, tension:
REAL, chords: RealSequence, coeffs: CoeffsSequence ←
NIL]
RETURNS [CoeffsSequence] ~ {
nCoeffs: NAT ← knots.length-1;
IF coeffs =
NIL
OR coeffs.maxLength < nCoeffs
THEN coeffs ← NEW[CoeffsSequenceRec[nCoeffs]];
IF tension # 1.0
THEN
FOR n:
NAT
IN[0..nCoeffs]
DO
tangents[n] ← Vector3d.Mul[tangents[n], tension];
ENDLOOP;
FOR m:
NAT
IN[0..nCoeffs)
DO
coeffs[m] ← CoeffsFromHermite[
[knots[m],
knots[m+1],
Vector3d.Mul[tangents[m], chords[m]],
Vector3d.Mul[tangents[m+1], chords[m]]],
coeffs[m]];
l: REAL ← chords[m];
dknots: Triple ← Vector3d.Sub[knots[m+1], knots[m]];
a: Triple ← Vector3d.Mul[tangents[m], l];
b: Triple ← Vector3d.Add[Vector3d.Mul[tangents[m+1], l], a];
c: Triple ← Vector3d.Add[Vector3d.Mul[dknots, -2.0], b];
d: Triple ← Vector3d.Sub[Vector3d.Sub[Vector3d.Mul[dknots, 3.0], b], a];
IF coeffs[m] = NIL THEN coeffs[m] ← NEW[CoeffsRep];
coeffs[m][0] ← [c.x, c.y, c.z, 0.0];
coeffs[m][1] ← [d.x, d.y, d.z, 0.0];
coeffs[m][2] ← [a.x, a.y, a.z, 0.0];
coeffs[m][3] ← [knots[m].x, knots[m].y, knots[m].z, 1.0];
ENDLOOP;
RETURN[coeffs];
};
Conversion Procedures
hermiteToCoeffs: Matrix ←
NEW[MatrixRep ← [
[ 2.0, -2.0, 1.0, 1.0],
[-3.0, 3.0, -2.0, -1.0],
[ 0.0, 0.0, 1.0, 0.0],
[ 1.0, 0.0, 0.0, 0.0]]];
coeffsToHermite: Matrix ←
NEW[MatrixRep ← [
[0.0, 0.0, 0.0, 1.0],
[1.0, 1.0, 1.0, 1.0],
[0.0, 0.0, 1.0, 0.0],
[3.0, 2.0, 1.0, 0.0]]];
bezierToCoeffs: Matrix ←
NEW[MatrixRep ← [
[-1.0, 3.0, -3.0, 1.0],
[ 3.0, -6.0, 3.0, 0.0],
[-3.0, 3.0, 0.0, 0.0],
[ 1.0, 0.0, 0.0, 0.0]]];
coeffsToBezier: Matrix ←
NEW[MatrixRep ← [
[0.0, 0.0, 0.0, 1.0],
[0.0, 0.0, 1.0/3.0, 1.0],
[0.0, 1.0/3.0, 2.0/3.0, 1.0],
[1.0, 1.0, 1.0, 1.0]]];
bsplineToCoeffs: Matrix ←
NEW[MatrixRep ← [
[-1.0/6.0, 3.0/6.0, -3.0/6.0, 1.0/6.0],
[ 3.0/6.0, -6.0/6.0, 3.0/6.0, 0.0/6.0],
[-3.0/6.0, 0.0/6.0, 3.0/6.0, 0.0/6.0],
[ 1.0/6.0, 4.0/6.0, 1.0/6.0, 0.0/6.0]]];
coeffsToBspline: Matrix ←
NEW[MatrixRep ← [
[0.0, 2.0/3.0, -1.0, 1.0],
[0.0, -1.0/3.0, 0.0, 1.0],
[0.0, 2.0/3.0, 1.0, 1.0],
[1.0, 11.0/3.0, 2.0, 1.0]]];
ConvertToCoeffs:
PROC [p0, p1, p2, p3: Triple, convert, out: Matrix, hermite:
BOOL ←
FALSE]
RETURNS [Coeffs] ~ {
w: REAL ← IF hermite THEN 0.0 ELSE 1.0;
m: Matrix ← Matrix3d.ObtainMatrix[];
m^ ← [
[p0.x, p0.y, p0.z, 1.0],
[p1.x, p1.y, p1.z, 1.0],
[p2.x, p2.y, p2.z, w],
[p3.x, p3.y, p3.z, w]];
IF out = NIL THEN out ← NEW[CoeffsRep];
[] ← Matrix3d.Mul[convert, m, out];
Matrix3d.ReleaseMatrix[m];
RETURN[out];
};
ConvertFromCoeffs:
PROC [c: Coeffs, convert: Matrix]
RETURNS [p0, p1, p2, p3: Triple] ~ {
m: Matrix ← Matrix3d.ObtainMatrix[];
[] ← Matrix3d.Mul[convert, c, m];
p0 ← [m[0][0], m[0][1], m[0][2]];
p1 ← [m[1][0], m[1][1], m[1][2]];
p2 ← [m[2][0], m[2][1], m[2][2]];
p3 ← [m[3][0], m[3][1], m[3][2]];
Matrix3d.ReleaseMatrix[m];
RETURN[p0, p1, p2, p3];
};
CoeffsFromBezier:
PUBLIC PROC [b: Bezier, out: Coeffs ←
NIL]
RETURNS [Coeffs] ~ {
RETURN[ConvertToCoeffs[b.b0, b.b1, b.b2, b.b3, bezierToCoeffs, out]];
};
CoeffsFromBspline:
PUBLIC PROC [b: Bspline, out: Coeffs ←
NIL]
RETURNS [Coeffs] ~ {
RETURN[ConvertToCoeffs[b.b0, b.b1, b.b2, b.b3, bsplineToCoeffs, out]];
};
CoeffsFromHermite:
PUBLIC
PROC [h: Hermite, out: Coeffs ←
NIL]
RETURNS [Coeffs] ~ {
RETURN[ConvertToCoeffs[h.p0, h.p1, h.tan0, h.tan1, hermiteToCoeffs, out, TRUE]];
};
BezierFromCoeffs:
PUBLIC PROC [c: Coeffs]
RETURNS [Bezier] ~ {
b0, b1, b2, b3: Triple;
[b0, b1, b2, b3] ← ConvertFromCoeffs[c, coeffsToBezier];
RETURN[[b0, b1, b2, b3]];
};
BsplineFromCoeffs:
PUBLIC PROC [c: Coeffs]
RETURNS [Bspline] ~ {
b0, b1, b2, b3: Triple;
[b0, b1, b2, b3] ← ConvertFromCoeffs[c, coeffsToBspline];
RETURN[[b0, b1, b2, b3]];
};
HermiteFromCoeffs:
PUBLIC PROC [c: Coeffs]
RETURNS [Hermite] ~ {
p0, p1, tan0, tan1: Triple;
[p0, p1, tan0, tan1] ← ConvertFromCoeffs[c, coeffsToHermite];
RETURN[[p0, p1, tan0, tan1]];
};
Evaluation Procedures
PerPointProc: TYPE = Spline3d.PerPointProc;
WalkBezier:
PUBLIC
PROC [b: Bezier, proc: PerPointProc, epsilon:
REAL ← 0.05, doLast:
BOOL ←
TRUE] ~ {
Inner:
PROC [b: Bezier] ~ {
IF FlatBezier[b, epsilon]
THEN proc[b.b0]
ELSE {
left, rite: Bezier;
[left, rite] ← SplitBezier[b];
Inner[left];
Inner[rite];
};
};
Inner[b];
IF doLast THEN proc[b.b3];
};
FwdDif:
PUBLIC
PROC [in: Coeffs, nSegments:
INTEGER, out: Coeffs←
NIL]
RETURNS [Coeffs] ~ {
fwdDif: Matrix3d.Matrix ← NEW[Matrix3d.MatrixRep];
d1, d2, d3, d6: REAL;
IF nSegments = 0 THEN RETURN[NIL];
d1 ← 1.0/nSegments;
d2 ← d1*d1;
d3 ← d1*d2;
d6 ← 6.0*d3;
fwdDif^ ← [
[0.0, 0.0, 0.0, 1.0],
[d3, d2, d1, 0.0],
[d6, d2+d2, 0.0, 0.0],
[d6, 0.0, 0.0, 0.0]];
RETURN[Matrix3d.Mul[fwdDif, in, out]];
};
Samples:
PUBLIC
PROC
[c: Coeffs, nPoints:
INTEGER, points: TripleSequence ←
NIL]
RETURNS
[TripleSequence] ~ {
p: ARRAY [0..2] OF REAL;
fwdDif: Coeffs ← FwdDif[c, nPoints-1];
IF points =
NIL
OR points.maxLength < nPoints
THEN
points ← NEW[TripleSequenceRec[nPoints]];
points[0] ← [fwdDif[0][0], fwdDif[0][1], fwdDif[0][2]];
FOR i: INTEGER IN[0..2] DO p[i] ← fwdDif[0][i]; ENDLOOP;
FOR i:
INTEGER
IN[1..nPoints)
DO
FOR j:
INTEGER
IN[0..2]
DO
p[j] ← p[j]+fwdDif[1][j];
fwdDif[1][j] ← fwdDif[1][j]+fwdDif[2][j];
fwdDif[2][j] ← fwdDif[2][j]+fwdDif[3][j];
ENDLOOP;
points[i] ← [p[0], p[1], p[2]];
ENDLOOP;
RETURN[points];
};
Position:
PUBLIC PROC [c: Coeffs, t:
REAL]
RETURNS [Triple] ~ {
ret: ARRAY [0..2] OF REAL;
IF t = 0.0 THEN FOR i: NAT IN[0..2] DO ret[i] ← c[3][i]; ENDLOOP
ELSE IF t = 1.0 THEN FOR i: NAT IN[0..2] DO ret[i] ← c[0][i]+c[1][i]+c[2][i]+c[3][i]; ENDLOOP
ELSE {
t2: REAL ← t*t;
t3: REAL ← t*t2;
FOR i: NAT IN[0..2] DO ret[i] ← t3*c[0][i]+t2*c[1][i]+t*c[2][i]+c[3][i]; ENDLOOP
};
RETURN[[ret[0], ret[1], ret[2]]];
};
Velocity:
PUBLIC PROC [c: Coeffs, t:
REAL]
RETURNS [Triple] ~ {
tan: ARRAY [0..2] OF REAL;
a: REAL ← 3.0;
b: REAL ← 2.0;
IF t = 0.0 THEN RETURN[[c[2][0], c[2][1], c[2][2]]];
IF t # 1.0 THEN {a ← a*t*t; b ← b*t};
FOR i: NAT IN[0..2] DO tan[i] ← a*c[0][i]+b*c[1][i]+c[2][i]; ENDLOOP;
RETURN[[tan[0], tan[1], tan[2]]];
};
Acceleration:
PUBLIC PROC [c: Coeffs, t:
REAL]
RETURNS [Triple] ~ {
a: REAL = 6.0*t;
RETURN[[a*c[0][0]+c[1][0]+c[1][0], a*c[0][1]+c[1][1]+c[1][1], a*c[0][2]+c[1][2]+c[1][2]]];
};
Tangent:
PUBLIC
PROC [c: Coeffs, t:
REAL]
RETURNS [Triple] ~ {
RETURN[Velocity[c, t]];
};
MinAcceleration:
PUBLIC PROC [c: Spline3d.Coeffs]
RETURNS [t:
REAL] ~ {
a0: Triple ← Acceleration[c, 0.0];
axis: Triple ← Vector3d.Sub[Acceleration[c, 1.0], a0];
t ← IF Vector3d.Null[axis] THEN 0.5 ELSE -Vector3d.Dot[a0, axis]/Vector3d.Square[axis];
IF t < 0.0 THEN t ← 0.0 ELSE IF t > 1.0 THEN t ← 1.0;
};
CurvatureMag:
PUBLIC
PROC [c: Coeffs, t:
REAL]
RETURNS [
REAL] ~ {
tan: Triple ← Tangent[c, t];
acc: Triple ← Acceleration[c, t];
m: REAL ← Vector3d.Mag[tan];
RETURN[IF m = 0.0 THEN 0.0 ELSE Vector3d.Mag[Vector3d.Cross[acc, tan]]/(m*m*m)];
};
Curvature:
PUBLIC PROC [c: Coeffs, t:
REAL]
RETURNS [Triple] ~ {
1 sqrt, 3 divides, 14 multiplies, 6 adds
tan: Triple ← Tangent[c, t];
IF tan = [0.0, 0.0, 0.0]
THEN
RETURN[tan]
ELSE {
acc: Triple ← Acceleration[c, t];
mag: REAL ← Vector3d.Mag[tan];
mag2: REAL ← mag*mag;
RETURN Vector3d.Div[Vector3d.Cross[Vector3d.Cross[tan, acc], tan], mag2*mag2];
};
};
RefVec:
PUBLIC PROC [c: Coeffs, t:
REAL]
RETURNS [Triple] ~ {
ref: Triple ← Curvature[c, t];
IF Vector3d.Square[ref] < 0.9 THEN ref ← Vector3d.Ortho[Tangent[c, t]];
RETURN[ref];
};
Size Procedures
Length:
PUBLIC PROC [c: Coeffs]
RETURNS [
REAL] ~ {
sum: REAL ← 0.0;
fd: Coeffs ← FwdDif[c, 100];
FOR i:
NAT
IN[0..100)
DO
sum ← sum+Vector3d.Mag[[fd[1][0], fd[1][1], fd[1][2]]];
FOR j:
NAT
IN[0..2]
DO
fd[1][j] ← fd[1][j]+fd[2][j];
fd[2][j] ← fd[2][j]+fd[3][j];
ENDLOOP;
ENDLOOP;
RETURN[sum];
};
ConvexHullArea:
PUBLIC PROC [b: Bezier]
RETURNS [
REAL] ~ {
TwiceTriArea:
PROC [p0, p1, p2: Triple]
RETURNS [
REAL] ~ {
RETURN[Vector3d.Mag[Vector3d.Cross[Vector3d.Sub[p1, p0], Vector3d.Sub[p2, p1]]]];
};
a1: REAL ← TwiceTriArea[b.b0, b.b1, b.b2];
a2: REAL ← TwiceTriArea[b.b2, b.b3, b.b0];
a3: REAL ← TwiceTriArea[b.b0, b.b1, b.b3];
a4: REAL ← TwiceTriArea[b.b1, b.b2, b.b3];
RETURN[0.25*(a1+a2+a3+a4)];
};
ConvexHullLength:
PUBLIC
PROC [b: Bezier]
RETURNS [
REAL] ~ {
d0: Triple ← Vector3d.Sub[b.b1, b.b0];
d1: Triple ← Vector3d.Sub[b.b2, b.b1];
d2: Triple ← Vector3d.Sub[b.b3, b.b2];
d3: Triple ← Vector3d.Sub[b.b3, b.b0];
RETURN[Vector3d.Mag[d0]+Vector3d.Mag[d1]+Vector3d.Mag[d2]+Vector3d.Mag[d3]];
};
FlatBezier:
PUBLIC PROC [b: Bezier, epsilon:
REAL ← 0.05]
RETURNS [
BOOL] ~ {
d3mag: REAL;
d0: Triple ← Vector3d.Sub[b.b1, b.b0];
d1: Triple ← Vector3d.Sub[b.b2, b.b1];
d2: Triple ← Vector3d.Sub[b.b3, b.b2];
d3: Triple ← Vector3d.Sub[b.b3, b.b0];
IF Vector3d.Dot[d3, d0] < 0.0 OR Vector3d.Dot[d3, d2] < 0.0 THEN RETURN[FALSE]; -- bulge
IF (d3mag ← Vector3d.Mag[d3]) < 0.000001 THEN RETURN[TRUE];
RETURN[
(Vector3d.Mag[d0]+Vector3d.Mag[d1]+Vector3d.Mag[d2])/Vector3d.Mag[d3]<1.0+epsilon];
};
Tiny:
PUBLIC
PROC [c: Coeffs, epsilon:
REAL ← 0.05]
RETURNS [
BOOL] ~ {
RETURN[Vector3d.Distance[Position[c, 0.0], Position[c, 1.0]] < epsilon];
};
Resolution:
PUBLIC PROC [c: Coeffs, epsilon:
REAL]
RETURNS [
INTEGER] ~ {
This produces inconsistent results:
MaxAccel:
PROC [curve: Coeffs]
RETURNS [
REAL] ~ {
Bigger: PROC [r0, r1: REAL] RETURNS [REAL] ~ {RETURN[IF r0 > r1 THEN r0 ELSE r1]};
max: REAL ← 0.0;
a0: Triple ← Acceleration[curve, 0.0];
a1: Triple ← Acceleration[curve, 1.0];
max ← max+Bigger[ABS[a0.x], ABS[a1.x]];
max ← max+Bigger[ABS[a0.y], ABS[a1.y]];
RETURN[max];
};
RETURN[MAX[1, Real.RoundI[Real.SqRt[MaxAccel[c]/(8.0*epsilon)]]]];
};
Search Procedures
Nearest
Point:
PUBLIC
PROC [p: Triple, c: Coeffs, t0:
REAL ← 0.0, t1:
REAL ← 1.0, epsilon:
REAL ← 0.01]
RETURNS [cPt: Triple, t, dist:
REAL] ~ {
InnerNear:
PROC [t0, t1:
REAL, p0, p1: Triple, d0, d1:
REAL]
RETURNS [cPtI: Triple, tI, distI: REAL]~ {
tt: REAL ← 0.5*(t0+t1);
cp: Triple ← Position[c, tt];
d: REAL ← Vector3d.Distance[p, cp];
dc0: REAL ← Vector3d.Distance[p0, cp];
dc1: REAL ← Vector3d.Distance[p1, cp];
IF d < min THEN min ← d;
IF Vector3d.Distance[p0, p1] < MAX[0.0001, epsilon*d] THEN RETURN[cp, tt, d];
IF dc0 < d0
AND dc1 < d1
THEN
SELECT
TRUE
FROM
MIN[d0, d, d1] > min => {cPtI ← cp; tI ← tt; distI ← d;};
d0 < d1 => [cPtI, tI, distI] ← InnerNear[t0, tt, p0, cp, d0, d];
ENDCASE => [cPtI, tI, distI] ← InnerNear[tt, t1, cp, p1, d, d1]
ELSE {
tt0, tt1, dist0, dist1: REAL;
pt0, pt1: Triple;
[pt0, tt0, dist0] ← InnerNear[t0, tt, p0, cp, d0, d];
[pt1, tt1, dist1] ← InnerNear[tt, t1, cp, p1, d, d1];
IF dist0 < dist1
THEN RETURN[pt0, tt0, dist0]
ELSE RETURN[pt1, tt1, dist1];
};
};
min: REAL ← 10000.0;
p0: Triple ← Position[c, t0];
p1: Triple ← Position[c, t1];
[cPt, t, dist] ← InnerNear[t0, t1, p0, p1, Vector3d.Distance[p, p0], Vector3d.Distance[p, p1]];
NearestLine:
PUBLIC
PROC [line: Line, c: Coeffs, t0:
REAL ← 0.0, t1:
REAL ← 1.0, epsilon:
REAL ← 0.01]
RETURNS [cPt, lPt: Triple, t, dist:
REAL] ~ {
InnerNear:
PROC [t0, t1:
REAL, p0, p1: Triple, d0, d1:
REAL]
RETURNS [cPtI, lPtI: Triple, tI, distI: REAL] ~ {
tt: REAL ← 0.5*(t0+t1);
cp: Triple ← Position[c, tt];
lp: Triple ← Vector3d.LinePoint[cp, line];
d: REAL ← Vector3d.Distance[lp, cp];
dc0: REAL ← Vector3d.Distance[p0, cp];
dc1: REAL ← Vector3d.Distance[p1, cp];
IF d < min THEN min ← d;
IF Vector3d.Distance[p0, p1] < MAX[0.0001, epsilon*d] THEN RETURN[cp, lp, tt, d];
IF dc0 < d0
AND dc1 < d1
THEN
SELECT
TRUE
FROM
MIN[d0, d, d1] > min => {cPtI ← cp; lPtI ← lp; tI ← tt; distI ← d;};
d0 < d1 => [cPtI, lPtI, tI, distI] ← InnerNear[t0, tt, p0, cp, d0, d];
ENDCASE => [cPtI, lPtI, tI, distI] ← InnerNear[tt, t1, cp, p1, d, d1]
ELSE {
tt0, tt1, dist0, dist1: REAL;
cPt0, lPt0, cPt1, lPt1: Triple;
[cPt0, lPt0, tt0, dist0] ← InnerNear[t0, tt, p0, cp, d0, d];
[cPt1, lPt1, tt1, dist1] ← InnerNear[tt, t1, cp, p1, d, d1];
IF dist0 < dist1
THEN RETURN[cPt0, lPt0, tt0, dist0]
ELSE RETURN[cPt1, lPt1, tt1, dist1];
};
};
min: REAL ← 10000.0;
p0: Triple ← Position[c, t0];
p1: Triple ← Position[c, t1];
[cPt, lPt, t, dist] ←
InnerNear[t0, t1, p0, p1, DistanceToLine[p0, line], DistanceToLine[p1, line]];
DistanceToLine:
PROC [p: Triple, l: Line]
RETURNS [
REAL] ~ {
lp: Triple ← Vector3d.LinePoint[p, l];
RETURN[Vector3d.Distance[p, lp]];
};
Nearest
Spline:
PUBLIC
PROC [c1, c2: Coeffs, epsilon:
REAL ← 0.01]
RETURNS [t1, t2:
REAL] ~ {
This doesn't always work right.
InnerNear:
PROC [t0, t1:
REAL, p0, p1: Triple, d0, d1:
REAL]
RETURNS [tI, dI:
REAL] ~ {
Return point on c1 closest to c2 within t0,t1
tt: REAL ← 0.5*(t0+t1);
pp: Triple ← Position[c1, tt];
dc0: REAL ← Vector3d.Distance[p0, pp];
dc1: REAL ← Vector3d.Distance[p1, pp];
d: REAL ← NearestPoint[pp, c2, 0.0, 1.0].dist;
IF d < min THEN min ← d;
IF Vector3d.Distance[p0, p1] < MAX[0.0001, epsilon*d] THEN RETURN[tt, d];
IF dc0 < d0
AND dc1 < d1
THEN {
t, d: REAL;
SELECT
TRUE
FROM
MIN[d0, d, d1] > min => RETURN[tt, d];
d0 < d1 => {[t, d] ← InnerNear[t0, tt, p0, pp, d0, d]; RETURN[t, d]};
ENDCASE => {[t, d] ← InnerNear[tt, t1, pp, p1, d, d1]; RETURN[t, d]};
}
ELSE {
tt0, tt1, dist0, dist1: REAL;
[tt0, dist0] ← InnerNear[t0, tt, p0, pp, d0, d];
[tt1, dist1] ← InnerNear[tt, t1, pp, p1, d, d1];
IF dist0 < dist1
THEN RETURN[tt0, dist0]
ELSE RETURN[tt1, dist1];
};
};
min: REAL ← 10000.0;
p0: Triple ← Position[c1, 0.0];
p1: Triple ← Position[c1, 1.0];
d0: REAL ← NearestPoint[p0, c2, 0.0, 1.0, epsilon].dist;
d1: REAL ← NearestPoint[p1, c2, 0.0, 1.0, epsilon].dist;
t1 ← InnerNear[0.0, 1.0, p0, p1, d0, d1].tI;
t2 ← NearestPoint[Position[c1, t1], c2, 0.0, 1.0, epsilon].t;
Nearest
Pair:
PUBLIC
PROC [p: Pair, c: Coeffs, persp:
BOOL ←
FALSE, t0:
REAL ← 0.0, t1:
REAL ← 1.0]
RETURNS [pRet: Pair, t, dist:
REAL] ~ {
InnerNear:
PROC [t0, t1:
REAL, p0, p1: Pair, d0, d1:
REAL] ~ {
tt: REAL ← 0.5*(t0+t1);
pp: Pair ← PairPosition[c, tt, persp];
dd: REAL ← PairToPairDistance[[p.x, p.y], pp];
IF PairToPairDistance[p0, p1] < 0.01*dd
THEN {
pRet ← pp;
t ← tt;
dist ← dd;
RETURN;
};
IF d0 < d1
THEN InnerNear[t0, tt, p0, pp, d0, dd]
ELSE InnerNear[tt, t1, pp, p1, dd, d1];
};
p0: Pair ← PairPosition[c, t0, persp];
p1: Pair ← PairPosition[c, t1, persp];
InnerNear[t0, t1, p0, p1, PairToPairDistance[[p.x, p.y], p0], PairToPairDistance[[p.x, p.y], p1]];
PairToPairDistance:
PROC [p0, p1: Pair]
RETURNS [
REAL] ~ {
a: REAL ← p0.x-p1.x;
b: REAL ← p0.y-p1.y;
RETURN[Real.SqRt[a*a+b*b]];
};
PairPosition:
PROC [c: Coeffs, t:
REAL, persp:
BOOL ←
FALSE]
RETURNS [Pair] ~ {
ret: ARRAY [0..3] OF REAL;
t2, t3: REAL;
nCoords: NAT ← IF persp THEN 4 ELSE 2;
IF t = 0.0
THEN
FOR i: NAT IN[0..nCoords) DO ret[i] ← c[3][i]; ENDLOOP
ELSE
IF t = 1.0
THEN
FOR i: NAT IN[0..nCoords) DO ret[i] ← c[0][i]+c[1][i]+c[2][i]+c[3][i]; ENDLOOP
ELSE {
t2 ← t*t;
t3 ← t*t2;
FOR i: NAT IN[0..nCoords) DO ret[i] ← t3*c[0][i]+t2*c[1][i]+t*c[2][i]+c[3][i]; ENDLOOP;
};
IF persp
THEN {
w: REAL ← ret[3];
IF ret[0]+w < 0.0
OR ret[0]+w < 0.0
OR ret[1]+w < 0.0
OR ret[1]-w < 0.0
OR ret[2]+w < 0.0
THEN RETURN[[2000.0, 2000.0]]
ELSE RETURN[[ret[0]/w, ret[1]/w]];
};
RETURN[[ret[0], ret[1]]];
};
FurthestPoint:
PUBLIC PROC [c: Coeffs]
RETURNS [p: Triple, t, dist:
REAL] ~ {
max: REAL ← 0.0;
base: Triple ← Position[c, 0.0];
axis: Triple ← Vector3d.Sub[Position[c, 1.0], base];
FOR tt:
REAL ← 0.0, tt+0.01
WHILE tt <= 1.0
DO
pc: Triple ← Position[c, tt];
pl: Triple ← Vector3d.LinePoint[pc, [base, axis]];
d: REAL ← Vector3d.Distance[pc, pl];
IF d > max THEN {max ← d; p ← pc; t ← tt; dist ← d};
ENDLOOP;
};
Subdivision Procedures
csplit0: Matrix ←
NEW[MatrixRep ← [
[1.0/8.0, 0.0, 0.0, 0.0],
[0.0, 1.0/4.0, 0.0, 0.0],
[0.0, 0.0, 1.0/2.0, 0.0],
[0.0, 0.0, 0.0, 1.0]]];
csplit1: Matrix ←
NEW[MatrixRep ← [
[1.0/8.0, 0.0, 0.0, 0.0],
[3.0/8.0, 1.0/4.0, 0.0, 0.0],
[3.0/8.0, 1.0/2.0, 1.0/2.0, 0.0],
[1.0/8.0, 1.0/4.0, 1.0/2.0, 1.0]]];
SplitCurve:
PUBLIC
PROC [c: Coeffs, out1, out2: Coeffs ←
NIL]
RETURNS [c1, c2: Coeffs] ~ {
aa, bb, cc, dd, ee, ff: REAL;
c1 ← IF out1 # NIL THEN out1 ELSE NEW[CoeffsRep];
c2 ← IF out2 # NIL THEN out2 ELSE NEW[CoeffsRep];
FOR i:
NAT
IN[0..2]
DO
c1[0][i] ← aa ← (1.0/8.0)*c[0][i];
c1[1][i] ← bb ← (1.0/4.0)*c[1][i];
c1[2][i] ← cc ← (1.0/2.0)*c[2][i];
c1[3][i] ← dd ← c[3][i];
ee ← 3.0*aa;
c2[0][i] ← aa;
c2[1][i] ← ff ← ee+bb;
c2[2][i] ← ff+bb+cc;
c2[3][i] ← aa+bb+cc+dd;
ENDLOOP;
};
SplitBezier:
PUBLIC PROC [b: Bezier]
RETURNS [b1, b2: Bezier] ~ {
Ave:
PROC [p, q: Triple]
RETURNS [Triple] ~
INLINE {
RETURN[[0.5*(p.x+q.x), 0.5*(p.y+q.y), 0.5*(p.z+q.z)]];
};
b01: Triple ← Ave[b.b0, b.b1];
b12: Triple ← Ave[b.b1, b.b2];
b23: Triple ← Ave[b.b2, b.b3];
b0112: Triple ← Ave[b01, b12];
b1223: Triple ← Ave[b12, b23];
b01121223: Triple ← Ave[b0112, b1223];
RETURN[[b.b0, b01, b0112, b01121223], [b01121223, b1223, b23, b.b3]];
};
Subdivide:
PUBLIC PROC [c: Coeffs, t:
REAL]
RETURNS [c1, c2: Coeffs] ~ {
RETURN[Reparameterize[c, 0.0, t], Reparameterize[c, t, 1.0]];
};
Reparameterize:
PUBLIC
PROC [in: Coeffs, t0, t1:
REAL, out: Coeffs←
NIL]
RETURNS [Coeffs] ~ {
dt1: REAL ← t1-t0;
dt2: REAL ← dt1*dt1;
t02: REAL ← t0*t0;
IF out = NIL THEN out ← NEW[CoeffsRep];
FOR i:
NAT
IN[0..2]
DO
x: REAL ← in[0][i];
y: REAL ← in[1][i];
z: REAL ← in[2][i];
x3: REAL ← 3.0*x;
out[0][i] ← x*dt1*dt2;
out[1][i] ← x3*dt2*t0+y*dt2;
out[2][i] ← x3*dt1*t02+2.0*y*dt1*t0+z*dt1;
out[3][i] ← x*t0*t02+y*t02+z*t0+in[3][i];
ENDLOOP;
FOR i: NAT IN[0..3] DO out[i][3] ← in[i][3]; ENDLOOP;
RETURN[out];
};
Miscellaneous procedures
Tame:
PUBLIC
PROC [in: Coeffs, out: Coeffs ←
NIL]
RETURNS [Coeffs] ~ {
v0, v2, m: REAL;
d0, d2, d3, s0, s2, r0, r2: Triple;
b: Bezier ← BezierFromCoeffs[in]; -- convert to bezier
d0 ← Vector3d.Sub[b.b1, b.b0];
d2 ← Vector3d.Sub[b.b3, b.b2];
d3 ← Vector3d.Sub[b.b3, b.b0];
m ← Vector3d.Mag[d3];
v0 ← m*Vector3d.Dot[d0, d3];
v2 ← m*Vector3d.Dot[d2, d3];
s0 ← Vector3d.Mul[d3, v0]; -- projection of d0 onto d3
s2 ← Vector3d.Mul[d3, v2]; -- projection of d2 onto d3
r0 ← Vector3d.Sub[d0, s0];
r2 ← Vector3d.Sub[d2, s2];
IF Vector3d.Dot[d3, d0] < 0.0
THEN {
-- bulge at start
d0 ← r0;
s0 ← [0.0, 0.0, 0.0];
};
IF Vector3d.Dot[d3, d2] < 0.0
THEN {
-- bulge at end
d2 ← r2;
s2 ← [0.0, 0.0, 0.0];
};
IF Vector3d.Dot[r0, r2] > 0.0
THEN {
-- sides point oppositely
IF Vector3d.Square[r0] > Vector3d.Square[r2]
THEN d2 ← s2
ELSE d0 ← s0;
};
IF v0+v2 > m
THEN {
-- sides too long
scale: REAL ← m/(v0+v2);
d0 ← Vector3d.Mul[d0, scale];
d2 ← Vector3d.Mul[d2, scale];
};
b.b1 ← Vector3d.Add[b.b0, d0];
b.b2 ← Vector3d.Sub[b.b3, d2];
RETURN [CoeffsFromBezier[b, out]];
};
Same:
PUBLIC PROC [c1, c2: Coeffs]
RETURNS [
BOOL] ~ {
FOR i:
NAT
IN[0..3]
DO
FOR j: NAT IN[0..3] DO IF c1[i][j] # c2[i][j] THEN RETURN[FALSE]; ENDLOOP;
ENDLOOP;
RETURN[TRUE];
};
debugView: Coeffs ← NIL;
DebugView: PUBLIC PROC [view: Coeffs] ~ {debugView ← view};
debugStream: IO.STREAM ← NIL;
DebugStream: PUBLIC PROC [stream: IO.STREAM] ~ {debugStream ← stream};
TPosFromCoeffs: PUBLIC PROC [c: Coeffs, num: NAT, t0: REAL ← 0.0, t1: REAL ← 1.0, tpos: TPosSequence ← NIL] RETURNS [TPosSequence] ~ {
Tri: TYPE ~ RECORD [t0, t1, t, area: REAL, p0, p1, p: Triple];
TriSequenceRec: TYPE ~ RECORD [element: SEQUENCE length: NAT OF Tri];
n, nTris: NAT ← 0;
tris: REF TriSequenceRec ← NEW[TriSequenceRec[num]];
LargestTri: PROC RETURNS [n: NAT] ~ {
max: REAL ← 0.0;
FOR i: NAT IN[0..nTris) DO IF tris[i].area > max THEN {max ← tris[i].area; n ← i} ENDLOOP;
};
AddTri: PROC [tri: Tri] ~ {tris[nTris] ← tri; nTris ← nTris+1};
RelArea: PROC [p0, p1, p2: Triple] RETURNS [REAL] ~ {
RETURN[Vector3d.Square[Vector3d.Cross[Vector3d.Sub[p1, p0], Vector3d.Sub[p2, p0]]]];
};
NewTri: PROC [t0, t1: REAL, p0, p1: Triple] RETURNS [Tri] ~ {
t: REAL ← 0.5*(t0+t1);
p: Triple ← Position[c, t];
RETURN[[t0, t1, t, RelArea[p0, p, p1], p0, p1, p]];
};
p0: Triple ← Position[c, t0];
p1: Triple ← Position[c, t1];
IF tpos = NIL THEN tpos ← NEW[TPosSequenceRec[num]];
AddTri[NewTri[0.0, 1.0, p0, p1]];
THROUGH [0..num-4] DO
tri: Tri ← tris[n ← LargestTri[]];
tris[n] ← NewTri[tri.t0, tri.t, tri.p0, tri.p];
AddTri[NewTri[tri.t, tri.t1, tri.p, tri.p1]];
ENDLOOP;
tpos[0] ← [t0, p0];
tpos[num-1] ← [t1, p1];
FOR n: NAT IN[1..nTris] DO
isave: NAT;
min: REAL ← 1.0;
FOR i: NAT IN[0..nTris) DO IF tris[i].t < min THEN {min ← tris[i].t; isave ← i} ENDLOOP;
tpos[n] ← [min, tris[isave].p];
tris[isave].t ← 1.0;
ENDLOOP;
RETURN[tpos];
};