<<>> <> <> <> <> <> DIRECTORY G2dBasic, G3dBasic, G3dMatrix, G3dVector, Real, RealFns; G3dVectorImpl: CEDAR PROGRAM IMPORTS G2dBasic, G3dMatrix, RealFns EXPORTS G3dVector ~ BEGIN <> Box: TYPE ~ G3dBasic.Box; Pair: TYPE ~ G3dBasic.Pair; Triple: TYPE ~ G3dBasic.Triple; Quad: TYPE ~ G3dBasic.Quad; Ray: TYPE ~ G3dBasic.Ray; TripleSequence: TYPE ~ G3dBasic.TripleSequence; Matrix: TYPE ~ G3dMatrix.Matrix; NearSegment: TYPE ~ G3dVector.NearSegment; origin: Triple ~ G3dBasic.origin; <> Null: PUBLIC PROC [v: Triple] RETURNS [BOOL] ~ { RETURN[v.x = 0.0 AND v.y = 0.0 AND v.z = 0.0]; }; Negate: PUBLIC PROC [v: Triple] RETURNS [Triple] ~ { RETURN[[-v.x, -v.y, -v.z]]; }; Unit: PUBLIC PROC [v: Triple] RETURNS [ret: Triple] ~ { m: REAL ¬ v.x*v.x+v.y*v.y+v.z*v.z; SELECT m FROM 0.0, 1.0 => RETURN[v]; ENDCASE => { m ¬ RealFns.SqRt[m]; RETURN[[v.x/m, v.y/m, v.z/m]]; }; }; Mul: PUBLIC PROC [v: Triple, s: REAL] RETURNS [Triple] ~ { RETURN[[v.x*s, v.y*s, v.z*s]]; }; Div: PUBLIC PROC [v: Triple, s: REAL] RETURNS [Triple] ~ { RETURN[IF s # 0.0 THEN [v.x/s, v.y/s, v.z/s] ELSE [v.x, v.y, v.z]]; }; Abs: PUBLIC PROC [v: Triple] RETURNS [Triple] ~ { RETURN[[ABS[v.x], ABS[v.y], ABS[v.z]]]; }; <> Add: PUBLIC PROC [v1, v2: Triple] RETURNS [Triple] ~ { RETURN[[v2.x+v1.x, v2.y+v1.y, v2.z+v1.z]]; }; Sub: PUBLIC PROC [v1, v2: Triple] RETURNS [Triple] ~ { RETURN[[v1.x-v2.x, v1.y-v2.y, v1.z-v2.z]]; }; Equal: PUBLIC PROC [v1, v2: Triple, epsilon: REAL ¬ 0.001] RETURNS [BOOL] ~ { RETURN[ABS[v1.x-v2.x]> Interp: PUBLIC PROC [t: REAL, v1, v2: Triple] RETURNS [Triple] ~ { RETURN [[v1.x+t*(v2.x-v1.x), v1.y+t*(v2.y-v1.y), v1.z+t*(v2.z-v1.z)]]; }; InterpQuad: PUBLIC PROC [t: REAL, v1, v2: Quad] RETURNS [Quad] ~ { RETURN[[v1.x+t*(v2.x-v1.x), v1.y+t*(v2.y-v1.y), v1.z+t*(v2.z-v1.z), v1.w+t*(v2.w-v1.w)]]; }; Combine: PUBLIC PROC [v1: Triple, s1: REAL, v2: Triple, s2: REAL] RETURNS [Triple] ~ { RETURN[[s1*v1.x+s2*v2.x, s1*v1.y+s2*v2.y, s1*v1.z+s2*v2.z]]; }; ScaleRay: PUBLIC PROC [ray: Ray, d: REAL] RETURNS [Triple] ~ { RETURN[[ray.base.x+d*ray.axis.x, ray.base.y+d*ray.axis.y, ray.base.z+d*ray.axis.z]]; }; MulVectors: PUBLIC PROC [v1, v2: Triple] RETURNS [Triple] ~ { RETURN[[v1.x*v2.x, v1.y*v2.y, v1.z*v2.z]]; }; DivVectors: PUBLIC PROC [v1, v2: Triple] RETURNS [Triple] ~ { ret: Triple ¬ v1; IF v2.x # 0.0 THEN ret.x ¬ ret.x/v2.x; IF v2.y # 0.0 THEN ret.y ¬ ret.y/v2.y; IF v2.z # 0.0 THEN ret.z ¬ ret.z/v2.z; RETURN[ret]; }; <> UnitizeSequence: PUBLIC PROC [triples: TripleSequence] ~ { IF triples # NIL THEN FOR n: NAT IN [0..triples.length) DO triples[n] ¬ Unit[triples[n]]; ENDLOOP; }; NegateSequence: PUBLIC PROC [triples: TripleSequence] ~ { IF triples # NIL THEN FOR n: NAT IN [0..triples.length) DO triples[n] ¬ Negate[triples[n]]; ENDLOOP; }; MinMaxSequence: PUBLIC PROC [triples: TripleSequence] RETURNS [b: Box] ~ { huge: REAL ~ Real.LargestNumber; b.min ¬ [huge, huge, huge]; b.max ¬ [-huge, -huge, -huge]; IF triples = NIL THEN RETURN[[origin, origin]]; FOR n: NAT IN [0..triples.length) DO t: Triple ~ triples[n]; IF t.x < b.min.x THEN b.min.x ¬ t.x; IF t.x > b.max.x THEN b.max.x ¬ t.x; IF t.y < b.min.y THEN b.min.y ¬ t.y; IF t.y > b.max.y THEN b.max.y ¬ t.y; IF t.z < b.min.z THEN b.min.z ¬ t.z; IF t.z > b.max.z THEN b.max.z ¬ t.z; ENDLOOP; }; <> Length: PUBLIC PROC [v: Triple] RETURNS [REAL] ~ { RETURN[RealFns.SqRt[v.x*v.x+v.y*v.y+v.z*v.z]]; }; Square: PUBLIC PROC [v: Triple] RETURNS [REAL] ~ { RETURN[v.x*v.x+v.y*v.y+v.z*v.z]; }; Distance: PUBLIC PROC [p1, p2: Triple] RETURNS [REAL] ~ { a: REAL ¬ p2.x-p1.x; b: REAL ¬ p2.y-p1.y; c: REAL ¬ p2.z-p1.z; RETURN[RealFns.SqRt[a*a+b*b+c*c]]; }; SquareDistance: PUBLIC PROC [p1, p2: Triple] RETURNS [REAL] ~ { RETURN[Square[Sub[p1, p2]]]; }; SameLength: PUBLIC PROC [v1, v2: Triple] RETURNS [Triple] ~ { lengthV2: REAL ¬ Length[v2]; RETURN[IF lengthV2 # 0.0 THEN Mul[v2, Length[v1]/lengthV2] ELSE v2]; }; SetVectorLength: PUBLIC PROC [v: Triple, length: REAL] RETURNS [Triple] ~ { sqLength: REAL ¬ v.x*v.x+v.y*v.y+v.z*v.z; IF sqLength = 0.0 THEN RETURN[v]; IF sqLength # 1.0 THEN length ¬ length/RealFns.SqRt[sqLength]; RETURN[[v.x*length, v.y*length, v.z*length]]; }; <> Collinear: PUBLIC PROC [p1, p2, p3: Triple, epsilon: REAL ¬ 0.01] RETURNS [BOOL] ~ { RETURN[Parallel[Sub[p1, p2], Sub[p2, p3], epsilon]]; }; VecsCoplanar: PUBLIC PROC [v1, v2, v3: Triple, epsilon: REAL ¬ 0.01] RETURNS [BOOL] ~ { <> e1: REAL ¬ v1.x*(v2.y*v3.z-v3.y*v2.z); e2: REAL ¬ v1.y*(v2.x*v3.z-v3.x*v2.z); e3: REAL ¬ v1.z*(v2.x*v3.y-v3.x*v2.y); RETURN[ABS[e2-e1-e3] < epsilon]; }; PointsCoplanar: PUBLIC PROC [p1, p2, p3, p4: Triple, epsilon: REAL ¬ 0.01] RETURNS [BOOL] ~ { RETURN[VecsCoplanar[Sub[p4, p1], Sub[p3, p1], Sub[p2, p1], epsilon]]; }; Parallel: PUBLIC PROC [v1, v2: Triple, epsilon: REAL ¬ 0.005] RETURNS [BOOL] ~ { RETURN[ABS[CosineBetween[v1, v2, TRUE]] > 1.-epsilon]; <> <> <> <> <> <> <> <> <> < epsilon THEN RETURN[FALSE];>> <<};>> <> <> }; Perpendicular: PUBLIC PROC [v1, v2: Triple, epsilon: REAL ¬ 0.005] RETURNS [BOOL] ~ { RETURN[ABS[CosineBetween[v1, v2]] < epsilon]; }; FrontFacing: PUBLIC PROC [vector, base: Triple, view: Matrix] RETURNS [BOOL] ~ { IF G3dMatrix.HasPerspective[view] THEN RETURN[FrontFacingWithPerspective[vector, base, G3dMatrix.Invert[view]]] ELSE RETURN[FrontFacingNoPerspective[vector, view]]; }; FrontFacingNoPerspective: PUBLIC PROC [vector: Triple, view: Matrix] RETURNS [BOOL] ~ { RETURN[vector.x*view[0][2]+vector.y*view[1][2]+vector.z*view[2][2] < 0.0]; }; FrontFacingWithPerspective: PUBLIC PROC [vector, base: Triple, invView: Matrix] RETURNS [BOOL] -- New, improved! Faster!! This should always work. <> ~ { <> q: Quad ¬ [invView[3][0], invView[3][1], invView[3][2], invView[3][3]]; camera: Triple ¬ IF q.w = 1.0 THEN [q.x, q.y, q.z] ELSE [q.x/q.w, q.y/q.w, q.z/q.w]; RETURN[Dot[Sub[camera, base], vector] > 0.0]; <> <> <> <> <> <> <> }; <<>> <> <> <<~ {>> <> <> <> <> <<};>> <<>> <> <> <<(this probably corrects above as long as vector doesn't go behind the eye!)>> <<~ {>> <> <> <> <> <> <> <<};>> <> NearnessAccelerator: PUBLIC PROC [p0, p1: Triple] RETURNS [n: Quad] ~ { n.x ¬ p1.x-p0.x; n.y ¬ p1.y-p0.y; n.z ¬ p1.z-p0.z; n.w ¬ n.x*n.x+n.y*n.y+n.z*n.z; IF n.w = 0.0 THEN RETURN; n.x ¬ n.x/n.w; n.y ¬ n.y/n.w; n.z ¬ n.z/n.w; }; NearestToSegment: PUBLIC PROC [p0, p1, q: Triple, acc: Quad ¬ [0.0, 0.0, 0.0, 0.0]] RETURNS [nearest: NearSegment] ~ { IF acc # [0.0, 0.0, 0.0, 0.0] THEN { alpha: REAL ¬ (q.x-p0.x)*acc.x+(q.y-p0.y)*acc.y+(q.z-p0.z)*acc.z; SELECT alpha FROM <= 0.0 => { nearest.inside ¬ FALSE; nearest.point ¬ p0; nearest.w0 ¬ 1.0; nearest.w1 ¬ 0.0; }; >= 1.0 => { nearest.inside ¬ FALSE; nearest.point ¬ p1; nearest.w0 ¬ 0.0; nearest.w1 ¬ 1.0; }; ENDCASE => { nearest.inside ¬ TRUE; nearest.w1 ¬ alpha; nearest.w0 ¬ 1.0-nearest.w1; alpha ¬ alpha*acc.w; nearest.point ¬ [p0.x+alpha*acc.x, p0.y+alpha*acc.y, p0.z+alpha*acc.z]; }; } ELSE { delta: Triple ¬ [p1.x-p0.x, p1.y-p0.y, p1.z-p0.z]; nearest.point ¬ p0; IF delta = origin THEN RETURN ELSE { ua: Triple ~ [q.x-p0.x, q.y-p0.y, q.z-p0.z]; deltaSq: REAL ~ delta.x*delta.x+delta.y*delta.y+delta.z*delta.z; alpha: REAL ¬ (ua.x*delta.x+ua.y*delta.y+ua.z*delta.z)/deltaSq; SELECT alpha FROM < 0.0 => {nearest.inside ¬ FALSE; alpha ¬ 0.0}; > 1.0 => {nearest.inside ¬ FALSE; alpha ¬ 1.0}; ENDCASE => nearest.inside ¬ TRUE; nearest.point ¬ [p0.x+alpha*delta.x, p0.y+alpha*delta.y, p0.z+alpha*delta.z]; nearest.w1 ¬ alpha; nearest.w0 ¬ 1.0-alpha; }; }; }; NearestToLine: PUBLIC PROC [line: Ray, q: Triple] RETURNS [Triple] ~ { << [Artwork node; type 'Artwork on' to command tool] >> u: Triple ¬ Unit[line.axis]; RETURN[Add[line.base, Mul[u, Dot[Sub[q, line.base], u]]]]; }; PointOnLine: PUBLIC PROC [p: Triple, line: Ray, epsilon: REAL ¬ 0.005] RETURNS [BOOL] ~ { RETURN[Distance[p, NearestToLine[line, p]] < epsilon]; }; NearestToSequence: PUBLIC PROC [p: Triple, points: TripleSequence] RETURNS [index: NAT ¬ 0] ~ { minSqDist: REAL ¬ Real.LargestNumber; IF points # NIL THEN FOR n: NAT IN [0..points.length) DO sqDist: REAL ¬ SquareDistance[p, points[n]]; IF sqDist < minSqDist THEN {index ¬ n; minSqDist ¬ sqDist}; ENDLOOP; }; ClosestApproach2Lines: PUBLIC PROC [line1, line2: Ray] RETURNS [p1, p2: Triple] ~ { <> <> <<(p2-p1).line1.axis = 0 and (p2-p1).line2.axis = 0.>> axis1: Triple ¬ Unit[line1.axis]; axis2: Triple ¬ Unit[line2.axis]; den: REAL ¬ Dot[axis1, axis2]; densq: REAL ¬ den*den; dif: Triple ¬ Sub[line1.base, line2.base]; dot1: REAL ¬ Dot[axis1, dif]; dot2: REAL ¬ Dot[axis2, dif]; s: REAL ¬ (dot1/densq-dot2/den)/(1.0-1.0/densq); t: REAL ¬ (s+dot1)/den; p1 ¬ Add[line1.base, Mul[axis1, s]]; p2 ¬ Add[line2.base, Mul[axis2, t]]; }; DistanceBetween2Lines: PUBLIC PROC [line1, line2: Ray] RETURNS [REAL] ~ { <> <> <> <> <> <> <> <> <> <> <> <> <> <> <> <> <> x21: REAL ¬ line2.base.x-line1.base.x; y21: REAL ¬ line2.base.y-line1.base.y; z21: REAL ¬ line2.base.z-line1.base.z; ab: REAL ¬ line1.axis.x*line2.axis.y-line2.axis.x*line1.axis.y; bc: REAL ¬ line1.axis.y*line2.axis.z-line2.axis.y*line1.axis.z; ca: REAL ¬ line1.axis.z*line2.axis.x-line2.axis.z*line1.axis.x; den: REAL ¬ ab*ab+bc*bc+ca*ca; IF den < 0.0001 THEN RETURN[Distance[line1.base, NearestToLine[line2, line1.base]]] ELSE RETURN[ABS[x21*bc+y21*ca+z21*ab]/RealFns.SqRt[den]]; }; <> <> <> <> <> <> <> <> <> <> <> <<};>> <> V90: PUBLIC PROC [v0, v1: Triple, unitize: BOOL ¬ TRUE] RETURNS [t: Triple] ~ { dot: REAL ¬ Dot[v0, v1]; t ¬ IF ABS[dot] > 0.000001 THEN [v1.x-dot*v0.x, v1.y-dot*v0.y, v1.z-dot*v0.z] ELSE v1; IF unitize THEN t ¬ Unit[t]; }; Ortho: PUBLIC PROC [v: Triple, crosser: Triple ¬ [0.0, 0.0, 0.0]] RETURNS [Triple] ~ { length: REAL ¬ Length[v]; IF crosser = [0.0, 0.0, 0.0] THEN crosser ¬ [v.z, -v.x, v.y]; <> RETURN[IF length = 0.0 THEN [0.0, 0.0, 0.0] ELSE Mul[Unit[Cross[v, crosser]], length]]; }; RotateAbout: PUBLIC PROC [v, axis: Triple, a: REAL, degrees: BOOL ¬ TRUE] RETURNS [t: Triple] ~ { rotate: Matrix ¬ G3dMatrix.MakeRotate[axis, a, degrees, G3dMatrix.ObtainMatrix[]]; t ¬ G3dMatrix.TransformVec[v, rotate]; G3dMatrix.ReleaseMatrix[rotate]; }; <> << [Artwork node; type 'Artwork on' to command tool] >> <> <> <> <> <<>> PolarFromCartesian: PUBLIC PROC [cartesian: Triple] RETURNS [Triple] ~ { xySum: REAL ¬ cartesian.x*cartesian.x+cartesian.y*cartesian.y; lng: REAL ¬ RealFns.ArcTanDeg[cartesian.y, cartesian.x]; lat: REAL ¬ RealFns.ArcTanDeg[cartesian.z, RealFns.SqRt[xySum]]; mag: REAL ¬ RealFns.SqRt[xySum+cartesian.z*cartesian.z]; RETURN[[lng, lat, mag]]; }; CartesianFromPolar: PUBLIC PROC [polar: Triple] RETURNS [Triple] ~ { cosmag: REAL ¬ RealFns.CosDeg[polar.y]*polar.z; RETURN[[ cosmag*RealFns.CosDeg[polar.x], cosmag*RealFns.SinDeg[polar.x], polar.z*RealFns.SinDeg[polar.y]]]; }; <> Clip: PUBLIC PROC [p: Triple, box: Box] RETURNS [clipped: Triple] ~ { clipped ¬ [ MIN[MAX[box.min.x, p.x], box.max.x], MIN[MAX[box.min.y, p.y], box.max.y], MIN[MAX[box.min.z, p.z], box.max.z]]; }; Project: PUBLIC PROC [v1, v2: Triple] RETURNS [Triple] ~ { RETURN[IF v2 = origin THEN v2 ELSE Mul[v2, Dot[v1, v2]/Square[v2]]]; }; Average: PUBLIC PROC [triples: TripleSequence] RETURNS [average: Triple] ~ { average ¬ origin; FOR n: NAT IN [0..triples.length) DO average ¬ Add[average, triples[n]]; ENDLOOP; IF triples.length # 0 THEN average ¬ Div[average, triples.length]; }; CosineBetween: PUBLIC PROC [v0, v1: Triple, unitize: BOOL ¬ FALSE] RETURNS [r: REAL] ~ { IF v0 = origin OR v1 = origin THEN RETURN[0.0]; r ¬ (v0.x*v1.x+v0.y*v1.y+v0.z*v1.z); IF unitize THEN r ¬ r/RealFns.SqRt[(v0.x*v0.x+v0.y*v0.y+v0.z*v0.z)*(v1.x*v1.x+v1.y*v1.y+v1.z*v1.z)] }; <<>> AngleBetween: PUBLIC PROC [ v0, v1: Triple, degrees: BOOL ¬ TRUE, unitize: BOOL ¬ FALSE] RETURNS [REAL] ~ { RETURN[IF degrees THEN G2dBasic.ArcCosDeg[CosineBetween[v0, v1, unitize]] ELSE G2dBasic.ArcCos[CosineBetween[v0, v1, unitize]]]; }; <<>> END.