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 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);
}
}
}
public ObstacleSegment GetOtherAdjacentSegment(ObstacleSegment s)
{
return(_segments[0] == s ? _segments[1] : _segments[0]);
}
private void InitializeNodeVisibility()
{
foreach (RotationTreeNode obstaclePoint in _nodes)
{
double minDist = double.MaxValue;
ObstacleSegment minDistSeg = null;
foreach (ObstacleSegment obstacleSegment in _obstacles.SelectMany(o => o.Segments))
{
if (obstacleSegment.Contains(obstaclePoint))
{
continue;
}
if (obstaclePoint.IsSinglePoint && obstaclePoint.Obstacle == obstacleSegment.Obstacle)
{
continue;
}
double pointX = obstaclePoint.X;
double pointY = obstaclePoint.Y;
double segPoint1X = obstacleSegment.Point1.X;
double segPoint1Y = obstacleSegment.Point1.Y;
double segPoint2X = obstacleSegment.Point2.X;
double segPoint2Y = obstacleSegment.Point2.Y;
double segMinX = Math.Min(segPoint1X, segPoint2X);
double segMinY = Math.Min(segPoint1Y, segPoint2Y);
double segMaxX = Math.Max(segPoint1X, segPoint2X);
double segMaxY = Math.Max(segPoint1Y, segPoint2Y);
double dist;
if (segMinX <= pointX && segMaxX > pointX)
{
// if segment stretches across the point in x direction
// We have to calculate the distance from the point
// straight down to the segment.
// Because the segment have a slope, we have to do a little math.
if (Math.Abs(segMinY - segMaxY) < double.Epsilon)
{
// special case: the segment is horizontal
dist = segMinY - pointY;
}
else if (Math.Abs(segMinX - segMaxX) < double.Epsilon)
{
// special case: the segment is vertical
dist = Math.Min(segMinY - pointY, segMaxY - pointY);
}
else
{
// case: angled segment
double pointOnSegmentX = pointX;
double pointOnSegmentY;
if (Math.Abs(pointOnSegmentX - segMinX) > double.Epsilon)
{
pointOnSegmentY = segMinY + ((segMaxY - segMinY) / (segMaxX - segMinX) * (pointOnSegmentX - segMinX));
}
else
{
pointOnSegmentY = segMinY;
}
dist = pointOnSegmentY - pointY;
}
}
else
{
continue;
}
if (dist > 0)
{ // segment lies above point not below it
continue;
}
dist = Math.Abs(dist);
if (dist < minDist)
{ // check if current distance is smaller than previous ones
minDist = dist;
minDistSeg = obstacleSegment;
}
}
obstaclePoint.VisibleSegment = minDistSeg;
}
}
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 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);
}
