<> <> <> <> DIRECTORY RealFns, GGAngle, GGModelTypes, GGVector, GGLines; GGLinesImpl: CEDAR PROGRAM IMPORTS RealFns, GGAngle, GGVector EXPORTS GGLines = BEGIN Point: TYPE = GGModelTypes.Point; Edge: TYPE = REF EdgeObj; EdgeObj: TYPE = GGModelTypes.EdgeObj; Line: TYPE = REF LineObj; LineObj: TYPE = GGModelTypes.LineObj; Ray: TYPE = REF RayObj; RayObj: TYPE = GGModelTypes.RayObj; Vector: TYPE = GGModelTypes.Vector; CreateEmptyLine: PUBLIC PROC RETURNS [line: Line] = { line _ NEW[LineObj]; }; CopyLine: PUBLIC PROC [from: Line, to: Line] = { to.c _ from.c; to.s _ from.s; to.theta _ from.theta; to.d _ from.d; to.slope _ from.slope; to.yInt _ from.yInt; }; FillLineFromPoints: PUBLIC PROC [v1, v2: Point, line: Line] = { <> <> epsilon: REAL = 1.0e-5; -- changed from 1.0e-6 on August 13, 1985 6:29:49 pm PDT by Bier 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 FillLineFromPoints FillLineFromPointAndVector: PUBLIC PROC [pt: Point, vec: Vector, line: Line] = { pt2: Point; pt2 _ GGVector.Add[pt, vec]; FillLineFromPoints[pt, pt2, line]; }; FillLineFromCoefficients: PUBLIC PROC [sineOfTheta, cosineOfTheta, distance: REAL, line: Line] = { <> <> 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 FillLineFromCoefficients FillLineFromPointAndAngle: PUBLIC PROC [pt: Point, degrees: REAL, line: Line] = { direction: Vector; direction _ GGVector.VectorFromAngle[degrees]; FillLineFromPointAndVector[pt, direction, line]; }; -- end of FillLineFromPointAndAngle FillLineAsNormal: PUBLIC PROC [line: Line, pt: Point, normalLine: Line] = { <> <> <> <> <> <> normalLine.s _ line.c; normalLine.c _ -line.s; normalLine.d _ -pt[2]*line.s - pt[1]*line.c; normalLine.theta _ GGAngle.Add[line.theta, 90]; IF normalLine.c #0 THEN { normalLine.slope _ normalLine.s/normalLine.c; <> line.yInt _ normalLine.d/normalLine.c}; }; -- end of FillLineAsNormal FillLineLeftOfLine: PUBLIC PROC [line: Line, dist: REAL, parallelLine: Line] = { parallelLine.s _ line.s; parallelLine.c _ line.c; parallelLine.d _ line.d + dist; parallelLine.theta _ line.theta; parallelLine.slope _ line.slope; }; FillLineRightOfLine: PUBLIC PROC [line: Line, dist: REAL, parallelLine: Line] = { parallelLine.s _ line.s; parallelLine.c _ line.c; parallelLine.d _ line.d - dist; parallelLine.theta _ line.theta; parallelLine.slope _ line.slope; }; LineFromPoints: PUBLIC PROC [v1, v2: Point] RETURNS [line: Line] = { line _ CreateEmptyLine[]; FillLineFromPoints[v1, v2, line]; }; LineFromPointAndVector: PUBLIC PROC [pt: Point, vec: Vector] RETURNS [line: Line] = { pt2: Point; pt2 _ GGVector.Add[pt, vec]; line _ LineFromPoints[pt, pt2]; }; LineFromCoefficients: PUBLIC PROC [sineOfTheta, cosineOfTheta, distance: REAL] RETURNS [line: Line] = { line _ CreateEmptyLine[]; FillLineFromCoefficients[sineOfTheta, cosineOfTheta, distance, line]; }; LineFromPointAndAngle: PUBLIC PROC [pt: Point, degrees: REAL] RETURNS [line: Line] = { line _ CreateEmptyLine[]; FillLineFromPointAndAngle[pt, degrees, line]; }; LineNormalToLineThruPoint: PUBLIC PROC [line: Line, pt: Point] RETURNS [normalLine: Line] = { normalLine _ CreateEmptyLine[]; FillLineAsNormal[line, pt, normalLine]; }; LineLeftOfLine: PUBLIC PROC [line: Line, dist: REAL] RETURNS [parallelLine: Line] = { parallelLine _ CreateEmptyLine[]; FillLineLeftOfLine[line, dist, parallelLine]; }; LineRightOfLine: PUBLIC PROC [line: Line, dist: REAL] RETURNS [parallelLine: Line] = { parallelLine _ CreateEmptyLine[]; FillLineRightOfLine[line, dist, parallelLine]; }; LineMeetsLine: PUBLIC PROC [line1, line2: Line] RETURNS [intersection: Point, 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; }; LineMeetsYAxis: PUBLIC PROC [line: Line] 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;} }; LineMeetsEdge: PUBLIC PROC [line: Line, edge: Edge] RETURNS [intersection: Point, noHit: BOOL] = { <> }; SignedLineDistance: PUBLIC PROC [pt: Point, line: Line] RETURNS [d: REAL] = { <> <> d _ pt[2]*line.c - pt[1]*line.s - line.d; }; -- SignedLineDistance LineDistance: PUBLIC PROC [pt: Point, line: Line] RETURNS [d: REAL] = { <> <> d _ ABS[pt[2]*line.c - pt[1]*line.s - line.d]; }; -- LineDistance PointProjectedOntoLine: PUBLIC PROC [pt: Point, line: Line] RETURNS [projectedPt: Point] = { <> <> <> <> <> <> 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; }; PointOnLine: PUBLIC PROC [line: Line] RETURNS [pt: Point] = { <> IF ABS[line.c] > ABS[line.s] THEN { pt[1] _ 0.0; pt[2] _ line.d/line.c; } ELSE { pt[2] _ 0.0; pt[1] _ -line.d/line.s; }; }; <<>> DirectionOfLine: PUBLIC PROC [line: Line] RETURNS [direction: Vector] = { <> direction[1] _ line.c; direction[2] _ line.s; }; <> CreateEdge: PUBLIC PROC [v1, v2: Point] RETURNS [edge: Edge] = { edge _ CreateEmptyEdge[]; FillEdge[v1, v2, edge]; }; -- end of CreateEdge CreateEmptyEdge: PUBLIC PROC RETURNS [edge: Edge] = { edge _ NEW[EdgeObj]; edge.line _ CreateEmptyLine[]; }; FillEdge: PUBLIC PROC [v1, v2: Point, edge: Edge] = { y2Minusy1: REAL; FillLineFromPoints[v1, v2, edge.line]; y2Minusy1 _ v2[2] - v1[2]; IF y2Minusy1 = 0 THEN IF v2[1] > v1[1] THEN {edge.end _ v2; edge.start _ v1; edge.startIsFirst _ TRUE} ELSE {edge.end _ v1; edge.start _ v2; edge.startIsFirst _ FALSE} ELSE IF v2[2] > v1[2] THEN {edge.end _ v2; edge.start _ v1; edge.startIsFirst _ TRUE} ELSE {edge.end _ v1; edge.start _ v2; edge.startIsFirst _ FALSE}; }; -- end of FillEdge CopyEdge: PUBLIC PROC [from: Edge, to: Edge] = { CopyLine[from.line, to.line]; to.start _ from.start; to.end _ from.end; to.startIsFirst _ from.startIsFirst; }; -- end of CopyEdge LinePointOnEdge: PUBLIC PROC [pt: Point, edge: Edge] RETURNS [BOOL] = { <> IF ABS[edge.end[1] - edge.start[1]] >= ABS[edge.end[2] - edge.start[2]] THEN -- line is more horizontal RETURN[Between[pt[1], edge.start[1], edge.end[1]]] ELSE -- line is more vertical RETURN[Between[pt[2], edge.start[2], edge.end[2]]]; }; -- end of LinePointOnEdge NearestEndpoint: PUBLIC PROC [pt: Point, edge: Edge] RETURNS [endpoint: Point] = { <> BEGIN IF ABS[pt[1]-edge.start[1]] <= ABS[pt[1]-edge.end[1]] THEN IF ABS[pt[2]-edge.start[2]] <= ABS[pt[2]-edge.end[2]] THEN RETURN[edge.start] ELSE GOTO DoMath ELSE IF ABS[pt[2]-edge.start[2]] > ABS[pt[2]-edge.end[2]] THEN RETURN[edge.end] ELSE GOTO DoMath; EXITS DoMath => IF DistanceSquaredPointToPoint[pt, edge.start] < DistanceSquaredPointToPoint[pt, edge.end] THEN endpoint _ edge.start ELSE endpoint _ edge.end; END; }; DistanceSquaredToNearestEndpoint: PUBLIC PROC [pt: Point, edge: Edge] RETURNS [distanceSquared: REAL] = { distance2ToPLo, distance2ToPHi: REAL; distance2ToPLo _ DistanceSquaredPointToPoint[pt, edge.start]; distance2ToPHi _ DistanceSquaredPointToPoint[pt, edge.end]; RETURN[MIN[distance2ToPLo, distance2ToPHi]]; }; NearestPointOnEdge: PUBLIC PROC [pt: Point, edge: Edge] RETURNS [onEdge: Point] = { projectedPt: Point _ PointProjectedOntoLine[pt, edge.line]; IF LinePointOnEdge[projectedPt, edge] THEN onEdge _ projectedPt ELSE onEdge _ NearestEndpoint[pt, edge]; }; DistancePointToEdge: PUBLIC PROC [pt: Point, edge: Edge] RETURNS [distance: REAL] = { <> projectedPt: Point _ PointProjectedOntoLine[pt, edge.line]; nearEndpoint: Point; IF LinePointOnEdge[projectedPt, edge] THEN distance _ ABS[LineDistance[pt, edge.line]] ELSE { nearEndpoint _ NearestEndpoint[pt, edge]; distance _ DistancePointToPoint[pt, nearEndpoint]; }; }; DistanceSquaredPointToEdge: PUBLIC PROC [pt: Point, edge: Edge] RETURNS [distanceSquared: REAL] = { <> projectedPt: Point _ PointProjectedOntoLine[pt, edge.line]; IF LinePointOnEdge[projectedPt, edge] THEN {distanceSquared _ LineDistance[pt, edge.line]; distanceSquared _ distanceSquared*distanceSquared} ELSE distanceSquared _ DistanceSquaredToNearestEndpoint[pt, edge]; }; <> DistancePointToPoint: PUBLIC PROC [p1, p2: Point] 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: Point] RETURNS [distance: REAL] = { distance _ (p2[2]-p1[2])*(p2[2]-p1[2]) + (p2[1]-p1[1])*(p2[1]-p1[1]); }; PointLeftOfLine: PUBLIC PROC [distance: REAL, pOnLine: Point, line: Line] RETURNS [point: Point] = { <> <> <<1) The line parallel to "line" but distance to its left>> <<2) The line perpendicular to "line" at pOnLine.>> <> lineParallel, linePerp: Line; parallel: BOOL; lineParallel _ CreateEmptyLine[]; linePerp _ CreateEmptyLine[]; FillLineFromCoefficients[line.s, line.c, line.d + distance, lineParallel]; FillLineAsNormal[line, pOnLine, linePerp]; [point, parallel] _ LineMeetsLine[lineParallel, linePerp]; IF parallel THEN ERROR; -- perpendicular lines are not parallel }; CreateRay: PUBLIC PROC [basePoint: Point, direction: Vector] RETURNS [ray: Ray] = { ray _ NEW[RayObj _ [basePoint, direction]]; }; CreateRayFromPoints: PUBLIC PROC [p1, p2: Point] RETURNS [ray: Ray] = { ray _ NEW[RayObj _ [p1, GGVector.Sub[p2, p1]]]; }; LineRayMeetsBox: PUBLIC PROC [ray: Ray, 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: Ray, param: REAL] RETURNS [point: Point] = { point[1] _ ray.p[1] + param*ray.d[1]; point[2] _ ray.p[2] + param*ray.d[2]; }; <> 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.