<<>> <> <> <> DIRECTORY G2dBasic, G2dVector, Real, RealFns; G2dVectorImpl: CEDAR PROGRAM IMPORTS G2dBasic, Real, RealFns EXPORTS G2dVector ~ BEGIN <> Box: TYPE ~ G2dBasic.Box; NatPair: TYPE ~ G2dBasic.NatPair; NearSegment: TYPE ~ G2dBasic.NearSegment; Pair: TYPE ~ G2dBasic.Pair; PairSequence: TYPE ~ G2dBasic.PairSequence; PairSequenceRep: TYPE ~ G2dBasic.PairSequenceRep; Triple: TYPE ~ G2dBasic.Triple; PI: REAL ~ G2dBasic.PI; <> Negate: PUBLIC PROC [p: Pair] RETURNS [Pair] ~ { RETURN[[-p.x, -p.y]]; }; Unit: PUBLIC PROC [p: Pair] RETURNS [Pair] ~ { m: REAL ~ RealFns.SqRt[p.x*p.x+p.y*p.y]; RETURN[[p.x/m, p.y/m]] }; <<>> Null: PUBLIC PROC [v: Pair] RETURNS [BOOL] ~ { RETURN[v.x = 0.0 AND v.y = 0.0]; }; <> Add: PUBLIC PROC [p1, p2: Pair] RETURNS [Pair] ~ {RETURN[[p1.x+p2.x, p1.y+p2.y]]}; <<>> Sub: PUBLIC PROC [p1, p2: Pair] RETURNS [Pair] ~ {RETURN[[p1.x-p2.x, p1.y-p2.y]]}; <<>> Mul: PUBLIC PROC [p: Pair, s: REAL] RETURNS [Pair] ~ {RETURN[[s*p.x, s*p.y]]}; <<>> Div: PUBLIC PROC [p: Pair, s: REAL] RETURNS [Pair] ~ {RETURN[[p.x/s, p.y/s]]}; <<>> Dot: PUBLIC PROC [p1, p2: Pair] RETURNS [REAL] ~ {RETURN[p1.x*p2.x+p1.y*p2.y]}; <<>> Cross: PUBLIC PROC [p1, p2: Pair] RETURNS [REAL] ~ {RETURN[p1.x*p2.y-p1.y*p2.x]}; Equal: PUBLIC PROC [v1, v2: Pair, epsilon: REAL ¬ 0.001] RETURNS [BOOL] ~ { RETURN[ABS[v1.x-v2.x] < epsilon AND ABS[v1.y-v2.y] < epsilon]; }; Midpoint: PUBLIC PROC [v1, v2: Pair] RETURNS [Pair] ~ { RETURN[[0.5*(v1.x+v2.x), 0.5*(v1.y+v2.y)]]; }; <<>> Interp: PUBLIC PROC [t: REAL, v1, v2: Pair] RETURNS [Pair] ~ { RETURN [[v1.x+t*(v2.x-v1.x), v1.y+t*(v2.y-v1.y)]]; }; Combine: PUBLIC PROC [v1: Pair, s1: REAL, v2: Pair, s2: REAL] RETURNS [Pair] ~ { RETURN[[s1*v1.x+s2*v2.x, s1*v1.y+s2*v2.y]]; }; MulVectors: PUBLIC PROC [v1, v2: Pair] RETURNS [Pair] ~ { RETURN[[v1.x*v2.x, v1.y*v2.y]]; }; DivVectors: PUBLIC PROC [v1, v2: Pair] RETURNS [Pair] ~ { ret: Pair ¬ 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; RETURN[ret]; }; <> UnitizeSequence: PUBLIC PROC [pairs: PairSequence] ~ { IF pairs # NIL THEN FOR n: NAT IN [0..pairs.length) DO pairs[n] ¬ Unit[pairs[n]]; ENDLOOP; }; AverageSequence: PUBLIC PROC [pairs: PairSequence] RETURNS [average: Pair] ~ { average ¬ [0.0, 0.0]; FOR n: NAT IN [0..pairs.length) DO average ¬ Add[average, pairs[n]]; ENDLOOP; IF pairs.length # 0 THEN average ¬ Div[average, pairs.length]; }; MinMaxSequence: PUBLIC PROC [pairs: PairSequence] RETURNS [mm: Box] ~ { mm.min ¬ [Real.LargestNumber, Real.LargestNumber]; mm.max ¬ [-Real.LargestNumber, -Real.LargestNumber]; FOR n: NAT IN [0..pairs.length) DO t: Pair ~ pairs[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; ENDLOOP; }; NegateSequence: PUBLIC PROC [pairs: PairSequence] ~ { IF pairs # NIL THEN FOR n: NAT IN [0..pairs.length) DO pairs[n] ¬ Negate[pairs[n]]; ENDLOOP; }; ReverseSequence: PUBLIC PROC [src: PairSequence, dst: PairSequence ¬ NIL] RETURNS [PairSequence] ~ { IF src = NIL THEN RETURN[NIL]; IF dst = NIL OR dst.maxLength < src.length THEN dst ¬ NEW[PairSequenceRep[src.length]]; dst.length ¬ src.length; IF dst = src THEN FOR n: NAT IN [0..src.length/2) DO -- in place nn: NAT ¬ src.length-n-1; t: Pair ¬ src[n]; src[n] ¬ src[nn]; src[nn] ¬ t; ENDLOOP ELSE FOR n: NAT IN [0..src.length) DO -- not in place dst[n] ¬ src[src.length-n-1]; ENDLOOP; RETURN[dst]; }; <> Length: PUBLIC PROC [p: Pair] RETURNS [REAL] ~ { RETURN[RealFns.SqRt[p.x*p.x+p.y*p.y]]; }; Square: PUBLIC PROC [p: Pair] RETURNS [REAL] ~ {RETURN[p.x*p.x+p.y*p.y]}; <<>> Distance: PUBLIC PROC [p1, p2: Pair] RETURNS [REAL] ~ { a: REAL ¬ p2.x-p1.x; b: REAL ¬ p2.y-p1.y; RETURN[RealFns.SqRt[a*a+b*b]]; }; SquareDistance: PUBLIC PROC [p1, p2: Pair] RETURNS [REAL] ~ { RETURN[Square[Sub[p1, p2]]]; }; SameLength: PUBLIC PROC [v1, v2: Pair] RETURNS [Pair] ~ { lengthV2: REAL ¬ Length[v2]; RETURN[IF lengthV2 # 0.0 THEN Mul[v2, Length[v1]/lengthV2] ELSE v2]; }; SetVectorLength: PUBLIC PROC [v: Pair, length: REAL] RETURNS [Pair] ~ { sqLength: REAL ¬ v.x*v.x+v.y*v.y; IF sqLength = 0.0 THEN RETURN[v]; IF sqLength # 1.0 THEN length ¬ length/RealFns.SqRt[sqLength]; RETURN[[v.x*length, v.y*length]]; }; <> GetNear2dAccelerator: PUBLIC PROC [p0, p1: Pair] RETURNS [n: Triple] ~ { n.x ¬ p1.x-p0.x; n.y ¬ p1.y-p0.y; n.z ¬ n.x*n.x+n.y*n.y; IF n.z = 0.0 THEN RETURN; n.x ¬ n.x/n.z; n.y ¬ n.y/n.z; }; NearestToSegment: PUBLIC PROC [p0, p1, q: Pair, acc: Triple ¬ [0, 0, 0]] RETURNS [nearest: NearSegment] ~ { <> IF acc # [0.0, 0.0, 0.0] THEN { alpha: REAL ¬ (q.x-p0.x)*acc.x+(q.y-p0.y)*acc.y; 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-alpha; alpha ¬ alpha*acc.z; nearest.point ¬ [p0.x+alpha*acc.x, p0.y+alpha*acc.y]; }; } ELSE { delta: Pair ¬ [p1.x-p0.x, p1.y-p0.y]; nearest.point ¬ p0; IF delta = [0.0, 0.0] THEN RETURN ELSE { ua: Pair ~ [q.x-p0.x, q.y-p0.y]; deltaSq: REAL ~ delta.x*delta.x+delta.y*delta.y; alpha: REAL ¬ (ua.x*delta.x+ua.y*delta.y)/deltaSq; nearest.w1 ¬ alpha; nearest.w0 ¬ 1.0-alpha; nearest.point ¬ [p0.x+alpha*delta.x, p0.y+alpha*delta.y]; }; }; }; NearestToSequence: PUBLIC PROC [p: Pair, points: PairSequence] 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; }; <> Line: PUBLIC PROC [p0, p1: Pair] RETURNS [line: Triple] ~ { line.x ¬ p0.y-p1.y; line.y ¬ p1.x-p0.x; line.z ¬ p0.x*p1.y-p1.x*p0.y; }; DistanceToLine: PUBLIC PROC [p: Pair, line2d: Triple] RETURNS [REAL] ~ { RETURN[p.x*line2d.x+p.y*line2d.y+line2d.z]; }; IntersectTwoLines: PUBLIC PROC [line0, line1: Triple] RETURNS [Pair] ~ { <> <> cross: Triple ¬ [line0.y*line1.z-line0.z*line1.y,line0.z*line1.x-line0.x*line1.z,line0.x*line1.y-line0.y*line1.x]; IF cross.z = 0.0 THEN ERROR; RETURN[[cross.x/cross.z, cross.y/cross.z]]; }; TriangleArea: PUBLIC PROC [p0, p1, p2: Pair] RETURNS [a: REAL] ~ { a ¬ ABS[(p1.x-p0.x)*(p2.y-p1.y)-(p1.y-p0.y)*(p2.x-p1.x)]; }; <> NatPairLerp: PUBLIC PROC [t: REAL, a, b: NatPair] RETURNS [Pair] ~ { RETURN [[a.x+t*(b.x-a.x), a.y+t*(b.y-a.y)]]; }; NatPairDistance: PUBLIC PROC [a, b: NatPair] RETURNS [REAL] ~ { dx: INT16 ¬ a.x-b.x; dy: INT16 ¬ a.y-b.y; RETURN[RealFns.SqRt[dx*dx+dy*dy]]; }; <> AnglesAgree: PUBLIC PROC [a0, a1: REAL] RETURNS [REAL] ~ { aa0: REAL ¬ G2dBasic.RealMod[a0, 2.0*PI]; aa1: REAL ¬ G2dBasic.RealMod[a1, 2.0*PI]; dAngle: REAL ¬ ABS[aa1-aa0]; IF dAngle > PI THEN { IF aa0 > PI THEN aa0 ¬ aa0-2.0*PI; IF aa1 > PI THEN aa1 ¬ aa1-2.0*PI; dAngle ¬ ABS[aa1-aa0]; }; RETURN[1.0-(1.0/PI)*dAngle]; }; AngleFromVector: PUBLIC PROC [v: Pair, degrees: BOOL ¬ FALSE] RETURNS [a: REAL] ~ { sd: REAL ¬ v.x*v.x+v.y*v.y; IF sd # 1.0 THEN v.x ¬ v.x/RealFns.SqRt[sd]; -- let it die if v = [0, 0] a ¬ G2dBasic.ArcCos[v.x]; IF v.y < 0.0 THEN a ¬ 2.0*PI-a; IF degrees THEN a ¬ a*(180.0/PI); }; END.