public void UpdateHeight(double x, double y)
{
double largeU = pu_hat.GetCell(x, y) / pij_sum.GetCell(x, y);
double largeSig = (psig_u_hat_square.GetCell(x, y) + pu_hat_square.GetCell(x, y)) / pij_sum.GetCell(x, y) - largeU * largeU;
uhatGM.SetCell(x, y, largeU);
sigSqrGM.SetCell(x, y, largeSig);
}
public IPath GetPathToGoal(IPath[] otherPaths, out IPath sparsePath, out bool inObstacle, out bool wpInObstacle)
{
inObstacle = false;
wpInObstacle = false;
sparsePath = null;
if (waypoints == null)
{
return(null);
}
if (waypointsAchieved >= waypoints.Count)
{
return(null);
}
if (og == null)
{
return(null);
}
if (bloatedObstacles == null)
{
Console.WriteLine("Convolved obstacles fail!");
return(null);
}
// Add current pose position to filteredWaypoints. (FilteredWaypoints is what we will be
// planning over. Waypoints is list of user given waypoints.)
List <Waypoint> filteredWaypoints = new List <Waypoint>();
filteredWaypoints.Add(new Waypoint(pose.ToVector2(), true, 0));
lock (ogLock)
{
if (og.GetCell(pose.x, pose.y) == 255)
{
inObstacle = true;
og.SetCell(pose.x, pose.y, 0);
}
}
// Only add user waypoints to PathPoints if they have not been achieved yet.
// If they are inside an obstacle, return a null path.
foreach (Waypoint wp in waypoints)
{
if (!wp.Achieved)
{
lock (bloatedObstacleLock)
{
foreach (Polygon p in bloatedObstacles)
{
if (p.IsInside(wp.Coordinate))
{
wpInObstacle = true;
return(null);
}
}
}
filteredWaypoints.Add(wp);
}
}
// Plan each segment of the filteredWaypoints individually and stitch them together.
List <Waypoint> completeRawPath = new List <Waypoint>();
for (int i = 0; i < filteredWaypoints.Count - 1; i++)
{
bool pathOk;
List <Waypoint> rawPath;
lock (ogLock)
{
rawPath = dstar.FindPath(filteredWaypoints[i], filteredWaypoints[i + 1], og, out pathOk);
}
if (!pathOk)
{
return(null);
}
else
{
int j;
// We want to only add the first path waypoint if it is the first
// segment of the whole path.
if (i == 0)
{
j = 0;
}
else
{
j = 1;
}
for (; j < rawPath.Count; j++)
{
completeRawPath.Add(rawPath.ElementAt(j));
}
}
}
if (completeRawPath.Count > 0)
{
IPath bezierPath;
PointPath segmentedBezierPath;
lock (bloatedObstacleLock)
{
bezierPath = smoother.Wp2BezierPath(completeRawPath, bloatedObstacles, collapseThreshold);
}
sparsePath = new PointPath(completeRawPath);
segmentedBezierPath = smoother.ConvertBezierPathToPointPath(bezierPath, bezierSegmentation);
// We want to check our sparse path against other sparse paths to see
// if there are any intersections. If there are, we want to know if the
// intersections will be a problem. If it is a problem, bloat that sparse path,
// add it to obstacles, and replan.
for (int i = 0; i < otherPaths.Length; i++)
{
if (otherPaths[i] != null && otherPaths[i].Count > 0 && i + 1 != robotID)
{
double intersectDist = DoPathsCollide(sparsePath, otherPaths[i]);
if (intersectDist != -1)
{
PointOnPath collisionPt;
int collisionSegmentIndex;
double tempDist = intersectDist - collideBackoff;
tempDist = Math.Max(0, tempDist);
collisionPt = segmentedBezierPath.AdvancePoint(segmentedBezierPath.StartPoint, ref tempDist);
collisionSegmentIndex = segmentedBezierPath.IndexOf(collisionPt.segment);
segmentedBezierPath.RemoveRange(collisionSegmentIndex, segmentedBezierPath.Count - collisionSegmentIndex);
tempDist = intersectDist - collideBackoff;
tempDist = Math.Max(0, tempDist);
collisionPt = sparsePath.AdvancePoint(sparsePath.StartPoint, ref tempDist);
collisionSegmentIndex = sparsePath.IndexOf(collisionPt.segment);
sparsePath.RemoveRange(collisionSegmentIndex, sparsePath.Count - collisionSegmentIndex - 1);
sparsePath[collisionSegmentIndex].End = collisionPt.pt;
}
}
}
return(segmentedBezierPath);
}
return(null);
}
