<<>> <> <> <> <> DIRECTORY IO, G2dSpline, G3dBasic, G3dMatrix, G3dSpline, G3dVector, Polynomial, Real, RealFns, Rope; G3dSplineImpl: CEDAR MONITOR IMPORTS G3dMatrix, G3dVector, Polynomial, Real, RealFns EXPORTS G3dSpline ~ BEGIN <> 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; <> InterpolateCyclic: PUBLIC PROC [knots: TripleSequence, tension: REAL ¬ 1.0] RETURNS [spline: SplineSequence] ~ { <> <> <> 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]]; <> FOR i: NAT IN [0..knots.length) DO spline[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])); <> 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; <> 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; <> 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; <> 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] ~ { <> 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 <> <<[knots[m],>> << knots[m+1],>> << G3dVector.Mul[tangents[m], chords[m]],>> << G3dVector.Mul[tangents[m+1], chords[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]; }; <> 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] ~ { <> 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]]; }; <> 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]; <> <> << [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[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]; <> 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]]; }; <> 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] ~ { <> 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)]]]]]; }; <> <> <> <> ShortRealRootRec: TYPE = Polynomial.ShortRealRootRec; CheapRealRoots: PROC [poly: Polynomial.Ref, lo, hi: REAL] RETURNS [roots: ShortRealRootRec] ~ { RootBetween: PROC [x0, x1: REAL] RETURNS [x: REAL] ~ { <> <> y0: REAL ¬ Eval[x0]; y1: REAL ¬ Eval[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 { 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]; <> 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, 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]; { <> 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]; }; }; NearestPair: 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]]; }; NearestSpline: PUBLIC PROC [s1, s2: Spline, 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[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; }; <> 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] ~ { <> 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] ¬ 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]; }; <> CopySplineSequence: PUBLIC PROC [splines: SplineSequence] RETURNS [copy: SplineSequence] ~ { IF splines # NIL THEN { copy ¬ NEW[SplineSequenceRep[splines.length]]; copy.length ¬ splines.length; FOR n: NAT IN [0..splines.length) DO copy[n] ¬ G3dMatrix.CopyMatrix[splines[n]]; ENDLOOP; }; }; AddToSplineSequence: PUBLIC PROC [splines: SplineSequence, spline: Spline] RETURNS [SplineSequence] ~ { IF splines = NIL THEN splines ¬ NEW[SplineSequenceRep[1]]; IF splines.length = splines.maxLength THEN splines ¬ LengthenSplineSequence[splines]; splines[splines.length] ¬ spline; splines.length ¬ splines.length+1; RETURN[splines]; }; LengthenSplineSequence: PUBLIC PROC [splines: SplineSequence, amount: REAL ¬ 1.3] RETURNS [new: SplineSequence] ~ { newLength: NAT ¬ MAX[Real.Ceiling[amount*splines.maxLength], 3]; new ¬ NEW[SplineSequenceRep[newLength]]; FOR i: NAT IN [0..splines.length) DO new[i] ¬ splines[i]; ENDLOOP; new.length ¬ splines.length; }; <> 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]; }; <> 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]; }; <> <> <> <<>> END. .. <> SplineFromPointsAndNormals: PUBLIC PROC [ point0, point1, normal0, normal1: Triple, tension: REAL ¬ 1.0, spline: Spline ¬ NIL] RETURNS [Spline] ~ { Ortho: PROC [normal, chord: Triple] RETURNS [Triple] ~ { <> 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]]; <> <> <> }; chord: Triple ¬ G3dVector.Sub[point1, point0]; chordSqLen: REAL ¬ G3dVector.SquareLength[chord]; chordLen: REAL ¬ RealFns.SqRt[chordSqLen]; <> tangent0: Triple ¬ Ortho[normal0, chord]; tangent1: Triple ¬ Ortho[normal1, chord]; <> <> 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]; }; < thriceChordLength>> <> < thriceChordLength>> <> IF tension # 1.0 THEN { tangent0 ¬ G3dVector.Mul[tangent0, tension]; tangent1 ¬ G3dVector.Mul[tangent1, tension]; }; RETURN[SplineFromHermite[[point0, point1, tangent0, tangent1], spline]]; }; <> <<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>> <> <> <> <> <<};>> <<};>> <> <> <> <> <> <> <> <> <> <<};>> <> <<};>> <<>>