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];
};
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.
x2Minusx1: REAL ← v2[1] - v1[1];
y2Minusy1: REAL ← v2[2] - v1[2];
IF x2Minusx1 = 0
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
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] = {
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
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;
};
TrigLineFromPoints:
PUBLIC
PROC [v1, v2: Point2d]
RETURNS [line: TrigLine] = {
line ← CreateEmptyTrigLine[];
FillTrigLineFromPoints[v1, v2, line];
};
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
-cy - s*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);
};
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.Difference[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;
};