<> <> <> <> DIRECTORY Matrix3d, RealFns, SV2d, SV3d, SVPolygon2d, SVPolygon3d; SVPolygon3dImpl: PROGRAM IMPORTS Matrix3d, RealFns--, SVPolygon2d EXPORTS SVPolygon3d = BEGIN Matrix4by4: TYPE = Matrix3d.Matrix4by4; Plane: TYPE = REF PlaneObj; PlaneObj: TYPE = SVPolygon3d.PlaneObj; Point3d: TYPE = SV3d.Point3d; Poly3d: TYPE = REF Poly3dObj; Poly3dObj: TYPE = SV3d.Poly3dObj; Polygon: TYPE = REF PolygonObj; PolygonObj: TYPE = SV2d.PolygonObj; Vector: TYPE = SV3d.Vector; <> root3, root3Over3, twoRoot3Over3: REAL; -- constants computed at load time in Init (see below). CreatePoly: PUBLIC PROC [len: NAT] RETURNS [poly3d: Poly3d] = { poly3d _ NEW[Poly3dObj[len]]; poly3d.len _ 0; }; CircumHexagon: PUBLIC PROC [y: REAL, r: REAL] RETURNS [hex: Poly3d] = { <> hex _ CreatePoly[6]; hex _ AddPolyPoint[hex, [r*root3Over3, y, r]]; -- front right hex _ AddPolyPoint[hex, [r*twoRoot3Over3, y, 0]];-- right hex _ AddPolyPoint[hex, [r*root3Over3, y, -r]];-- back right hex _ AddPolyPoint[hex, [-r*root3Over3, y, -r]];-- back left hex _ AddPolyPoint[hex, [-r*twoRoot3Over3, y, 0]];-- left hex _ AddPolyPoint[hex, [-r*root3Over3, y, r]];-- front left }; ClearPoly: PUBLIC PROC [poly: Poly3d] = { poly.len _ 0; }; AddPolyPoint: PUBLIC PROC [poly: Poly3d, point: Point3d] RETURNS [polyPlusPoint: Poly3d] = { IF poly.len = poly.maxVerts THEN { polyPlusPoint _ CreatePoly[poly.maxVerts+1]; polyPlusPoint _ PartPolyGetsPartPoly[poly, 0, polyPlusPoint, 0, poly.maxVerts]; polyPlusPoint[poly.maxVerts] _ point; polyPlusPoint.len _ poly.maxVerts + 1; } ELSE { poly[poly.len] _ point; poly.len _ poly.len + 1; polyPlusPoint _ poly; }; }; -- end of AddPolyPoint PutPolyPoint: PUBLIC PROC [poly: Poly3d, index: NAT, point: Point3d] RETURNS [newPoly: Poly3d] = { IF index+1 > poly.maxVerts THEN { newPoly _ CreatePoly[index+1]; newPoly _ PartPolyGetsPartPoly[poly, 0, newPoly, 0, poly.maxVerts]; newPoly[index] _ point; newPoly.len _ index+1; } ELSE { IF index+1 > poly.len THEN poly.len _ index+1; poly[index] _ point; newPoly _ poly}; }; PartPolyGetsPartPoly: PUBLIC PROC [fromPoly: Poly3d, fromStart: NAT, toPoly: Poly3d, toStart: NAT, duration: NAT] RETURNS [newPoly: Poly3d] = { <> j: NAT _ fromStart; IF toStart+duration > toPoly.maxVerts THEN { newPoly _ CreatePoly[toStart+duration]; newPoly _ PartPolyGetsPartPoly[toPoly, 0, newPoly, 0, toPoly.len]; newPoly.len _ toStart+duration} ELSE { newPoly _ toPoly; IF toStart+duration > newPoly.len THEN newPoly.len _ toStart+duration}; IF fromStart+duration > fromPoly.len THEN ERROR; -- don't use bad data FOR i: NAT IN[toStart..toStart+duration-1] DO newPoly[i] _ fromPoly[j]; j _ j + 1; ENDLOOP; }; PlaneFromPoly3d: PUBLIC PROC [poly: Poly3d] RETURNS [plane: Plane] = { <> <> <> iPlusOne: NAT; iPoint, iPlusOnePoint: Point3d; plane _ NEW[PlaneObj]; plane.A _ 0; plane.B _ 0; plane.C _ 0; FOR i: NAT IN[1..poly.len] DO iPlusOne _ IF i = poly.len THEN 1 ELSE i + 1; iPoint _ poly[i - 1]; iPlusOnePoint _ poly[iPlusOne - 1]; plane.A _ plane.A + (iPoint[3] + iPlusOnePoint[3])*(iPlusOnePoint[2] - iPoint[2]); plane.B _ plane.B + (iPoint[1] + iPlusOnePoint[1])*(iPlusOnePoint[3] - iPoint[3]); plane.C _ plane.C + (iPoint[2] + iPlusOnePoint[2])*(iPlusOnePoint[1] - iPoint[1]); ENDLOOP; <> iPoint _ poly[0]; plane.D _ -(iPoint[1]*plane.A + iPoint[2]*plane.B + iPoint[3]*plane.C); }; -- end of PlaneFromPoly3d PlaneFromPointAndNormal: PUBLIC PROC [point: Point3d, normal: Vector] RETURNS [plane: Plane] = { plane _ NEW[PlaneObj]; plane.A _ normal[1]; plane.B _ normal[2]; plane.C _ normal[3]; <> plane.D _ -(plane.A*point[1] + plane.B*point[2] + plane.C*point[3]); }; -- end of PlaneFromPointAndNormal PlaneFromCoefficients: PUBLIC PROC [A, B, C, D: REAL] RETURNS [plane: Plane] = { plane _ NEW[PlaneObj _ [A, B, C, D]]; }; ClipPolyToPlane: PUBLIC PROC [poly: Poly3d, plane: Plane] RETURNS [clippedPoly: Poly3d] = { <> lastSide, thisSide: BOOL; lastDotProduct, thisDotProduct: REAL; lastPoint, thisPoint, intersectionPoint: Point3d; clippedPoly _ CreatePoly[poly.len]; -- the clipped poly won't have more vertices than the old poly. IF poly.len = 0 THEN RETURN; lastPoint _ poly[poly.len-1]; -- the last point lastDotProduct _ (plane.A*lastPoint[1] + plane.B*lastPoint[2] + plane.C*lastPoint[3] + plane.D); lastSide _ lastDotProduct <= 0; FOR i: NAT IN [0..poly.len) DO thisPoint _ poly[i]; thisDotProduct _ (plane.A*thisPoint[1] + plane.B*thisPoint[2] + plane.C*thisPoint[3] + plane.D); thisSide _ thisDotProduct <= 0; IF thisSide THEN { IF lastSide THEN clippedPoly _ AddPolyPoint[clippedPoly, thisPoint] ELSE { -- we have travelled from the badSide to the keepSide. Add the intersection point first. intersectionPoint _ LineSegmentMeetsPlaneFast[lastPoint, thisPoint, lastDotProduct, thisDotProduct]; clippedPoly _ AddPolyPoint[clippedPoly, intersectionPoint]; clippedPoly _ AddPolyPoint[clippedPoly, thisPoint]; }; } ELSE { -- this point is on the bad side. IF lastSide THEN { -- we have gone from good side to bad side <> intersectionPoint _ LineSegmentMeetsPlaneFast[lastPoint, thisPoint, lastDotProduct, thisDotProduct]; clippedPoly _ AddPolyPoint[clippedPoly, intersectionPoint]; } ELSE {}; -- bad side to bad side. Do nothing. }; lastSide _ thisSide; lastPoint _ thisPoint; lastDotProduct _ thisDotProduct; ENDLOOP; }; ClipPolyToPlanes: PUBLIC PROC [poly: Poly3d, planes: LIST OF Plane] RETURNS [clippedPoly: Poly3d] = { clippedPoly _ poly; FOR planeList: LIST OF Plane _ planes, planeList.rest UNTIL planeList = NIL DO clippedPoly _ ClipPolyToPlane[clippedPoly, planeList.first]; ENDLOOP; }; LineSegmentMeetsPlaneFast: PRIVATE PROC [p1, p2: Point3d, d1, d2: REAL] RETURNS [intersection: Point3d] = { <> total: REAL; d1 _ IF d1 < 0 THEN -d1 ELSE d1; d2 _ IF d2 < 0 THEN -d2 ELSE d2; total _ d1+d2; intersection[1] _ (d1/total)*(p2[1]-p1[1])+p1[1]; intersection[2] _ (d1/total)*(p2[2]-p1[2])+p1[2]; intersection[3] _ (d1/total)*(p2[3]-p1[3])+p1[3]; }; ClipLineSegmentToPlane: PUBLIC PROC [p1, p2: Point3d, plane: Plane] RETURNS [newP1, newP2: Point3d, newP1isP1, newP2isP2, nullSegment: BOOL] = { dotProduct1, dotProduct2: REAL; nullSegment _ FALSE; dotProduct1 _ (plane.A*p1[1] + plane.B*p1[2] + plane.C*p1[3] + plane.D); newP1isP1 _ dotProduct1 <= 0; dotProduct2 _ (plane.A*p2[1] + plane.B*p2[2] + plane.C*p2[3] + plane.D); newP2isP2 _ dotProduct2 <= 0; IF newP1isP1 THEN { newP1 _ p1; IF newP2isP2 THEN newP2 _ p2 ELSE newP2 _ LineSegmentMeetsPlaneFast[p1, p2, dotProduct1, dotProduct2]; } ELSE { IF newP2isP2 THEN { newP1 _ LineSegmentMeetsPlaneFast[p1, p2, dotProduct1, dotProduct2]; newP2 _ p2; } ELSE nullSegment _ TRUE; }; }; -- end of ClipLineSegmentToPlane ClipLineSegmentToPlanes: PUBLIC PROC [p1, p2: Point3d, planes: LIST OF Plane] RETURNS [newP1, newP2: Point3d, newP1isP1, newP2isP2, nullSegment: BOOL] = { plane: Plane; keepSide1, keepSide2: BOOL; newP1isP1 _ newP2isP2 _ TRUE; newP1 _ p1; newP2 _ p2; FOR planeList: LIST OF Plane _ planes, planeList.rest UNTIL planeList = NIL DO plane _ planeList.first; [newP1, newP2, keepSide1, keepSide2, nullSegment] _ ClipLineSegmentToPlane[newP1, newP2, plane]; IF nullSegment THEN RETURN; newP1isP1 _ newP1isP1 AND keepSide1; newP2isP2 _ newP2isP2 AND keepSide2; ENDLOOP; }; -- end of ClipLineSegmentToPlanes NormalOfPlane: PUBLIC PROC [plane: Plane] RETURNS [normal: Vector] = { normal[1] _ plane.A; normal[2] _ plane.B; normal[3] _ plane.C; }; -- end of NormalFromPlane MaximumDeviationWithinPoly3d: PROC [poly: Poly3d] RETURNS [distance: REAL] = { <> plane: Plane _ PlaneFromPoly3d[poly]; SIGNAL NotYetImplemented;-- **** not yet implemented }; NotYetImplemented: SIGNAL = CODE; SignedPointToPlaneDistance: PUBLIC PROC [point: Point3d, plane: Plane] RETURNS [distance: REAL] = { distance _ (plane.A*point[1] + plane.B*point[2] + plane.C*point[3] + plane.D)/ RealFns.SqRt[plane.A*plane.A + plane.B*plane.B + plane.C*plane.C]; }; -- end of SignedPointToPlaneDistance PointOnNormalSideOfPlane: PUBLIC PROC [point: Point3d, plane: Plane] RETURNS [BOOL] = { plugInPoint: REAL _ (plane.A*point[1] + plane.B*point[2] + plane.C*point[3] + plane.D); RETURN [plugInPoint > 0]; }; Quadrant: TYPE = SVPolygon3d.Quadrant;-- {inin, inout, outin, outout}; QuadrantOfPoint: PUBLIC PROC [point: Point3d, plane1, plane2: Plane] RETURNS [Quadrant] = { <> IF PointOnNormalSideOfPlane[point, plane1] THEN { IF PointOnNormalSideOfPlane[point, plane2] THEN RETURN[outout] ELSE RETURN[outin]} ELSE { IF PointOnNormalSideOfPlane[point, plane2] THEN RETURN[inout] ELSE RETURN[inin]}; }; -- QuadrantOfPoint TransformByMat: PUBLIC PROC [poly3d: Poly3d, mat: Matrix4by4] RETURNS [newPoly: Poly3d] = { <> tPoint: Point3d; newPoly _ CreatePoly[poly3d.len]; FOR i: NAT IN [0..poly3d.len) DO tPoint _ Matrix3d.Update[mat, poly3d[i]]; newPoly _ AddPolyPoint[newPoly, tPoint]; ENDLOOP; }; <<>> <> <> <> <> <> <> <> <<};>> <> <> <> <> <> <> <<};>> <> <> <> <> <> <> <<};>> <<>> Init: PROC = { root3 _ RealFns.SqRt[3]; root3Over3 _ root3/3; twoRoot3Over3 _ 2*root3Over3; }; Init[]; END.