<> <> <> DIRECTORY IO, Matrix3d, Polynomial, Real, RealFns, Rope, Spline3d, Vector3d; Spline3dImpl: CEDAR PROGRAM IMPORTS Matrix3d, Polynomial, Real, RealFns, Vector3d EXPORTS Spline3d ~ BEGIN OPEN Spline3d; <> InterpolateCyclic: PUBLIC PROC [knots: TripleSequence, tension: REAL _ 1.0] RETURNS [coeffs: CoeffsSequence] ~ { <> <> <> 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]]; <> FOR i: NAT IN [0..knots.length) DO coeffs[i][3] _ [knots[i].x, knots[i].y, knots[i].z, 1.0]; ENDLOOP; <> h _ ComputeChords[knots, h]; <> 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; <> FOR i: NAT IN [0..3) DO p2: REAL; <> 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; <> 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; <> 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])); <<>> <> 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; <> 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; <<>> <> 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; <> 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] ~ { <> 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 <> <<[knots[m],>> << knots[m+1],>> << Vector3d.Mul[tangents[m], chords[m]],>> << Vector3d.Mul[tangents[m+1], chords[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]; }; <> 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]]; }; <> 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]]; }; <> 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] ~ { <> 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)]]]]; }; <> <> <> 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 <> <> OPEN Polynomial; y0: REAL _ Polynomial.Eval[poly,x0]; y1: REAL _ Polynomial.Eval[poly,x1]; xx: REAL; xx0: REAL _ IF y0yy0 THEN {xx0 _ x; yy0 _ y}}; IF y>0 THEN {IF y 0 AND y1 > 0) OR (y0 < 0 AND y1 < 0) THEN RETURN [x1+100]; xx _ x _ (x0+x1)/2; <> WHILE x IN [x0..x1] DO newx:REAL _ NewtonStep[x]; IF x=newx THEN RETURN; x _ NewtonStep[newx]; xx _ NewtonStep[xx]; <> IF xx=x THEN EXIT; ENDLOOP; <> <> 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]; <> 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; }; }; NearestPoint: 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; { <> 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]; }; }; NearestPair: 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]]; }; NearestSpline: PUBLIC PROC [c1, c2: Coeffs, epsilon: REAL _ 0.01] RETURNS [t1, t2: REAL] ~ { <> InnerNear: PROC [t0, t1: REAL, p0, p1: Triple, d0, d1: REAL] RETURNS [tI, dI: REAL] ~ { <> 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; }; <> 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]; }; <> 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]; }; END. .. References: /Cedar/CedarChest6.0/CubicSplinePackage/*.mesa Rogers and Adams: Mathematical Elements for Computer Graphics Notes: <> <<1 sqrt, 3 divides, 8 multiplies, 7 adds but this gives wrong length>> <> <> <> <> <> <> <> <> <> <<};>> <<};>> <<>> <> <> <> <> <> <> <> < max THEN {max _ tris[i].area; n _ i} ENDLOOP;>> <<};>> <> <> <> <<};>> <> <> <> <> <<};>> <> <> <> <> <> <> <> <> <> <> <> <> <> <> <> <> <> <> <> <<};>> <<>> <> <> <> <<~ {>> <> <> <> <> <> <> <> < 0>> <> < min => n, -- no need recurse further this segment>> < InnerNear[n0, n, level+1], -- an easy test>> < InnerNear[n, n1, level+1]] -- an easy test>> <> <> <> <> <<};>> <<};>> <> <> <> <> <> <> <> <> <> <<};>> <> <<};>> <<>> <<>>