File: SVLines2dImpl.mesa
Author: Eric Bier on October 1, 1982 9:03 pm
Last edited by Bier on January 28, 1987 2:24:26 pm PST
Contents: Routines for finding the intersections of various types of lines and line segments used in the SolidViews package.
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] = {
Recall y*c - x*s - d = 0;
Calculates the different parts of a line given an ordered pair of points (the tail and the head). Trig lines are directed in sense since 0 <= line.theta <= 180implies that v1 is lower than or to the right of) v2.
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;
we have -x*s = d. where s = 1. Plug in v1. -v1[1]*s = d
line.d ← -v1[1]}
ELSE { -- line goes down
line.theta ← -90;
line.s ← -1;
we have -x*s = d. where s = -1. Plug in v1. -v1[1]*s = d
line.d ← v1[1]};
line.c ← 0;
line.slope and line.yInt are meaningless. Leave them uninitialized.
}
s, c, theta, d, slope, yInt
ELSE {
line.slope ← y2Minusy1/x2Minusx1;
line.theta ← RealFns.ArcTanDeg[y2Minusy1, x2Minusx1];
line.c ← RealFns.CosDeg[line.theta];
line.s ← RealFns.SinDeg[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)
d ← y1c - x1s. Subsitute in a point to find d.
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] = {
recall y*c - x*s - d = 0;
Calculates the different parts of a line given c, s and d.
line.s ← sineOfTheta;
line.c ← cosineOfTheta;
line.d ← distance;
line.theta ← RealFns.ArcTanDeg[sineOfTheta, cosineOfTheta];
If line is not vertical we can find its slope and y intercept.
IF cosineOfTheta # 0 THEN {
line.slope ← sineOfTheta/cosineOfTheta;
y intercept occurs when x = 0, ie when y*c = d. y = d/c;
line.yInt ← line.d/line.c};
}; -- end of FillTrigLineFromCoefficients
FillTrigLineAsNormal: PUBLIC PROC [line: TrigLine, pt: Point2d, normalLine: TrigLine] = {
Find a line which is perpendicular to "line" and passes thru "pt". Useful for dropping perpendiculars.
If line has the form: y*cos(theta) - x*sin(theta) - d = 0, then normalLine will have the form:
y*cos(theta+90) - x*sin(theta+90) - D = 0;
or -y*sin(theta) - (x*cos(theta)) - D = 0;
to find D, we substitute in pt:
D = -pt[2]*sin(theta) - pt[1]*cos(theta);
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;
y intercept occurs when x = 0, ie when y*c = d. y = d/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;
If line1 is of the form: c1*y - s1*x - d1 = 0;
and line2 of the form: c2*y - s2*x - d2 = 0;
then we solve simultaneously.
c1c2*y - s1c2*x - c2d1 = 0
c1c2*y - c1s2*x - c1d2 = 0
(-s1c2+c1s2)*x - c2d1 + c1d2 = 0;
x = (c2d1 - c1d2)/(s2c1 -s1c2);
s2c1*y - s1s2*x - s2d1 = 0;
s1c2*y - s1s2*x -s1d2 = 0;
(s2c1 - s1c2)*y -s2d1 + s1d2 = 0;
y = (s2d1 - s1d2)/(s2c1 - s1c2);
(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] = {
Find the intersection of line with the line of seg. See if this point is within the bounds of seg.
};
TrigLineDistance: PUBLIC PROC [pt: Point2d, line: TrigLine] RETURNS [d: REAL] = {
Because of the data representation of a TrigLineSeg, plugging the point into the line equation gives us the distance.
ie. distance = y*cos(theta) - x*sin(theta) - d;
d ← pt[2]*line.c - pt[1]*line.s - line.d;
}; -- end of TrigDistance
PointProjectedOntoTrigLine: PUBLIC PROC [pt: Point2d, line: TrigLine] RETURNS [projectedPt: Point2d] = {
We drop a normal from the point onto the line and find where it hits.
The line equation of the normal we drop can be found as in FillTrigLineAsNormal above.
We will have line equations:
c*y - s*x - d = 0, and
-sy - c*x - D = 0
Solving simultaneously for this special case, we have:
x = (d + D)/-2*s
y = (d - D)/2*c
This equation is singular when the determinant -2sc = 0, ie when s = 0 or c = 0
corresponding to horizontal and vertical lines. We handle these separately.
D: REAL ← -pt[2]*line.c - pt[1]*line.s;
IF line.s = 0 THEN { -- horizontal line. c = +-1. So y = d/c or equivalently y = cd;
projectedPt[1] ← pt[1];
projectedPt[2] ← IF line.c < 0 THEN -line.d ELSE line.d;
RETURN};
IF line.c = 0 THEN { -- vertical line. s = +-1. So x = -d/s or equivalently x = -ds.
projectedPt[1] ← IF line.s < 0 THEN line.d ELSE -line.d;
projectedPt[2] ← pt[2];
RETURN};
projectedPt[1] ← -(line.d + D)/(2*line.s);
projectedPt[2] ← (line.d - D)/(2*line.c);
};
PointProjectedOntoTrigLine: PUBLIC PROC [pt: Point2d, line: TrigLine] RETURNS [projectedPt: Point2d] = {
We drop a normal from the point onto the line and find where it hits.
The line equation of the normal we drop can be found using FillTrigLineAsNormal above.
We will have line equations:
c*y - s*x - d = 0. The vector v = [c, s] is the unit vector parallel to line. The vector
l = [-s, c] is the unit vector 90 degrees counter-clockwise of v. If pt is distance D from line (along l), then the new point we want is pt-D*l. Componentwise:
projectedPt[1] ← pt[1] + D*s.
projectedPt[2] ← pt[2] - D*c.
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;
};
TRIGLINESEGS
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] = {
Assumes pt is on seg.line. Is it on seg?
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] = {
Look for an obvious winner first. If that fails, do math.
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] = {
perpendicular distance if possible, else distance to nearest endpoint.
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] = {
Perpendicular distance if possible, else distance to nearest endpoint.
projectedPt: Point2d ← PointProjectedOntoTrigLine[pt, seg.line];
IF TrigLinePointOnTrigLineSeg[projectedPt, seg]
THEN {distanceSquared ← TrigLineDistance[pt, seg.line];
distanceSquared ← distanceSquared*distanceSquared}
ELSE distanceSquared ← DistanceSquaredToNearestEndpoint[pt, seg];
};
POINT2DS
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] = {
point is a point to the left of the directed line, on the normal to the line which intersects the line at pOnLine. If distance is negative, the point will be to the right of the directed line.
Method: The point we want will be at the intersection these two lines
1) The line parallel to "line" but distance to its left
2) The line perpendicular to "line" at pOnLine.
We can generate both of these easily as follows:
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
};
LINESEGS
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;
to find y intercept, substitute in the first point. yInt = y - slope*x;
seg.yInt ← v1[2] - seg.slope*v1[1];
RETURN};
};
RAYS
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] = {
Find intersections of the directed half-line only.
We can take advantage of the horizontal and vertical lines of the box to do an easy intersection test. Note that we are really testing for line intersections rather than ray intersections.
almostZero: REAL ← 1.0e-12;
x, y, t: REAL;
count ← 0;
Top Line
The top line has equation y = ymax. If ray.d[2] = 0, we don't hit this line. Otherwise, we use y(t) = ymax = ray.p[2]+t*ray.d[2]; Solve for t: t = (ymax-ray.p[2])/ray.d[2].
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;
};
Bottom Line
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;
};
};
Right Line
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;
};
Left Line
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] = {
We can take advantage of the horizontal and vertical lines of the box to do an easy intersection test. Note that we are really testing for line intersections rather than ray intersections.
almostZero: REAL ← 1.0e-12;
x, y, t: REAL;
count ← 0;
Top Line
The top line has equation y = ymax. If ray.d[2] = 0, we don't hit this line. Otherwise, we use y(t) = ymax = ray.p[2]+t*ray.d[2]; Solve for t: t = (ymax-ray.p[2])/ray.d[2].
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;
};
Bottom Line
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;
};
};
Right Line
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;
};
Left Line
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];
};
UTILITY FUNCTIONS
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.