Type Declarations
Bezier: TYPE ~ G3dSpline.Bezier;
Bspline: TYPE ~ G3dSpline.Bspline;
Hermite: TYPE ~ G3dSpline.Hermite;
NearSpline: TYPE ~ G3dSpline.NearSpline;
Spline: TYPE ~ G3dSpline.Spline;
SplinePlaneIntersect: TYPE ~ G3dSpline.SplinePlaneIntersect;
SplineRep: TYPE ~ G3dSpline.SplineRep;
SplineSequence: TYPE ~ G3dSpline.SplineSequence;
SplineSequenceRep: TYPE ~ G3dSpline.SplineSequenceRep;
Pair: TYPE ~ G3dBasic.Pair;
Triple: TYPE ~ G3dBasic.Triple;
Quad: TYPE ~ G3dBasic.Quad;
Ray: TYPE ~ G3dBasic.Ray;
TripleSequence: TYPE ~ G3dBasic.TripleSequence;
NearSpline2d: TYPE ~ G2dSpline.NearSpline;
RealSequence: TYPE ~ G3dBasic.RealSequence;
RealSequenceRep: TYPE ~ G3dBasic.RealSequenceRep;
TripleSequenceRep: TYPE ~ G3dBasic.TripleSequenceRep;
Matrix: TYPE ~ G3dMatrix.Matrix;
MatrixRep: TYPE ~ G3dMatrix.MatrixRep;
PerPointProc: TYPE ~ G3dSpline.PerPointProc;
Interpolation Procedures
InterpolateCyclic:
PUBLIC
PROC [knots: TripleSequence, tension:
REAL ¬ 1.0]
RETURNS [spline: SplineSequence] ~ {
From /Cedar/CedarChest6.1/CubicSplinePackage/RegularALSplineImpl.mesa by Baudelaire
and Stone, in which spline.t3, .t2, .t1, and .t0 refer to the 3rd, 2nd and 1st derivatives, and
position of the curve. In G3dSpline, these correspond to spline[0], [1], [2], and [3].
IF
NOT G3dVector.Equal[knots[0], knots[knots.length-1], 0.0]
THEN {
old: TripleSequence ¬ knots;
knots ¬ NEW[TripleSequenceRep[old.length+1]];
knots.length ¬ old.length+1;
FOR n: NAT IN [0..old.length) DO knots[n] ¬ old[n]; ENDLOOP;
knots[knots.length-1] ¬ knots[0];
};
spline ¬ NEW[SplineSequenceRep[knots.length]];
spline.length ¬ knots.length;
FOR n: NAT IN [0..spline.length) DO spline[n] ¬ NEW[SplineRep]; ENDLOOP;
SELECT knots.length
FROM
< 1 => spline ¬ NIL;
< 3 => {
spline[0][3] ¬ [knots[0].x, knots[0].y, knots[0].z, 1.0];
IF knots.length = 2
THEN {
dif: Triple ~ G3dVector.Sub[knots[1], knots[0]];
spline[0][2] ¬ [dif.x, dif.y, dif.z, 0.0];
};
};
ENDCASE => {
lastKnot: NAT ¬ knots.length-1; -- last knot index
lastK: NAT ¬ lastKnot-1; -- max k for intervals h,a,b,c,d,r,s
h: RealSequence ¬ NEW[RealSequenceRep[lastKnot]];
a: RealSequence ¬ NEW[RealSequenceRep[lastKnot]];
b: RealSequence ¬ NEW[RealSequenceRep[lastKnot]];
d: RealSequence ¬ NEW[RealSequenceRep[lastKnot]];
c: RealSequence ¬ NEW[RealSequenceRep[lastKnot]];
r: RealSequence ¬ NEW[RealSequenceRep[lastKnot]];
s: RealSequence ¬ NEW[RealSequenceRep[lastKnot]];
set cubic constants:
FOR i:
NAT
IN [0..knots.length)
DO
spline[i][3] ¬ [knots[i].x, knots[i].y, knots[i].z, 1.0];
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 k: NAT IN [1..lastK) DO a[k] ¬ 2.0*(h[k-1]+h[k])-h[k-1]*h[k-1]/a[k-1]; ENDLOOP;
c[0] ¬ h[lastK];
FOR k: NAT IN [1..lastK) DO c[k] ¬ -h[k-1]*c[k-1]/a[k-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: spline[lastK+1].t2.[i] = spline[0].t2[i]
spline[lastK][1][i] ¬ 0.5*p2;
FOR k: NAT IN [0..lastK) DO spline[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
spline[k][0][i] ¬ (spline[k+1][1][i]-spline[k][1][i])/3.0;
spline[k][2][i] ¬ d[k]-h[k]*(2.0*spline[k][1][i]+spline[k+1][1][i])/3.0;
ENDLOOP;
note again: spline[lastK+1].t2[i] = spline[0].t2[i] (0 for natural end conditions)
spline[lastK][0][i] ¬ (spline[0][1][i]-spline[lastK][1][i])/3.0;
spline[lastK][2][i] ¬ d[lastK]-h[lastK]*(2.0*spline[lastK][1][i]+spline[0][1][i])/3.0;
reparametrize coefficients to interval [0..1]:
FOR k:
NAT
IN [0..lastK]
DO
spline[k][0][i] ¬ h[k]*h[k]*spline[k][0][i];
spline[k][1][i] ¬ h[k]*h[k]*spline[k][1][i];
spline[k][2][i] ¬ h[k]*spline[k][2][i];
ENDLOOP;
ENDLOOP;
};
Interpolate:
PUBLIC PROC [
knots: TripleSequence,
tan0, tan1: Triple ¬ [0.0, 0.0, 0.0],
tension: REAL ¬ 1.0,
c: SplineSequence ¬ NIL]
RETURNS [SplineSequence]
~ {
Mostly from Rogers and Adams, with tension added:
SELECT knots.length
FROM
0 => RETURN[c];
1, 2 => {
p0: Triple ~ knots[0];
p1: Triple ~ IF knots.length = 2 THEN knots[1] ELSE knots[0];
len: REAL ¬ G3dVector.Distance[p0, p1];
IF c = NIL OR c.maxLength < 1 THEN c ¬ NEW[SplineSequenceRep[1]];
c.length ¬ 1;
c[0] ¬ SplineFromHermite[[p0, p1, G3dVector.Mul[tan0, len], G3dVector.Mul[tan1, len]]];
RETURN[c];
};
ENDCASE => {
chords: RealSequence ¬ ComputeChords[knots, NIL];
tangents: TripleSequence ¬ ComputeTangents[knots, chords, NIL, tan0, tan1];
RETURN[ComputeSpline[knots, tangents, tension, chords, c]];
};
};
ComputeChords:
PROC [knots: TripleSequence, chords: RealSequence ¬
NIL]
RETURNS [RealSequence] ~ {
max: INTEGER ~ knots.length-1;
IF chords =
NIL
OR chords.maxLength < knots.length
THEN chords ¬ NEW[RealSequenceRep[knots.length]];
FOR n:
NAT
IN [0..max)
DO
IF (chords[n] ¬ G3dVector.Distance[knots[n+1], knots[n]]) = 0.0 THEN chords[n] ¬ 1.0;
ENDLOOP;
IF (chords[max] ¬ G3dVector.Distance[knots[0], knots[max]]) = 0.0 THEN chords[max] ¬ 1.0;
RETURN[chords];
};
ComputeTangents:
PROC [
knots: TripleSequence,
chords: RealSequence,
tangents: TripleSequence ¬ NIL,
tan0, tan1: Triple]
RETURNS [TripleSequence]
~ {
N: TripleSequence ¬ NEW[TripleSequenceRep[knots.length]]; -- nonzero elements of M
B: TripleSequence ¬ NEW[TripleSequenceRep[knots.length]]; -- a control matrix
IF tangents =
NIL
OR tangents.maxLength < knots.length
THEN tangents ¬ NEW[TripleSequenceRep[knots.length]];
SetEndConditions[knots, N, B, tangents, chords, tan0, tan1];
GaussianEliminate[knots, N, B, tangents, chords];
RETURN[tangents];
};
SetEndConditions:
PROC [
knots: TripleSequence,
N, B, tangents: TripleSequence,
chords: RealSequence,
tan0, tan1: Triple]
~ {
z: NAT ¬ knots.length-1;
IF G3dVector.Null[tan0]
THEN {
N[0] ¬ [0.0, 1.0, 0.5];
B[0] ¬ G3dVector.Mul[G3dVector.Sub[knots[1], knots[0]], 1.5/chords[0]];
}
ELSE {
tangents[0] ¬ B[0] ¬ tan0;
N[0] ¬ [0.0, 1.0, 0.0];
};
IF G3dVector.Null[tan1]
THEN {
N[z] ¬ [2.0, 4.0, 0.0];
B[z] ¬ G3dVector.Mul[G3dVector.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: TripleSequence,
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] ¬ G3dVector.Mul[
G3dVector.Add[
G3dVector.Mul[G3dVector.Sub[knots[n+1], knots[n]], l0*l0],
G3dVector.Mul[G3dVector.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] ¬ G3dVector.Sub[G3dVector.Mul[N[n], d], N[n-1]];
B[n] ¬ G3dVector.Sub[G3dVector.Mul[B[n], d], B[n-1]];
q ¬ 1.0/N[n].y;
N[n] ¬ G3dVector.Mul[N[n], q];
B[n] ¬ G3dVector.Mul[B[n], q];
ENDLOOP;
tangents[nPts-1] ¬ G3dVector.Div[B[nPts-1], N[nPts-1].y]; -- back substitution
FOR n:
NAT
IN[2..nPts]
DO
tangents[nPts-n] ¬
G3dVector.Div[
G3dVector.Sub[
B[nPts-n],
G3dVector.Mul[tangents[nPts-n+1], N[nPts-n].z]],
N[nPts-n].y];
ENDLOOP;
};
ComputeSpline:
PROC [
knots: TripleSequence,
tangents: TripleSequence,
tension: REAL,
chords: RealSequence,
in: SplineSequence ¬ NIL]
RETURNS [SplineSequence]
~ {
nSpline: NAT ¬ knots.length-1;
IF in = NIL OR in.maxLength < nSpline THEN in ¬ NEW[SplineSequenceRep[nSpline]];
in.length ¬ nSpline;
IF tension # 1.0
THEN
FOR n:
NAT
IN[0..nSpline]
DO
tangents[n] ¬ G3dVector.Mul[tangents[n], tension];
ENDLOOP;
FOR m:
NAT
IN[0..nSpline)
DO
in[m] ← SplineFromHermite[
[knots[m],
knots[m+1],
G3dVector.Mul[tangents[m], chords[m]],
G3dVector.Mul[tangents[m+1], chords[m]]],
in[m]];
l: REAL ¬ chords[m];
dknots: Triple ¬ G3dVector.Sub[knots[m+1], knots[m]];
a: Triple ¬ G3dVector.Mul[tangents[m], l];
b: Triple ¬ G3dVector.Add[G3dVector.Mul[tangents[m+1], l], a];
c: Triple ¬ G3dVector.Add[G3dVector.Mul[dknots, -2.0], b];
d: Triple ¬ G3dVector.Sub[G3dVector.Sub[G3dVector.Mul[dknots, 3.0], b], a];
IF in[m] = NIL THEN in[m] ¬ NEW[SplineRep];
in[m][0] ¬ [c.x, c.y, c.z, 0.0];
in[m][1] ¬ [d.x, d.y, d.z, 0.0];
in[m][2] ¬ [a.x, a.y, a.z, 0.0];
in[m][3] ¬ [knots[m].x, knots[m].y, knots[m].z, 1.0];
ENDLOOP;
RETURN[in];
};
Conversion Procedures
hermiteToSpline: 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]]];
splineToHermite: 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]]];
bezierToSpline: 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]]];
splineToBezier: 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]]];
bsplineToSpline: 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]]];
splineToBspline: 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]]];
ConvertToSpline:
PROC [p0, p1, p2, p3: Triple, convert, out: Matrix, hermite:
BOOL ¬
FALSE]
RETURNS [Spline] ~ {
w: REAL ¬ IF hermite THEN 0.0 ELSE 1.0;
m: Matrix ¬ G3dMatrix.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[SplineRep];
[] ¬ G3dMatrix.Mul[convert, m, out];
G3dMatrix.ReleaseMatrix[m];
RETURN[out];
};
ConvertFromSpline:
PROC [c: Spline, convert: Matrix]
RETURNS [p0, p1, p2, p3: Triple] ~ {
m: Matrix ¬ G3dMatrix.ObtainMatrix[];
[] ¬ G3dMatrix.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]];
G3dMatrix.ReleaseMatrix[m];
RETURN[p0, p1, p2, p3];
};
SplineFromBezier:
PUBLIC PROC [b: Bezier, out: Spline ¬
NIL]
RETURNS [Spline] ~ {
RETURN[ConvertToSpline[b.b0, b.b1, b.b2, b.b3, bezierToSpline, out]]; -- but, let's optimize:
x0X3: REAL ¬ 3.0*b.b0.x;
y0X3: REAL ¬ 3.0*b.b0.y;
z0X3: REAL ¬ 3.0*b.b0.z;
x1X3: REAL ¬ 3.0*b.b1.x;
y1X3: REAL ¬ 3.0*b.b1.y;
z1X3: REAL ¬ 3.0*b.b1.z;
x2X3: REAL ¬ 3.0*b.b2.x;
y2X3: REAL ¬ 3.0*b.b2.y;
z2X3: REAL ¬ 3.0*b.b2.z;
IF out = NIL THEN out ¬ NEW[G3dSpline.SplineRep];
out[0][0] ¬ -b.b0.x+x1X3-x2X3+b.b3.x;
out[0][1] ¬ -b.b0.y+y1X3-y2X3+b.b3.y;
out[0][2] ¬ -b.b0.z+z1X3-z2X3+b.b3.z;
out[0][3] ¬ 0.0;
out[1][0] ¬ x0X3-x1X3-x1X3+x2X3;
out[1][1] ¬ y0X3-y1X3-y1X3+y2X3;
out[1][2] ¬ z0X3-z1X3-z1X3+z2X3;
out[1][3] ¬ 0.0;
out[2][0] ¬ -x0X3+x1X3;
out[2][1] ¬ -y0X3+y1X3;
out[2][2] ¬ -z0X3+z1X3;
out[2][3] ¬ 0.0;
out[3][0] ¬ b.b0.x;
out[3][1] ¬ b.b0.y;
out[3][2] ¬ b.b0.z;
out[3][3] ¬ 1.0;
RETURN[out];
};
SplineFromBspline:
PUBLIC PROC [b: Bspline, out: Spline ¬
NIL]
RETURNS [Spline] ~ {
RETURN[ConvertToSpline[b.b0, b.b1, b.b2, b.b3, bsplineToSpline, out]];
};
SplineFromHermite:
PUBLIC
PROC [h: Hermite, out: Spline ¬
NIL]
RETURNS [Spline] ~ {
RETURN[ConvertToSpline[h.p0, h.p1, h.tan0, h.tan1, hermiteToSpline, out, TRUE]];
};
BezierFromSpline:
PUBLIC PROC [s: Spline]
RETURNS [Bezier] ~ {
b0, b1, b2, b3: Triple;
[b0, b1, b2, b3] ¬ ConvertFromSpline[s, splineToBezier];
RETURN[[b0, b1, b2, b3]];
};
BsplineFromSpline:
PUBLIC PROC [s: Spline]
RETURNS [Bspline] ~ {
b0, b1, b2, b3: Triple;
[b0, b1, b2, b3] ¬ ConvertFromSpline[s, splineToBspline];
RETURN[[b0, b1, b2, b3]];
};
HermiteFromSpline:
PUBLIC PROC [s: Spline]
RETURNS [Hermite] ~ {
p0, p1, tan0, tan1: Triple;
[p0, p1, tan0, tan1] ¬ ConvertFromSpline[s, splineToHermite];
RETURN[[p0, p1, tan0, tan1]];
};
BezierFromHermite:
PUBLIC
PROC [p0, p1, v0, v1: Triple]
RETURNS [Bezier] ~ {
RETURN[[
p0,
G3dVector.Add[p0, G3dVector.Mul[v0, 1.0/3.0]],
G3dVector.Sub[p1, G3dVector.Mul[v1, 1.0/3.0]],
p1]];
};
CopySpline:
PUBLIC PROC [spline: Spline, out: Spline ¬
NIL]
RETURNS [Spline] ~ {
RETURN[G3dMatrix.CopyMatrix[spline, out]];
};
Evaluation Procedures
Position:
PUBLIC PROC [s: Spline, t:
REAL]
RETURNS [Triple] ~ {
ret: ARRAY [0..2] OF REAL;
IF t = 0.0 THEN FOR i: NAT IN[0..2] DO ret[i] ¬ s[3][i]; ENDLOOP
ELSE IF t = 1.0 THEN FOR i: NAT IN[0..2] DO ret[i] ¬ s[0][i]+s[1][i]+s[2][i]+s[3][i]; ENDLOOP
ELSE {
t2: REAL ¬ t*t;
t3: REAL ¬ t*t2;
FOR i: NAT IN[0..2] DO ret[i] ¬ t3*s[0][i]+t2*s[1][i]+t*s[2][i]+s[3][i]; ENDLOOP
};
RETURN[[ret[0], ret[1], ret[2]]];
};
Velocity:
PUBLIC PROC [s: Spline, t:
REAL]
RETURNS [Triple] ~ {
tan: ARRAY [0..2] OF REAL;
a: REAL ¬ 3.0;
b: REAL ¬ 2.0;
IF t = 0.0 THEN RETURN[[s[2][0], s[2][1], s[2][2]]];
IF t # 1.0 THEN {a ¬ a*t*t; b ¬ b*t};
FOR i: NAT IN[0..2] DO tan[i] ¬ a*s[0][i]+b*s[1][i]+s[2][i]; ENDLOOP;
RETURN[[tan[0], tan[1], tan[2]]];
};
Tangent:
PUBLIC
PROC [s: Spline, t:
REAL]
RETURNS [Triple] ~ {
RETURN[Velocity[s, t]];
};
PositionAndVelocity:
PUBLIC PROC [s: Spline, t:
REAL]
RETURNS [pos, vel: Triple] ~ {
IF t = 0.0
THEN {
s2: ARRAY [0..4) OF REAL ¬ s[2];
s3: ARRAY [0..4) OF REAL ¬ s[3];
RETURN[[s3[0], s3[1], s3[2]], [s2[0], s2[1], s2[2]]];
}
ELSE {
a: REAL ¬ 3.0;
b: REAL ¬ 2.0;
s0: ARRAY [0..4) OF REAL ¬ s[0];
s1: ARRAY [0..4) OF REAL ¬ s[1];
s2: ARRAY [0..4) OF REAL ¬ s[2];
s3: ARRAY [0..4) OF REAL ¬ s[3];
IF t = 1.0
THEN
pos ¬ [s0[0]+s1[0]+s2[0]+s3[0], s0[1]+s1[1]+s2[1]+s3[1], s0[2]+s1[2]+s2[2]+s3[2]]
ELSE {
t2: REAL ¬ t*t;
t3: REAL ¬ t*t2;
a ¬ a*t2;
b ¬ b*t;
pos.x ¬ t3*s0[0]+t2*s1[0]+t*s2[0]+s3[0];
pos.y ¬ t3*s0[1]+t2*s1[1]+t*s2[1]+s3[1];
pos.z ¬ t3*s0[2]+t2*s1[2]+t*s2[2]+s3[2];
};
vel ¬ [a*s0[0]+b*s1[0]+s2[0], a*s0[1]+b*s1[1]+s2[1], a*s0[2]+b*s1[2]+s2[2]];
};
};
Acceleration:
PUBLIC PROC [s: Spline, t:
REAL]
RETURNS [Triple] ~ {
a: REAL = 6.0*t;
RETURN[[a*s[0][0]+s[1][0]+s[1][0], a*s[0][1]+s[1][1]+s[1][1], a*s[0][2]+s[1][2]+s[1][2]]];
};
MinAcceleration:
PUBLIC PROC [s: Spline]
RETURNS [t:
REAL] ~ {
a0: Triple ¬ Acceleration[s, 0.0];
axis: Triple ¬ G3dVector.Sub[Acceleration[s, 1.0], a0];
t ¬ IF G3dVector.Null[axis] THEN 0.5 ELSE -G3dVector.Dot[a0, axis]/G3dVector.Square[axis];
t ¬ MIN[1.0, MAX[0.0, t]];
};
Curvature:
PUBLIC PROC [s: Spline, t:
REAL]
RETURNS [Triple] ~ {
1 sqrt, 3 divides, 14 multiplies, 6 adds
tan: Triple ¬ Tangent[s, t];
IF tan = [0.0, 0.0, 0.0]
THEN
RETURN[tan]
ELSE {
acc: Triple ¬ Acceleration[s, t];
length: REAL ¬ G3dVector.Length[tan];
length2: REAL ¬ length*length;
RETURN G3dVector.Div[G3dVector.Cross[G3dVector.Cross[tan, acc], tan], length2*length2];
};
};
CurvatureMag:
PUBLIC
PROC [s: Spline, t:
REAL]
RETURNS [
REAL] ~ {
tan: Triple ¬ Tangent[s, t];
acc: Triple ¬ Acceleration[s, t];
l: REAL ¬ G3dVector.Length[tan];
RETURN[IF l = 0.0 THEN 0.0 ELSE G3dVector.Length[G3dVector.Cross[acc, tan]]/(l*l*l)];
};
WalkBezier:
PUBLIC
PROC [
b: Bezier, proc: PerPointProc, epsilon: REAL ¬ 0.05, doLast: BOOL ¬ TRUE]
~ {
Inner:
PROC [b: Bezier] ~ {
IF FlatBezier[b, epsilon]
THEN {IF NOT proc[b.b0] THEN RETURN}
ELSE {
left, rite: Bezier;
[left, rite] ¬ SplitBezier[b];
Inner[left];
Inner[rite];
};
};
Inner[b];
IF doLast THEN [] ¬ proc[b.b3];
};
ForwardDifference:
PUBLIC
PROC [in: Spline, nSegments:
INTEGER, out: Spline ¬
NIL]
RETURNS [Spline]
~ {
d1, d2, d22, d3, d6: REAL;
IF nSegments = 0 THEN RETURN[NIL];
d1 ¬ 1.0/REAL[nSegments];
d2 ¬ d1*d1;
d22 ¬ d2+d2;
d3 ¬ d1*d2;
d6 ¬ 6.0*d3;
IF out = NIL THEN out ¬ NEW[MatrixRep];
We wish to return the input spline multiplied by the forward differencing matrix:
forwardDifference^ ← [
[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]];
out ← G3dMatrix.Mul[forwardDifference, in, out];
which may be optimized to:
out[0] ¬ in[3];
out[1][0] ¬ d3*in[0][0]+d2*in[1][0]+d1*in[2][0];
out[1][1] ¬ d3*in[0][1]+d2*in[1][1]+d1*in[2][1];
out[1][2] ¬ d3*in[0][2]+d2*in[1][2]+d1*in[2][2];
out[1][3] ¬ d3*in[0][3]+d2*in[1][3]+d1*in[2][3];
out[2][0] ¬ (out[3][0] ¬ d6*in[0][0])+d22*in[1][0];
out[2][1] ¬ (out[3][1] ¬ d6*in[0][1])+d22*in[1][1];
out[2][2] ¬ (out[3][2] ¬ d6*in[0][2])+d22*in[1][2];
out[2][3] ¬ (out[3][3] ¬ d6*in[0][3])+d22*in[1][3];
RETURN[out];
};
Samples:
PUBLIC
PROC
[s: Spline, nPoints:
NAT, points: TripleSequence ¬
NIL]
RETURNS [TripleSequence]
~ {
p: ARRAY [0..2] OF REAL;
forwardDifference: Spline ¬ G3dMatrix.ObtainMatrix[];
forwardDifference ¬ ForwardDifference[s, nPoints-1, forwardDifference];
IF points =
NIL
OR points.maxLength < nPoints
THEN
points ¬ NEW[TripleSequenceRep[nPoints]];
points[0] ¬ [forwardDifference[0][0], forwardDifference[0][1], forwardDifference[0][2]];
FOR i: INTEGER IN[0..2] DO p[i] ¬ forwardDifference[0][i]; ENDLOOP;
FOR i:
INTEGER
IN[1..nPoints)
DO
FOR j:
INTEGER
IN[0..2]
DO
p[j] ¬ p[j]+forwardDifference[1][j];
forwardDifference[1][j] ¬ forwardDifference[1][j]+forwardDifference[2][j];
forwardDifference[2][j] ¬ forwardDifference[2][j]+forwardDifference[3][j];
ENDLOOP;
points[i] ¬ [p[0], p[1], p[2]];
ENDLOOP;
G3dMatrix.ReleaseMatrix[forwardDifference];
RETURN[points];
};
RefVec:
PUBLIC PROC [s: Spline, t:
REAL]
RETURNS [Triple] ~ {
ref: Triple ¬ Curvature[s, t];
We assume that, for any point along s, if acceleration and tangent are 0, then spline is linear
IF G3dVector.Square[ref] < 0.9
THEN {
tan: Triple ¬ Tangent[s, t];
IF G3dVector.Square[tan] < 0.01 THEN tan ¬ G3dVector.Sub[Position[s, 1.0], Position[s, 0.0]];
ref ¬ G3dVector.Ortho[tan];
};
RETURN[ref];
};
IsStraight:
PUBLIC PROC [s: Spline, epsilon:
REAL ¬ 0.01]
RETURNS [
BOOL] ~ {
b: Bezier ~ BezierFromSpline[s];
RETURN[
G3dVector.Collinear[b.b0, b.b1, b.b2, epsilon] AND
G3dVector.Collinear[b.b1, b.b2, b.b3, epsilon]];
};
Size Procedures
Length:
PUBLIC
PROC [s: Spline, nSteps:
NAT ¬ 100]
RETURNS [
REAL] ~ {
sum: REAL ¬ 0.0;
fd: Spline ¬ ForwardDifference[s, nSteps, G3dMatrix.ObtainMatrix[]];
FOR i:
NAT
IN [0..nSteps)
DO
sum ¬ sum+G3dVector.Length[[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;
G3dMatrix.ReleaseMatrix[fd];
RETURN[sum];
};
ConvexHullArea:
PUBLIC PROC [b: Bezier]
RETURNS [
REAL] ~ {
TwiceTriArea:
PROC [p0, p1, p2: Triple]
RETURNS [r:
REAL] ~ {
r ¬ G3dVector.Length[G3dVector.Cross[G3dVector.Sub[p1, p0], G3dVector.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 ¬ G3dVector.Sub[b.b1, b.b0];
d1: Triple ¬ G3dVector.Sub[b.b2, b.b1];
d2: Triple ¬ G3dVector.Sub[b.b3, b.b2];
g3d: Triple ¬ G3dVector.Sub[b.b3, b.b0];
RETURN[G3dVector.Length[d0]+
G3dVector.Length[d1]+G3dVector.Length[d2]+G3dVector.Length[g3d]];
};
FlatBezier:
PUBLIC PROC [b: Bezier, epsilon:
REAL ¬ 0.05]
RETURNS [
BOOL] ~ {
g3dlength: REAL;
d0: Triple ¬ G3dVector.Sub[b.b1, b.b0];
d1: Triple ¬ G3dVector.Sub[b.b2, b.b1];
d2: Triple ¬ G3dVector.Sub[b.b3, b.b2];
g3d: Triple ¬ G3dVector.Sub[b.b3, b.b0];
IF G3dVector.Dot[g3d, d0] < 0.0 OR G3dVector.Dot[g3d, d2] < 0.0 THEN RETURN[FALSE]; -- bulge
IF (g3dlength ¬ G3dVector.Length[g3d]) < 0.000001 THEN RETURN[TRUE];
RETURN[
(G3dVector.Length[d0]+G3dVector.Length[d1]+G3dVector.Length[d2])/G3dVector.Length[g3d]
< 1.0+epsilon];
};
Tiny:
PUBLIC
PROC [s: Spline, epsilon:
REAL ¬ 0.05]
RETURNS [
BOOL] ~ {
RETURN[G3dVector.Distance[Position[s, 0.0], Position[s, 1.0]] < epsilon];
};
Resolution:
PUBLIC PROC [s: Spline, epsilon:
REAL]
RETURNS [
INTEGER] ~ {
This produces inconsistent results:
MaxAccel:
PROC [curve: Spline]
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, INTEGER[Real.Round[RealFns.SqRt[MaxAccel[s]/(8.0*epsilon)]]]]];
};
Search Procedures
Modified Polynomial.CheapRealRoots by Eric Bier
Limits consideration of the solutions to the domain [lo..hi].
Modified by Bloomenthal: EvalDeriv and Eval don't recompute degree, minimized arg passing.
ShortRealRootRec: TYPE = Polynomial.ShortRealRootRec;
CheapRealRoots:
PROC [poly: Polynomial.Ref, lo, hi:
REAL]
RETURNS [roots: ShortRealRootRec]
~ {
RootBetween:
PROC [x0, x1:
REAL]
RETURNS [x:
REAL] ~ {
debug xi: ARRAY [0..50) OF REAL;
debug i: NAT ← 0;
y0: REAL ¬ Eval[x0];
y1: REAL ¬ Eval[x1];
xx: REAL;
xx0: REAL ¬ IF y0<y1 THEN x0 ELSE x1;
xx1: REAL ¬ IF y0<y1 THEN x1 ELSE x0;
yy0: REAL ¬ MIN[y0,y1];
yy1: REAL ¬ MAX[y0,y1];
EvalDeriv:
PROC [x:
REAL]
RETURNS [y:
REAL] ~ {
y ¬ 0.0;
FOR i:NAT DECREASING IN (0..degree] DO y ¬ i*poly[i] + y*x; ENDLOOP
};
Eval:
PROC [x:
REAL]
RETURNS [y:
REAL] ~ {
y ¬ 0.0;
FOR i:NAT DECREASING IN [0..degree] DO y ¬ y*x + poly[i]; ENDLOOP
};
NewtonStep:
PROC [x:
REAL]
RETURNS [newx:
REAL] ~ {
y: REAL ¬ Eval[x];
deriv: REAL ¬ EvalDeriv[x];
IF y<0 THEN {IF y>yy0 THEN {xx0 ¬ x; yy0 ¬ y}};
IF y>0 THEN {IF y<yy1 THEN {xx1 ¬ x; yy1 ¬ y}};
newx ¬ IF deriv=0 THEN x+y ELSE x-y/deriv;
};
IF y0=0 THEN RETURN[x0];
IF y1=0 THEN RETURN[x1];
IF (y0 > 0 AND y1 > 0) OR (y0 < 0 AND y1 < 0) THEN RETURN [x1+100];
xx ¬ x ¬ (x0+x1)/2;
first try Newton's method
WHILE x
IN [x0..x1]
DO
newx:REAL ¬ NewtonStep[x];
IF x=newx THEN RETURN;
x ¬ NewtonStep[newx];
xx ¬ NewtonStep[xx];
debug xi[i] ← xx; i ← i+1;
IF xx=x THEN EXIT;
ENDLOOP;
If it oscillated or went out of range, try bisection
debug SIGNAL Bisection;
IF xx0
IN [x0..x1]
AND xx1
IN [x0..x1]
THEN {
x0 ¬ MIN[xx0,xx1];
x1 ¬ MAX[xx0,xx1];
};
y0 ¬ Eval[x0];
y1 ¬ Eval[x1];
THROUGH [0..500)
DO
y: REAL ¬ Eval[x ¬ 0.5*(x0+x1)];
IF x=x0
OR x=x1
THEN
{IF ABS[y0] < ABS[y1] THEN RETURN[x0] ELSE RETURN[x1]};
IF (y > 0 AND y0 < 0) OR (y < 0 AND y0 > 0) THEN {x1 ¬ x; y1 ¬ y}
ELSE {x0 ¬ x; y0 ¬ y};
IF (y0 > 0 AND y1 > 0) OR (y0 < 0 AND y1 < 0) OR (y0=0 OR y1=0) THEN {IF ABS[y0] < ABS[y1] THEN RETURN[x0] ELSE RETURN[x1]}
ENDLOOP;
ERROR;
};
degree: NAT ¬ Polynomial.Degree[poly];
roots.nRoots ¬ 0;
IF degree<=1
THEN {
IF degree=1 THEN {roots.nRoots ¬ 1; roots.realRoot[0] ¬ -poly[0]/poly[1]}
}
ELSE {
savedCoeff: ARRAY [0..5] OF REAL;
FOR i: NAT IN [0..degree] DO savedCoeff[i] ¬ poly[i] ENDLOOP;
Polynomial.Differentiate[poly];
{
extrema: ShortRealRootRec ¬ CheapRealRoots[poly, lo, hi];
x: REAL;
FOR i: NAT IN [0..degree] DO poly[i] ¬ savedCoeff[i] ENDLOOP;
IF extrema.nRoots>0
THEN {
x ¬ RootBetween[lo, extrema.realRoot[0]];
IF x<= extrema.realRoot[0] THEN {roots.nRoots ¬ 1; roots.realRoot[0] ¬ x};
FOR i:
NAT
IN [0..extrema.nRoots-1)
DO
x ¬ RootBetween[extrema.realRoot[i], extrema.realRoot[i+1]];
IF x <= extrema.realRoot[i+1] THEN {roots.realRoot[roots.nRoots] ¬ x; roots.nRoots ¬ roots.nRoots + 1};
ENDLOOP;
x ¬ RootBetween[extrema.realRoot[extrema.nRoots-1], hi];
IF x <= hi THEN {roots.realRoot[roots.nRoots] ¬ x; roots.nRoots ¬ roots.nRoots + 1}
}
ELSE {
x ¬ RootBetween[lo, hi];
IF x <= hi THEN {roots.realRoot[0] ¬ x; roots.nRoots ¬ 1}
};
}
};
};
Square:
PROC [p0, p1: Triple]
RETURNS [
REAL] ~ {
d: Triple ¬ [p1.x-p0.x, p1.y-p0.y, p1.z-p0.z];
RETURN[d.x*d.x+d.y*d.y+d.z*d.z];
};
InflectionPoint:
PUBLIC PROC [s: Spline]
RETURNS [t:
REAL] ~ {
InflectionTest:
PROC [t:
REAL]
RETURNS [
BOOL] ~ {
RETURN[t IN [0.0..1.0] AND CurvatureMag[s, t] < 0.001];
};
Root:
PROC [a, b, c:
REAL]
RETURNS [
REAL] ~ {
ref: Polynomial.Ref ~ Polynomial.Quadratic[[a, b, c]];
roots: Polynomial.ShortRealRootRec ~ CheapRealRoots[ref, 0.0, 1.0];
FOR n:
NAT
IN [0..roots.nRoots)
DO
IF InflectionTest[roots.realRoot[n]] THEN RETURN[roots.realRoot[n]];
ENDLOOP;
RETURN[-1.0];
};
IF IsStraight[s] THEN RETURN[-2.0];
{
a: Triple ~ GetA[s];
b: Triple ~ GetB[s];
c: Triple ~ GetC[s];
Test each of the components of A V for zero:
IF (t ¬ Root[6*(a.y*b.z-a.z*b.y),6*(a.y*c.z-a.z*c.y),2*(b.y*c.z-b.z*c.y)])#-1 THEN RETURN;
IF (t ¬ Root[6*(a.z*b.x-a.x*b.z),6*(a.z*c.x-a.x*c.z),2*(b.z*c.x-b.x*c.z)])#-1 THEN RETURN;
IF (t ¬ Root[6*(a.x*b.y-a.y*b.x),6*(a.x*c.y-a.y*c.x),2*(b.x*c.y-b.y*c.x)])#-1 THEN RETURN;
};
};
Nearest
Point:
PUBLIC
PROC [
p: Triple, s: Spline, t0: REAL ¬ 0.0, t1: REAL ¬ 1.0, epsilon: REAL ¬ 0.01]
RETURNS [near: NearSpline]
~ {
Eval: TYPE ~ RECORD [p, v: Triple, t, dot: REAL];
maxCos: REAL ~ RealFns.CosDeg[MAX[0.0, 90.0-ABS[epsilon]]];
EvalCurve:
PROC [t:
REAL]
RETURNS [e: Eval] ~ {
e.t ¬ t;
e.p ¬ Position[s, t];
e.v ¬ Velocity[s, t];
e.dot ¬ G3dVector.Dot[G3dVector.Unit[e.v], G3dVector.Unit[G3dVector.Sub[p, e.p]]];
};
NearFromT:
PROC [t:
REAL]
RETURNS [NearSpline] ~ {
q: Triple ~ Position[s, t];
RETURN[[q, t, G3dVector.Distance[p, q]]];
};
NearFromEval:
PROC [e: Eval]
RETURNS [NearSpline] ~ {
RETURN[[e.p, e.t, G3dVector.Distance[p, e.p]]];
};
InnerNear:
PROC [e0, e1: Eval, t0, t1:
REAL]
RETURNS [NearSpline] ~ {
nTries: NAT ¬ 0;
IF e0.dot < 0.0
OR e1.dot > 0.0
THEN {
IF e0.dot < 0.0 AND e1.dot > 0.0 THEN {e: Eval ~ e0; e0 ¬ e1; e1 ¬ e}
ELSE {
WHILE e0.dot < 0.0
DO
IF (nTries ¬ nTries+1) > 9 THEN EXIT;
IF e0.t <= t0 THEN RETURN[NearFromEval[e0]];
e0 ¬ EvalCurve[MAX[t0, e0.t+e0.t-e1.t]];
ENDLOOP;
nTries ¬ 0;
WHILE e1.dot > 0.0
DO
IF (nTries ¬ nTries+1) > 9 THEN EXIT;
IF e1.t >= t1 THEN RETURN[NearFromEval[e1]];
e1 ¬ EvalCurve[MIN[t1, e1.t+e1.t-e0.t]];
ENDLOOP;
};
};
nTries ¬ 0;
DO
e: Eval;
IF (nTries ¬ nTries+1) > 9 THEN RETURN[NearFromEval[EvalCurve[0.5*(e0.t+e1.t)]]];
IF e0.dot < maxCos THEN RETURN[NearFromEval[e0]];
IF e1.dot > -maxCos THEN RETURN[NearFromEval[e1]];
e ¬ EvalCurve[0.5*(e0.t+e1.t)];
IF e.dot < 0.0 THEN e1 ¬ e ELSE e0 ¬ e;
ENDLOOP;
};
n: NearSpline;
IF epsilon = 0.0
THEN n ¬ PreciseNearestPoint[p, s]
ELSE {
TDivide: PROC RETURNS [t: REAL] ~ {t ¬ InflectionPoint[s]; IF t < 0.0 THEN t ¬ 0.5};
t: REAL ~ 0.5; -- TDivide[];
e: Eval ~ EvalCurve[t];
near0: NearSpline ~ NearFromT[0.0];
near1: NearSpline ~ NearFromT[1.0];
nearA: NearSpline ~ InnerNear[EvalCurve[t0], e, 1.0, t];
nearB: NearSpline ~ InnerNear[e, EvalCurve[t1], t, 1.0];
n ¬
SELECT
MIN[near0.distance, near1.distance, nearA.distance, nearB.distance]
FROM
near0.distance => near0,
near1.distance => near1,
nearA.distance => nearA,
ENDCASE => nearB;
};
RETURN[n];
PreciseNearestPoint:
PUBLIC PROC [p: Triple, s: Spline]
RETURNS [n: NearSpline] ~ {
Test:
PROC [testT:
REAL] ~ {
testP: Triple ¬ Position[s, testT];
testDistance: REAL ¬ Square[testP, p];
IF testDistance < n.distance
THEN {
n.point ¬ testP;
n.t ¬ testT;
n.distance ¬ testDistance;
};
};
c0: ARRAY [0..4) OF REAL ¬ s[0];
c1: ARRAY [0..4) OF REAL ¬ s[1];
c2: ARRAY [0..4) OF REAL ¬ s[2];
c3: ARRAY [0..4) OF REAL ¬ s[3];
{
Dot product of tangent(t) with [q-position(t)]:
realRoots: Polynomial.ShortRealRootRec;
a: Triple ~ [c0[0], c0[1], c0[2]];
b: Triple ~ [c1[0], c1[1], c1[2]];
c: Triple ~ [c2[0], c2[1], c2[2]];
d: Triple ~ [c3[0], c3[1], c3[2]];
e: Triple ~ [d.x-p.x, d.y-p.y, d.z-p.z];
aa: REAL ~ a.x*a.x+a.y*a.y+a.z*a.z;
ab: REAL ~ a.x*b.x+a.y*b.y+a.z*b.z;
ac: REAL ~ a.x*c.x+a.y*c.y+a.z*c.z;
ae: REAL ~ a.x*e.x+a.y*e.y+a.z*e.z;
bb: REAL ~ b.x*b.x+b.y*b.y+b.z*b.z;
bc: REAL ~ b.x*c.x+b.y*c.y+b.z*c.z;
be: REAL ~ b.x*e.x+b.y*e.y+b.z*e.z;
cc: REAL ~ c.x*c.x+c.y*c.y+c.z*c.z;
ce: REAL ~ c.x*e.x+c.y*e.y+c.z*e.z;
poly: Polynomial.Ref ¬ ObtainQuintic[];
poly[0] ¬ ce;
poly[1] ¬ be+be+cc;
poly[2] ¬ 3.0*(ae+bc);
poly[3] ¬ 4.0*ac+bb+bb;
poly[4] ¬ 5.0*ab;
poly[5] ¬ 3.0*aa ;
realRoots ¬ CheapRealRoots[poly, 0.0, 1.0];
n.distance ¬ 100000.0;
FOR n:
NAT
IN [0..realRoots.nRoots)
DO
IF realRoots.realRoot[n] IN (0.0..1.0) THEN Test[realRoots.realRoot[n]];
ENDLOOP;
Test[0.0];
Test[1.0];
n.distance ¬ RealFns.SqRt[n.distance];
ReleaseQuintic[poly];
};
};
Nearest
Pair:
PUBLIC
PROC [
p: Pair, s: Spline, persp: BOOL ¬ FALSE, t0: REAL ¬ 0.0, t1: REAL ¬ 1.0]
RETURNS [nearSpline2d: NearSpline2d]
~ {
InnerNear:
PROC [t0, t1:
REAL, p0, p1: Pair, d0, d1:
REAL] ~ {
tt: REAL ¬ 0.5*(t0+t1);
pp: Pair ¬ PairPosition[s, tt, persp];
dd: REAL ¬ PairToPairDistance[[p.x, p.y], pp];
IF PairToPairDistance[p0, p1] < 0.01*dd
THEN {
nearSpline2d.point ¬ pp;
nearSpline2d.t ¬ tt;
nearSpline2d.distance ¬ dd;
RETURN;
};
IF d0 < d1
THEN InnerNear[t0, tt, p0, pp, d0, dd]
ELSE InnerNear[tt, t1, pp, p1, dd, d1];
};
p0: Pair ¬ PairPosition[s, t0, persp];
p1: Pair ¬ PairPosition[s, t1, persp];
InnerNear[t0, t1, p0, p1, PairToPairDistance[[p.x, p.y], p0], PairToPairDistance[[p.x, p.y], p1]];
NearestLine:
PUBLIC
PROC [
line: Ray, s: Spline, t0: REAL ¬ 0.0, t1: REAL ¬ 1.0, epsilon: REAL ¬ 0.01]
RETURNS [sPt, lPt: Triple, t, dist: REAL]
~ {
DistanceToLine:
PROC [p: Triple, l: Ray]
RETURNS [
REAL] ~ {
lp: Triple ¬ G3dVector.NearestToLine[l, p];
RETURN[G3dVector.Distance[p, lp]];
};
InnerNear:
PROC [t0, t1:
REAL, p0, p1: Triple, d0, d1:
REAL]
RETURNS [sPtI, lPtI: Triple, tI, distI: REAL] ~ {
tt: REAL ¬ 0.5*(t0+t1);
sp: Triple ¬ Position[s, tt];
lp: Triple ¬ G3dVector.NearestToLine[line, sp];
d: REAL ¬ G3dVector.Distance[lp, sp];
ds0: REAL ¬ G3dVector.Distance[p0, sp];
ds1: REAL ¬ G3dVector.Distance[p1, sp];
IF d < min THEN min ¬ d;
IF G3dVector.Distance[p0, p1] < MAX[0.0001, epsilon*d] THEN RETURN[sp, lp, tt, d];
IF ds0 < d0
AND ds1 < d1
THEN
SELECT
TRUE
FROM
MIN[d0, d, d1] > min => {sPtI ¬ sp; lPtI ¬ lp; tI ¬ tt; distI ¬ d;};
d0 < d1 => [sPtI, lPtI, tI, distI] ¬ InnerNear[t0, tt, p0, sp, d0, d];
ENDCASE => [sPtI, lPtI, tI, distI] ¬ InnerNear[tt, t1, sp, p1, d, d1]
ELSE {
tt0, tt1, dist0, dist1: REAL;
sPt0, lPt0, sPt1, lPt1: Triple;
[sPt0, lPt0, tt0, dist0] ¬ InnerNear[t0, tt, p0, sp, d0, d];
[sPt1, lPt1, tt1, dist1] ¬ InnerNear[tt, t1, sp, p1, d, d1];
IF dist0 < dist1
THEN RETURN[sPt0, lPt0, tt0, dist0]
ELSE RETURN[sPt1, lPt1, tt1, dist1];
};
};
min: REAL ¬ 10000.0;
p0: Triple ¬ Position[s, t0];
p1: Triple ¬ Position[s, t1];
[sPt, lPt, t, dist] ¬
InnerNear[t0, t1, p0, p1, DistanceToLine[p0, line], DistanceToLine[p1, line]];
Nearest
Spline:
PUBLIC
PROC [s1, s2: Spline, 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 s1 closest to s2 within t0, t1
tt: REAL ¬ 0.5*(t0+t1);
pp: Triple ¬ Position[s1, tt];
ds0: REAL ¬ G3dVector.Distance[p0, pp];
ds1: REAL ¬ G3dVector.Distance[p1, pp];
d: REAL ¬ NearestPoint[pp, s2, 0.0, 1.0].distance;
IF d < min THEN min ¬ d;
IF G3dVector.Distance[p0, p1] < MAX[0.0001, epsilon*d] THEN RETURN[tt, d];
IF ds0 < d0
AND ds1 < d1
THEN
SELECT
TRUE
FROM
MIN[d0, d, d1] > min => RETURN[tt, d];
d0 < d1 => {t, dd: REAL; [t, dd] ¬ InnerNear[t0, tt, p0, pp, d0, d]; RETURN[t, dd]};
ENDCASE => {t, dd: REAL; [t, dd] ¬ InnerNear[tt, t1, pp, p1, d, d1]; RETURN[t, dd]}
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[s1, 0.0];
p1: Triple ¬ Position[s1, 1.0];
d0: REAL ¬ NearestPoint[p0, s2, 0.0, 1.0, epsilon].distance;
d1: REAL ¬ NearestPoint[p1, s2, 0.0, 1.0, epsilon].distance;
t1 ¬ InnerNear[0.0, 1.0, p0, p1, d0, d1].tI;
t2 ¬ NearestPoint[Position[s1, t1], s2, 0.0, 1.0, epsilon].t;
PairToPairDistance:
PROC [p0, p1: Pair]
RETURNS [
REAL] ~ {
a: REAL ¬ p0.x-p1.x;
b: REAL ¬ p0.y-p1.y;
RETURN[RealFns.SqRt[a*a+b*b]];
};
PairPosition:
PROC [s: Spline, 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] ¬ s[3][i]; ENDLOOP
ELSE
IF t = 1.0
THEN
FOR i: NAT IN [0..nCoords) DO ret[i] ¬ s[0][i]+s[1][i]+s[2][i]+s[3][i]; ENDLOOP
ELSE {
t2 ¬ t*t;
t3 ¬ t*t2;
FOR i: NAT IN[0..nCoords) DO ret[i] ¬ t3*s[0][i]+t2*s[1][i]+t*s[2][i]+s[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 [s: Spline]
RETURNS [near: NearSpline] ~ {
max: REAL ¬ 0.0;
base: Triple ¬ Position[s, 0.0];
axis: Triple ¬ G3dVector.Sub[Position[s, 1.0], base];
FOR tt:
REAL ¬ 0.0, tt+0.01
WHILE tt <= 1.0
DO
ps: Triple ¬ Position[s, tt];
pl: Triple ¬ G3dVector.NearestToLine[[base, axis], ps];
d: REAL ¬ G3dVector.Distance[ps, pl];
IF d > max THEN {max ¬ d; near.point ¬ ps; near.t ¬ tt; near.distance ¬ d};
ENDLOOP;
};
Subdivision Procedures
split0: 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]]];
split1: 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 [s: Spline, out1, out2: Spline ¬
NIL]
RETURNS [s1, s2: Spline] ~ {
aa, bb, cc, dd, ee, ff: REAL;
s1 ¬ IF out1 # NIL THEN out1 ELSE NEW[SplineRep];
s2 ¬ IF out2 # NIL THEN out2 ELSE NEW[SplineRep];
FOR i:
NAT
IN[0..2]
DO
s1[0][i] ¬ aa ¬ (1.0/8.0)*s[0][i];
s1[1][i] ¬ bb ¬ (1.0/4.0)*s[1][i];
s1[2][i] ¬ cc ¬ (1.0/2.0)*s[2][i];
s1[3][i] ¬ dd ¬ s[3][i];
ee ¬ 3.0*aa;
s2[0][i] ¬ aa;
s2[1][i] ¬ ff ¬ ee+bb;
s2[2][i] ¬ ff+bb+cc;
s2[3][i] ¬ aa+bb+cc+dd;
ENDLOOP;
};
SplitBezier:
PUBLIC PROC [b: Bezier]
RETURNS [b1, b2: Bezier] ~ {
There might be a faster to way to weight all these points:
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 [s: Spline, t:
REAL]
RETURNS [s1, s2: Spline] ~ {
RETURN[Reparameterize[s, 0.0, t], Reparameterize[s, t, 1.0]];
};
Reparameterize:
PUBLIC
PROC
[in:
Spline,
t0,
t1:
REAL,
out:
Spline
¬
NIL]
RETURNS [Spline] ~ {
dt1: REAL ¬ t1-t0;
dt2: REAL ¬ dt1*dt1;
t02: REAL ¬ t0*t0;
IF out = NIL THEN out ¬ NEW[SplineRep];
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[1][i] ¬ dt2*(x3*t0+y);
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];
};
Special Purpose
SlowInOut:
PUBLIC PROC [value0, value1, t:
REAL]
RETURNS [
REAL] ~ {
t2: REAL ¬ t*t;
t3: REAL ¬ t*t2;
RETURN[value0+(3.0*t2-t3-t3)*(value1-value0)];
};
TameType:
TYPE ~ G3dSpline.TameType;
Tame:
PUBLIC
PROC [in: Spline, tameType: TameType, out: Spline ¬
NIL]
RETURNS [Spline] ~ {
v0, v2, l: REAL;
d0, d2, g3d, s0, s2, r0, r2: Triple;
b: Bezier ¬ BezierFromSpline[in]; -- convert to bezier
d0 ¬ G3dVector.Sub[b.b1, b.b0];
d2 ¬ G3dVector.Sub[b.b3, b.b2];
g3d ¬ G3dVector.Sub[b.b3, b.b0];
l ¬ G3dVector.Length[g3d];
v0 ¬ l*G3dVector.Dot[d0, g3d];
v2 ¬ l*G3dVector.Dot[d2, g3d];
s0 ¬ G3dVector.Mul[g3d, v0]; -- projection of d0 onto g3d
s2 ¬ G3dVector.Mul[g3d, v2]; -- projection of d2 onto g3d
r0 ¬ G3dVector.Sub[d0, s0];
r2 ¬ G3dVector.Sub[d2, s2];
IF G3dVector.Dot[g3d, d0] < 0.0
THEN {
-- bulge at start
d0 ¬ r0;
s0 ¬ [0.0, 0.0, 0.0];
};
IF G3dVector.Dot[g3d, d2] < 0.0
THEN {
-- bulge at end
d2 ¬ r2;
s2 ¬ [0.0, 0.0, 0.0];
};
IF G3dVector.Dot[r0, r2] > 0.0
THEN {
-- sides point oppositely
IF G3dVector.Square[r0] > G3dVector.Square[r2]
THEN d2 ¬ s2
ELSE d0 ¬ s0;
};
IF v0+v2 > l
THEN {
-- sides too long
scale: REAL ¬ l/(v0+v2);
d0 ¬ G3dVector.Mul[d0, scale];
d2 ¬ G3dVector.Mul[d2, scale];
};
b.b1 ¬ G3dVector.Add[b.b0, d0];
b.b2 ¬ G3dVector.Sub[b.b3, d2];
RETURN [SplineFromBezier[b, out]];
};
SplineFromPointsAndNormals:
PUBLIC PROC [
point0, point1, normal0, normal1: Triple,
tension: REAL ¬ 1.0,
spline: Spline ¬ NIL]
RETURNS [Spline]
~ {
GetTangent:
PROC [point, normal: Triple]
RETURNS [Triple] ~ {
dProj: Triple ¬ G3dVector.Project[d, normal];
tangent: Triple ¬ G3dVector.Sub[d, dProj];
normalLen: REAL ¬ G3dVector.Length[normal];
cos: REAL ¬ ABS[G3dVector.Dot[normal, d]/(dLen*normalLen)];
radius: REAL ¬ dLen/cos;
lLen: REAL ¬ 4.0*(radius-RealFns.SqRt[radius*radius-dSqLen]);
q: Triple ¬ G3dVector.Sub[tangent, G3dVector.Project[tangent, d]];
scale: REAL ¬ tension*lLen/G3dVector.Length[q];
RETURN[G3dVector.Mul[tangent, scale]];
};
d: Triple ¬ G3dVector.Mul[G3dVector.Sub[point1, point0], 0.5];
dSqLen: REAL ¬ G3dVector.Square[d];
dLen: REAL ¬ RealFns.SqRt[dSqLen];
tangent0, tangent1: Triple ¬ [0.0, 0.0, 0.0];
tangent0 ¬ GetTangent[point0, normal0 ! Real.RealException => CONTINUE];
tangent1 ¬ GetTangent[point1, normal1 ! Real.RealException => CONTINUE];
RETURN[SplineFromHermite[[point0, point1, tangent0, tangent1], spline]];
};
IntersectSplineAndPlane:
PUBLIC PROC [spline: Spline, plane: Quad]
RETURNS [ret: SplinePlaneIntersect]
~ {
a: Triple ~ [spline[0][0], spline[0][1], spline[0][2]];
b: Triple ~ [spline[1][0], spline[1][1], spline[1][2]];
c: Triple ~ [spline[2][0], spline[2][1], spline[2][2]];
d: Triple ~ [spline[3][0], spline[3][1], spline[3][2]];
pln: Triple ~ [plane.x, plane.y, plane.z];
realRoots: Polynomial.ShortRealRootRec;
poly: Polynomial.Ref ¬ ObtainCubic[];
poly[3] ¬ G3dVector.Dot[a, pln];
poly[2] ¬ G3dVector.Dot[b, pln];
poly[1] ¬ G3dVector.Dot[c, pln];
poly[0] ¬ G3dVector.Dot[d, pln]+plane.w;
realRoots ¬ Polynomial.CheapRealRoots[poly];
FOR n:
NAT
IN [0..realRoots.nRoots)
DO
IF realRoots.realRoot[n]
IN [0.0..1.0]
THEN {
ret.roots[ret.nRoots] ¬ realRoots.realRoot[n];
ret.nRoots ¬ ret.nRoots+1;
};
ENDLOOP;
ReleaseCubic[poly];
};
Miscellaneous procedures
nScratchCubics: NAT ~ 6;
scratchCubics:
ARRAY [0..nScratchCubics)
OF Polynomial.Ref ¬
ALL[
NIL];
ObtainCubic:
ENTRY
PROC
RETURNS [Polynomial.Ref] ~ {
FOR i:
NAT
IN [0..nScratchCubics)
DO
ref: Polynomial.Ref ~ scratchCubics[i];
IF ref = NIL THEN LOOP;
scratchCubics[i] ¬ NIL;
RETURN[ref];
ENDLOOP;
RETURN[Polynomial.Create[3]];
};
ReleaseCubic:
ENTRY
PROC [ref: Polynomial.Ref] ~ {
FOR i:
NAT
IN [0..nScratchCubics)
DO
IF scratchCubics[i] # NIL THEN LOOP;
scratchCubics[i] ¬ ref;
EXIT;
ENDLOOP;
};
nScratchQuintics: NAT ~ 6;
scratchQuintics:
ARRAY [0..nScratchQuintics)
OF Polynomial.Ref ¬
ALL[
NIL];
ObtainQuintic:
ENTRY
PROC
RETURNS [Polynomial.Ref] ~ {
FOR i:
NAT
IN [0..nScratchQuintics)
DO
ref: Polynomial.Ref ~ scratchQuintics[i];
IF ref = NIL THEN LOOP;
scratchQuintics[i] ¬ NIL;
RETURN[ref];
ENDLOOP;
RETURN[Polynomial.Create[5]];
};
ReleaseQuintic:
ENTRY
PROC [ref: Polynomial.Ref] ~ {
FOR i:
NAT
IN [0..nScratchQuintics)
DO
IF scratchQuintics[i] # NIL THEN LOOP;
scratchQuintics[i] ¬ ref;
EXIT;
ENDLOOP;
};
GetA: PUBLIC PROC [s: Spline] RETURNS [Triple] ~ {RETURN[[s[0][0], s[0][1], s[0][2]]]};
GetB: PUBLIC PROC [s: Spline] RETURNS [Triple] ~ {RETURN[[s[1][0], s[1][1], s[1][2]]]};
GetC: PUBLIC PROC [s: Spline] RETURNS [Triple] ~ {RETURN[[s[2][0], s[2][1], s[2][2]]]};
GetD:
PUBLIC PROC [s: Spline]
RETURNS [Triple] ~ {
RETURN[[s[3][0], s[3][1], s[3][2]]]};
Same:
PUBLIC PROC [s1, s2: Spline]
RETURNS [
BOOL] ~ {
FOR i:
NAT
IN[0..3]
DO
FOR j: NAT IN[0..3] DO IF s1[i][j] # s2[i][j] THEN RETURN[FALSE]; ENDLOOP;
ENDLOOP;
RETURN[TRUE];
};
Notes
SplineFromPointsAndNormals:
PUBLIC PROC [
point0, point1, normal0, normal1: Triple,
tension: REAL ¬ 1.0,
spline: Spline ¬ NIL]
RETURNS [Spline]
~ {
Ortho:
PROC [normal, chord: Triple]
RETURNS [Triple] ~ {
orthoLength, normalLength: REAL ← G3dVector.Length[normal];
chordProjectedOnNormal: Triple ¬ G3dVector.Project[chord, normal];
ortho: Triple ¬ G3dVector.Sub[chord, chordProjectedOnNormal];
normalLen: REAL ¬ G3dVector.Length[normal];
cos: REAL ¬ G3dVector.Dot[normal, chord]/(chordLen*normalLen);
radius: REAL ¬ chordLen/cos;
l: REAL ¬ (4.0/3.0)*(radius-RealFns.SqRt[radius*radius-chordSqLen]);
q: Triple ¬ G3dVector.Sub[ortho, G3dVector.Project[ortho, chord]];
scale: REAL ¬ l/G3dVector.Length[q];
RETURN[G3dVector.Mul[ortho, scale]];
orthoLength ← G3dVector.Length[ortho];
IF normalLength # 0.0 AND orthoLength # 0.0
THEN ortho ← G3dVector.Mul[ortho, 0.5/(normalLength*orthoLength)];
};
chord: Triple ¬ G3dVector.Sub[point1, point0];
chordSqLen: REAL ¬ G3dVector.SquareLength[chord];
chordLen: REAL ¬ RealFns.SqRt[chordSqLen];
thriceChordLength: REAL ← 3.0*G3dVector.Length[chord];
tangent0: Triple ¬ Ortho[normal0, chord];
tangent1: Triple ¬ Ortho[normal1, chord];
tangent0Length: REAL ← G3dVector.Length[tangent0];
tangent1Length: REAL ← G3dVector.Length[tangent1];
IF G3dVector.Dot[normal0, normal1] < 0.0
THEN {
IF
ABS[G3dVector.Dot[G3dVector.Normalize[normal0], chord]] <
ABS[G3dVector.Dot[G3dVector.Normalize[normal1], chord]]
THEN tangent1 ¬ G3dVector.Negate[tangent1]
ELSE tangent0 ¬ G3dVector.Negate[tangent0];
};
IF tangent0Length > thriceChordLength
THEN tangent0 ← G3dVector.Mul[tangent0, thriceChordLength/tangent0Length];
IF tangent1Length > thriceChordLength
THEN tangent1 ← G3dVector.Mul[tangent1, thriceChordLength/tangent1Length];
IF tension # 1.0
THEN {
tangent0 ¬ G3dVector.Mul[tangent0, tension];
tangent1 ¬ G3dVector.Mul[tangent1, tension];
};
RETURN[SplineFromHermite[[point0, point1, tangent0, tangent1], spline]];
};
Curvature: PUBLIC PROC [c: Spline, t: REAL] RETURNS [Triple] ~ {
1 sqrt, 3 divides, 8 multiplies, 7 adds but this gives wrong length
tan: Triple ← Tangent[c, t];
IF tan = [0.0, 0.0, 0.0] THEN RETURN[tan] ELSE {
acc: Triple ← Acceleration[c, t];
u: REAL ← tan.x*tan.x+tan.y*tan.y+tan.z*tan.z;
RETURN G3dVector.Div[
G3dVector.Sub[
G3dVector.Mul[acc, u],
G3dVector.Mul[tan, G3dVector.Dot[acc, tan]]],
Real.SqRt[u*u*u]];
};
};
TPosFromSpline: PUBLIC PROC [c: Spline, 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];
TriSequenceRep: TYPE ~ RECORD [element: SEQUENCE length: NAT OF Tri];
n, nTris: NAT ← 0;
tris: REF TriSequenceRep ← NEW[TriSequenceRep[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[G3dVector.Square[G3dVector.Cross[G3dVector.Sub[p1, p0], G3dVector.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[TPosSequenceRep[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];
};
NearestPoint: PUBLIC PROC [
p: Triple, c: Spline, t0: REAL ← 0.0, t1: REAL ← 1.0, epsilon: REAL ← 0.01]
RETURNS [cPt: Triple, t, dist: REAL]
~ {
For highest precision, set epsilon to 0.0 to use PreciseNearestPoint[].
Near: TYPE ~ RECORD [p: Triple, t, d: REAL];
InnerNear: PROC [n0, n1: Near, level: NAT] RETURNS [Near] ~ {
n: Near;
n.d ← Square[p, n.p ← Position[c, n.t ← 0.5*(n0.t+n1.t)]];
min ← MIN[min, n.d];
IF Square[n0.p, n1.p] < MAX[0.0001, epsilon*n.d] THEN RETURN[n];
IF --Square[n0.p, n.p] < n0.d AND Square[n1.p, n.p] < n1.d -- level > 0
THEN RETURN[SELECT TRUE FROM
MIN[n0.d, n.d, n1.d] > min => n, -- no need recurse further this segment
n0.d < n1.d => InnerNear[n0, n, level+1], -- an easy test
ENDCASE => InnerNear[n, n1, level+1]] -- an easy test
ELSE {
nA: Near ← InnerNear[n0, n, level+1]; -- a hard test
nB: Near ← InnerNear[n, n1, level+1]; -- a hard test
RETURN[IF nA.d < nB.d THEN nA ELSE nB];
};
};
n: Near;
min: REAL ← 10000.0;
p0: Triple ← Position[c, t0];
p1: Triple ← Position[c, t1];
IF epsilon = 0.0
THEN [n.p, n.t, n.d] ← PreciseNearestPoint[p, c, t0, t1]
ELSE {
n ← InnerNear[[p0, t0, Square[p, p0]], [p1, t1, Square[p, p1]], 0];
n.d ← Real.SqRt[n.d];
};
RETURN[n.p, n.t, n.d];