public virtual void ReplanGlobal(Platform platform)
{
// create searchGrid data structure for the EpPathFinding
BaseGrid searchGrid = new StaticGrid(platform.Map.Rows, platform.Map.Columns);
List <Tuple <double, GridPos> > searchPoses = new List <Tuple <double, GridPos> > (); // possible search poses
for (int i = 0; i < platform.Map.Rows; i++)
{
for (int j = 0; j < platform.Map.Columns; j++)
{
if (platform.Map.MapMatrix[i, j] < platform.OccupiedThreshold)
{
searchGrid.SetWalkableAt(i, j, true);
}
if ((platform.Map.MapMatrix[i, j] >= platform.FreeThreshold) && (platform.Map.MapMatrix[i, j] <= platform.OccupiedThreshold))
{
RegionLimits limits = platform.Map.CalculateLimits(i, j, 1);
List <Pose> posesl = limits.GetPosesWithinLimits();
foreach (Pose p in posesl)
{
if (platform.Map.MapMatrix[p.X, p.Y] < platform.FreeThreshold)
{
double d = Math.Sqrt(Math.Pow(p.X - platform.Pose.X, 2) + Math.Pow(p.Y - platform.Pose.Y, 2));
searchPoses.Add(new Tuple <double, GridPos>(d, new GridPos(i, j)));
break;
}
}
}
}
}
// set unaccessable for those places where are platforms and their enviroment within 1 step radius
foreach (Platform plt in platform.ObservedPlatforms)
{
if (plt.Equals(platform))
{
continue;
}
RegionLimits limits = platform.Map.CalculateLimits(plt.Pose.X, plt.Pose.Y, 1);
List <Pose> posesl = limits.GetPosesWithinLimits();
foreach (Pose p in posesl)
{
searchGrid.SetWalkableAt(p.X, p.Y, false);
}
}
// bound the search to avoid large computation
// select the first closest 50 candidates based on L2 distance
int maxNumOfSearchPoses = 50;
searchPoses.Sort((t1, t2) => t1.Item1.CompareTo(t2.Item1));
if (searchPoses.Count > maxNumOfSearchPoses)
{
searchPoses.RemoveRange(maxNumOfSearchPoses, searchPoses.Count - maxNumOfSearchPoses);
}
// init search
GridPos startPos = new GridPos(platform.Pose.X, platform.Pose.Y);
GridPos endPos = new GridPos(20, 10);
JumpPointParam jpParam = new JumpPointParam(searchGrid, startPos, endPos, false, true, true);
// find the best path
double bestPathScore = Double.PositiveInfinity;
List <GridPos> bestPath = null;
foreach (Tuple <double, GridPos> p in searchPoses)
{
//jpParam.Reset(startPos, p);
jpParam.Reset(new GridPos(platform.Pose.X, platform.Pose.Y), p.Item2);
List <GridPos> resultPathList = JumpPointFinder.FindPath(jpParam);
if (resultPathList.Count > 2)
{
double score = 0;
for (int i = 1; i < resultPathList.Count; i++)
{
score += Math.Sqrt(Math.Pow(resultPathList[i].x - resultPathList[i - 1].x, 2) + Math.Pow(resultPathList[i].y - resultPathList[i - 1].y, 2));
}
if (score < bestPathScore)
{
bestPathScore = score;
bestPath = resultPathList;
bestFronterier = new Pose(resultPathList.Last().x, resultPathList.Last().y);
}
}
}
// convert the best path to command sequence
if ((bestPath != null) && (bestPath.Count > 2))
{
List <Pose> bestPathConv = new List <Pose>();
bestPathConv.Add(platform.Pose);
for (int i = 1; i < bestPath.Count; i++)
{
Pose prevPose = bestPathConv.Last();
Pose goalPose = new Pose(bestPath[i].x, bestPath[i].y);
int dxl = Math.Sign(goalPose.X - prevPose.X);
int dyl = Math.Sign(goalPose.Y - prevPose.Y);
while (!prevPose.Equals(goalPose)) // it's a bit dangerous here
{
Pose newPose = new Pose(prevPose.X + dxl, prevPose.Y + dyl);
prevPose = newPose;
bestPathConv.Add(newPose);
}
}
for (int i = bestPathConv.Count - 2; i > 0; i--)
{
int dx = bestPathConv[i + 1].X - bestPathConv[i].X;
int dy = bestPathConv[i + 1].Y - bestPathConv[i].Y;
double dalpha = Math.Atan2(dy, dx);
Pose newPose = new Pose(bestPathConv[i].X, bestPathConv[i].Y, dalpha);
commandSequence.Push(newPose);
}
}
}
private GraphNode FindTrack(Pose startPose, Platform platform, int searchRadius)
{
Queue <GraphNode> candidates = new Queue <GraphNode>();
distMap = Matrix.Create <double>(platform.Map.Rows, platform.Map.Columns, Double.PositiveInfinity);
GraphNode startNode = new GraphNode(startPose, null, 0, 0);
candidates.Enqueue(startNode);
GraphNode bestFronterier = null;
double bestFronterierScore = Double.PositiveInfinity;
int fronterierNum = 0;
distMap[startPose.X, startPose.Y] = 0;
minDistMap = 0;
maxDistMap = 0;
// calculate safe zones around platforms
List <Pose> safeZone = new List <Pose>();
foreach (Platform plt in platform.ObservedPlatforms)
{
RegionLimits limits = platform.Map.CalculateLimits(plt.Pose.X, plt.Pose.Y, 1);
List <Pose> poses = limits.GetPosesWithinLimits();
foreach (Pose p in poses)
{
safeZone.Add(p);
}
}
//graph search
while (candidates.Count != 0)
{
GraphNode cp = candidates.Dequeue();
int k = cp.Depth + 1;
if ((k > searchRadius) && (searchRadius != -1))
{
break;
}
if (k > maxDeep)
{
break;
}
RegionLimits limits = platform.Map.CalculateLimits(cp.Pose.X, cp.Pose.Y, 1);
List <Pose> poses = limits.GetPosesWithinLimits();
// 1. Generate map based on the previous actions
MapObject infoMap = platform.Map;
infoMap = (MapObject)platform.Map.Clone();
foreach (Pose p in cp.DiscoverCells)
{
// this is an approximation here
infoMap.MapMatrix[p.X, p.Y] = 0;
}
foreach (Pose p in poses)
{
// is there any other platform on this bin?
if (safeZone.Exists(pt => pt.Equals(p)))
{
continue;
}
double score = cp.Score + 1;
double dalpha = Math.Abs(p.GetHeadingTo(cp.Pose)) / 45.0;
score = score + dalpha;
// don't override "(distMap[p.X, p.Y] > score)", speed up calculation, this is why this is an approximation algorithm
// for this, a correct score/info weighting procedure is needed
if (distMap[p.X, p.Y] != Double.PositiveInfinity)
{
continue;
}
// 2. Compute info that is gained by the next step
// this is an approximation here
RegionLimits nlimits = platform.Map.CalculateLimits(p.X, p.Y, 1);
List <Pose> neighp = nlimits.GetPosesWithinLimits();
//double info = neighp.Sum(x => (1 - Math.Abs(infoMap.MapMatrix[x.X, x.Y] - 0.5)) * 2) / 9; // using for-loop instead, cause it's faster
// using for-loop instead, cause it's faster
double info = 0;
for (int i = 0; i < neighp.Count; i++)
{
info += (1 - Math.Abs(infoMap.MapMatrix[neighp[i].X, neighp[i].Y] - 0.5)) * 2;
}
info = info / 9;
/*List<Tuple<int, Pose>> neighp = platform.CalculateBinsInFOV(p, 2);
* double info = neighp.Sum(x => (0.5 - Math.Abs(infoMap.MapMatrix[x.Item2.X, x.Item2.Y] - 0.5)) * 2) / 9;*/
score = score - info;
// we found a solution if it is not discovered yet
if ((platform.Map.MapMatrix[p.X, p.Y] > platform.FreeThreshold) && (platform.Map.MapMatrix[p.X, p.Y] < platform.OccupiedThreshold))
{
fronterierNum++;
if (((bestFronterierScore > score) && ((allocationMap[p.X, p.Y] == platform.ID))))
{
bestFronterier = new GraphNode(p, cp, k, score);
bestFronterierScore = score;
bestFronterier.DiscoverCells = new List <Pose>(cp.DiscoverCells);
foreach (Pose lp in neighp)
{
bestFronterier.DiscoverCells.Add(lp);
}
candidates.Enqueue(bestFronterier);
// if search radius is -1, then give back the first fronterier that we found, neverthless the score
if (searchRadius == -1)
{
return(bestFronterier);
}
continue;
}
}
// this pose is not occupied and has a higher score than the pervious, so expend it
if ((platform.Map.MapMatrix[p.X, p.Y] < platform.OccupiedThreshold) && (distMap[p.X, p.Y] > score))
{
GraphNode newNode = new GraphNode(p, cp, k, score);
newNode.DiscoverCells = new List <Pose>(cp.DiscoverCells);
foreach (Pose lp in neighp)
{
newNode.DiscoverCells.Add(lp);
}
candidates.Enqueue(newNode);
// maintain distance map
distMap[p.X, p.Y] = (double)score;
if (minDistMap > score)
{
minDistMap = (double)score;
}
if (maxDistMap < score)
{
maxDistMap = (double)score;
}
}
}
}
return(bestFronterier);
}
private GraphNode FindTrack(Pose startPose, Platform platform, int searchRadius)
{
Queue <GraphNode> candidates = new Queue <GraphNode>();
distMap = Matrix.Create <double>(platform.Map.Rows, platform.Map.Columns, Double.PositiveInfinity);
candidates.Enqueue(new GraphNode(startPose, null, 0, 0));
GraphNode bestFronterier = null;
int fronterierNum = 0;
distMap[startPose.X, startPose.Y] = 0;
minDistMap = 0;
maxDistMap = 0;
// calculate safe zones around platforms
List <Pose> safeZone = new List <Pose>();
foreach (Platform plt in platform.ObservedPlatforms)
{
RegionLimits limits = platform.Map.CalculateLimits(plt.Pose.X, plt.Pose.Y, 1);
List <Pose> poses = limits.GetPosesWithinLimits();
foreach (Pose p in poses)
{
safeZone.Add(p);
}
}
//graph search
while (candidates.Count != 0)
{
GraphNode cp = candidates.Dequeue();
int k = cp.Depth + 1;
if ((k > searchRadius) && (searchRadius != -1))
{
break;
}
if (k > maxDeep)
{
break;
}
RegionLimits limits = platform.Map.CalculateLimits(cp.Pose.X, cp.Pose.Y, 1);
List <Pose> poses = limits.GetPosesWithinLimits();
foreach (Pose p in poses)
{
// is there any other platform on this bin?
if (safeZone.Exists(pt => pt.Equals(p)))
{
continue;
}
double score = cp.Score + 1;
double dalpha = Math.Abs(p.GetHeadingTo(cp.Pose)) / 45.0;
score = score + dalpha;
// we found a solution if it is not discovered yet
if ((platform.Map.MapMatrix[p.X, p.Y] > platform.FreeThreshold) && (platform.Map.MapMatrix[p.X, p.Y] < platform.OccupiedThreshold))
{
fronterierNum++;
if ((bestFronterier == null) || (bestFronterier.Score > score))
{
bestFronterier = new GraphNode(p, cp, k, score);
candidates.Enqueue(bestFronterier);
// if search radius is -1, then give back the first fronterier that we found, neverthless the score
if (searchRadius == -1)
{
return(bestFronterier);
}
}
}
// this pose is not occupied and has a higher score than the pervious, so expend it
if ((platform.Map.MapMatrix[p.X, p.Y] < platform.OccupiedThreshold) && (distMap[p.X, p.Y] > score))
{
candidates.Enqueue(new GraphNode(p, cp, k, score));
// maintain distance map
distMap[p.X, p.Y] = (double)score;
if (minDistMap > score)
{
minDistMap = (double)score;
}
if (maxDistMap < score)
{
maxDistMap = (double)score;
}
}
}
}
return(bestFronterier);
}
