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); }