<> <> <> DIRECTORY Matrix3d, RealFns, Vector3d; Vector3dImpl: CEDAR PROGRAM IMPORTS Matrix3d, RealFns EXPORTS Vector3d ~ BEGIN OPEN Vector3d; <> 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]]; }; Normalize: PUBLIC PROC [v: Triple] RETURNS [Triple] ~ { m: REAL ~ RealFns.SqRt[v.x*v.x+v.y*v.y+v.z*v.z]; RETURN[SELECT m FROM 0.0 => origin, 1.0 => v, ENDCASE => [v.x/m, v.y/m, v.z/m]]; }; Unitize: PUBLIC PROC [v: Triple] RETURNS [Triple] ~ {RETURN[Normalize[v]]}; 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]]; }; <> 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]> 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]]; }; Ray: PUBLIC PROC [l: Line, d: REAL] RETURNS [Triple] ~ { RETURN[[l.base.x+d*l.axis.x, l.base.y+d*l.axis.y, l.base.z+d*l.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.x/v2.y; IF v2.z # 0.0 THEN ret.z _ ret.x/v2.z; RETURN[ret]; }; <> Length: PUBLIC PROC [v: Triple] RETURNS [REAL] ~ { RETURN[RealFns.SqRt[v.x*v.x+v.y*v.y+v.z*v.z]]; }; SquareLength: 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[SquareLength[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]; }; <> 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]; }; PtsCoplanar: 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] ~ { par: BOOL _ FALSE; temp1: Triple _ Normalize[v1]; temp2: Triple _ Normalize[v2]; RETURN[ABS[Dot[temp1, temp2]] > 1.-epsilon]; }; Perpendicular: PUBLIC PROC [v1, v2: Triple, epsilon: REAL _ 0.005] RETURNS [BOOL _ FALSE] ~ { perp: BOOL _ FALSE; temp1: Triple _ Normalize[v1]; temp2: Triple _ Normalize[v2]; RETURN[ABS[Dot[temp1, temp2]] < epsilon]; }; <> LinePoint: PUBLIC PROC [p: Triple, l: Line] RETURNS [Triple] ~ { lineV: Triple _ Normalize[l.axis]; RETURN[Add[l.base, Mul[lineV, Dot[Sub[p, l.base], lineV]]]]; }; Project: PUBLIC PROC [v1, v2: Triple] RETURNS [Triple] ~ { v2unit: Triple _ Normalize[v2]; RETURN[Mul[v2unit, Dot[v1, v2unit]]]; }; PtOnLine: PUBLIC PROC [p: Triple, l: Line, epsilon: REAL _ 0.005] RETURNS [BOOL] ~ { RETURN[Distance[p, LinePoint[p, l]] < epsilon]; }; <> V90: PUBLIC PROC [v0, v1: Triple] RETURNS [Triple] ~ { dot: REAL _ Dot[v0, v1]; RETURN [IF ABS[dot] > 0.000001 THEN Normalize[[v1.x-dot*v0.x, v1.y-dot*v0.y, v1.z-dot*v0.z]] ELSE v1]; }; Ortho: PUBLIC PROC [v: Triple, crosser: Triple _ [0.0, 0.0, 0.0]] RETURNS [Triple] ~ { length: REAL _ Length[v]; IF crosser = v THEN crosser _ [crosser.z, crosser.y, crosser.x]; RETURN[IF length = 0.0 THEN [0.0, 0.0, 0.0] ELSE Mul[Normalize[Cross[v, crosser]], length]]; }; RotateAbout: PUBLIC PROC [v, axis: Triple, a: REAL, degrees: BOOL _ TRUE] RETURNS [Triple] ~ { rotate: Matrix3d.Matrix _ Matrix3d.MakePureRotate[axis, a, degrees]; RETURN[Matrix3d.TransformVec[v, rotate]]; }; <> PolarFromCartesian: PUBLIC PROC [cartesian: Triple] RETURNS [Triple] ~ { xzSum: REAL _ cartesian.x*cartesian.x+cartesian.z*cartesian.z; lng: REAL _ RealFns.ArcTanDeg[cartesian.z, cartesian.x]; lat: REAL _ RealFns.ArcTanDeg[cartesian.y, RealFns.SqRt[xzSum]]; mag: REAL _ RealFns.SqRt[xzSum+cartesian.y*cartesian.y]; IF lat < 0.0 THEN lat _ 360.0+lat; 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], polar.z*RealFns.SinDeg[polar.y], cosmag*RealFns.SinDeg[polar.x]]]; }; <> CopyRealSequence: PUBLIC PROC [reals: RealSequence] RETURNS [RealSequence] ~ { copy: RealSequence _ NIL; IF reals # NIL THEN { copy _ NEW[RealSequenceRep[reals.length]]; copy.length _ reals.length; FOR n: NAT IN [0..reals.length) DO copy[n] _ reals[n]; ENDLOOP; }; RETURN[copy]; }; CopyPairSequence: PUBLIC PROC [pairs: PairSequence] RETURNS [PairSequence] ~ { copy: PairSequence _ NIL; IF pairs # NIL THEN { copy _ NEW[PairSequenceRep[pairs.length]]; copy.length _ pairs.length; FOR n: NAT IN [0..pairs.length) DO copy[n] _ pairs[n]; ENDLOOP; }; RETURN[copy]; }; CopyTripleSequence: PUBLIC PROC [triples: TripleSequence] RETURNS [TripleSequence] ~ { copy: TripleSequence _ NIL; IF triples # NIL THEN { copy _ NEW[TripleSequenceRep[triples.length]]; copy.length _ triples.length; FOR n: NAT IN [0..triples.length) DO copy[n] _ triples[n]; ENDLOOP; }; RETURN[copy]; }; <> ArcCos: PUBLIC PROC [cos: REAL, degrees: BOOL _ TRUE] RETURNS [REAL] ~ { x: REAL ~ MIN[1.0, MAX[-1.0, cos]]; y: REAL ~ RealFns.SqRt[1.0-x*x]; RETURN[IF degrees THEN RealFns.ArcTanDeg[y, x] ELSE RealFns.ArcTan[y, x]]; }; CosineBetween: PUBLIC PROC [v0, v1: Triple] RETURNS [REAL] ~ { RETURN[Dot[Normalize[v0], Normalize[v1]]]; }; <<>> AngleBetween: PUBLIC PROC [v0, v1: Triple, degrees: BOOL _ TRUE] RETURNS [REAL] ~ { RETURN[ArcCos[CosineBetween[v0, v1], degrees]]; }; <<>> MinMaxOfTriples: PUBLIC PROC [triples: TripleSequence] RETURNS [mm: MinMax] ~ { huge: REAL ~ 100000000.0; mm.min _ [huge, huge, huge]; mm.max _ [-huge, -huge, -huge]; FOR n: NAT IN [0..triples.length) DO t: Triple ~ triples[n]; IF t.x < mm.min.x THEN mm.min.x _ t.x; IF t.x > mm.max.x THEN mm.max.x _ t.x; IF t.y < mm.min.y THEN mm.min.y _ t.y; IF t.y > mm.max.y THEN mm.max.y _ t.y; IF t.z < mm.min.z THEN mm.min.z _ t.z; IF t.y > mm.max.y THEN mm.max.y _ t.y; ENDLOOP; }; END.