<> <> <> <> <<>> 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; <> 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] = { <> }; <<>> FillLineFromPoints: PUBLIC PROC [v1, v2: Point3d, line: Line3d] = { <> <> OPEN SVVector3d; line.direction _ Normalize[Sub[v2, v1]]; line.base _ Add[v1, Scale[line.direction, DotProduct[Sub[[0,0,0], v1], line.direction]]]; }; <> <> <> <> <> <> <> <> <> <> <> <> <> <<};>> <<>> 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] = { }; <> <> 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; <> }; -- 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 <> DirectionOfLine: PUBLIC PROC [line: Line3d] RETURNS [direction: Vector3d] = { <> 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] >> <> DropPerpendicular: PUBLIC PROC [pt: Point3d, line: Line3d] RETURNS [projectedPt: Point3d] = { <> <> <> OPEN SVVector3d; projectedPt _ Add[line.base, Scale[line.direction, DotProduct[Sub[pt,line.base], line.direction]]]; }; << [Artwork node; type 'Artwork on' to command tool] >> <> NearLinePoints: PUBLIC PROC [l1, l2: Line3d] RETURNS [p1, p2: Point3d, parallel: BOOL _ FALSE] = { <> <> <<(U-V) (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]; }; <> <> NearEndpointToPoint: PUBLIC PROC [pt: Point3d, edge: Edge3d] RETURNS [endpoint: Point3d] = { <> 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] = { <> 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] = { <> 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] = { <> 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 <<>> <> 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]; <> <> }; }; }; <<>> <> 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.