SVLines3dImpl.mesa
Copyright © 1986 by Xerox Corporation. All rights reserved.
Last edited by Bier on February 18, 1987
Contents: Procedures for performing geometric operations on lines and points in 3-space.
DIRECTORY
Matrix3d, RealFns, SV3d, SVLines3d, SVVector3d;
SVLines3dImpl: CEDAR PROGRAM
IMPORTS Matrix3d, RealFns, SVVector3d
EXPORTS SVLines3d
=
BEGIN
Edge3d: TYPE = SV3d.Edge3d;
Edge3dObj: TYPE = SV3d.Edge3dObj;
Line3d: TYPE = SV3d.Line3d;
Line3dObj: TYPE = SV3d.Line3dObj;
Matrix4by4: TYPE = SV3d.Matrix4by4;
Point3d: TYPE = SV3d.Point3d;
Vector3d: TYPE = SV3d.Vector3d;
Making Lines
CreateEmptyLine: PUBLIC PROC [] RETURNS [line: Line3d] = {
line ← NEW[Line3dObj];
};
CopyLine: PUBLIC PROC [from: Line3d, to: Line3d] = {
to^ ← from^;
};
EqualLine: PUBLIC PROC [a: Line3d, b: Line3d] RETURNS [BOOL] = {
RETURN[a.base = b.base AND a.direction = b.direction];
};
AlmostEqualLine: PUBLIC PROC [a: Line3d, b: Line3d, errorDegrees: REAL, errorDistance: REAL] RETURNS [BOOL] = {
angle: REAL ← SVVector3d.AngleCCWBetweenVectors[a.direction, b.direction];
angleOK: BOOL ← ABS[angle] < errorDegrees OR ABS[angle-180.0] < errorDegrees;
IF NOT angleOK THEN RETURN[FALSE];
RETURN[ABS[
DistancePointToLine[[0.0, 0.0, 0.0], a] -
DistancePointToLine[[0.0, 0.0, 0.0], b]] < errorDistance
];
};
LineFromPoints: PUBLIC PROC [v1, v2: Point3d] RETURNS [line: Line3d] = {
line ← CreateEmptyLine[];
FillLineFromPoints[v1, v2, line];
};
LineFromPointAndVector: PUBLIC PROC [pt: Point3d, vec: Vector3d] RETURNS [line: Line3d] = {
line ← CreateEmptyLine[];
FillLineFromPointAndVector[pt, vec, line];
};
LineNormalToLineThruPoint: PUBLIC PROC [line: Line3d, pt: Point3d] RETURNS [normalLine: Line3d] = {
line and pt determine a plane. Within that plane, compute the line that is perpendicular to line and passes throught pt.
};
FillLineFromPoints: PUBLIC PROC [v1, v2: Point3d, line: Line3d] = {
Our first job is to find the nearest point on the line to the origin.
base = v1 + [(O-v1)N] N.
OPEN SVVector3d;
line.direction ← Normalize[Sub[v2, v1]];
line.base ← Add[v1, Scale[line.direction, DotProduct[Sub[[0,0,0], v1], line.direction]]];
};
FillLineFromPoints: PUBLIC PROC [v1, v2: Point3d, line: Line3d] = {
Our first job is to find the nearest point on the line to the origin.
originInWorld: Point3d = [0,0,0];
originInLine: Point3d;
lineInWorld, worldInLine: Matrix4by4;
zInLine: REAL;
line.direction ← SVVector3d.Normalize[SVVector3d.Sub[v2, v1]];
lineInWorld ← Matrix3d.MakeHorizontalMatFromZAxis[line.direction, v1];
worldInLine ← Matrix3d.Inverse[lineInWorld];
originInLine ← Matrix3d.Update[originInWorld, worldInLine];
zInLine ← originInLine[3];
line.base ← SVVector3d.Add[v1, SVVector3d.Scale[line.direction, zInLine]];
line.mat ← Matrix3d.SetOrigin[lineInWorld, line.base];
};
FillLineFromPointAndVector: PUBLIC PROC [pt: Point3d, vec: Vector3d, line: Line3d] = {
p2: Point3d ← SVVector3d.Add[pt, vec];
FillLineFromPoints[pt, p2, line];
};
FillLineNormalToLineThruPoint: PUBLIC PROC [line: Line3d, pt: Point3d, normalLine: Line3d] = {
};
line and pt determine a plane. Within that plane, compute the line that is perpendicular to line and passes throught pt.
Making Edges
CreateEmptyEdge: PUBLIC PROC RETURNS [edge: Edge3d] = {
edge ← NEW[Edge3dObj];
edge.line ← CreateEmptyLine[];
};
CopyEdge: PUBLIC PROC [from: Edge3d, to: Edge3d] = {
CopyLine[from.line, to.line];
to.start ← from.start;
to.end ← from.end;
to.startIsFirst ← from.startIsFirst;
}; -- end of CopyEdge
FillEdge: PUBLIC PROC [v1, v2: Point3d, edge: Edge3d] = {
FillLineFromPoints[v1, v2, edge.line];
edge.start ← v1;
edge.end ← v2;
}; -- end of FillEdge
FillEdgeTransform: PUBLIC PROC [fixed: Edge3d, transform: Matrix4by4, edge: Edge3d] = {
start, end: Point3d;
start ← Matrix3d.Update[fixed.start, transform];
end ← Matrix3d.Update[fixed.end, transform];
FillEdge[start, end, edge];
};
CreateEdge: PUBLIC PROC [v1, v2: Point3d] RETURNS [edge: Edge3d] = {
edge ← CreateEmptyEdge[];
FillEdge[v1, v2, edge];
}; -- end of CreateEdge
Direction and Distance for Lines
DirectionOfLine: PUBLIC PROC [line: Line3d] RETURNS [direction: Vector3d] = {
Returns the unit direction vector of line.
direction ← line.direction;
};
DistancePointToLine: PUBLIC PROC [pt: Point3d, line: Line3d] RETURNS [d: REAL] = {
U: Point3d;
U ← DropPerpendicular[pt, line];
d ← SVVector3d.Distance[pt, U];
};
Distance2PointToLine: PUBLIC PROC [pt: Point3d, line: Line3d] RETURNS [d2: REAL] = {
U: Point3d;
U ← DropPerpendicular[pt, line];
d2 ← SVVector3d.DistanceSquared[pt, U];
};
[Artwork node; type 'Artwork on' to command tool]
Dropping a perpendicular from V to line [A, N].
DropPerpendicular: PUBLIC PROC [pt: Point3d, line: Line3d] RETURNS [projectedPt: Point3d] = {
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 FillLineNormalToLineThruPoint above.
The projectedPt U is:
U = A + [(V-A)N] N.
OPEN SVVector3d;
projectedPt ← Add[line.base, Scale[line.direction, DotProduct[Sub[pt,line.base], line.direction]]];
};
[Artwork node; type 'Artwork on' to command tool]
Find the points of closest approach of lines [A, N] and [B, M].
NearLinePoints: PUBLIC PROC [l1, l2: Line3d] RETURNS [p1, p2: Point3d, parallel: BOOL ← FALSE] = {
Method: Let l1 be (A, N) and l2 be (B, M). Let p1 be U and p2 be V.
Find U on (A, N) and V on (B, M) such that
(U-V)M = 0
(U-V)N = 0.
Let k = MN. If k H 1 then d = DistancePointToLine[A, (B, M)].
Otherwise,
U = A + [((B-A)P)/(1-k2)] N, where P = N- kM.
V = B + [(U-B)M] M.
d = Distance[U, V].
OPEN SVVector3d;
A, B: Point3d;
N, M, P: Vector3d;
k: REAL;
A ← l1.base; N ← l1.direction; B ← l2.base; M ← l2.direction;
k ← DotProduct[M,N];
IF ABS[1.0-ABS[k]] < 3.808737e-5 -- within 0.5 degrees of parallel. Numerically stable this ain't.
THEN {parallel ← TRUE; RETURN};
P ← Sub[N, Scale[M, k]];
p1 ← Add[A, Scale[N, DotProduct[Sub[B,A], P]/(1.0-k*k)]];
p2 ← Add[B, Scale[M, DotProduct[Sub[p1,B], M]]];
};
DistanceLineToLine: PUBLIC PROC [l1, l2: Line3d] RETURNS [d: REAL] = {
OPEN SVVector3d;
U, V: Point3d;
parallel: BOOL;
[U, V, parallel] ← NearLinePoints[l1, l2];
IF parallel THEN RETURN[DistancePointToLine[l1.base, l2]];
d ← Distance[U,V];
};
Distance for Edges
Comparing Edges to Points
NearEndpointToPoint: PUBLIC PROC [pt: Point3d, edge: Edge3d] RETURNS [endpoint: Point3d] = {
Look for an obvious winner first. If that fails, do math.
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]] AND
ABS[pt[3]-edge.start[3]] <= ABS[pt[3]-edge.end[3]] THEN RETURN[edge.start]
ELSE GOTO DoMath
}
ELSE {
IF ABS[pt[2]-edge.start[2]] > ABS[pt[2]-edge.end[2]] AND
ABS[pt[3]-edge.start[3]] > ABS[pt[3]-edge.end[3]] THEN RETURN[edge.end]
ELSE GOTO DoMath;
};
EXITS
DoMath =>
IF SVVector3d.DistanceSquared[pt, edge.start] <= SVVector3d.DistanceSquared[pt, edge.end]
THEN endpoint ← edge.start
ELSE endpoint ← edge.end;
};
Distance2PointToEndpoint: PUBLIC PROC [pt: Point3d, edge: Edge3d] RETURNS [distanceSquared: REAL] = {
distance2ToPLo, distance2ToPHi: REAL;
distance2ToPLo ← SVVector3d.DistanceSquared[pt, edge.start];
distance2ToPHi ← SVVector3d.DistanceSquared[pt, edge.end];
RETURN[MIN[distance2ToPLo, distance2ToPHi]];
};
NearEdgePointToPoint: PUBLIC PROC [pt: Point3d, edge: Edge3d] RETURNS [onEdge: Point3d] = {
projectedPt: Point3d ← DropPerpendicular[pt, edge.line];
IF LinePointOnEdge[projectedPt, edge] THEN onEdge ← projectedPt
ELSE onEdge ← NearEndpointToPoint[pt, edge];
};
Distance2PointToEdge: PUBLIC PROC [pt: Point3d, edge: Edge3d] RETURNS [distanceSquared: REAL] = {
Perpendicular distance if possible, else distance to nearest endpoint.
projectedPt: Point3d ← DropPerpendicular[pt, edge.line];
IF LinePointOnEdge[projectedPt, edge]
THEN distanceSquared ← SVVector3d.DistanceSquared[pt, projectedPt]
ELSE distanceSquared ← Distance2PointToEndpoint[pt, edge];
};
DistancePointToEdge: PUBLIC PROC [pt: Point3d, edge: Edge3d] RETURNS [distance: REAL] = {
perpendicular distance if possible, else distance to nearest endpoint.
projectedPt: Point3d ← DropPerpendicular[pt, edge.line];
nearEndpoint: Point3d;
IF LinePointOnEdge[projectedPt, edge]
THEN distance ← SVVector3d.Distance[pt, projectedPt]
ELSE {
nearEndpoint ← NearEndpointToPoint[pt, edge];
distance ← SVVector3d.Distance[pt, nearEndpoint];
};
};
LinePointOnEdge: PUBLIC PROC [pt: Point3d, edge: Edge3d] RETURNS [BOOL] = {
Assumes pt is on edge.line. Is it on edge?
dx, dy, dz, max: REAL;
dx ← ABS[edge.end[1] - edge.start[1]];
dy ← ABS[edge.end[2] - edge.start[2]];
dz ← ABS[edge.end[3] - edge.start[3]];
max ← MAX[dx, dy, dz];
IF dx = max THEN RETURN[Between[pt[1], edge.start[1], edge.end[1]]]
ELSE IF dy = max THEN RETURN[Between[pt[2], edge.start[2], edge.end[2]]]
ELSE RETURN[Between[pt[3], edge.start[3], edge.end[3]]];
}; -- end of LinePointOnEdge
Comparing Edges to Lines
NearEndpointToLine: PUBLIC PROC [line: Line3d, edge: Edge3d] RETURNS [endpoint: Point3d] = {
distance2ToPLo, distance2ToPHi: REAL;
distance2ToPLo ← Distance2PointToLine[edge.start, line];
distance2ToPHi ← Distance2PointToLine[edge.end, line];
IF distance2ToPLo <= distance2ToPHi THEN endpoint ← edge.start
ELSE endpoint ← edge.end;
};
DistanceLineToEndpoint: PROC [line: Line3d, edge: Edge3d] RETURNS [distance: REAL] = {
distance2ToPLo, distance2ToPHi: REAL;
distance2ToPLo ← Distance2PointToLine[edge.start, line];
distance2ToPHi ← Distance2PointToLine[edge.end, line];
IF distance2ToPLo <= distance2ToPHi THEN distance ← RealFns.SqRt[distance2ToPLo]
ELSE distance ← RealFns.SqRt[distance2ToPHi];
};
DistanceAndEndpointLineToEdge: PROC [line: Line3d, edge: Edge3d] RETURNS [distance: REAL, endpoint: Point3d] = {
distance2ToPLo, distance2ToPHi: REAL;
distance2ToPLo ← Distance2PointToLine[edge.start, line];
distance2ToPHi ← Distance2PointToLine[edge.end, line];
IF distance2ToPLo <= distance2ToPHi THEN {
endpoint ← edge.start;
distance ← RealFns.SqRt[distance2ToPLo];
}
ELSE {
endpoint ← edge.end;
distance ← RealFns.SqRt[distance2ToPHi];
};
};
DistanceLineToEdge: PUBLIC PROC [line: Line3d, edge: Edge3d] RETURNS [distance: REAL] = {
U, V: Point3d;
parallel: BOOL;
[U, V, parallel] ← NearLinePoints[line, edge.line];
IF parallel THEN RETURN[DistancePointToLine[line.base, edge.line]];
IF LinePointOnEdge[V, edge] THEN distance ← SVVector3d.Distance[U,V]
ELSE distance ← DistanceLineToEndpoint[line, edge];
};
NearEdgePointToLine: PUBLIC PROC [line: Line3d, edge: Edge3d] RETURNS [edgePoint: Point3d] = {
U, V: Point3d;
parallel: BOOL;
[U, V, parallel] ← NearLinePoints[line, edge.line];
IF parallel THEN RETURN[edge.start]; -- any point on edge will do
IF LinePointOnEdge[V, edge] THEN edgePoint ← V
ELSE edgePoint ← NearEndpointToLine[line, edge];
};
DistanceAndPointLineToEdge: PUBLIC PROC [line: Line3d, edge: Edge3d] RETURNS [distance: REAL, edgePoint: Point3d] = {
U, V: Point3d;
parallel: BOOL;
[U, V, parallel] ← NearLinePoints[line, edge.line];
IF parallel THEN {
distance ← DistancePointToLine[line.base, edge.line];
edgePoint ← edge.start; -- any point on edge will do
}
ELSE {
IF LinePointOnEdge[V, edge] THEN {
distance ← SVVector3d.Distance[U,V];
edgePoint ← V;
}
ELSE {
[distance, edgePoint] ← DistanceAndEndpointLineToEdge[line, edge];
distance ← DistanceLineToEndpoint[line, edge];
edgePoint ← NearEndpointToLine[line, edge];
};
};
};
UTILITY FUNCTIONS
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.