public BaseGrid PNGtoGrid(Bitmap input)
{
BaseGrid output = new StaticGrid(
input.Width,
input.Height
);
for (int x = 0; x < input.Width; x++)
{
for (int y = 0; y < input.Height; y++)
{
Color c = input.GetPixel(
x,
y
);
if (c.GetBrightness() < threshold)
output.SetWalkableAt(
x,
y,
false
);
else
output.SetWalkableAt(
x,
y,
true
);
}
}
if (debug) Console.WriteLine("converted PNG to grid");
return output;
}
public void LoadZone()
{
using (Stream stream = new MemoryStream(Data))
{
int numBytesToRead = 1;
int numBytesRead = 0;
byte[] bytes = new byte[numBytesToRead];
byte[] xlength = new byte[2];
byte[] ylength = new byte[2];
stream.Read(bytes, numBytesRead, numBytesToRead);
xlength[0] = bytes[0];
stream.Read(bytes, numBytesRead, numBytesToRead);
xlength[1] = bytes[0];
stream.Read(bytes, numBytesRead, numBytesToRead);
ylength[0] = bytes[0];
stream.Read(bytes, numBytesRead, numBytesToRead);
ylength[1] = bytes[0];
YLength = BitConverter.ToInt16(ylength, 0);
XLength = BitConverter.ToInt16(xlength, 0);
Grid = new StaticGrid(XLength, YLength);
for (int i = 0; i < YLength; ++i)
{
for (int t = 0; t < XLength; ++t)
{
stream.Read(bytes, numBytesRead, numBytesToRead);
Grid.SetWalkableAt(new GridPos(t, i), bytes[0]);
}
}
}
}
public void LoadZone()
{
Stream stream = new MemoryStream(Data);
byte[] bytes = new byte[stream.Length];
int numBytesToRead = 1;
int numBytesRead = 0;
byte[] xlength = new byte[2];
byte[] ylength = new byte[2];
stream.Read(bytes, numBytesRead, numBytesToRead);
xlength[0] = bytes[0];
stream.Read(bytes, numBytesRead, numBytesToRead);
xlength[1] = bytes[0];
stream.Read(bytes, numBytesRead, numBytesToRead);
ylength[0] = bytes[0];
stream.Read(bytes, numBytesRead, numBytesToRead);
ylength[1] = bytes[0];
YLength = BitConverter.ToInt16(ylength, 0);
XLength = BitConverter.ToInt16(xlength, 0);
_grid = new StaticGrid(XLength, YLength);
for (int i = 0; i < YLength; ++i)
{
for (int t = 0; t < XLength; ++t)
{
stream.Read(bytes, numBytesRead, numBytesToRead);
_grid.SetWalkableAt(new GridPos(t, i), Convert.ToBoolean(Convert.ToInt16(bytes[0]) == 0 ? true : false));
}
}
// initialize JPS _tempgrid = ConvertToGrid(_grid);
JumpPointParameters = new JumpPointParam(_grid, new GridPos(0, 0), new GridPos(0, 0), false, true, true, HeuristicMode.MANHATTAN);
}
public BaseGrid ConvertToGrid(short[,] _grid)
{
BaseGrid grid = new StaticGrid(XLength, YLength);
for (int y = 0; y < YLength; ++y)
{
for (int x = 0; x < XLength; ++x)
{
grid.SetWalkableAt(x, y, !IsBlockedZone(x, y));
}
}
return(grid);
}
public BaseGrid ConvertToGrid(short[,] _grid)
{
BaseGrid grid = new StaticGrid(XLength, YLength);
for (int i = 0; i < YLength; ++i)
{
for (int t = 0; t < XLength; ++t)
{
grid.SetWalkableAt(t, i, (_grid[t, i] == 0 ? true : false));
}
}
return(grid);
}
private JumpPointParam GetJumpPointParam()
{
if (_cachedJpParam == null)
{
BaseGrid searchGrid = new StaticGrid(WorldController.instance.Width, WorldController.instance.Height);
for (int x = 0; x < WorldController.instance.Width; x++)
{
for (int z = 0; z < WorldController.instance.Height; z++)
{
searchGrid.SetWalkableAt(x, z, true);
}
}
_cachedJpParam = new JumpPointParam(searchGrid); // Cant configure it to not allow diagonally moving, has to be enforced by us
}
return(_cachedJpParam);
}
public void LoadMap(string jsonData)
{
Debug.LogError("LoadMap");
var data = SimpleJSON.JSON.Parse(jsonData);
width = System.Convert.ToInt32(data[0]["width"].AsFloat);
height = Convert.ToInt32(data[0]["height"].AsFloat);
var cenArray = data[0]["center"].AsObject;
var cx = cenArray["x"].AsFloat;
var cy = cenArray["y"].AsFloat;
var cz = cenArray["z"].AsFloat;
center = new Vector3(cx, cy, cz);
nodeSize = data[0]["nodeSize"].AsFloat;
var mapData = data[0]["mapdata"].AsArray;
grids = new StaticGrid(width, height);
var i = 0;
foreach (SimpleJSON.JSONNode d in mapData)
{
var v = d.AsInt;
if (v == 0)
{
var r = i / width;
var c = i % width;
grids.SetWalkableAt(c, r, true);
}
i++;
}
var mh = data[0]["mapHeight"].AsArray;
mapHeight = new List <float>(width * height);
foreach (JSONNode d in mh)
{
mapHeight.Add(d.AsFloat);
}
}
private void LoadMapJsonDict(JSONClass jclass)
{
width = System.Convert.ToInt32(jclass["width"].AsFloat);
height = Convert.ToInt32(jclass["height"].AsFloat);
var cenArray = jclass["center"].AsObject;
var cx = cenArray["x"].AsFloat;
var cy = cenArray["y"].AsFloat;
var cz = cenArray["z"].AsFloat;
center = new Vector3(cx, cy, cz);
nodeSize = jclass["nodeSize"].AsFloat;
var mapData = jclass["mapdata"].AsArray;
grids = new StaticGrid(width, height);
var i = 0;
foreach (SimpleJSON.JSONNode d in mapData)
{
var v = d.AsInt;
if (v == 0)
{
var r = i / width;
var c = i % width;
grids.SetWalkableAt(c, r, true);
}
i++;
}
var mh = jclass["mapHeight"].AsArray;
mapHeight = new List <float>(width * height);
foreach (JSONNode d in mh)
{
mapHeight.Add(d.AsFloat);
}
}
public static double CalculateFitness(Chromosome chromosome)
{
double fitness = 0;
var board = chromosome2Board(chromosome);
if (board[0, 0] != 2)
{
return(0);
}
if (board[boardSize / 2 - 1, boardSize / 2 - 1] != 0)
{
return(0);
}
BaseGrid searchGrid = new StaticGrid(boardSize, boardSize);
searchGrid.SetWalkableAt(0, 0, true);
for (var i = 0; i < boardSize; i++)
{
for (var j = 0; j < boardSize; j++)
{
if (board[i, j] != 1)
{
searchGrid.SetWalkableAt(i, j, true);
}
}
}
GridPos startPos = new GridPos(0, 0);
GridPos endPos;
JumpPointParam jpParam;
var resultPathLists = new List <List <GridPos> >();
for (var i = 0; i < boardSize; i++)
{
for (var j = 0; j < boardSize; j++)
{
if (board[i, j] != 1)
{
endPos = new GridPos(i, j);
jpParam = new JumpPointParam(searchGrid, startPos, endPos);
resultPathLists.Add(JumpPointFinder.FindPath(jpParam));
}
}
}
var counter = 0;
foreach (var list in resultPathLists)
{
if (list.Count > 0)
{
counter++;
}
}
fitness = ((double)counter) / ((double)resultPathLists.Count);
if (!fieldProportions(board) && (fitness > 0.9)) // && (fitness > 0.75))
{
fitness = fitness / 2; // fitness/2;// fitness *3/4;
}
return(fitness);
}
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);
}
}
}
public BaseGrid ShowFloor1()
{
BaseGrid searchGrid = new StaticGrid(20, 40);
// //LEFT BOTTOM
// searchGrid.SetWalkableAt(new GridPos(7, 31), true);
// searchGrid.SetWalkableAt(new GridPos(6, 31), true);
// searchGrid.SetWalkableAt(new GridPos(6, 32), true);
// searchGrid.SetWalkableAt(new GridPos(7, 32), true);
// searchGrid.SetWalkableAt(new GridPos(7, 33), true);
// //searchGrid.SetWalkableAt(new GridPos(8, 33), true);
// searchGrid.SetWalkableAt(new GridPos(8, 34), true);
// searchGrid.SetWalkableAt(new GridPos(9, 34), true);
// searchGrid.SetWalkableAt(new GridPos(10, 34), true);
// //RIGHT BOTTOM
// searchGrid.SetWalkableAt(new GridPos(11, 33), true);
// searchGrid.SetWalkableAt(new GridPos(11, 34), true);
// searchGrid.SetWalkableAt(new GridPos(12, 33), true);
// searchGrid.SetWalkableAt(new GridPos(12, 34), true);
// searchGrid.SetWalkableAt(new GridPos(13, 34), true);
// searchGrid.SetWalkableAt(new GridPos(14, 34), true);
// searchGrid.SetWalkableAt(new GridPos(15, 34), true);
// searchGrid.SetWalkableAt(new GridPos(16, 34), true);
// searchGrid.SetWalkableAt(new GridPos(13, 33), true);
// searchGrid.SetWalkableAt(new GridPos(14, 33), true);
// searchGrid.SetWalkableAt(new GridPos(15, 33), true);
// searchGrid.SetWalkableAt(new GridPos(14, 32), true);
// searchGrid.SetWalkableAt(new GridPos(15, 32), true);
// searchGrid.SetWalkableAt(new GridPos(15, 31), true);
// //RIGHT WALKWAY
// searchGrid.SetWalkableAt(new GridPos(16, 30), true);
// searchGrid.SetWalkableAt(new GridPos(16, 29), true);
// searchGrid.SetWalkableAt(new GridPos(16, 28), true);
// searchGrid.SetWalkableAt(new GridPos(16, 27), true);
// searchGrid.SetWalkableAt(new GridPos(16, 26), true);
// searchGrid.SetWalkableAt(new GridPos(16, 25), true);
// searchGrid.SetWalkableAt(new GridPos(14, 25), true);
// searchGrid.SetWalkableAt(new GridPos(15, 25), true);
// searchGrid.SetWalkableAt(new GridPos(16, 24), true);
// searchGrid.SetWalkableAt(new GridPos(15, 24), true);
// searchGrid.SetWalkableAt(new GridPos(14, 24), true);
// searchGrid.SetWalkableAt(new GridPos(16, 23), true);
// searchGrid.SetWalkableAt(new GridPos(16, 22), true);
// searchGrid.SetWalkableAt(new GridPos(16, 21), true);
// searchGrid.SetWalkableAt(new GridPos(16, 20), true);
// searchGrid.SetWalkableAt(new GridPos(16, 19), true);
// searchGrid.SetWalkableAt(new GridPos(16, 18), true);
// searchGrid.SetWalkableAt(new GridPos(15, 18), true);
// searchGrid.SetWalkableAt(new GridPos(14, 18), true);
// //LEFT WALKWAY
// searchGrid.SetWalkableAt(new GridPos(6, 30), true);
// searchGrid.SetWalkableAt(new GridPos(6, 29), true);
// searchGrid.SetWalkableAt(new GridPos(6, 28), true);
// searchGrid.SetWalkableAt(new GridPos(6, 27), true);
// searchGrid.SetWalkableAt(new GridPos(6, 26), true);
// searchGrid.SetWalkableAt(new GridPos(6, 25), true);
// searchGrid.SetWalkableAt(new GridPos(7, 25), true);
// searchGrid.SetWalkableAt(new GridPos(8, 25), true);
// searchGrid.SetWalkableAt(new GridPos(6, 24), true);
// searchGrid.SetWalkableAt(new GridPos(7, 24), true);
// searchGrid.SetWalkableAt(new GridPos(8, 24), true);
// searchGrid.SetWalkableAt(new GridPos(6, 23), true);
// searchGrid.SetWalkableAt(new GridPos(6, 22), true);
// searchGrid.SetWalkableAt(new GridPos(6, 21), true);
// searchGrid.SetWalkableAt(new GridPos(6, 20), true);
// searchGrid.SetWalkableAt(new GridPos(6, 19), true);
// searchGrid.SetWalkableAt(new GridPos(6, 18), true);
// searchGrid.SetWalkableAt(new GridPos(6, 17), true);
// searchGrid.SetWalkableAt(new GridPos(7, 17), true);
// //searchGrid.SetWalkableAt(new GridPos(7, 18), true);
// //searchGrid.SetWalkableAt(new GridPos(7, 19), true);
//// searchGrid.SetWalkableAt(new GridPos(7, 20), true);
// searchGrid.SetWalkableAt(new GridPos(8, 18), true);
// //searchGrid.SetWalkableAt(new GridPos(8, 19), true);
////searchGrid.SetWalkableAt(new GridPos(8, 20), true);
///
searchGrid.SetWalkableAt(new GridPos(11, 35), true);
searchGrid.SetWalkableAt(new GridPos(11, 34), true);
//searchGrid.SetWalkableAt(new GridPos(10, 33), true);
searchGrid.SetWalkableAt(new GridPos(10, 35), true);
//searchGrid.SetWalkableAt(new GridPos(9, 33), true);
searchGrid.SetWalkableAt(new GridPos(9, 35), true);
//searchGrid.SetWalkableAt(new GridPos(8, 33), true);
searchGrid.SetWalkableAt(new GridPos(8, 35), true);
searchGrid.SetWalkableAt(new GridPos(7, 35), true);
searchGrid.SetWalkableAt(new GridPos(7, 33), true);
searchGrid.SetWalkableAt(new GridPos(7, 34), true);
searchGrid.SetWalkableAt(new GridPos(7, 32), true);
searchGrid.SetWalkableAt(new GridPos(7, 31), true);
searchGrid.SetWalkableAt(new GridPos(6, 33), true);
searchGrid.SetWalkableAt(new GridPos(6, 32), true);
searchGrid.SetWalkableAt(new GridPos(6, 31), true);
searchGrid.SetWalkableAt(new GridPos(6, 30), true);
searchGrid.SetWalkableAt(new GridPos(6, 29), true);
searchGrid.SetWalkableAt(new GridPos(6, 28), true);
searchGrid.SetWalkableAt(new GridPos(6, 27), true);
searchGrid.SetWalkableAt(new GridPos(6, 26), true);
searchGrid.SetWalkableAt(new GridPos(6, 25), true);
searchGrid.SetWalkableAt(new GridPos(6, 24), true);
searchGrid.SetWalkableAt(new GridPos(6, 23), true);
searchGrid.SetWalkableAt(new GridPos(6, 22), true);
searchGrid.SetWalkableAt(new GridPos(6, 21), true);
searchGrid.SetWalkableAt(new GridPos(6, 20), true);
searchGrid.SetWalkableAt(new GridPos(6, 19), true);
searchGrid.SetWalkableAt(new GridPos(7, 19), true);
searchGrid.SetWalkableAt(new GridPos(6, 19), true);
//jpParam.Reset(startPos, endPos);
//List<GridPos> resultPathList = JumpPointFinder.FindPath(jpParam);
//System.Diagnostics.Debug.WriteLine(resultPathList.Count);
//Convert convert = new Convert();
//List<GridPos> realList = convert.Points(resultPathList);
//for (int i = 0; i < realList.Count - 1; i++)
//{
// System.Diagnostics.Debug.WriteLine("path.LineTo(" + realList[i].x + ", " + realList[i].y + ");");
//}
return(searchGrid);
}
