public ObstacleSegment(Obstacle obstacle, RotationTreeNode point1, RotationTreeNode point2)
{
_obstacle = obstacle;
_point1 = point1;
_point1.Segments.Add(this);
_point2 = point2;
_point2.Segments.Add(this);
}
public ObstacleSegment(Obstacle obstacle, RotationTreeNode point1, RotationTreeNode point2)
{
_obstacle = obstacle;
_point1 = point1;
_point1.Segments.Add(this);
_point2 = point2;
_point2.Segments.Add(this);
}
public RotationTreeNode GetOtherPoint(RotationTreeNode p)
{
return(_point1 == p ? _point2 : _point1);
}
public bool Contains(RotationTreeNode p)
{
return(_point1 == p || _point2 == p);
}
public RotationTreeNode GetOtherPoint(RotationTreeNode p)
{
return _point1 == p ? _point2 : _point1;
}
public bool Contains(RotationTreeNode p)
{
return _point1 == p || _point2 == p;
}
private bool SameObstacle(RotationTreeNode p, RotationTreeNode q)
{
return !p.IsSinglePoint && !q.IsSinglePoint && p.Obstacle.Nodes.Contains(q);
}
private static ObstacleSegment GetHigherSegment(RotationTreeNode node, RotationTreeNode p)
{
foreach (ObstacleSegment seg in node.Segments)
{
RotationTreeNode other = seg.GetOtherPoint(node);
if (other != p && other.Y > node.Y)
return seg;
}
return null;
}
private void HandleWithPoints(RotationTreeNode p, RotationTreeNode q)
{
if (p == null || q == null)
return;
if (p.X > q.X)
return;
if (p.IsSinglePoint && !q.IsSinglePoint && p.Obstacle == q.Obstacle)
{
p.VisibleSegment = q.VisibleSegment;
//AddEdge(p, q);
}
else if (q.IsSinglePoint && !p.IsSinglePoint && q.Obstacle == p.Obstacle)
{
//AddEdge(p, q);
}
else if (q.IsSinglePoint && p.VisibleSegment != null && q.Obstacle == p.VisibleSegment.Obstacle)
{
AddEdge(p, q);
}
else if (p.IsAdjacent(q))
{
ObstacleSegment higher = GetHigherSegment(q, p);
p.VisibleSegment = higher ?? q.VisibleSegment;
if (!SameObstacle(p, q))
AddEdge(p, q);
}
else if (p.VisibleSegment != null && p.VisibleSegment.Contains(q))
{
ObstacleSegment left = GetLeftSegment(q, p);
p.VisibleSegment = left ?? q.VisibleSegment;
if (!SameObstacle(p, q))
AddEdge(p, q);
}
else if (PointLiesNearerThanSegment(p, q))
{
p.VisibleSegment = q.IsSinglePoint ? q.VisibleSegment : GetNearerSegment(q, p);
if (!SameObstacle(p, q))
AddEdge(p, q);
}
}
private void Initialize()
{
_graph = new UndirectedGraph<PointVertex, Edge<PointVertex>>();
_nodes.Clear();
foreach (Obstacle obstacle in _obstacles)
{
foreach (RotationTreeNode node in obstacle.Nodes)
{
_nodes.Add(node);
_graph.AddVertex(new PointVertex(node.Point));
}
_graph.AddEdgeRange(obstacle.Segments.Select(s => new Edge<PointVertex>(new PointVertex(s.Point1.Point), new PointVertex(s.Point2.Point))));
}
foreach (Point point in _singlePoints)
{
Obstacle obstacle = _obstacles.FirstOrDefault(o => o.Contains(point));
var newPoint = new RotationTreeNode(obstacle, point, true);
_graph.AddVertex(new PointVertex(point));
_nodes.Add(newPoint);
}
double maxX = _nodes.Max(p => p.Point.X);
_plusInf = new RotationTreeNode(new Point(maxX + 100, double.PositiveInfinity));
_minusInf = new RotationTreeNode(new Point(maxX + 100, double.NegativeInfinity));
_plusInf.AddChild(_minusInf);
foreach (RotationTreeNode node in _nodes.OrderByDescending(n => n))
_minusInf.AddChild(node);
}
private void AddEdge(RotationTreeNode p, RotationTreeNode q)
{
var pVertex = new PointVertex(p.Point);
var qVertex = new PointVertex(q.Point);
if (_graph.ContainsVertex(pVertex) && _graph.ContainsVertex(qVertex))
_graph.AddEdge(new Edge<PointVertex>(pVertex, qVertex));
}
private static bool PointLiesNearerThanSegment(RotationTreeNode p, RotationTreeNode q)
{
ObstacleSegment visSegment = p.VisibleSegment;
if (visSegment == null) // if there is no segment, it can not lie nearer (to p) than q
return true;
// using a linear equation: y = mx + b
// for the line pq
double m1 = (q.Y - p.Y) / (q.X - p.X);
double b1 = p.Y - (m1 * p.X);
// getting the points of the segment
RotationTreeNode point1 = visSegment.Point1;
RotationTreeNode point2 = visSegment.Point2;
// check if both segment points lie on the
// same side of that line.
// if they do, there is no crossing.
if (!double.IsInfinity(m1))
{
double valuePoint1 = (m1 * point1.X) + b1;
double valuePoint2 = (m1 * point2.X) + b1;
if ((point1.Y > valuePoint1 && point2.Y > valuePoint2)
|| (point1.Y < valuePoint1 && point2.Y < valuePoint2))
{
return true;
}
}
else if (point1.X < p.X && point2.X < p.X || point1.X > p.X && point2.X > p.X)
{
return true;
}
// get the line for the segment
double m2 = (point2.Y - point1.Y) / (point2.X - point1.X);
double b2 = point1.Y - (m2 * point1.X);
if (Math.Abs(m1 - m2) < double.Epsilon) // avoiding divide-by-zero; lines are parallel
return true; // if pq and the segment are parallel, the segment can be discarded and the point "lies nearer"
// calculate the crossing point
double x;
if (!double.IsInfinity(m2) && !double.IsInfinity(m1))
x = (b2 - b1) / (m1 - m2);
else if (!double.IsInfinity(m2) && double.IsInfinity(m1))
x = p.X;
else
x = point1.X;
double y = m1 * x + b1;
var crossingPoint = new Point(x, y);
// check if distancePQ is the smallest
return CalcDistance(p.Point, q.Point) <= CalcDistance(p.Point, crossingPoint);
}
private static bool LeftTurn(RotationTreeNode p, RotationTreeNode q, RotationTreeNode r)
{
if (r == null)
return false;
if (double.IsNegativeInfinity(p.Y) || double.IsNegativeInfinity(q.Y) || double.IsNegativeInfinity(r.Y)
|| double.IsPositiveInfinity(p.Y) || double.IsPositiveInfinity(q.Y))
{
return false;
}
if (double.IsPositiveInfinity(r.Y))
return p.X <= q.X || (Math.Abs(p.X - q.X) < double.Epsilon && p.Y > q.Y);
double m = (q.Y - p.Y) / (q.X - p.X);
double b = p.Y - (m * p.X);
double rValue = (m * r.X) + b;
if (r.Y > rValue)
return p.X <= q.X;
if (r.Y < rValue)
return p.X > q.X;
return false;
}
private static ObstacleSegment GetNearerSegment(RotationTreeNode node, RotationTreeNode p)
{
ObstacleSegment segment1 = node.Segments[0];
ObstacleSegment segment2 = node.Segments[1];
return CalcDistance(p.Point, segment1.Point1.Point, segment1.Point2.Point) < CalcDistance(p.Point, segment2.Point1.Point, segment2.Point2.Point) ? segment1 : segment2;
}
private static ObstacleSegment GetLeftSegment(RotationTreeNode node, RotationTreeNode p)
{
ObstacleSegment seg1 = node.Segments[0];
ObstacleSegment seg2 = node.Segments[1];
RotationTreeNode pointSeg1 = seg1.GetOtherPoint(node);
RotationTreeNode pointSeg2 = seg2.GetOtherPoint(node);
// using a linear equation: y = mx + b
double m = (node.Y - p.Y) / (node.X - p.X);
double b = p.Y - (m * p.X);
// there now are several cases:
// 1) one of the two segments lies to the left
// 2) none lies to the left
// 3) both lie to the left
// 4) one segment lies directly behind the line p to q
// in case
// 1) we return that segment
// 2) we return null
// 3) we return the segment nearer to p
// 4) we return that segment (if the other does not lie to the left)
// check the cases if the line p-to-this
// is vertical
if (double.IsInfinity(m))
{
if (p.Y > node.Y)
{ // consider the direction of the vertical line
// case 1
if (pointSeg1.X < node.X && node.X <= pointSeg2.X)
return seg1; // Segment 1 lies to the left of pq
if (pointSeg2.X < node.X && node.X <= pointSeg1.X)
return seg2; // Segment 2 lies to the left of pq
// case 3
if (pointSeg1.X < node.X && pointSeg2.X < node.X)
//return node.GetNearestSegment(p);
return GetNearerSegment(node, p);
// case 4
if (Math.Abs(pointSeg1.X - node.X) < double.Epsilon)
return seg1;
if (Math.Abs(pointSeg2.X - node.X) < double.Epsilon)
return seg2;
// case 2
if (pointSeg1.X > node.X && pointSeg2.X > node.X)
return null;
}
else
{
// case 1
if (pointSeg1.X > node.X && node.X >= pointSeg2.X)
return seg1; // Segment 1 lies to the left of pq
if (pointSeg2.X > node.X && node.X >= pointSeg1.X)
return seg2; // Segment 2 lies to the left of pq
// case 3
if (pointSeg1.X > node.X && pointSeg2.X > node.X)
//return node.GetNearestSegment(p);
return GetNearerSegment(node, p);
// case 4
if (Math.Abs(pointSeg1.X - node.X) < double.Epsilon)
return seg1;
if (Math.Abs(pointSeg2.X - node.X) < double.Epsilon)
return seg2;
// case 2
if (pointSeg1.X < node.X && pointSeg2.X < node.X)
return null;
}
}
else
{ // check for non-vertical lines
// function value for end point of segment 1
double value1 = (m * pointSeg1.X) + b;
// function value for end point of segment 2
double value2 = (m * pointSeg2.X) + b;
// case 1
if (pointSeg1.Y > value1 && value2 >= pointSeg2.Y)
return seg1; // Segment 1 lies to the left of pq
if (pointSeg2.Y > value2 && value1 >= pointSeg1.Y)
return seg2; // Segment 2 lies to the left of pq
// case 3
if (pointSeg1.Y > value1 && pointSeg2.Y > value2)
//return node.GetNearestSegment(p);
return GetNearerSegment(node, p);
// case 4
if (Math.Abs(pointSeg1.Y - value1) < double.Epsilon)
return seg1;
if (Math.Abs(pointSeg2.Y - value2) < double.Epsilon)
return seg2;
// case 2
if (pointSeg1.Y < value1 && pointSeg2.Y < value2)
return null;
}
// return null if this.equals(p)
return null;
}
