G3dNavigateImpl.mesa
Copyright Ó 1988, 1992 by Xerox Corporation. All rights reserved.
Bloomenthal, July 15, 1992 4:10 pm PDT
DIRECTORY G2dContour, G3dNavigate, G3dBasic, G3dMatrix, G3dPlane, G3dVector, Real, RefTab, RuntimeError, Vector2;
G3dNavigateImpl: CEDAR PROGRAM
IMPORTS Contours, G3dMatrix, G3dPlane, G3dVector, RefTab, RuntimeError, Vector2
EXPORTS G3dNavigate
~ BEGIN
OPEN G3dNavigate;
NatPair: TYPE ~ G3dBasic.NatPair;
Errors
Error: PUBLIC SIGNAL [reason: ATOM] = CODE;
Navigating along a Polygonal Surface
ModelFromPtsPolys: PUBLIC PROC [pts: TripleSequence, polys: SurfaceSequence]
RETURNS [model: Model]
~ {
model ← NEW[ModelRep ← [pts: pts]];
model.polys ← NEW[PolysRep[polys.length]];
model.polys.length ← polys.length;
FOR n: NAT IN [0..polys.length) DO
p: Poly ← model.polys[n] ← NEW[PolyRep];
p.indices ← polys[n];
p.plane ← G3dPlane.Normalize[G3dPlane.FromPolygon[pts, p.indices]];
p.lines ← NEW[TripleSequenceRep[p.indices.length]];
p.neighs ← NEW[PolysRep[p.indices.length]];
p.pairs ← NEW[PairSequenceRep[p.indices.length]];
p.majorPlane ← G3dPlane.GetMajorPlane[p.plane];
p.pairs.length ← p.neighs.length ← p.lines.length ← p.indices.length;
FOR n: NAT IN [0..p.indices.length) DO
p.pairs[n] ← G3dPlane.ProjectPointToMajorPlane[pts[p.indices[n]], p.majorPlane];
ENDLOOP;
FOR n: NAT IN [0..p.indices.length) DO
p.lines[n] ← G3dVector.Line2d[p.pairs[n], p.pairs[(n+1) MOD p.indices.length]];
ENDLOOP;
p.convex ← G3dPolygon.IsConvex[p];
ENDLOOP;
SetPolygonNeighbors[model];
};
PointFromPair: PUBLIC PROC [pair: Pair, poly: Polygon] RETURNS [point: Triple] ~ {
plane: Quad ← poly.plane;
point ← SELECT poly.majorPlane FROM
xy   => [pair.x, pair.y, (-plane.w-plane.x*pair.x-plane.y*pair.y)/plane.z],
yz   => [(-plane.w-plane.y*pair.x-plane.z*pair.y)/plane.x, pair.x, pair.y],
ENDCASE => [pair.x, (-plane.w-plane.x*pair.x-plane.z*pair.y)/plane.y, pair.y];
};
PairOn2dLine: PUBLIC PROC [pair: Pair, line: Triple] RETURNS [Border] ~ {
RETURN[SELECT pair.x*line.x+pair.y*line.y+line.z FROM
> 0 => inside, < 0 => outside, ENDCASE => edge];
};
Intersect: PUBLIC PROC [s0, s1: Seg] RETURNS [i: Intersection] ~ {
l0: Triple ← IF s0.line = [0, 0, 0] THEN G3dVector.Line2d[s0.p0, s0.p1] ELSE s0.line;
l1: Triple ← IF s1.line = [0, 0, 0] THEN G3dVector.Line2d[s1.p0, s1.p1] ELSE s1.line;
i.disjoint ←
(s0.p0.x*l1.x+s0.p0.y*l1.y+l1.z > 0) = (s0.p1.x*l1.x+s0.p1.y*l1.y+l1.z > 0) OR
(s1.p0.x*l0.x+s1.p0.y*l0.y+l0.z > 0) = (s1.p1.x*l0.x+s1.p1.y*l0.y+l0.z > 0);
IF NOT i.disjoint THEN i.pt ← G3dVector.IntersectTwoLines2d[l0, l1];
};
MoveSpot: PUBLIC PROC [old: Spot, direction: Triple, distance: REAL] RETURNS [new: Spot] ~ {
new.point ← G3dVector.ScaleRay[[old.point, direction], distance];
new.pair ← G3dPlane.ProjectPointToMajorPlane[new.point, old.poly.majorPlane];
new.poly ← old.poly;
};
IntersectNextPoly: PUBLIC PROC [in, out: Spot] RETURNS [next: Spot] ~ {
i: Intersection;
poly: Poly ← in.poly;
s0: Seg ← [in.pair, out.pair, G3dVector.Line2d[out.pair, in.pair]];
IF poly.convex
THEN
FOR n: NAT IN [0..poly.pairs.length) DO
s1: Seg ← [poly.pairs[n], poly.pairs[(n+1) MOD poly.pairs.length], poly.lines[n]];
IF NOT (i ← Intersect[s0, s1]).disjoint THEN {
next ← [PointFromPair[i.pt, poly], i.pt, poly.neighs[n]];
EXIT;
};
ENDLOOP
ELSE {
iPair: Pair;
iEdge: NAT;
d: REAL ← Real.LargestNumber;
FOR n: NAT IN [0..poly.pairs.length) DO
s1: Seg ← [poly.pairs[n], poly.pairs[(n+1) MOD poly.pairs.length], poly.lines[n]];
IF NOT (i ← Intersect[s0, s1]).disjoint THEN {
dd: REAL ← Vector2.Square[Vector2.Sub[out.pair, i.pt]];
IF dd < d THEN {d ← dd; iPair ← i.pt; iEdge ← n};
};
ENDLOOP;
next ← [PointFromPair[iPair, poly], iPair, poly.neighs[iEdge]];
};
};
MoveOnPolygon: PUBLIC PROC [old: Spot, direction: Triple, distance: REAL] RETURNS [Spot]
~ {
ENABLE {
RuntimeError.BoundsFault => Error[$BadPolygon];
};
poly: Poly ← old.poly;
new: Spot ← MoveSpot[old, direction, distance];
IF Contours.InsideContour[new.pair, poly.pairs] = inside
THEN RETURN[new]
ELSE {
next: Spot ← IntersectNextPoly[old, new];
IF (distance ← distance-G3dVector.Distance[next.point, old.point]) < 0.0001
THEN RETURN[next]
ELSE {
n0: Triple ← [poly.plane.x, poly.plane.y, poly.plane.z];
n1: Triple ← [next.poly.plane.x, next.poly.plane.y, next.poly.plane.z];
axis: Triple ← G3dVector.Cross[n1, n0];
angle: REAL ← G3dVector.AngleBetween[n1, n0];
mat: G3dBasic.Matrix ← G3dMatrix.ObtainMatrix[];
mat ← G3dMatrix.MakeRotate[axis, angle,,, mat];
direction ← G3dMatrix.TransformVec[direction, mat];
G3dMatrix.ReleaseMatrix[mat];
RETURN[MoveOnPolygon[next, direction, distance]];
};
};
};
MoveToPolygonEdge: PUBLIC PROC [old: Spot, direction: Triple] RETURNS [edge: Spot] ~ {
distance: REAL ← 0.002;
edge ← MoveSpot[old, direction, distance];
WHILE Contours.InsideContour[edge.pair, old.poly.pairs] # outside DO
distance ← distance+distance;
edge ← MoveSpot[edge, direction, distance];
ENDLOOP;
RETURN[IntersectNextPoly[old, edge]];
};
END.