public static bool TestBlockage(IList<Polygon> obstaclePolygons, LinePath leftBound, LinePath rightBound, double expandDist, double trackWidth)
{
try
{
Circle robotCircle = new Circle(expandDist, Vector2.Zero);
Polygon robotPoly = robotCircle.ToPolygon(24);
List<BlockageData> obstacles = new List<BlockageData>(obstaclePolygons.Count);
foreach (Polygon poly in obstaclePolygons)
{
Polygon convolvedPoly = null;
try
{
convolvedPoly = Polygon.ConvexMinkowskiConvolution(robotPoly, poly);
}
catch (Exception)
{
// minkowski convolution failed, just expand that shit with the gaheezy inflate method
convolvedPoly = poly.Inflate(expandDist);
}
// add the entry to the obstacle collection
BlockageData data = new BlockageData(convolvedPoly);
obstacles.Add(data);
}
// shrink in the lanes by a half robot width
leftBound = leftBound.ShiftLateral(-trackWidth / 2.0);
rightBound = rightBound.ShiftLateral(-trackWidth / 2.0);
Queue<BlockageData> testQueue = new Queue<BlockageData>();
foreach (BlockageData obs in obstacles)
{
if (obs.convolvedPolygon.DoesIntersect(leftBound))
{
// check if this hits the right bound
if (obs.convolvedPolygon.DoesIntersect(rightBound))
{
// this extends across the entire lane, the segment is blocked
return true;
}
else
{
testQueue.Enqueue(obs);
obs.searchMark = true;
}
}
}
while (testQueue.Count > 0)
{
BlockageData obs = testQueue.Dequeue();
foreach (BlockageData neighbor in obstacles)
{
if (!neighbor.searchMark && Polygon.TestConvexIntersection(obs.convolvedPolygon, neighbor.convolvedPolygon))
{
if (neighbor.convolvedPolygon.DoesIntersect(rightBound))
return true;
testQueue.Enqueue(neighbor);
neighbor.searchMark = true;
}
}
}
return false;
}
catch (Exception) { }
return false;
}
public static bool TestBlockage(IList <Polygon> obstaclePolygons, LinePath leftBound, LinePath rightBound)
{
try
{
double expandDist = TahoeParams.T / 2.0 + 0.3;
Circle tahoeCircle = new Circle(expandDist, Coordinates.Zero);
Polygon tahoePoly = tahoeCircle.ToPolygon(24);
List <BlockageData> obstacles = new List <BlockageData>(obstaclePolygons.Count);
foreach (Polygon poly in obstaclePolygons)
{
Polygon convolvedPoly = null;
try
{
convolvedPoly = Polygon.ConvexMinkowskiConvolution(tahoePoly, poly);
}
catch (Exception)
{
// minkowski convolution failed, just expand that stuff with the gaheezy inflate method
convolvedPoly = poly.Inflate(expandDist);
}
// add the entry to the obstacle collection
BlockageData data = new BlockageData(convolvedPoly);
obstacles.Add(data);
}
// shrink in the lanes by a half tahoe width
leftBound = leftBound.ShiftLateral(-TahoeParams.T / 2.0);
rightBound = rightBound.ShiftLateral(-TahoeParams.T / 2.0);
Queue <BlockageData> testQueue = new Queue <BlockageData>();
foreach (BlockageData obs in obstacles)
{
if (obs.convolvedPolygon.DoesIntersect(leftBound))
{
// check if this hits the right bound
if (obs.convolvedPolygon.DoesIntersect(rightBound))
{
// this extends across the entire lane, the segment is blocked
return(true);
}
else
{
testQueue.Enqueue(obs);
obs.searchMark = true;
}
}
}
while (testQueue.Count > 0)
{
BlockageData obs = testQueue.Dequeue();
foreach (BlockageData neighbor in obstacles)
{
if (!neighbor.searchMark && Polygon.TestConvexIntersection(obs.convolvedPolygon, neighbor.convolvedPolygon))
{
if (neighbor.convolvedPolygon.DoesIntersect(rightBound))
{
return(true);
}
testQueue.Enqueue(neighbor);
neighbor.searchMark = true;
}
}
}
return(false);
}
catch (Exception) { }
return(false);
}