<> <> <> <> <> DIRECTORY PEBetaSpline USING [BetaSplinePoints], PERefresh USING [DisableTrajectoryRefresh, EnableTrajectoryRefresh], PESimplify, PETrajectoryOps USING [InsertSegment, InsertVertex, LinkSegments, UnlinkSegments], PETypes, RealFns USING [ArcTanDeg, CosDeg, Ln, SinDeg, SqRt]; PESimplifyImpl: CEDAR PROGRAM IMPORTS PEBetaSpline, PERefresh, PETrajectoryOps, RealFns EXPORTS PESimplify = BEGIN pi: CARDINAL _ 180; WeightedMidpoint: PUBLIC PROCEDURE [activeTrajectory: PETypes.TrajectoryNode, delta: REAL] RETURNS [filtered: BOOLEAN _ FALSE, collinearSegNode: PETypes.SegmentNode, newControlPolygon: PETypes.Trajectory, newSpline: PETypes.Trajectory, oldSegList: PETypes.SegmentList _ NIL] = { <> bias: REAL; center: PETypes.Point; equiCurveSegNode: PETypes.SegmentNode; factor1, factor2, factor3: REAL; firstSegment: PETypes.Segment; newSegmentList: PETypes.SegmentList; newSplineSegmentList: PETypes.SegmentList; numEquiCurveSegs: CARDINAL; point: PETypes.Point; prevSegment: PETypes.SegmentNode; radius: REAL; segment: PETypes.SegmentNode; tempSegList: PETypes.SegmentList _ NIL; tension: REAL; vertex1, vertex2, vertex3, vertex4: PETypes.Vertex; << >> <> collinearSegNode _ ComputeCollinear[activeTrajectory, delta]; IF collinearSegNode # NIL THEN RETURN; <> [numEquiCurveSegs, equiCurveSegNode] _ EquiCurvatureSegments[activeTrajectory, delta]; IF equiCurveSegNode # NIL AND numEquiCurveSegs > 4 THEN{ PERefresh.DisableTrajectoryRefresh[activeTrajectory.first]; [center, radius] _ OsculatingCircle[equiCurveSegNode, numEquiCurveSegs]; newSegmentList _ NewCurveSegs[equiCurveSegNode, numEquiCurveSegs, center, radius]; prevSegment _ equiCurveSegNode.preceding; [tempSegList, oldSegList] _ PETrajectoryOps.UnlinkSegments[activeTrajectory.first.segments, equiCurveSegNode, numEquiCurveSegs]; activeTrajectory.first.segments _ PETrajectoryOps.LinkSegments[tempSegList, prevSegment, newSegmentList]; <<>> PERefresh.EnableTrajectoryRefresh[activeTrajectory.first]; RETURN; }; filtered _ TRUE; <> firstSegment _ activeTrajectory.first.segments.first; segment _ activeTrajectory.first.segments.preceding; vertex1 _ segment.first.vertices.first; vertex2 _ segment.preceding.first.vertices.first; vertex3 _ segment.preceding.preceding.first.vertices.first; vertex4 _ segment.preceding.preceding.preceding.first.vertices.first; newSegmentList _ ComputeEndPoint[vertex1, vertex2, vertex3, vertex4, tempSegList]; tempSegList _ newSegmentList; <<-- The currently implemented code recomputes n new positions for n control points by using a weighted neighborhood averaging with a neighborhood of two.>> WHILE segment.preceding.first # firstSegment DO vertex1 _ segment.first.vertices.first; vertex2 _ segment.preceding.first.vertices.first; vertex3 _ segment.preceding.preceding.first.vertices.first; vertex4 _ segment.preceding.preceding.preceding.first.vertices.first; [factor1, factor2, factor3] _ ComputeWeightingFactors[vertex1, vertex2, vertex3]; point.x _ factor1*vertex1.point.x + factor2*vertex2.point.x + factor3*vertex3.point.x; point.y _ factor1*vertex1.point.y + factor2*vertex2.point.y + factor3*vertex3.point.y; bias _ factor1*vertex1.bias + factor2*vertex2.bias + factor3*vertex3.bias; tension _ factor1*vertex1.tension + factor2*vertex2.tension + factor3*vertex3.tension; newSegmentList _ LinkSegments[tempSegList, point, bias, tension]; tempSegList _ newSegmentList; segment _ segment.preceding; ENDLOOP; vertex1 _ segment.preceding.first.vertices.first; vertex2 _ segment.first.vertices.first; vertex3 _ segment.rest.first.vertices.first; vertex4 _ segment.rest.rest.first.vertices.first; newSegmentList _ ComputeEndPoint[vertex1, vertex2, vertex3, vertex4, newSegmentList]; newSegmentList.first.type _ moveTo; newSegmentList.first.fp _ NIL; IF newSegmentList # NIL THEN newControlPolygon _ NEW[PETypes.TrajectoryRec _ [segments: newSegmentList]]; <<>> <> [newSplineSegmentList] _ PEBetaSpline.BetaSplinePoints[newSegmentList, newSegmentList, FALSE]; IF newSplineSegmentList # NIL THEN{ newSplineSegmentList.first.type _ moveTo; newSplineSegmentList.first.fp _ NIL; newSpline _ NEW[PETypes.TrajectoryRec _ [segments: newSplineSegmentList]]; }; <<>> }; ComputeWeightingFactors: PRIVATE PROCEDURE [vertex1: PETypes.Vertex, vertex2: PETypes.Vertex, vertex3: PETypes.Vertex] RETURNS [factor1: REAL, factor2: REAL, factor3: REAL] = { <> weight1, weight2, weight3: REAL; denom: REAL; <<>> <<-- Using the natural log because the effects of tension and bias are non-linear.>> weight1 _ 0.1*(1.0 + RealFns.Ln[vertex1.tension + vertex1.bias]); weight2 _ 0.8*(1.0 + RealFns.Ln[vertex2.tension + vertex2.bias]); weight3 _ 0.1*(1.0 + RealFns.Ln[vertex3.tension + vertex3.bias]); denom _ weight1 + weight2 + weight3; IF denom # 0.0 THEN { factor1 _ weight1/denom; factor2 _ weight2/denom; factor3 _ weight3/denom; } ELSE {factor1 _ 0.1; factor2 _ 0.8; factor3 _ 0.1}; }; LinkSegments: PRIVATE PROCEDURE [changedSegList: PETypes.SegmentList, point: PETypes.Point, bias: REAL, tension: REAL] RETURNS [newSegmentList: PETypes.SegmentList] = { <> changedVertexList: PETypes.VertexList; newSegment: PETypes.Segment; newVertex: PETypes.Vertex; newVertexList: PETypes.VertexList; newVertex _ NEW[PETypes.VertexRec _ [point: point, bias: bias, tension: tension]]; newVertexList _ NEW[PETypes.VertexNodeRec]; newVertexList _ NIL; [changedVertexList, ] _ PETrajectoryOps.InsertVertex[newVertexList, newVertex, NIL]; newSegment _ NEW[PETypes.SegmentRec _ [vertices: changedVertexList]]; [newSegmentList, ] _ PETrajectoryOps.InsertSegment[changedSegList, newSegment, NIL]; }; ComputeCollinear: PRIVATE PROCEDURE [activeTrajectory: PETypes.TrajectoryNode, delta: REAL] RETURNS [collinearSegNode: PETypes.SegmentNode] = { <> firstSegment: PETypes.Segment; lastAcceptedDist: REAL _ delta; perpendicularDist: REAL; segment: PETypes.SegmentNode; segment0, segment1, segment2: PETypes.Vertex; tempSegList: PETypes.SegmentList _ NIL; u: PETypes.Point; vector1, vector2: PETypes.Point; vector1Length: REAL; xComponent, yComponent: REAL; firstSegment _ activeTrajectory.first.segments.rest.first; segment _ activeTrajectory.first.segments.preceding.preceding.preceding; WHILE segment.first # firstSegment DO segment0 _ segment.rest.first.vertices.first; segment1 _ segment.first.vertices.first; segment2 _ segment.preceding.first.vertices.first; vector1.x _ segment2.point.x - segment0.point.x; vector1.y _ segment2.point.y - segment0.point.y; vector2.x _ segment1.point.x - segment0.point.x; vector2.y _ segment1.point.y - segment0.point.y; vector1Length _ RealFns.SqRt[vector1.x*vector1.x + vector1.y*vector1.y]; u.x _ vector1.x/vector1Length; u.y _ vector1.y/vector1Length; xComponent _ vector2.x*u.x + vector2.y*u.y; yComponent _ vector2.y*u.x - vector2.x*u.y; IF (xComponent/vector1Length) >= 0.0 AND (xComponent/vector1Length) <= 1.0 THEN{ perpendicularDist _ ABS[yComponent]; IF perpendicularDist < lastAcceptedDist THEN{ collinearSegNode _ segment; lastAcceptedDist _ perpendicularDist; }; }; segment _ segment.preceding; ENDLOOP; }; EquiCurvatureSegments: PRIVATE PROCEDURE [activeTrajectory: PETypes.TrajectoryNode, delta: REAL] RETURNS [saveCount: CARDINAL, savePointer: PETypes.SegmentNode] = { <> count: INT; currentCos, nextCos: REAL; currentSine, nextSine: REAL; currentTheta, nextTheta: REAL; epsilon: REAL _ delta; firstSegment: PETypes.Segment; length1, length2, length3: REAL; point1, point2: PETypes.Point; pointer: PETypes.SegmentNode _ NIL; sameBias, sameTension: BOOLEAN; segment: PETypes.SegmentNode; vertex1, vertex2, vertex3, vertex4: PETypes.Vertex; firstSegment _ activeTrajectory.first.segments.first; segment _ activeTrajectory.first.segments.preceding.preceding.preceding; count _ 1; saveCount _ 1; point1.x _ segment.first.vertices.first.point.x - segment.rest.first.vertices.first.point.x; point1.y _ segment.first.vertices.first.point.y - segment.rest.first.vertices.first.point.y; point2.x _ segment.preceding.first.vertices.first.point.x - segment.first.vertices.first.point.x; point2.y _ segment.preceding.first.vertices.first.point.y - segment.first.vertices.first.point.y; [currentCos, currentSine, length1, length2] _ ComputeCurvatureElements[point1, point2]; WHILE segment.first # firstSegment DO vertex1 _ segment.rest.first.vertices.first; vertex2 _ segment.first.vertices.first; vertex3 _ segment.preceding.first.vertices.first; vertex4 _ segment.preceding.preceding.first.vertices.first; IF vertex1.bias = vertex2.bias AND vertex2.bias = vertex3.bias AND vertex3.bias = vertex4.bias THEN sameBias _ TRUE; IF vertex1.tension = vertex2.tension AND vertex2.tension = vertex3.tension AND vertex3.tension = vertex4.tension THEN sameTension _ TRUE; <> point1.x _ vertex3.point.x - vertex2.point.x; point1.y _ vertex3.point.y - vertex2.point.y; point2.x _ vertex4.point.x - vertex3.point.x; point2.y _ vertex4.point.y - vertex3.point.y; [nextCos, nextSine, , length3] _ ComputeCurvatureElements[point1, point2]; currentTheta _ RealFns.ArcTanDeg[currentSine, currentCos]; IF currentSine < 0 THEN currentTheta _ pi + currentTheta; IF currentTheta = 0.0 THEN IF currentCos < 0.0 THEN currentTheta _ pi; nextTheta _ RealFns.ArcTanDeg[nextSine, nextCos]; IF nextSine < 0 THEN nextTheta _ pi + nextTheta; IF nextTheta = 0.0 THEN IF nextCos < 0.0 THEN nextTheta _ pi; <> IF ABS[nextTheta - currentTheta] < epsilon THEN{ IF (ABS[length1-length2] < epsilon) AND (ABS[length2-length3] < epsilon) AND (ABS[length1-length3] < epsilon) THEN{ IF sameBias AND sameTension THEN { count _ count + 1; pointer _ segment.preceding; }; }; } ELSE IF count > saveCount THEN { -- At the end of a run of equicurvature segments saveCount _ count; savePointer _ pointer; count _ 1; pointer _ segment.preceding; }; segment _ segment.preceding; currentCos _ nextCos; currentSine _ nextSine; length1 _ length2; length2 _ length3; sameBias _ FALSE; sameTension _ FALSE; ENDLOOP; IF count > saveCount THEN { -- Pick up last equicurvature segments, if any saveCount _ count; savePointer _ pointer; }; IF saveCount > 1 THEN{ saveCount _ saveCount + 2; savePointer _ savePointer.preceding; }; }; ComputeCurvatureElements: PRIVATE PROCEDURE [a: PETypes.Point, b: PETypes.Point] RETURNS [cosTheta: REAL, sineTheta: REAL, aLength: REAL, bLength: REAL] = { <> aCrossB: REAL; aDotB: REAL; aLength _ RealFns.SqRt[a.x*a.x + a.y*a.y]; bLength _ RealFns.SqRt[b.x*b.x + b.y*b.y]; aDotB _ a.x*b.x + a.y*b.y; cosTheta _ aDotB/(aLength*bLength); aCrossB _ a.x*b.y - a.y*b.x; sineTheta _ aCrossB/(aLength*bLength); }; ComputeEndPoint: PRIVATE PROCEDURE [vertex1: PETypes.Vertex, vertex2: PETypes.Vertex, vertex3: PETypes.Vertex, vertex4: PETypes.Vertex, oldSegmentList: PETypes.SegmentList] RETURNS [newSegmentList: PETypes.SegmentNode] = { <> bias: REAL; factor, factor1, factor2, factor3: REAL; midPoint: PETypes.Point; midSegLength: REAL; newPoint: PETypes.Point; nextPoint, afterNextPoint: PETypes.Point; oldSegLength: REAL; <> tension: REAL; midPoint.x _ vertex2.point.x + .5*(vertex1.point.x - vertex2.point.x); midPoint.y _ vertex2.point.y + .5*(vertex1.point.y - vertex2.point.y); bias _ vertex2.bias + .5*(vertex1.bias - vertex2.bias); tension _ vertex2.tension + .5*(vertex1.tension - vertex2.tension); [factor1, factor2, factor3] _ ComputeWeightingFactors[vertex1, vertex2, vertex3]; nextPoint.x _ factor1*vertex1.point.x + factor2*vertex2.point.x + factor3*vertex3.point.x; nextPoint.y _ factor1*vertex1.point.y + factor2*vertex2.point.y + factor3*vertex3.point.y; [factor1, factor2, factor3] _ ComputeWeightingFactors[vertex2, vertex3, vertex4]; afterNextPoint.x _ factor1*vertex2.point.x + factor2*vertex3.point.x + factor3*vertex4.point.x; afterNextPoint.y _ factor1*vertex2.point.y + factor2*vertex3.point.y + factor3*vertex4.point.y; midSegLength _ RealFns.SqRt[(nextPoint.x-midPoint.x)*(nextPoint.x-midPoint.x) + (nextPoint.y-midPoint.y)*(nextPoint.y-midPoint.y)]; oldSegLength _ RealFns.SqRt[(nextPoint.x-afterNextPoint.x)*(nextPoint.x-afterNextPoint.x) + (nextPoint.y-afterNextPoint.y)*(nextPoint.y-afterNextPoint.y)]; factor _ MAX[oldSegLength/midSegLength, 1.0]; newPoint.x _ nextPoint.x + factor*(midPoint.x - nextPoint.x); newPoint.y _ nextPoint.y + factor*(midPoint.y - nextPoint.y); newSegmentList _ LinkSegments[oldSegmentList, newPoint, bias, tension]; }; NewCurveSegs: PRIVATE PROCEDURE [firstVertex: PETypes.SegmentNode, numVertices: INT, center: PETypes.Point, radius: REAL] RETURNS [newSegmentList: PETypes.SegmentNode] = { <> angle: REAL; cosStep: REAL; i: INT _ 0; lastVertex: PETypes.SegmentNode _ firstVertex; newPoint: PETypes.Point; numPts: REAL; point: PETypes.Point; sineStep: REAL; stepAngle: REAL; tempSegList: PETypes.SegmentList _ NIL; theta: REAL; <> WHILE i < numVertices-1 DO lastVertex _ lastVertex.rest; i _ i+1; ENDLOOP; point.x _ lastVertex.first.vertices.first.point.x - center.x; point.y _ lastVertex.first.vertices.first.point.y - center.y; theta _ PolygonAngle[center, firstVertex, numVertices]; numPts _ ABS[(2*pi)/theta]; stepAngle _ 2*pi/(numPts-2); IF theta > 0 THEN stepAngle _ -stepAngle; <> angle _ 0.0; i _ 1; WHILE i <= numVertices-2 DO cosStep _ RealFns.CosDeg[angle]; sineStep _ RealFns.SinDeg[angle]; newPoint.x _ point.x*cosStep - point.y*sineStep + center.x; newPoint.y _ point.y*cosStep + point.x*sineStep + center.y; newSegmentList _ LinkSegments[tempSegList, newPoint, 1.0, 0.0]; tempSegList _ newSegmentList; angle _ angle + stepAngle; i _ i+1; ENDLOOP; newSegmentList _ LinkSegments[tempSegList, firstVertex.first.vertices.first.point, 1.0, 0.0]; }; OsculatingCircle: PRIVATE PROCEDURE [firstVertex: PETypes.SegmentNode, numVertices: INT] RETURNS [center: PETypes.Point, radius: REAL] = { <> denom: REAL; i: INT _ 1; numer: REAL; point1, point2, point3, point4: PETypes.Point; segment: PETypes.SegmentNode; sumCenter: PETypes.Point; sumRadius: REAL _ 0.0; t: REAL; tempPoint: PETypes.Point; IF numVertices < 4 THEN RETURN; sumCenter.x _ 0.0; sumCenter.y _ 0.0; segment _ firstVertex; point1 _ segment.first.vertices.first.point; point2 _ segment.rest.first.vertices.first.point; point3 _ segment.rest.rest.first.vertices.first.point; point4 _ segment.rest.rest.rest.first.vertices.first.point; WHILE i <= numVertices-3 DO tempPoint.x _ point1.x + point3.x - point2.x; tempPoint.y _ point1.y + point3.y - point2.y; denom _ -point1.x + 3*point2.x - 3*point3.x + point4.x; numer _ point2.x - point3.x; IF denom = 0.0 THEN{ denom _ -point1.y + 3*point2.y - 3*point3.y + point4.y; numer _ point2.y - point3.y; }; t _ numer/denom; sumCenter.x _ sumCenter.x + (point2.x + t*(tempPoint.x - point2.x)); sumCenter.y _ sumCenter.y + (point2.y + t*(tempPoint.y - point2.y)); center.x _ point2.x + t*(tempPoint.x - point2.x); center.y _ point2.y + t*(tempPoint.y - point2.y); sumRadius _ sumRadius + RealFns.SqRt[(center.x-point2.x)*(center.x-point2.x) + (center.y-point2.y)*(center.y-point2.y)]; segment _ segment.rest; point1 _ segment.first.vertices.first.point; point2 _ segment.rest.first.vertices.first.point; point3 _ segment.rest.rest.first.vertices.first.point; point4 _ segment.rest.rest.rest.first.vertices.first.point; i _ i+1; ENDLOOP; center.x _ sumCenter.x/(numVertices-3); center.y _ sumCenter.y/(numVertices-3); radius _ sumRadius/numVertices; }; PolygonAngle: PRIVATE PROCEDURE [center: PETypes.Point, firstVertex: PETypes.SegmentNode, numVertices: INT] RETURNS [theta: REAL] = { <> cosTheta: REAL; i: INT _ 1; point1, point2: PETypes.Point; segment: PETypes.SegmentNode; sineTheta: REAL; sumAngles: REAL _ 0.0; segment _ firstVertex; point1.x _ segment.first.vertices.first.point.x - center.x; point1.y _ segment.first.vertices.first.point.y - center.y; point2.x _ segment.rest.first.vertices.first.point.x - center.x; point2.y _ segment.rest.first.vertices.first.point.y - center.y; WHILE i < numVertices DO [cosTheta, sineTheta, ] _ ComputeCurvatureElements[point1, point2]; sumAngles _ sumAngles + RealFns.ArcTanDeg[sineTheta, cosTheta]; segment _ segment.rest; point1 _ point2; point2.x _ segment.rest.first.vertices.first.point.x - center.x; point2.y _ segment.rest.first.vertices.first.point.y - center.y; i _ i+1; ENDLOOP; theta _ sumAngles/(numVertices-1); }; END.