Interpolation Procedures
InterpolateCyclic:
PUBLIC
PROC [knots: TripleSequence, tension:
REAL ← 1.0]
RETURNS [coeffs: CoeffsSequence] ~ {
From /Cedar/CedarChest6.1/CubicSplinePackage/RegularALSplineImpl.mesa by Baudelaire
and Stone, in which coeffs.t3, .t2, .t1, and .t0 refer to the 3rd, 2nd and 1st derivatives,
and position of the curve. In Spline3d, these correspond to coeffs[0], [1], [2], and [3].
IF
NOT Vector3d.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];
};
coeffs ← NEW[CoeffsSequenceRep[knots.length]];
coeffs.length ← knots.length;
FOR n: NAT IN [0..coeffs.length) DO coeffs[n] ← NEW[CoeffsRep]; ENDLOOP;
SELECT knots.length
FROM
< 1 => coeffs ← NIL;
< 3 => {
coeffs[0][3] ← [knots[0].x, knots[0].y, knots[0].z, 1.0];
IF knots.length = 2
THEN {
dif: Triple ~ Vector3d.Sub[knots[1], knots[0]];
coeffs[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
coeffs[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: 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;
ENDLOOP;
};
Interpolate:
PUBLIC PROC [knots: TripleSequence, tan0, tan1: Triple ← [0.0, 0.0, 0.0], tension:
REAL ← 1.0, c: CoeffsSequence ←
NIL]
RETURNS [CoeffsSequence] ~ {
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 ← Vector3d.Distance[p0, p1];
IF c = NIL OR c.maxLength < 1 THEN c ← NEW[CoeffsSequenceRep[1]];
c.length ← 1;
c[0] ← CoeffsFromHermite[[p0, p1, Vector3d.Mul[tan0, len], Vector3d.Mul[tan1, len]]];
RETURN[c];
};
ENDCASE => {
chords: RealSequence ← ComputeChords[knots, NIL];
tangents: TripleSequence ← ComputeTangents[knots, chords, NIL, tan0, tan1];
RETURN[ComputeCoeffs[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] ← Vector3d.Distance[knots[n+1], knots[n]]) = 0.0 THEN chords[n] ← 1.0;
ENDLOOP;
IF (chords[max] ← Vector3d.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 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: 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] ← 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: TripleSequence,
tangents: TripleSequence,
tension: REAL,
chords: RealSequence,
in: CoeffsSequence ← NIL]
RETURNS [CoeffsSequence]
~ {
nCoeffs: NAT ← knots.length-1;
IF in = NIL OR in.maxLength < nCoeffs THEN in ← NEW[CoeffsSequenceRep[nCoeffs]];
in.length ← 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
in[m] ← CoeffsFromHermite[
[knots[m],
knots[m+1],
Vector3d.Mul[tangents[m], chords[m]],
Vector3d.Mul[tangents[m+1], chords[m]]],
in[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 in[m] = NIL THEN in[m] ← NEW[CoeffsRep];
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
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
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:
NAT, 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[TripleSequenceRep[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.SquareLength[axis];
t ← MIN[1.0, MAX[0.0, t]];
};
CurvatureMag:
PUBLIC
PROC [c: Coeffs, t:
REAL]
RETURNS [
REAL] ~ {
tan: Triple ← Tangent[c, t];
acc: Triple ← Acceleration[c, t];
l: REAL ← Vector3d.Length[tan];
RETURN[IF l = 0.0 THEN 0.0 ELSE Vector3d.Length[Vector3d.Cross[acc, tan]]/(l*l*l)];
};
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];
length: REAL ← Vector3d.Length[tan];
length2: REAL ← length*length;
RETURN Vector3d.Div[Vector3d.Cross[Vector3d.Cross[tan, acc], tan], length2*length2];
};
};
RefVec:
PUBLIC PROC [c: Coeffs, t:
REAL]
RETURNS [Triple] ~ {
ref: Triple ← Curvature[c, t];
IF Vector3d.SquareLength[ref] < 0.9 THEN ref ← Vector3d.Ortho[Tangent[c, t]];
RETURN[ref];
};
IsStraight:
PUBLIC PROC [c: Coeffs, epsilon:
REAL ← 0.01]
RETURNS [
BOOL] ~ {
b: Bezier ~ BezierFromCoeffs[c];
RETURN[
Vector3d.Collinear[b.b0, b.b1, b.b2, epsilon] AND
Vector3d.Collinear[b.b1, b.b2, b.b3, epsilon]];
};
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.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;
RETURN[sum];
};
ConvexHullArea:
PUBLIC PROC [b: Bezier]
RETURNS [
REAL] ~ {
TwiceTriArea:
PROC [p0, p1, p2: Triple]
RETURNS [
REAL] ~ {
RETURN[Vector3d.Length[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.Length[d0]+Vector3d.Length[d1]+Vector3d.Length[d2]+Vector3d.Length[d3]];
};
FlatBezier:
PUBLIC PROC [b: Bezier, epsilon:
REAL ← 0.05]
RETURNS [
BOOL] ~ {
d3length: 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 (d3length ← Vector3d.Length[d3]) < 0.000001 THEN RETURN[TRUE];
RETURN[
(Vector3d.Length[d0]+Vector3d.Length[d1]+Vector3d.Length[d2])/Vector3d.Length[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
Modified Polynomial.CheapRealRoots by Eric Bier
Limits consideration of the solutions to the domain [loo..hi].
ShortRealRootRec: TYPE = Polynomial.ShortRealRootRec;
CheapRealRoots:
PROC [poly: Polynomial.Ref, lo, hi:
REAL]
RETURNS [roots: ShortRealRootRec] = {
d: NAT ← Polynomial.Degree[poly];
roots.nRoots ← 0;
IF d<=1
THEN {
IF d=1 THEN {roots.nRoots ← 1; roots.realRoot[0] ← -poly[0]/poly[1]}
}
ELSE {
savedCoeff: ARRAY [0..5] OF REAL;
FOR i: NAT IN [0..d] DO savedCoeff[i] ← poly[i] ENDLOOP;
Polynomial.Differentiate[poly];
BEGIN
extrema: ShortRealRootRec ← CheapRealRoots[poly, lo, hi];
x: REAL;
FOR i: NAT IN [0..d] DO poly[i] ← savedCoeff[i] ENDLOOP;
IF extrema.nRoots>0
THEN {
x ← RootBetween[poly, 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[poly, 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[poly, extrema.realRoot[extrema.nRoots-1], hi];
IF x <= hi THEN {roots.realRoot[roots.nRoots] ← x; roots.nRoots ← roots.nRoots + 1}
}
ELSE {
x ← RootBetween[poly, lo, hi];
IF x <= hi THEN {roots.realRoot[0] ← x; roots.nRoots ← 1}
};
END
};
};
RootBetween:
PROC [poly: Polynomial.Ref, x0, x1:
REAL]
RETURNS [x: REAL] =
BEGIN
debug xi: ARRAY [0..50) OF REAL;
debug i: NAT ← 0;
OPEN Polynomial;
y0: REAL ← Polynomial.Eval[poly,x0];
y1: REAL ← Polynomial.Eval[poly,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];
NewtonStep:
PROC [x:
REAL]
RETURNS [newx:
REAL] =
BEGIN
y: REAL ← Polynomial.Eval[poly, x];
deriv: REAL ← Polynomial.EvalDeriv[poly, 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;
END;
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
BEGIN
x0 ← MIN[xx0,xx1];
x1 ← MAX[xx0,xx1];
END;
y0 ← Polynomial.Eval[poly,x0];
y1 ← Polynomial.Eval[poly,x1];
THROUGH [0..500)
DO
y: REAL ← Polynomial.Eval[poly, x ← (x0+x1)/2];
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;
END;
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 [c: Coeffs]
RETURNS [t:
REAL] ~ {
InflectionTest:
PROC [t:
REAL]
RETURNS [
BOOL] ~ {
RETURN[t IN [0.0..1.0] AND CurvatureMag[c, 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];
};
coeffs: Coeffs ← c;
IF IsStraight[coeffs] THEN RETURN[-2.0];
{
a: Triple ~ GetA[coeffs];
b: Triple ~ GetB[coeffs];
c: Triple ~ GetC[coeffs];
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, c: Coeffs, t0: REAL ← 0.0, t1: REAL ← 1.0, epsilon: REAL ← 0.01]
RETURNS [near3d: Near3d]
~ {
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[c, t];
e.v ← Velocity[c, t];
e.dot ← Vector3d.Dot[Vector3d.Normalize[e.v], Vector3d.Normalize[Vector3d.Sub[p, e.p]]];
};
NearFromT:
PROC [t:
REAL]
RETURNS [Near3d] ~ {
q: Triple ~ Position[c, t];
RETURN[[q, t, Vector3d.Distance[p, q]]];
};
NearFromEval:
PROC [e: Eval]
RETURNS [Near3d] ~ {
RETURN[[e.p, e.t, Vector3d.Distance[p, e.p]]];
};
InnerNear:
PROC [e0, e1: Eval, t0, t1:
REAL]
RETURNS [Near3d] ~ {
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: Near3d;
IF epsilon = 0.0
THEN n ← PreciseNearestPoint[p, c]
ELSE {
TDivide: PROC RETURNS [t: REAL] ~ {t ← InflectionPoint[c]; IF t < 0.0 THEN t ← 0.5};
t: REAL ~ 0.5; -- TDivide[];
e: Eval ~ EvalCurve[t];
near0: Near3d ~ NearFromT[0.0];
near1: Near3d ~ NearFromT[1.0];
nearA: Near3d ~ InnerNear[EvalCurve[t0], e, 1.0, t];
nearB: Near3d ~ 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, c: Coeffs]
RETURNS [n: Near3d] ~ {
Test:
PROC [testT:
REAL] ~ {
testP: Triple ← Position[coeffs, testT];
testDistance: REAL ← Square[testP, p];
IF testDistance < n.distance
THEN {
n.point ← testP;
n.t ← testT;
n.distance ← testDistance;
};
};
coeffs: Coeffs ← c;
{
Dot product of tangent(t) with [q-position(t)]:
a: Triple ~ [coeffs[0][0], coeffs[0][1], coeffs[0][2]];
b: Triple ~ [coeffs[1][0], coeffs[1][1], coeffs[1][2]];
c: Triple ~ [coeffs[2][0], coeffs[2][1], coeffs[2][2]];
d: Triple ~ [coeffs[3][0], coeffs[3][1], coeffs[3][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 ←
Polynomial.Quintic[[ce, be+be+cc, 3.0*(ae+bc), 4.0*ac+bb+bb, 5.0*ab, 3.0*aa]];
realRoots: Polynomial.ShortRealRootRec ← 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 ← Real.SqRt[n.distance];
};
};
Nearest
Pair:
PUBLIC
PROC [
p: Pair, c: Coeffs, persp: BOOL ← FALSE, t0: REAL ← 0.0, t1: REAL ← 1.0]
RETURNS [near2d: Near2d]
~ {
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 {
near2d.point ← pp;
near2d.t ← tt;
near2d.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[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]];
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]
~ {
DistanceToLine:
PROC [p: Triple, l: Line]
RETURNS [
REAL] ~ {
lp: Triple ← Vector3d.LinePoint[p, l];
RETURN[Vector3d.Distance[p, lp]];
};
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]];
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].distance;
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].distance;
d1: REAL ← NearestPoint[p1, c2, 0.0, 1.0, epsilon].distance;
t1 ← InnerNear[0.0, 1.0, p0, p1, d0, d1].tI;
t2 ← NearestPoint[Position[c1, t1], c2, 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[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 [near3d: Near3d] ~ {
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; near3d.point ← pc; near3d.t ← tt; near3d.distance ← 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] ~ {
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 [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
GetA: PUBLIC PROC [c: Coeffs] RETURNS [Triple] ~ {RETURN[[c[0][0], c[0][1], c[0][2]]]};
GetB: PUBLIC PROC [c: Coeffs] RETURNS [Triple] ~ {RETURN[[c[1][0], c[1][1], c[1][2]]]};
GetC: PUBLIC PROC [c: Coeffs] RETURNS [Triple] ~ {RETURN[[c[2][0], c[2][1], c[2][2]]]};
GetD: PUBLIC PROC [c: Coeffs] RETURNS [Triple] ~ {RETURN[[c[3][0], c[3][1], c[3][2]]]};
CopyCoeffs:
PUBLIC PROC [in: Coeffs, out: Coeffs ←
NIL]
RETURNS [Coeffs] ~ {
RETURN[Matrix3d.CopyMatrix[in, out]];
};
CopyCoeffsSequence:
PUBLIC
PROC [in: CoeffsSequence]
RETURNS [CoeffsSequence] ~ {
copy: CoeffsSequence ← NIL;
IF in #
NIL
THEN {
copy ← NEW[CoeffsSequenceRep[in.maxLength]];
FOR n: NAT IN [0..in.maxLength) DO copy[n] ← Matrix3d.CopyMatrix[in[n]]; ENDLOOP;
};
RETURN[copy];
};
Tame:
PUBLIC
PROC [in: Coeffs, out: Coeffs ←
NIL]
RETURNS [Coeffs] ~ {
v0, v2, l: 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];
l ← Vector3d.Length[d3];
v0 ← l*Vector3d.Dot[d0, d3];
v2 ← l*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.SquareLength[r0] > Vector3d.SquareLength[r2]
THEN d2 ← s2
ELSE d0 ← s0;
};
IF v0+v2 > l
THEN {
-- sides too long
scale: REAL ← l/(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];
};
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];
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[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[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: Coeffs, 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];