767 lines
38 KiB
C#
767 lines
38 KiB
C#
|
//
|
||
|
|
// ObstacleTree.cs
|
||
|
|
// MSAGL class for wrapping a RectangleNode<Obstacle> hierarchy for Rectilinear Edge Routing.
|
||
|
|
//
|
||
|
|
// Copyright Microsoft Corporation.
|
||
|
|
using System.Collections.Generic;
|
||
|
|
using System.Diagnostics;
|
||
|
|
using System.Linq;
|
||
|
|
using Microsoft.Msagl.Core.DataStructures;
|
||
|
|
using Microsoft.Msagl.Core.Geometry;
|
||
|
|
using Microsoft.Msagl.Core.Geometry.Curves;
|
||
|
|
using Microsoft.Msagl.Core.GraphAlgorithms;
|
||
|
|
|
||
|
|
namespace Microsoft.Msagl.Routing.Rectilinear {
|
||
|
|
using System;
|
||
|
|
|
||
|
|
internal class ObstacleTree {
|
||
|
|
/// <summary>
|
||
|
|
/// The root of the hierarchy.
|
||
|
|
/// </summary>
|
||
|
|
internal RectangleNode<Obstacle> Root { get; private set; }
|
||
|
|
internal Rectangle GraphBox { get { return Root.Rectangle; } }
|
||
|
|
|
||
|
|
/// <summary>
|
||
|
|
/// Dictionary of sets of ancestors for each shape, for evaluating necessary group-boundary crossings.
|
||
|
|
/// </summary>
|
||
|
|
internal Dictionary<Shape, Set<Shape>> AncestorSets;
|
||
|
|
|
||
|
|
/// <summary>
|
||
|
|
/// Indicates whether we adjusted spatial ancestors due to blocked paths.
|
||
|
|
/// </summary>
|
||
|
|
internal bool SpatialAncestorsAdjusted;
|
||
|
|
|
||
|
|
/// <summary>
|
||
|
|
/// The map of shapes to obstacles.
|
||
|
|
/// </summary>
|
||
|
|
private Dictionary<Shape, Obstacle> shapeIdToObstacleMap;
|
||
|
|
|
||
|
|
/// <summary>
|
||
|
|
/// The map of all group boundary crossings for the current RestrictSegmentWithObstacles call.
|
||
|
|
/// </summary>
|
||
|
|
internal readonly GroupBoundaryCrossingMap CurrentGroupBoundaryCrossingMap = new GroupBoundaryCrossingMap();
|
||
|
|
|
||
|
|
/// <summary>
|
||
|
|
/// The list of all obstacles (not just those in the Root, which may have accretions of obstacles in convex hulls).
|
||
|
|
/// </summary>
|
||
|
|
private List<Obstacle> allObstacles;
|
||
|
|
|
||
|
|
/// <summary>
|
||
|
|
/// For accreting obstacles for clumps or convex hulls.
|
||
|
|
/// </summary>
|
||
|
|
private readonly Set<IntPair> overlapPairs = new Set<IntPair>();
|
||
|
|
|
||
|
|
/// <summary>
|
||
|
|
/// Indicates whether one or more obstacles overlap.
|
||
|
|
/// </summary>
|
||
|
|
private bool hasOverlaps;
|
||
|
|
|
||
|
|
/// <summary>
|
||
|
|
/// Member to avoid unnecessary class creation just to do a lookup.
|
||
|
|
/// </summary>
|
||
|
|
private readonly IntPair lookupIntPair = new IntPair(-1, -1);
|
||
|
|
|
||
|
|
/// <summary>
|
||
|
|
/// Create the tree hierarchy from the enumeration.
|
||
|
|
/// </summary>
|
||
|
|
/// <param name="obstacles"></param>
|
||
|
|
/// <param name="ancestorSets"></param>
|
||
|
|
/// <param name="idToObstacleMap"></param>
|
||
|
|
internal void Init(IEnumerable<Obstacle> obstacles, Dictionary<Shape, Set<Shape>> ancestorSets
|
||
|
|
, Dictionary<Shape, Obstacle> idToObstacleMap) {
|
||
|
|
CreateObstacleListAndOrdinals(obstacles);
|
||
|
|
this.AncestorSets = ancestorSets;
|
||
|
|
this.CreateRoot();
|
||
|
|
this.shapeIdToObstacleMap = idToObstacleMap;
|
||
|
|
}
|
||
|
|
|
||
|
|
private void CreateObstacleListAndOrdinals(IEnumerable<Obstacle> obstacles) {
|
||
|
|
this.allObstacles = obstacles.ToList();
|
||
|
|
int scanlineOrdinal = Obstacle.FirstNonSentinelOrdinal;
|
||
|
|
foreach (var obstacle in this.allObstacles) {
|
||
|
|
obstacle.Ordinal = scanlineOrdinal++;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
private Obstacle OrdinalToObstacle(int index) {
|
||
|
|
Debug.Assert(index >= Obstacle.FirstNonSentinelOrdinal, "index too small");
|
||
|
|
Debug.Assert(index < this.allObstacles.Count + Obstacle.FirstNonSentinelOrdinal, "index too large");
|
||
|
|
return this.allObstacles[index - Obstacle.FirstNonSentinelOrdinal];
|
||
|
|
}
|
||
|
|
|
||
|
|
/// <summary>
|
||
|
|
/// Create the root with overlapping non-rectangular obstacles converted to their convex hulls, for more reliable calculations.
|
||
|
|
/// </summary>
|
||
|
|
private void CreateRoot() {
|
||
|
|
this.Root = CalculateHierarchy(this.GetAllObstacles());
|
||
|
|
if (!OverlapsExist()) {
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
AccreteClumps();
|
||
|
|
AccreteConvexHulls();
|
||
|
|
GrowGroupsToAccommodateOverlaps();
|
||
|
|
this.Root = CalculateHierarchy(this.GetAllObstacles().Where(obs => obs.IsPrimaryObstacle));
|
||
|
|
}
|
||
|
|
|
||
|
|
private bool OverlapsExist() {
|
||
|
|
if (this.Root == null) {
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
RectangleNodeUtils.CrossRectangleNodes<Obstacle>(this.Root, this.Root, this.CheckForInitialOverlaps);
|
||
|
|
return this.hasOverlaps;
|
||
|
|
}
|
||
|
|
|
||
|
|
private bool OverlapPairAlreadyFound(Obstacle a, Obstacle b) {
|
||
|
|
// If we already found it then we'll have enqueued it in the reverse order.
|
||
|
|
this.lookupIntPair.x = b.Ordinal;
|
||
|
|
this.lookupIntPair.y = a.Ordinal;
|
||
|
|
return this.overlapPairs.Contains(this.lookupIntPair);
|
||
|
|
}
|
||
|
|
|
||
|
|
private void CheckForInitialOverlaps(Obstacle a, Obstacle b) {
|
||
|
|
if (this.hasOverlaps) {
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
bool aIsInsideB, bIsInsideA;
|
||
|
|
if (ObstaclesIntersect(a, b, out aIsInsideB, out bIsInsideA)) {
|
||
|
|
this.hasOverlaps = true;
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
if (!aIsInsideB && !bIsInsideA) {
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
// One obstacle is inside the other. If they're both groups, or a non-group is inside a group, nothing
|
||
|
|
// further is needed; we process groups differently because we can go through their sides.
|
||
|
|
if (a.IsGroup && b.IsGroup) {
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
if ((a.IsGroup && bIsInsideA) || (b.IsGroup && aIsInsideB)) {
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
this.hasOverlaps = true;
|
||
|
|
}
|
||
|
|
|
||
|
|
private void AccreteClumps() {
|
||
|
|
// Clumps are only created once. After that, as the result of convex hull creation, we may
|
||
|
|
// overlap an obstacle of a clump, in which case we enclose the clump in the convex hull as well.
|
||
|
|
// We only allow clumps of rectangular obstacles, to avoid angled sides in the scanline.
|
||
|
|
this.AccumulateObstaclesForClumps();
|
||
|
|
if (this.overlapPairs.Count == 0) {
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
this.CreateClumps();
|
||
|
|
}
|
||
|
|
|
||
|
|
private void AccreteConvexHulls() {
|
||
|
|
// Convex-hull creation is transitive, because the created hull may overlap additional obstacles.
|
||
|
|
for (; ; ) {
|
||
|
|
this.AccumulateObstaclesForConvexHulls();
|
||
|
|
if (!this.CreateConvexHulls()) {
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
internal static RectangleNode<Obstacle> CalculateHierarchy(IEnumerable<Obstacle> obstacles) {
|
||
|
|
var rectNodes = obstacles.Select(obs => new RectangleNode<Obstacle>(obs, obs.VisibilityBoundingBox)).ToList();
|
||
|
|
return RectangleNode<Obstacle>.CreateRectangleNodeOnListOfNodes(rectNodes);
|
||
|
|
}
|
||
|
|
|
||
|
|
private void AccumulateObstaclesForClumps() {
|
||
|
|
this.overlapPairs.Clear();
|
||
|
|
var rectangularObstacles = CalculateHierarchy(this.GetAllObstacles().Where(obs => !obs.IsGroup && obs.IsRectangle));
|
||
|
|
if (rectangularObstacles == null) {
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
RectangleNodeUtils.CrossRectangleNodes<Obstacle>(rectangularObstacles, rectangularObstacles, this.EvaluateOverlappedPairForClump);
|
||
|
|
}
|
||
|
|
|
||
|
|
private void EvaluateOverlappedPairForClump(Obstacle a, Obstacle b) {
|
||
|
|
Debug.Assert(!a.IsGroup && !b.IsGroup, "Groups should not come here");
|
||
|
|
Debug.Assert(a.IsRectangle && b.IsRectangle, "Only rectangles should come here");
|
||
|
|
if ((a == b) || this.OverlapPairAlreadyFound(a, b)) {
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
bool aIsInsideB, bIsInsideA;
|
||
|
|
if (!ObstaclesIntersect(a, b, out aIsInsideB, out bIsInsideA) && !aIsInsideB && !bIsInsideA) {
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
this.overlapPairs.Insert(new IntPair(a.Ordinal, b.Ordinal));
|
||
|
|
}
|
||
|
|
|
||
|
|
private void AccumulateObstaclesForConvexHulls() {
|
||
|
|
this.overlapPairs.Clear();
|
||
|
|
var allPrimaryNonGroupObstacles = CalculateHierarchy(this.GetAllObstacles().Where(obs => obs.IsPrimaryObstacle && !obs.IsGroup));
|
||
|
|
if (allPrimaryNonGroupObstacles == null) {
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
RectangleNodeUtils.CrossRectangleNodes<Obstacle>(allPrimaryNonGroupObstacles, allPrimaryNonGroupObstacles, this.EvaluateOverlappedPairForConvexHull);
|
||
|
|
}
|
||
|
|
|
||
|
|
private void EvaluateOverlappedPairForConvexHull(Obstacle a, Obstacle b) {
|
||
|
|
Debug.Assert(!a.IsGroup && !b.IsGroup, "Groups should not come here");
|
||
|
|
if ((a == b) || this.OverlapPairAlreadyFound(a, b)) {
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
bool aIsInsideB, bIsInsideA;
|
||
|
|
if (!ObstaclesIntersect(a, b, out aIsInsideB, out bIsInsideA) && !aIsInsideB && !bIsInsideA) {
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
// If either is in a convex hull, those must be coalesced.
|
||
|
|
if (!a.IsInConvexHull && !b.IsInConvexHull) {
|
||
|
|
// If the obstacles are rectangles, we don't need to do anything (for this pair).
|
||
|
|
if (a.IsRectangle && b.IsRectangle) {
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
this.overlapPairs.Insert(new IntPair(a.Ordinal, b.Ordinal));
|
||
|
|
AddClumpToConvexHull(a);
|
||
|
|
AddClumpToConvexHull(b);
|
||
|
|
AddConvexHullToConvexHull(a);
|
||
|
|
AddConvexHullToConvexHull(b);
|
||
|
|
}
|
||
|
|
|
||
|
|
void GrowGroupsToAccommodateOverlaps() {
|
||
|
|
// Group growth is transitive, because the created hull may overlap additional obstacles.
|
||
|
|
for (; ; ) {
|
||
|
|
this.AccumulateObstaclesForGroupOverlaps();
|
||
|
|
if (!this.GrowGroupsToResolveOverlaps()) {
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
private void AccumulateObstaclesForGroupOverlaps() {
|
||
|
|
var groupObstacles = CalculateHierarchy(this.GetAllObstacles().Where(obs => obs.IsGroup));
|
||
|
|
var allPrimaryObstacles = CalculateHierarchy(this.GetAllObstacles().Where(obs => obs.IsPrimaryObstacle));
|
||
|
|
if ((groupObstacles == null) || (allPrimaryObstacles == null)) {
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
RectangleNodeUtils.CrossRectangleNodes<Obstacle>(groupObstacles, allPrimaryObstacles, this.EvaluateOverlappedPairForGroup);
|
||
|
|
}
|
||
|
|
|
||
|
|
private void EvaluateOverlappedPairForGroup(Obstacle a, Obstacle b) {
|
||
|
|
Debug.Assert(a.IsGroup, "Inconsistency in overlapping group enumeration");
|
||
|
|
if ((a == b) || this.OverlapPairAlreadyFound(a, b)) {
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
bool aIsInsideB, bIsInsideA;
|
||
|
|
var curvesIntersect = ObstaclesIntersect(a, b, out aIsInsideB, out bIsInsideA);
|
||
|
|
if (!curvesIntersect && !aIsInsideB && !bIsInsideA) {
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (a.IsRectangle && b.IsRectangle) {
|
||
|
|
// If these are already rectangles, we don't need to do anything here. Non-group VisibilityPolylines
|
||
|
|
// will not change by the group operations; we'll just grow the group if needed (if it is already
|
||
|
|
// nonrectangular, either because it came in that way or because it has intersected a non-rectangle).
|
||
|
|
// However, SparseVg needs to know about the overlap so it will create interior scansegments if the
|
||
|
|
// obstacle is not otherwise overlapped.
|
||
|
|
if (!b.IsGroup) {
|
||
|
|
if (aIsInsideB || FirstRectangleContainsACornerOfTheOther(b.VisibilityBoundingBox, a.VisibilityBoundingBox)) {
|
||
|
|
b.OverlapsGroupCorner = true;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (!curvesIntersect) {
|
||
|
|
// If the borders don't intersect, we don't need to do anything if both are groups or the
|
||
|
|
// obstacle or convex hull is inside the group. Otherwise we have to grow group a to encompass b.
|
||
|
|
if (b.IsGroup || bIsInsideA) {
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
this.overlapPairs.Insert(new IntPair(a.Ordinal, b.Ordinal));
|
||
|
|
}
|
||
|
|
|
||
|
|
private static bool FirstRectangleContainsACornerOfTheOther(Rectangle a, Rectangle b) {
|
||
|
|
return a.Contains(b.LeftBottom) || a.Contains(b.LeftTop) || a.Contains(b.RightTop) || a.Contains(b.RightBottom);
|
||
|
|
}
|
||
|
|
|
||
|
|
private static bool FirstPolylineStartIsInsideSecondPolyline(Polyline first, Polyline second) {
|
||
|
|
return Curve.PointRelativeToCurveLocation(first.Start, second) != PointLocation.Outside;
|
||
|
|
}
|
||
|
|
|
||
|
|
private void AddClumpToConvexHull(Obstacle obstacle) {
|
||
|
|
if (obstacle.IsOverlapped) {
|
||
|
|
foreach (var sibling in obstacle.Clump.Where(sib => sib.Ordinal != obstacle.Ordinal)) {
|
||
|
|
this.overlapPairs.Insert(new IntPair(obstacle.Ordinal, sibling.Ordinal));
|
||
|
|
}
|
||
|
|
|
||
|
|
// Clear this now so any overlaps with other obstacles in the clump won't doubly insert.
|
||
|
|
obstacle.Clump.Clear();
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
private void AddConvexHullToConvexHull(Obstacle obstacle) {
|
||
|
|
if (obstacle.IsInConvexHull) {
|
||
|
|
foreach (var sibling in obstacle.ConvexHull.Obstacles.Where(sib => sib.Ordinal != obstacle.Ordinal)) {
|
||
|
|
this.overlapPairs.Insert(new IntPair(obstacle.Ordinal, sibling.Ordinal));
|
||
|
|
}
|
||
|
|
|
||
|
|
// Clear this now so any overlaps with other obstacles in the ConvexHull won't doubly insert.
|
||
|
|
obstacle.ConvexHull.Obstacles.Clear();
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
private void CreateClumps() {
|
||
|
|
var graph = new BasicGraph<IntPair>(this.overlapPairs);
|
||
|
|
var connectedComponents = ConnectedComponentCalculator<IntPair>.GetComponents(graph);
|
||
|
|
foreach (var component in connectedComponents) {
|
||
|
|
// GetComponents returns at least one self-entry for each index - including the < FirstNonSentinelOrdinal ones.
|
||
|
|
if (component.Count() == 1) {
|
||
|
|
continue;
|
||
|
|
}
|
||
|
|
var clump = new Clump(component.Select(this.OrdinalToObstacle));
|
||
|
|
foreach (var obstacle in clump) {
|
||
|
|
obstacle.Clump = clump;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
private bool CreateConvexHulls() {
|
||
|
|
var found = false;
|
||
|
|
var graph = new BasicGraph<IntPair>(this.overlapPairs);
|
||
|
|
var connectedComponents = ConnectedComponentCalculator<IntPair>.GetComponents(graph);
|
||
|
|
foreach (var component in connectedComponents) {
|
||
|
|
// GetComponents returns at least one self-entry for each index - including the < FirstNonSentinelOrdinal ones.
|
||
|
|
if (component.Count() == 1) {
|
||
|
|
continue;
|
||
|
|
}
|
||
|
|
found = true;
|
||
|
|
var obstacles = component.Select(this.OrdinalToObstacle);
|
||
|
|
var points = obstacles.SelectMany(obs => obs.VisibilityPolyline);
|
||
|
|
var och = new OverlapConvexHull(ConvexHull.CreateConvexHullAsClosedPolyline(points), obstacles);
|
||
|
|
foreach (var obstacle in obstacles) {
|
||
|
|
obstacle.SetConvexHull(och);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return found;
|
||
|
|
}
|
||
|
|
|
||
|
|
private bool GrowGroupsToResolveOverlaps() {
|
||
|
|
// This is one-at-a-time so not terribly efficient but there should be a very small number of such overlaps, if any.
|
||
|
|
var found = false;
|
||
|
|
foreach (var pair in this.overlapPairs) {
|
||
|
|
found = true;
|
||
|
|
var a = this.OrdinalToObstacle(pair.First);
|
||
|
|
var b = this.OrdinalToObstacle(pair.Second);
|
||
|
|
if (!ResolveGroupAndGroupOverlap(a, b)) {
|
||
|
|
ResolveGroupAndObstacleOverlap(a, b);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
this.overlapPairs.Clear();
|
||
|
|
return found;
|
||
|
|
}
|
||
|
|
|
||
|
|
private static bool ResolveGroupAndGroupOverlap(Obstacle a, Obstacle b) {
|
||
|
|
// For simplicity, pick the larger group and make grow its convex hull to encompass the smaller.
|
||
|
|
if (!b.IsGroup) {
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
if (a.VisibilityPolyline.BoundingBox.Area > b.VisibilityPolyline.BoundingBox.Area) {
|
||
|
|
ResolveGroupAndObstacleOverlap(a, b);
|
||
|
|
} else {
|
||
|
|
ResolveGroupAndObstacleOverlap(b, a);
|
||
|
|
}
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
|
||
|
|
private static void ResolveGroupAndObstacleOverlap(Obstacle group, Obstacle obstacle) {
|
||
|
|
// Create a convex hull for the group which goes outside the obstacle (which may also be a group).
|
||
|
|
// It must go outside the obstacle so we don't have coinciding angled sides in the scanline.
|
||
|
|
var loosePolyline = obstacle.LooseVisibilityPolyline;
|
||
|
|
GrowGroupAroundLoosePolyline(group, loosePolyline);
|
||
|
|
|
||
|
|
// Due to rounding we may still report this to be close or intersecting; grow it again if so.
|
||
|
|
bool aIsInsideB, bIsInsideA;
|
||
|
|
while (ObstaclesIntersect(obstacle, group, out aIsInsideB, out bIsInsideA) || !aIsInsideB) {
|
||
|
|
loosePolyline = Obstacle.CreateLoosePolyline(loosePolyline);
|
||
|
|
GrowGroupAroundLoosePolyline(group, loosePolyline);
|
||
|
|
}
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
private static void GrowGroupAroundLoosePolyline(Obstacle group, Polyline loosePolyline) {
|
||
|
|
var points = group.VisibilityPolyline.Select(p => p).Concat(loosePolyline.Select(p => p));
|
||
|
|
group.SetConvexHull(new OverlapConvexHull(ConvexHull.CreateConvexHullAsClosedPolyline(points), new[] { group }));
|
||
|
|
}
|
||
|
|
|
||
|
|
internal static bool ObstaclesIntersect(Obstacle a, Obstacle b, out bool aIsInsideB, out bool bIsInsideA) {
|
||
|
|
if (Curve.CurvesIntersect(a.VisibilityPolyline, b.VisibilityPolyline)) {
|
||
|
|
aIsInsideB = false;
|
||
|
|
bIsInsideA = false;
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
|
||
|
|
aIsInsideB = FirstPolylineStartIsInsideSecondPolyline(a.VisibilityPolyline, b.VisibilityPolyline);
|
||
|
|
bIsInsideA = !aIsInsideB && FirstPolylineStartIsInsideSecondPolyline(b.VisibilityPolyline, a.VisibilityPolyline);
|
||
|
|
if (a.IsRectangle && b.IsRectangle) {
|
||
|
|
// Rectangles do not require further evaluation.
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
if (ObstaclesAreCloseEnoughToBeConsideredTouching(a, b, aIsInsideB, bIsInsideA)) {
|
||
|
|
aIsInsideB = false;
|
||
|
|
bIsInsideA = false;
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
|
||
|
|
private static bool ObstaclesAreCloseEnoughToBeConsideredTouching(Obstacle a, Obstacle b, bool aIsInsideB, bool bIsInsideA) {
|
||
|
|
// This is only called when the obstacle.VisibilityPolylines don't intersect, thus one is inside the other
|
||
|
|
// or both are outside. If both are outside then either one's LooseVisibilityPolyline may be used.
|
||
|
|
if (!aIsInsideB && !bIsInsideA) {
|
||
|
|
return Curve.CurvesIntersect(a.LooseVisibilityPolyline, b.VisibilityPolyline);
|
||
|
|
}
|
||
|
|
|
||
|
|
// Otherwise see if the inner one is close enough to the outer border to consider them touching.
|
||
|
|
var innerLoosePolyline = aIsInsideB ? a.LooseVisibilityPolyline : b.LooseVisibilityPolyline;
|
||
|
|
var outerPolyline = aIsInsideB ? b.VisibilityPolyline : a.VisibilityPolyline;
|
||
|
|
foreach (Point innerPoint in innerLoosePolyline) {
|
||
|
|
if (Curve.PointRelativeToCurveLocation(innerPoint, outerPolyline) == PointLocation.Outside) {
|
||
|
|
var outerParamPoint = Curve.ClosestPoint(outerPolyline, innerPoint);
|
||
|
|
if (!ApproximateComparer.CloseIntersections(innerPoint, outerParamPoint)) {
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
|
||
|
|
/// <summary>
|
||
|
|
/// Add ancestors that are spatial parents - they may not be in the hierarchy, but we need to be
|
||
|
|
/// able to cross their boundaries if we're routing between obstacles on different sides of them.
|
||
|
|
/// </summary>
|
||
|
|
internal bool AdjustSpatialAncestors() {
|
||
|
|
if (this.SpatialAncestorsAdjusted) {
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Add each group to the AncestorSet of any spatial children (duplicate Insert() is ignored).
|
||
|
|
foreach (var group in GetAllGroups()) {
|
||
|
|
var groupBox = group.VisibilityBoundingBox;
|
||
|
|
|
||
|
|
foreach (var obstacle in Root.GetNodeItemsIntersectingRectangle(groupBox)) {
|
||
|
|
if ((obstacle != group) && Curve.ClosedCurveInteriorsIntersect(obstacle.VisibilityPolyline, group.VisibilityPolyline)) {
|
||
|
|
if (obstacle.IsInConvexHull)
|
||
|
|
{
|
||
|
|
Debug.Assert(obstacle.IsPrimaryObstacle, "Only primary obstacles should be in the hierarchy");
|
||
|
|
foreach (var sibling in obstacle.ConvexHull.Obstacles)
|
||
|
|
{
|
||
|
|
AncestorSets[sibling.InputShape].Insert(group.InputShape);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
AncestorSets[obstacle.InputShape].Insert(group.InputShape);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// Remove any hierarchical ancestors that are not spatial ancestors. Otherwise, when trying to route to
|
||
|
|
// obstacles that *are* spatial children of such a non-spatial-but-hierarchical ancestor, we won't enable
|
||
|
|
// crossing the boundary the first time and will always go to the full "activate all groups" path. By
|
||
|
|
// removing them here we not only get a better graph (avoiding some spurious crossings) but we're faster
|
||
|
|
// both in path generation and Nudging.
|
||
|
|
var nonSpatialGroups = new List<Shape>();
|
||
|
|
foreach (var child in Root.GetAllLeaves()) {
|
||
|
|
var childBox = child.VisibilityBoundingBox;
|
||
|
|
|
||
|
|
// This has to be two steps because we can't modify the Set during enumeration.
|
||
|
|
nonSpatialGroups.AddRange(AncestorSets[child.InputShape].Where(anc => !childBox.Intersects(this.shapeIdToObstacleMap[anc].VisibilityBoundingBox)));
|
||
|
|
foreach (var group in nonSpatialGroups) {
|
||
|
|
AncestorSets[child.InputShape].Remove(group);
|
||
|
|
}
|
||
|
|
nonSpatialGroups.Clear();
|
||
|
|
}
|
||
|
|
|
||
|
|
this.SpatialAncestorsAdjusted = true;
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
|
||
|
|
internal IEnumerable<Obstacle> GetAllGroups() {
|
||
|
|
return GetAllObstacles().Where(obs => obs.IsGroup);
|
||
|
|
}
|
||
|
|
|
||
|
|
/// <summary>
|
||
|
|
/// Clear the internal state.
|
||
|
|
/// </summary>
|
||
|
|
internal void Clear() {
|
||
|
|
Root = null;
|
||
|
|
AncestorSets = null;
|
||
|
|
}
|
||
|
|
|
||
|
|
/// <summary>
|
||
|
|
/// Create a LineSegment that contains the max visibility from startPoint in the desired direction.
|
||
|
|
/// </summary>
|
||
|
|
internal LineSegment CreateMaxVisibilitySegment(Point startPoint, Directions dir, out PointAndCrossingsList pacList) {
|
||
|
|
var graphBoxBorderIntersect = StaticGraphUtility.RectangleBorderIntersect(this.GraphBox, startPoint, dir);
|
||
|
|
if (PointComparer.GetDirections(startPoint, graphBoxBorderIntersect) == Directions. None) {
|
||
|
|
pacList = null;
|
||
|
|
return new LineSegment(startPoint, startPoint);
|
||
|
|
}
|
||
|
|
var segment = this.RestrictSegmentWithObstacles(startPoint, graphBoxBorderIntersect);
|
||
|
|
|
||
|
|
// Store this off before other operations which overwrite it.
|
||
|
|
pacList = this.CurrentGroupBoundaryCrossingMap.GetOrderedListBetween(segment.Start, segment.End);
|
||
|
|
return segment;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
/// <summary>
|
||
|
|
/// Convenience functions that call through to RectangleNode.
|
||
|
|
/// </summary>
|
||
|
|
/// <returns></returns>
|
||
|
|
internal IEnumerable<Obstacle> GetAllObstacles() {
|
||
|
|
return this.allObstacles;
|
||
|
|
}
|
||
|
|
|
||
|
|
/// <summary>
|
||
|
|
/// Returns a list of all primary obstacles - secondary obstacles inside a convex hull are not needed in the VisibilityGraphGenerator.
|
||
|
|
/// </summary>
|
||
|
|
/// <returns></returns>
|
||
|
|
internal IEnumerable<Obstacle> GetAllPrimaryObstacles() {
|
||
|
|
return this.Root.GetAllLeaves();
|
||
|
|
}
|
||
|
|
|
||
|
|
// Hit-testing.
|
||
|
|
internal bool IntersectionIsInsideAnotherObstacle(Obstacle sideObstacle, Obstacle eventObstacle, Point intersect, ScanDirection scanDirection) {
|
||
|
|
insideHitTestIgnoreObstacle1 = eventObstacle;
|
||
|
|
insideHitTestIgnoreObstacle2 = sideObstacle;
|
||
|
|
insideHitTestScanDirection = scanDirection;
|
||
|
|
RectangleNode<Obstacle> obstacleNode = Root.FirstHitNode(intersect, InsideObstacleHitTest);
|
||
|
|
return (null != obstacleNode);
|
||
|
|
}
|
||
|
|
|
||
|
|
internal bool PointIsInsideAnObstacle(Point intersect, Directions direction) {
|
||
|
|
return PointIsInsideAnObstacle(intersect, ScanDirection.GetInstance(direction));
|
||
|
|
}
|
||
|
|
|
||
|
|
internal bool PointIsInsideAnObstacle(Point intersect, ScanDirection scanDirection) {
|
||
|
|
insideHitTestIgnoreObstacle1 = null;
|
||
|
|
insideHitTestIgnoreObstacle2 = null;
|
||
|
|
insideHitTestScanDirection = scanDirection;
|
||
|
|
RectangleNode<Obstacle> obstacleNode = Root.FirstHitNode(intersect, InsideObstacleHitTest);
|
||
|
|
return (null != obstacleNode);
|
||
|
|
}
|
||
|
|
|
||
|
|
// Ignore one (always) or both (depending on location) of these obstacles on Obstacle hit testing.
|
||
|
|
Obstacle insideHitTestIgnoreObstacle1;
|
||
|
|
Obstacle insideHitTestIgnoreObstacle2;
|
||
|
|
ScanDirection insideHitTestScanDirection;
|
||
|
|
|
||
|
|
HitTestBehavior InsideObstacleHitTest(Point location, Obstacle obstacle) {
|
||
|
|
if ((obstacle == insideHitTestIgnoreObstacle1) || (obstacle == insideHitTestIgnoreObstacle2)) {
|
||
|
|
// It's one of the two obstacles we already know about.
|
||
|
|
return HitTestBehavior.Continue;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (obstacle.IsGroup) {
|
||
|
|
// Groups are handled differently from overlaps; we create ScanSegments (overlapped
|
||
|
|
// if within a non-group obstacle, else non-overlapped), and turn on/off access across
|
||
|
|
// the Group boundary vertices.
|
||
|
|
return HitTestBehavior.Continue;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (!StaticGraphUtility.PointIsInRectangleInterior(location, obstacle.VisibilityBoundingBox)) {
|
||
|
|
// The point is on the obstacle boundary, not inside it.
|
||
|
|
return HitTestBehavior.Continue;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Note: There are rounding issues using Curve.PointRelativeToCurveLocation at angled
|
||
|
|
// obstacle boundaries, hence this function.
|
||
|
|
Point high = StaticGraphUtility.RectangleBorderIntersect(obstacle.VisibilityBoundingBox, location
|
||
|
|
, insideHitTestScanDirection.Direction)
|
||
|
|
+ insideHitTestScanDirection.DirectionAsPoint;
|
||
|
|
Point low = StaticGraphUtility.RectangleBorderIntersect(obstacle.VisibilityBoundingBox, location
|
||
|
|
, insideHitTestScanDirection.OppositeDirection)
|
||
|
|
- insideHitTestScanDirection.DirectionAsPoint;
|
||
|
|
var testSeg = new LineSegment(low, high);
|
||
|
|
IList<IntersectionInfo> xxs = Curve.GetAllIntersections(testSeg, obstacle.VisibilityPolyline, true /*liftIntersections*/);
|
||
|
|
|
||
|
|
// If this is an extreme point it can have one intersection, in which case we're either on the border
|
||
|
|
// or outside; if it's a collinear flat boundary, there can be 3 intersections to this point which again
|
||
|
|
// means we're on the border (and 3 shouldn't happen anymore with the curve intersection fixes and
|
||
|
|
// PointIsInsideRectangle check above). So the interesting case is that we have 2 intersections.
|
||
|
|
if (2 == xxs.Count) {
|
||
|
|
Point firstInt = SpliceUtility.RawIntersection(xxs[0], location);
|
||
|
|
Point secondInt = SpliceUtility.RawIntersection(xxs[1], location);
|
||
|
|
|
||
|
|
// If we're on either intersection, we're on the border rather than inside.
|
||
|
|
if (!PointComparer.Equal(location, firstInt) && !PointComparer.Equal(location, secondInt)
|
||
|
|
&& (location.CompareTo(firstInt) != location.CompareTo(secondInt))) {
|
||
|
|
// We're inside. However, this may be an almost-flat side, in which case rounding
|
||
|
|
// could have reported the intersection with the start or end of the same side and
|
||
|
|
// a point somewhere on the interior of that side. Therefore if both intersections
|
||
|
|
// are on the same side (integral portion of the parameter), we consider location
|
||
|
|
// to be on the border. testSeg is always xxs[*].Segment0.
|
||
|
|
Debug.Assert(testSeg == xxs[0].Segment0, "incorrect parameter ordering to GetAllIntersections");
|
||
|
|
if (!ApproximateComparer.Close(Math.Floor(xxs[0].Par1), Math.Floor(xxs[1].Par1))) {
|
||
|
|
return HitTestBehavior.Stop;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return HitTestBehavior.Continue;
|
||
|
|
}
|
||
|
|
|
||
|
|
internal bool SegmentCrossesAnObstacle(Point startPoint, Point endPoint) {
|
||
|
|
stopAtGroups = true;
|
||
|
|
wantGroupCrossings = false;
|
||
|
|
LineSegment obstacleIntersectSeg = RestrictSegmentPrivate(startPoint, endPoint);
|
||
|
|
return !PointComparer.Equal(obstacleIntersectSeg.End, endPoint);
|
||
|
|
}
|
||
|
|
|
||
|
|
#if DEBUG
|
||
|
|
internal bool SegmentCrossesANonGroupObstacle(Point startPoint, Point endPoint) {
|
||
|
|
stopAtGroups = false;
|
||
|
|
wantGroupCrossings = false;
|
||
|
|
LineSegment obstacleIntersectSeg = RestrictSegmentPrivate(startPoint, endPoint);
|
||
|
|
return !PointComparer.Equal(obstacleIntersectSeg.End, endPoint);
|
||
|
|
}
|
||
|
|
#endif // DEBUG
|
||
|
|
|
||
|
|
internal LineSegment RestrictSegmentWithObstacles(Point startPoint, Point endPoint) {
|
||
|
|
stopAtGroups = false;
|
||
|
|
wantGroupCrossings = true;
|
||
|
|
return RestrictSegmentPrivate(startPoint, endPoint);
|
||
|
|
}
|
||
|
|
|
||
|
|
private LineSegment RestrictSegmentPrivate(Point startPoint, Point endPoint) {
|
||
|
|
GetRestrictedIntersectionTestSegment(startPoint, endPoint);
|
||
|
|
currentRestrictedRay = new LineSegment(startPoint, endPoint);
|
||
|
|
restrictedRayLengthSquared = (startPoint - endPoint).LengthSquared;
|
||
|
|
CurrentGroupBoundaryCrossingMap.Clear();
|
||
|
|
RecurseRestrictRayWithObstacles(Root);
|
||
|
|
return currentRestrictedRay;
|
||
|
|
}
|
||
|
|
|
||
|
|
private void GetRestrictedIntersectionTestSegment(Point startPoint, Point endPoint) {
|
||
|
|
// Due to rounding issues use a larger line span for intersection calculations.
|
||
|
|
Directions segDir = PointComparer.GetPureDirection(startPoint, endPoint);
|
||
|
|
double startX = (Directions.West == segDir) ? GraphBox.Right : ((Directions.East == segDir) ? GraphBox.Left : startPoint.X);
|
||
|
|
double endX = (Directions.West == segDir) ? GraphBox.Left : ((Directions.East == segDir) ? GraphBox.Right : endPoint.X);
|
||
|
|
double startY = (Directions.South == segDir) ? GraphBox.Top * 2: ((Directions.North == segDir) ? GraphBox.Bottom : startPoint.Y);
|
||
|
|
double endY = (Directions.South == segDir) ? GraphBox.Bottom : ((Directions.North == segDir) ? GraphBox.Top : startPoint.Y);
|
||
|
|
restrictedIntersectionTestSegment = new LineSegment(new Point(startX, startY), new Point(endX, endY));
|
||
|
|
}
|
||
|
|
|
||
|
|
// Due to rounding at the endpoints of the segment on intersection calculations, we need to preserve the original full-length segment.
|
||
|
|
LineSegment restrictedIntersectionTestSegment;
|
||
|
|
LineSegment currentRestrictedRay;
|
||
|
|
bool wantGroupCrossings;
|
||
|
|
bool stopAtGroups;
|
||
|
|
double restrictedRayLengthSquared;
|
||
|
|
|
||
|
|
private void RecurseRestrictRayWithObstacles(RectangleNode<Obstacle> rectNode) {
|
||
|
|
// A lineSeg that moves along the boundary of an obstacle is not blocked by it.
|
||
|
|
if (!StaticGraphUtility.RectangleInteriorsIntersect(currentRestrictedRay.BoundingBox, rectNode.Rectangle)) {
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
Obstacle obstacle = rectNode.UserData;
|
||
|
|
if (null != obstacle) {
|
||
|
|
// Leaf node. Get the interior intersections. Use the full-length original segment for the intersection calculation.
|
||
|
|
IList<IntersectionInfo> intersections = Curve.GetAllIntersections(restrictedIntersectionTestSegment, obstacle.VisibilityPolyline, true /*liftIntersections*/);
|
||
|
|
|
||
|
|
if (!obstacle.IsGroup || stopAtGroups) {
|
||
|
|
LookForCloserNonGroupIntersectionToRestrictRay(intersections);
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (wantGroupCrossings) {
|
||
|
|
AddGroupIntersectionsToRestrictedRay(obstacle, intersections);
|
||
|
|
}
|
||
|
|
|
||
|
|
Debug.Assert(rectNode.IsLeaf, "RectNode with UserData is not a Leaf");
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Not a leaf; recurse into children.
|
||
|
|
RecurseRestrictRayWithObstacles(rectNode.Left);
|
||
|
|
RecurseRestrictRayWithObstacles(rectNode.Right);
|
||
|
|
}
|
||
|
|
|
||
|
|
private void LookForCloserNonGroupIntersectionToRestrictRay(IList<IntersectionInfo> intersections) {
|
||
|
|
int numberOfGoodIntersections = 0;
|
||
|
|
IntersectionInfo closestIntersectionInfo = null;
|
||
|
|
var localLeastDistSquared = this.restrictedRayLengthSquared;
|
||
|
|
var testDirection = PointComparer.GetDirections(restrictedIntersectionTestSegment.Start, restrictedIntersectionTestSegment.End);
|
||
|
|
foreach (var intersectionInfo in intersections) {
|
||
|
|
var intersect = SpliceUtility.RawIntersection(intersectionInfo, currentRestrictedRay.Start);
|
||
|
|
var dirToIntersect = PointComparer.GetDirections(currentRestrictedRay.Start, intersect);
|
||
|
|
|
||
|
|
if (dirToIntersect == CompassVector.OppositeDir(testDirection)) {
|
||
|
|
continue;
|
||
|
|
}
|
||
|
|
++numberOfGoodIntersections;
|
||
|
|
|
||
|
|
if (Directions. None == dirToIntersect) {
|
||
|
|
localLeastDistSquared = 0.0;
|
||
|
|
closestIntersectionInfo = intersectionInfo;
|
||
|
|
continue;
|
||
|
|
}
|
||
|
|
|
||
|
|
var distSquared = (intersect - currentRestrictedRay.Start).LengthSquared;
|
||
|
|
if (distSquared < localLeastDistSquared) {
|
||
|
|
// Rounding may falsely report two intersections as different when they are actually "Close",
|
||
|
|
// e.g. a horizontal vs. vertical intersection on a slanted edge.
|
||
|
|
var rawDistSquared = (intersectionInfo.IntersectionPoint - currentRestrictedRay.Start).LengthSquared;
|
||
|
|
if (rawDistSquared < ApproximateComparer.SquareOfDistanceEpsilon) {
|
||
|
|
continue;
|
||
|
|
}
|
||
|
|
localLeastDistSquared = distSquared;
|
||
|
|
closestIntersectionInfo = intersectionInfo;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
if (null != closestIntersectionInfo) {
|
||
|
|
// If there was only one intersection and it is quite close to an end, ignore it.
|
||
|
|
// If there is more than one intersection, we have crossed the obstacle so we want it.
|
||
|
|
if (numberOfGoodIntersections == 1) {
|
||
|
|
var intersect = SpliceUtility.RawIntersection(closestIntersectionInfo, currentRestrictedRay.Start);
|
||
|
|
if (ApproximateComparer.CloseIntersections(intersect, this.currentRestrictedRay.Start) ||
|
||
|
|
ApproximateComparer.CloseIntersections(intersect, this.currentRestrictedRay.End)) {
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
this.restrictedRayLengthSquared = localLeastDistSquared;
|
||
|
|
currentRestrictedRay.End = SpliceUtility.MungeClosestIntersectionInfo(currentRestrictedRay.Start, closestIntersectionInfo
|
||
|
|
, !StaticGraphUtility.IsVertical(currentRestrictedRay.Start, currentRestrictedRay.End));
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
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);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|