<> <> <> <> DIRECTORY RealFns, SV2d, SVAngle, SVLines2d, SVVector2d; SVLines2dImpl: CEDAR PROGRAM IMPORTS RealFns, SVAngle, SVVector2d EXPORTS SVLines2d = BEGIN LineSeg: TYPE = REF LineSegObj; LineSegObj: TYPE = SV2d.LineSegObj; Path: TYPE = REF PathObj; PathObj: TYPE = SV2d.PathObj;Point2d: TYPE = SV2d.Point2d; Polygon: TYPE = REF PolygonObj; PolygonObj: TYPE = SV2d.PolygonObj; Ray2d: TYPE = REF Ray2dObj; Ray2dObj: TYPE = SV2d.Ray2dObj; TrigLine: TYPE = REF TrigLineObj; TrigLineObj: TYPE = SV2d.TrigLineObj; TrigLineSeg: TYPE = REF TrigLineSegObj; TrigLineSegObj: TYPE = SV2d.TrigLineSegObj; TrigPolygon: TYPE = REF TrigPolygonObj; TrigPolygonObj: TYPE = SV2d.TrigPolygonObj; Vector2d: TYPE = SV2d.Vector2d; CreateEmptyTrigLine: PUBLIC PROC RETURNS [line: TrigLine] = { line _ NEW[TrigLineObj]; }; CopyTrigLine: PUBLIC PROC [from: TrigLine, to: TrigLine] = { to.c _ from.c; to.s _ from.s; to.theta _ from.theta; to.d _ from.d; to.slope _ from.slope; to.yInt _ from.yInt; }; FillTrigLineFromPoints: PUBLIC PROC [v1, v2: Point2d, line: TrigLine] = { <> <> epsilon: REAL = 1.0e-6; x2Minusx1: REAL _ v2[1] - v1[1]; y2Minusy1: REAL _ v2[2] - v1[2]; IF ABS[x2Minusx1] < epsilon THEN {-- vertical line IF v2[2] > v1[2] THEN {-- line goes up line.theta _ 90.0; line.s _ 1; <> line.d _ -v1[1]} ELSE { -- line goes down line.theta _ -90; line.s _ -1; <> line.d _ v1[1]}; line.c _ 0; <> } <> ELSE { line.slope _ y2Minusy1/x2Minusx1; line.theta _ RealFns.ArcTanDeg[y2Minusy1, x2Minusx1]; line.c _ RealFns.CosDeg[line.theta]; <> line.s _ line.slope*line.c;-- gets around inaccuracies in the trig functions to make sure the line has the right slope. (also saves some time) <> line.d _ v1[2]*line.c - v1[1]*line.s; line.yInt _ line.d/line.c; }; }; -- end of FillTrigLineFromPoints FillTrigLineFromPointAndVector: PUBLIC PROC [pt: Point2d, vec: Vector2d, line: TrigLine] = { pt2: Point2d; pt2 _ SVVector2d.Add[pt, vec]; FillTrigLineFromPoints[pt, pt2, line]; }; FillTrigLineFromCoefficients: PUBLIC PROC [sineOfTheta, cosineOfTheta, distance: REAL, line: TrigLine] = { <> <> line.s _ sineOfTheta; line.c _ cosineOfTheta; line.d _ distance; line.theta _ RealFns.ArcTanDeg[sineOfTheta, cosineOfTheta]; <> IF cosineOfTheta # 0 THEN { line.slope _ sineOfTheta/cosineOfTheta; <> line.yInt _ line.d/line.c}; }; -- end of FillTrigLineFromCoefficients FillTrigLineAsNormal: PUBLIC PROC [line: TrigLine, pt: Point2d, normalLine: TrigLine] = { <> <> <> <> <> <> normalLine.s _ line.c; normalLine.c _ -line.s; normalLine.d _ -pt[2]*line.s - pt[1]*line.c; normalLine.theta _ SVAngle.Add[line.theta, 90]; IF normalLine.c #0 THEN { normalLine.slope _ normalLine.s/normalLine.c; <> line.yInt _ normalLine.d/normalLine.c}; }; -- end of FillTrigLineAsNormal TrigLineFromPoints: PUBLIC PROC [v1, v2: Point2d] RETURNS [line: TrigLine] = { line _ CreateEmptyTrigLine[]; FillTrigLineFromPoints[v1, v2, line]; }; TrigLineFromPointAndVector: PUBLIC PROC [pt: Point2d, vec: Vector2d] RETURNS [line: TrigLine] = { pt2: Point2d; pt2 _ SVVector2d.Add[pt, vec]; line _ TrigLineFromPoints[pt, pt2]; }; TrigLineFromCoefficients: PUBLIC PROC [sineOfTheta, cosineOfTheta, distance: REAL] RETURNS [line: TrigLine] = { line _ CreateEmptyTrigLine[]; FillTrigLineFromCoefficients[sineOfTheta, cosineOfTheta, distance, line]; }; TrigLineNormalToTrigLineThruPoint: PUBLIC PROC [line: TrigLine, pt: Point2d] RETURNS [normalLine: TrigLine] = { normalLine _ CreateEmptyTrigLine[]; FillTrigLineAsNormal[line, pt, normalLine]; }; TrigLineMeetsTrigLine: PUBLIC PROC [line1, line2: TrigLine] RETURNS [intersection: Point2d, parallel: BOOL] = { determinant: REAL; IF line1.theta = line2.theta THEN {parallel _ TRUE; RETURN}; parallel _ FALSE; <> <> <> <> <> <<(-s1c2+c1s2)*x - c2d1 + c1d2 = 0;>> <> <> <> <<(s2c1 - s1c2)*y -s2d1 + s1d2 = 0;>> <> <<(I might just implement Kramer's Rule with determinants some day)>> determinant _ line2.s*line1.c - line1.s*line2.c; IF determinant = 0 THEN {parallel _ TRUE; RETURN}; intersection[1] _ (line2.c*line1.d - line1.c*line2.d)/determinant; intersection[2] _ (line2.s*line1.d - line1.s*line2.d)/determinant; }; TrigLineMeetsYAxis: PUBLIC PROC [line: TrigLine] RETURNS [yInt: REAL, parallel: BOOL] = { IF line.theta = 90 OR line.theta = -90 THEN parallel _ TRUE ELSE {-- we just want the y Intercept which is calculated at line creation time for now. parallel _ FALSE; yInt _ line.yInt;} }; TrigLineMeetsTrigLineSeg: PUBLIC PROC [line: TrigLine, seg: TrigLineSeg] RETURNS [intersection: Point2d, noHit: BOOL] = { <> }; TrigLineDistance: PUBLIC PROC [pt: Point2d, line: TrigLine] RETURNS [d: REAL] = { <> <> d _ pt[2]*line.c - pt[1]*line.s - line.d; }; -- end of TrigDistance <> <> <> <> <> <<-sy - c*x - D = 0>> <> <> <> <> <> <> <> <> <> <> <> <> <> <> <> <> <<};>> <<>> PointProjectedOntoTrigLine: PUBLIC PROC [pt: Point2d, line: TrigLine] RETURNS [projectedPt: Point2d] = { <> <> <> <> <> <> D: REAL _ pt[2]*line.c - pt[1]*line.s - line.d; projectedPt[1] _ pt[1] + D*line.s; projectedPt[2] _ pt[2] - D*line.c; }; <> CreateTrigLineSeg: PUBLIC PROC [v1, v2: Point2d] RETURNS [seg: TrigLineSeg] = { seg _ CreateEmptyTrigLineSeg[]; FillTrigLineSeg[v1, v2, seg]; }; -- end of CreateTrigLineSeg CreateEmptyTrigLineSeg: PUBLIC PROC RETURNS [seg: TrigLineSeg] = { seg _ NEW[TrigLineSegObj]; seg.line _ CreateEmptyTrigLine[]; }; FillTrigLineSeg: PUBLIC PROC [v1, v2: Point2d, seg: TrigLineSeg] = { y2Minusy1: REAL; FillTrigLineFromPoints[v1, v2, seg.line]; y2Minusy1 _ v2[2] - v1[2]; IF y2Minusy1 = 0 THEN IF v2[1] > v1[1] THEN {seg.pHi _ v2; seg.pLo _ v1; seg.pLoIsFirst _ TRUE} ELSE {seg.pHi _ v1; seg.pLo _ v2; seg.pLoIsFirst _ FALSE} ELSE IF v2[2] > v1[2] THEN {seg.pHi _ v2; seg.pLo _ v1; seg.pLoIsFirst _ TRUE} ELSE {seg.pHi _ v1; seg.pLo _ v2; seg.pLoIsFirst _ FALSE}; }; -- end of FillTrigLineSeg CopyTrigLineSeg: PUBLIC PROC [from: TrigLineSeg, to: TrigLineSeg] = { CopyTrigLine[from.line, to.line]; to.pLo _ from.pLo; to.pHi _ from.pHi; to.pLoIsFirst _ from.pLoIsFirst; }; -- end of CopyTrigLineSeg TrigLinePointOnTrigLineSeg: PUBLIC PROC [pt: Point2d, seg: TrigLineSeg] RETURNS [BOOL] = { <> IF Abs[seg.pHi[1] - seg.pLo[1]] >= Abs[seg.pHi[2] - seg.pLo[2]] THEN -- line is more horizontal RETURN[Between[pt[1], seg.pLo[1], seg.pHi[1]]] ELSE -- line is more vertical RETURN[Between[pt[2], seg.pLo[2], seg.pHi[2]]]; }; -- end of TrigLinePointOnTrigLineSeg NearestEndpoint: PUBLIC PROC [pt: Point2d, seg: TrigLineSeg] RETURNS [endpoint: Point2d] = { <> BEGIN IF Abs[pt[1]-seg.pLo[1]] <= Abs[pt[1]-seg.pHi[1]] THEN IF Abs[pt[2]-seg.pLo[2]] <= Abs[pt[2]-seg.pHi[2]] THEN RETURN[seg.pLo] ELSE GOTO DoMath ELSE IF Abs[pt[2]-seg.pLo[2]] > Abs[pt[2]-seg.pHi[2]] THEN RETURN[seg.pHi] ELSE GOTO DoMath; EXITS DoMath => IF DistanceSquaredPointToPoint[pt, seg.pLo] < DistanceSquaredPointToPoint[pt, seg.pHi] THEN endpoint _ seg.pLo ELSE endpoint _ seg.pHi; END; }; DistanceSquaredToNearestEndpoint: PUBLIC PROC [pt: Point2d, seg: TrigLineSeg] RETURNS [distanceSquared: REAL] = { distance2ToPLo, distance2ToPHi: REAL; distance2ToPLo _ DistanceSquaredPointToPoint[pt, seg.pLo]; distance2ToPHi _ DistanceSquaredPointToPoint[pt, seg.pHi]; RETURN[Min[distance2ToPLo, distance2ToPHi]]; }; DistancePointToTrigLineSeg: PUBLIC PROC [pt: Point2d, seg: TrigLineSeg] RETURNS [distance: REAL] = { <> projectedPt: Point2d _ PointProjectedOntoTrigLine[pt, seg.line]; nearEndpoint: Point2d; IF TrigLinePointOnTrigLineSeg[projectedPt, seg] THEN distance _ TrigLineDistance[pt, seg.line] ELSE { nearEndpoint _ NearestEndpoint[pt, seg]; distance _ DistancePointToPoint[pt, nearEndpoint]; }; }; DistanceSquaredPointToTrigLineSeg: PUBLIC PROC [pt: Point2d, seg: TrigLineSeg] RETURNS [distanceSquared: REAL] = { <> projectedPt: Point2d _ PointProjectedOntoTrigLine[pt, seg.line]; IF TrigLinePointOnTrigLineSeg[projectedPt, seg] THEN {distanceSquared _ TrigLineDistance[pt, seg.line]; distanceSquared _ distanceSquared*distanceSquared} ELSE distanceSquared _ DistanceSquaredToNearestEndpoint[pt, seg]; }; <> DistancePointToPoint: PUBLIC PROC [p1, p2: Point2d] RETURNS [distance: REAL] = { distance _ RealFns.SqRt[(p2[2]-p1[2])*(p2[2]-p1[2]) + (p2[1]-p1[1])*(p2[1]-p1[1])]; }; DistanceSquaredPointToPoint: PUBLIC PROC [p1, p2: Point2d] RETURNS [distance: REAL] = { distance _ (p2[2]-p1[2])*(p2[2]-p1[2]) + (p2[1]-p1[1])*(p2[1]-p1[1]); }; PointLeftOfTrigLine: PUBLIC PROC [distance: REAL, pOnLine: Point2d, line: TrigLine] RETURNS [point: Point2d] = { <> <> <<1) The line parallel to "line" but distance to its left>> <<2) The line perpendicular to "line" at pOnLine.>> <> lineParallel, linePerp: TrigLine; parallel: BOOL; lineParallel _ CreateEmptyTrigLine[]; linePerp _ CreateEmptyTrigLine[]; FillTrigLineFromCoefficients[line.s, line.c, line.d + distance, lineParallel]; FillTrigLineAsNormal[line, pOnLine, linePerp]; [point, parallel] _ TrigLineMeetsTrigLine[lineParallel, linePerp]; IF parallel THEN ERROR; -- perpendicular lines are not parallel }; <> CreateLineSeg: PUBLIC PROC [v1, v2: Point2d] RETURNS [seg: LineSeg] = { x2MinusX1: REAL; seg _ NEW[LineSegObj]; seg.p1 _ v1; seg.p2 _ v2; x2MinusX1 _ v2[1] - v1[1]; IF x2MinusX1 = 0 THEN { seg.isVert _ TRUE; seg.xInt _ v1[1]; seg.yInt _ 0;-- a dummy value RETURN} ELSE { seg.isVert _ FALSE; seg.slope _ (v2[2] - v1[2])/x2MinusX1; <> seg.yInt _ v1[2] - seg.slope*v1[1]; RETURN}; }; <> CreateRay: PUBLIC PROC [basePoint: Point2d, direction: Vector2d] RETURNS [ray: Ray2d] = { ray _ NEW[Ray2dObj _ [basePoint, direction]]; }; CreateRayFromPoints: PUBLIC PROC [p1, p2: Point2d] RETURNS [ray: Ray2d] = { ray _ NEW[Ray2dObj _ [p1, SVVector2d.Sub[p2, p1]]]; }; RightHorizontalRay: PUBLIC PROC [point: Point2d] RETURNS [horizRayThruPoint: Ray2d] = { horizRayThruPoint _ NEW[Ray2dObj _ [point, [1,0]]]; }; UpVerticalRay: PUBLIC PROC [point: Point2d] RETURNS [vertRayThruPoint: Ray2d] = { vertRayThruPoint _ NEW[Ray2dObj _ [point, [0,1]]]; }; RayMeetsBox: PUBLIC PROC [ray: Ray2d, xmin, ymin, xmax, ymax: REAL] RETURNS [count: NAT, params: ARRAY[1..2] OF REAL] = { <> almostZero: REAL _ 1.0e-12; x, y, t: REAL; count _ 0; <> <> IF Abs[ray.d[2]] > almostZero THEN { -- intersection occurs t _ (ymax-ray.p[2])/ray.d[2]; x _ ray.p[1] + t*ray.d[1]; IF x >=xmin AND x<= xmax AND t>=0 THEN { -- hits box count _ count + 1; params[count] _ t; }; <> t _ (ymin-ray.p[2])/ray.d[2]; x _ ray.p[1] + t*ray.d[1]; IF x >=xmin AND x<= xmax AND t>=0 THEN { -- hits box count _ count + 1; params[count] _ t; }; }; <> IF Abs[ray.d[1]] > almostZero THEN { -- intersection occurs t _ (xmax-ray.p[1])/ray.d[1]; y _ ray.p[2] + t*ray.d[2]; IF y >=ymin AND y<= ymax AND t>=0 THEN { -- hits box count _ count + 1; params[count] _ t; }; <> t _ (xmin-ray.p[1])/ray.d[1]; y _ ray.p[2] + t*ray.d[2]; IF y >=ymin AND y<= ymax AND t>=0 THEN { -- hits box count _ count + 1; params[count] _ t; }; }; IF count = 2 THEN { IF params[2] < params[1] THEN { temp: REAL _ params[1]; params[1] _ params[2]; params[2] _ temp; }; }; -- make sure hits are sorted }; -- end of RayMeetsBox LineRayMeetsBox: PUBLIC PROC [ray: Ray2d, xmin, ymin, xmax, ymax: REAL] RETURNS [count: NAT, params: ARRAY[1..2] OF REAL] = { <> almostZero: REAL _ 1.0e-12; x, y, t: REAL; count _ 0; <> <> IF Abs[ray.d[2]] > almostZero THEN { -- intersection occurs t _ (ymax-ray.p[2])/ray.d[2]; x _ ray.p[1] + t*ray.d[1]; IF x >=xmin AND x<= xmax THEN { -- hits box count _ count + 1; params[count] _ t; }; <> t _ (ymin-ray.p[2])/ray.d[2]; x _ ray.p[1] + t*ray.d[1]; IF x >=xmin AND x<= xmax THEN { -- hits box count _ count + 1; params[count] _ t; }; }; <> IF Abs[ray.d[1]] > almostZero THEN { -- intersection occurs t _ (xmax-ray.p[1])/ray.d[1]; y _ ray.p[2] + t*ray.d[2]; IF y >=ymin AND y<= ymax THEN { -- hits box count _ count + 1; params[count] _ t; }; <> t _ (xmin-ray.p[1])/ray.d[1]; y _ ray.p[2] + t*ray.d[2]; IF y >=ymin AND y<= ymax THEN { -- hits box count _ count + 1; params[count] _ t; }; }; IF count = 2 THEN { IF params[2] < params[1] THEN { temp: REAL _ params[1]; params[1] _ params[2]; params[2] _ temp; }; }; -- make sure hits are sorted }; -- end of LineRayMeetsBox EvalRay: PUBLIC PROC [ray: Ray2d, param: REAL] RETURNS [point: Point2d] = { point[1] _ ray.p[1] + param*ray.d[1]; point[2] _ ray.p[2] + param*ray.d[2]; }; <> Abs: PRIVATE PROC [r: REAL] RETURNS [REAL] = { RETURN [IF r>=0 THEN r ELSE -r]; }; Min: PRIVATE PROC [a, b: REAL] RETURNS [REAL] = { RETURN [IF a <= b THEN a ELSE b]; }; Between: PRIVATE PROC [test, a, b: REAL] RETURNS [BOOL] = { SELECT a FROM < b => RETURN [a <= test AND test <= b]; = b => RETURN [test = b]; > b => RETURN [b <= test AND test <= a]; ENDCASE => ERROR; }; END.