private Set <VisibilityEdge> GetSegmentsOnPathsToInsertedNeighborsNotOnOldTrajectories(LgNodeInfo ni, Dictionary <SymmetricTuple <LgNodeInfo>, List <Point> > trajectories, LgPathRouter pathRouter)
{
var edges = new Set <VisibilityEdge>();
var neighbors = GetAdjacentProcessed(ni);
foreach (var neighb in neighbors)
{
var t1 = new SymmetricTuple <LgNodeInfo>(ni, neighb);
List <Point> trajectory;
if (trajectories.ContainsKey(t1))
{
trajectory = trajectories[t1];
}
else
{
continue;
}
for (int i = 0; i < trajectory.Count - 1; i++)
{
var p0 = trajectory[i];
var p1 = trajectory[i + 1];
var e = pathRouter.FindEdge(p0, p1);
Debug.Assert(e != null, "VisibilityEdge on trajectory not found!");
if (!pathRouter.IsOnOldTrajectory(e))
{
edges.Insert(e);
}
}
}
return(edges);
}
internal List <Point> GetTrajectory(LgNodeInfo s, LgNodeInfo t)
{
List <Point> path;
var tuple = new SymmetricTuple <LgNodeInfo>(s, t);
EdgeTrajectories.TryGetValue(tuple, out path);
return(path);
}
void RemoveTupleFromRailEdges(SymmetricTuple <LgNodeInfo> tuple)
{
var a = tuple.A.GeometryNode;
var b = tuple.B.GeometryNode;
foreach (var edge in EdgesBetween(a, b))
{
_railsOfEdges.Remove(edge);
}
}
internal void SetTrajectoryAndAddEdgesToUsed(LgNodeInfo s, LgNodeInfo t, List <Point> path)
{
var t1 = new SymmetricTuple <LgNodeInfo>(s, t);
if (_edgeTrajectories.ContainsKey(t1))
{
return;
}
_edgeTrajectories[t1] = path;
PathRouter.MarkEdgesUsedAlongPath(path);
}
void RemoveEdgeTrajectory(SymmetricTuple <LgNodeInfo> symmetricTuple)
{
List <Point> trajectory;
if (_edgeTrajectories.TryGetValue(symmetricTuple, out trajectory))
{
for (int i = 0; i < trajectory.Count - 1; i++)
{
DiminishUsed(trajectory[i], trajectory[i + 1]);
}
_edgeTrajectories.Remove(symmetricTuple);
}
}
List <Point> GetPointsOverlappingSeg(SymmetricTuple <int> seg, RTree <Point> tree, Point[] indexToPoints)
{
Point p0 = indexToPoints[seg.A];
Point p1 = indexToPoints[seg.B];
var rect = new Rectangle(p0, p1);
rect.Pad(1e-5);
Point[] vts = tree.GetAllIntersecting(rect).ToArray();
double t;
var vtsOverlapping = vts.Where(p => Point.DistToLineSegment(p, p0, p1, out t) < 1e-5).ToList();
vtsOverlapping = vtsOverlapping.OrderBy(p => (p - p0).Length).ToList();
return(vtsOverlapping);
}
internal bool HasSavedTrajectory(LgNodeInfo s, LgNodeInfo t)
{
var t1 = new SymmetricTuple <LgNodeInfo>(s, t);
return(EdgeTrajectories.ContainsKey(t1));
}
/*
void AddSegsHandlingOverlaps(SymmetricTuple<int> seg, RTree<Point> tree, Point[] indexToPoints) {
var overlaps = GetPointsOverlappingSeg(seg, tree,indexToPoints);
if (overlaps.Count <= 2) {
_segments.Insert(seg);
return;
}
Point ep0 = indexToPoints[seg.A];
AddSegment(ep0, overlaps[1]);
AddSegment(overlaps[overlaps.Count - 2], indexToPoints[seg.B]);
if (overlaps.Count > 3) {
AddSegsHandlingOverlaps(new SymmetricTuple<int>(_pointIndices[overlaps[1]],_pointIndices[overlaps[overlaps.Count - 2]]), tree, indexToPoints);
}
}
*/
List<Point> GetPointsOverlappingSeg(SymmetricTuple<int> seg, RTree<Point> tree, Point[] indexToPoints) {
Point p0 = indexToPoints[seg.A];
Point p1 = indexToPoints[seg.B];
var rect = new Rectangle(p0, p1);
rect.Pad(1e-5);
Point[] vts = tree.GetAllIntersecting(rect).ToArray();
double t;
var vtsOverlapping = vts.Where(p => Point.DistToLineSegment(p, p0, p1, out t) < 1e-5).ToList();
vtsOverlapping = vtsOverlapping.OrderBy(p => (p - p0).Length).ToList();
if (vtsOverlapping.Count > 2) {
Console.WriteLine("overlapping points");
foreach (var v in vtsOverlapping)
Console.WriteLine(v);
}
return vtsOverlapping;
}
void RemoveTupleFromRailEdges(SymmetricTuple<LgNodeInfo> tuple) {
var a = tuple.A.GeometryNode;
var b = tuple.B.GeometryNode;
foreach (var edge in EdgesBetween(a, b))
_railsOfEdges.Remove(edge);
}
private Set<VisibilityEdge> GetSegmentsOnPathsToInsertedNeighborsNotOnOldTrajectories(LgNodeInfo ni, Dictionary<SymmetricTuple<LgNodeInfo>, List<Point>> trajectories, LgPathRouter pathRouter)
{
var edges = new Set<VisibilityEdge>();
var neighbors = GetAdjacentProcessed(ni);
foreach (var neighb in neighbors) {
var t1 = new SymmetricTuple<LgNodeInfo>(ni, neighb);
List<Point> trajectory;
if (trajectories.ContainsKey(t1)) trajectory = trajectories[t1];
else continue;
for (int i = 0; i < trajectory.Count - 1; i++)
{
var p0 = trajectory[i];
var p1 = trajectory[i + 1];
var e = pathRouter.FindEdge(p0, p1);
Debug.Assert(e!=null, "VisibilityEdge on trajectory not found!");
if (!pathRouter.IsOnOldTrajectory(e))
{
edges.Insert(e);
}
}
}
return edges;
}
void RemoveEdgeTrajectory(SymmetricTuple<LgNodeInfo> symmetricTuple) {
List<Point> trajectory;
if (_edgeTrajectories.TryGetValue(symmetricTuple, out trajectory)) {
for (int i = 0; i < trajectory.Count - 1; i++) {
DiminishUsed(trajectory[i], trajectory[i + 1]);
}
_edgeTrajectories.Remove(symmetricTuple);
}
}
internal List<Point> GetTrajectory(LgNodeInfo s, LgNodeInfo t) {
List<Point> path;
var tuple = new SymmetricTuple<LgNodeInfo>(s, t);
EdgeTrajectories.TryGetValue(tuple, out path);
return path;
}
internal bool HasSavedTrajectory(LgNodeInfo s, LgNodeInfo t) {
var t1 = new SymmetricTuple<LgNodeInfo>(s, t);
return EdgeTrajectories.ContainsKey(t1);
}
internal void SetTrajectoryAndAddEdgesToUsed(LgNodeInfo s, LgNodeInfo t, List<Point> path) {
var t1 = new SymmetricTuple<LgNodeInfo>(s, t);
if (_edgeTrajectories.ContainsKey(t1)) return;
_edgeTrajectories[t1] = path;
PathRouter.MarkEdgesUsedAlongPath(path);
}