private void AddGroupIntersectionsToRestrictedRay(Obstacle obstacle, IList <IntersectionInfo> intersections)
{
// We'll let the lines punch through any intersections with groups, but track the location so we can enable/disable crossing.
foreach (var intersectionInfo in intersections)
{
var intersect = SpliceUtility.RawIntersection(intersectionInfo, currentRestrictedRay.Start);
// Skip intersections that are past the end of the restricted segment (though there may still be some
// there if we shorten it later, but we'll skip them later).
var distSquared = (intersect - currentRestrictedRay.Start).LengthSquared;
if (distSquared > restrictedRayLengthSquared)
{
continue;
}
var dirTowardIntersect = PointComparer.GetPureDirection(currentRestrictedRay.Start, currentRestrictedRay.End);
var polyline = (Polyline)intersectionInfo.Segment1; // this is the second arg to GetAllIntersections
var dirsOfSide = CompassVector.VectorDirection(polyline.Derivative(intersectionInfo.Par1));
// The derivative is always clockwise, so if the side contains the rightward rotation of the
// direction from the ray origin, then we're hitting it from the inside; otherwise from the outside.
var dirToInsideOfGroup = dirTowardIntersect;
if (0 != (dirsOfSide & CompassVector.RotateRight(dirTowardIntersect)))
{
dirToInsideOfGroup = CompassVector.OppositeDir(dirToInsideOfGroup);
}
CurrentGroupBoundaryCrossingMap.AddIntersection(intersect, obstacle, dirToInsideOfGroup);
}
}
/// <summary>
/// estimation from below for the distance
/// </summary>
/// <param name="point"></param>
/// <param name="entryDirToVertex"></param>
/// <returns></returns>
private double HeuristicDistanceFromVertexToTarget(Point point, Directions entryDirToVertex)
{
Point vectorToTarget = Target.Point - point;
if (ApproximateComparer.Close(vectorToTarget.X, 0) && ApproximateComparer.Close(vectorToTarget.Y, 0))
{
// We are at the target.
return(this.targetCostAdjustment);
}
Directions dirToTarget = CompassVector.VectorDirection(vectorToTarget);
int numberOfBends;
if (entryDirToVertex == Directions.None)
{
entryDirToVertex = Directions.East | Directions.North | Directions.West | Directions.South;
numberOfBends = GetNumberOfBends(entryDirToVertex, dirToTarget);
}
else
{
numberOfBends = GetNumberOfBends(entryDirToVertex, dirToTarget);
}
return(CombinedCost(ManhattanDistance(point, Target.Point), numberOfBends) + this.targetCostAdjustment);
}
/// <summary>
///
/// </summary>
/// <param name="direction"></param>
/// <param name="obstacles"></param>
/// <param name="axisEdgesToObstaclesTheyOriginatedFrom"></param>
/// <param name="pathOrders"></param>
/// <param name="axisEdges">edges to find the empty space around</param>
internal FreeSpaceFinder(Directions direction, IEnumerable <Polyline> obstacles, Dictionary <AxisEdge, Polyline> axisEdgesToObstaclesTheyOriginatedFrom, Dictionary <AxisEdge, List <PathEdge> > pathOrders,
IEnumerable <AxisEdge> axisEdges) : base(obstacles, new CompassVector(direction).ToPoint())
{
DirectionPerp = new CompassVector(direction).Right.ToPoint();
PathOrders = pathOrders;
xProjection = direction == Directions.North ? (PointProjection)X : MinusY;
#if SHARPKIT //https://code.google.com/p/sharpkit/issues/detail?id=301
edgeContainersTree = new RbTree <AxisEdgesContainer>(new FreeSpaceFinderComparer(this));
#else
edgeContainersTree = new RbTree <AxisEdgesContainer>(this);
#endif
SweepPole = CompassVector.VectorDirection(SweepDirection);
Debug.Assert(CompassVector.IsPureDirection(SweepPole));
AxisEdges = axisEdges;
AxisEdgesToObstaclesTheyOriginatedFrom = axisEdgesToObstaclesTheyOriginatedFrom;
}
/// <summary>
/// The usual Compare operation, with inputs that are assumed to have been Round()ed.
/// </summary>
/// <param name="lhs"></param>
/// <param name="rhs"></param>
/// <returns>0 if the inputs are close enough to be considered equal, else -1 if lhs is
/// less than rhs, else 1.</returns>
public static int Compare(double lhs, double rhs)
{
// If the inputs are not rounded, then two numbers that are close together at the
// middle of the rounding range may Compare as 0 but Round to different values
// (e.g., with rounding to 6 digits, xxx.yyyyyy49 and xxx.yyyyyy51 will exhibit this).
Assert_Rounded(lhs);
Assert_Rounded(rhs);
int cmp = 0;
if (lhs + DifferenceEpsilon < rhs)
{
cmp = -1;
}
else if (rhs + DifferenceEpsilon < lhs)
{
cmp = 1;
}
// Just to be sure we're in sync with CompassVector
Debug.Assert((cmp < 0) == (Directions.East == CompassVector.VectorDirection(new Point(lhs, 0), new Point(rhs, 0))));
Debug.Assert((0 == cmp) == (Directions.None == CompassVector.VectorDirection(new Point(lhs, 0), new Point(rhs, 0))));
return(cmp);
}
