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);
}
// Use this for initialization
void Start()
{
m_hidden = new Color(1.0f, 1.0f, 1.0f, 0.0f); //invisible
m_basic = new Color(1.0f, 1.0f, 1.0f, 0.03f); //visible
m_start = new Color(Color.green.r, Color.green.g, Color.green.b, 1.0f);
m_end = new Color(Color.red.r, Color.red.g, Color.red.b, 1.0f);
m_open = new Color(Color.blue.r, Color.blue.g, Color.blue.b, 0.3f);
m_close = new Color(Color.cyan.r, Color.cyan.g, Color.cyan.b, 0.3f);
m_unwalkable = new Color(Color.black.r, Color.black.g, Color.black.b, 1.0f);
//// UI ////
//loading & mapping
m_cube = Resources.Load("Prefabs/Cube") as GameObject;
GameObject.Find("Canvas/btnClear").GetComponent <Button>().onClick.AddListener(OnbtnClearClicked);
GameObject.Find("Canvas/btnSearch").GetComponent <Button>().onClick.AddListener(OnbtnSearchClicked);
GameObject.Find("Canvas/btnLoadMap").GetComponent <Button>().onClick.AddListener(OnbtnLoadMapClicked);
GameObject.Find("Canvas/btnLoadStartEnd").GetComponent <Button>().onClick.AddListener(OnbtnLoadStartEndClicked);
m_ddDiagonalMode = GameObject.Find("Canvas/DropdownMode").GetComponent <Dropdown>();
m_tRecursive = GameObject.Find("Canvas/ToggleRecursive").GetComponent <Toggle>();
m_tOpenedCell = GameObject.Find("Canvas/ToggleOpenedCell").GetComponent <Toggle>();
m_tTotalMap = GameObject.Find("Canvas/ToggleTotalMap").GetComponent <Toggle>();
m_tOpenedCell.onValueChanged.AddListener((value) => { OntOpenedCellValue(value); });
m_tTotalMap.onValueChanged.AddListener((value) => { OntTotalMapValue(value); });
////cube setup////
m_cubeMap = new Dictionary <int, GameObject>();
int width = 10, length = 10, height = 10;
buildCubes(width, length, height);
////Testbeds////
//2 testbeds
//m_testbedList.Add(new Testbed()); //default
//m_testbedList.Add(new Testbed(new GridPos(1, 2, 3), new GridPos(9, 8, 7))); //custom
////path grid setup////
//convert all matrix walkable
bool[][][] movableMatrix = new bool [width][][];
for (int widthTrav = 0; widthTrav < width; widthTrav++)
{
movableMatrix[widthTrav] = new bool[length][];
for (int lengthTrav = 0; lengthTrav < length; lengthTrav++)
{
movableMatrix[widthTrav][lengthTrav] = new bool[height];
for (int heightTrav = 0; heightTrav < height; heightTrav++)
{
movableMatrix[widthTrav][lengthTrav][heightTrav] = true;
}
}
}
BaseGrid searchGrid = new StaticGrid(width, length, height, movableMatrix);
m_jumpParam = new JumpPointParam(searchGrid, new GridPos(), new GridPos(), EndNodeUnWalkableTreatment.ALLOW, DiagonalMovement.Always);
}
public IEnumerable <Vector2D> Find(Vector2D origin, Vector2D destination)
{
BaseGrid searchGrid = new StaticGrid(map.Width, map.Height, matrix);
var jp = new JumpPointParam(searchGrid, new GridPos(origin.X, origin.Y), new GridPos(destination.X, destination.Y), EndNodeUnWalkableTreatment.Allow);
return(JumpPointFinder.FindPath(jp).Select(x => new Vector2D(x.X, x.Y)));
}
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 static List <GridPos> GetPath(Point2D start, Point2D end)
{
for (int i = 0; i < MovableMatrix.Length; i++)
{
var row = MovableMatrix[i];
for (int index = 0; index < row.Length; index++)
{
MovableMatrix[i][index] = true;
}
}
List <CircularUnit> circularUnits = new List <CircularUnit>(Tick.World.Minions);
circularUnits.AddRange(Tick.World.Buildings);
circularUnits.AddRange(Tick.World.Wizards.Where(x => !x.IsMe));
circularUnits.AddRange(Tick.World.Trees);
circularUnits = circularUnits.Where(x => Tick.Self.GetDistanceTo(x) < Tick.Self.VisionRange).ToList();
foreach (var nearestMinion in circularUnits)
{
var p1 = (int)nearestMinion.X / gridStep;
var p2 = (int)nearestMinion.Y / gridStep;
FillMovableCircle(p1, p2, (int)(nearestMinion.Radius + Tick.Self.Radius * 1.2) / gridStep, gridSize, MovableMatrix, false);
}
var startGridPos = GetGridPosByPoint2d(start, gridStep);
var endGridPos = GetGridPosByPoint2d(end, gridStep);
MovableMatrix[startGridPos.x][startGridPos.y] = true;
FillMovableCircle(endGridPos.x, endGridPos.y, (int)(Tick.Self.Radius * 2) / gridStep, gridSize, MovableMatrix, true);
BaseGrid searchGrid = new StaticGrid(gridSize, gridSize, MovableMatrix);
JumpPointParam jpParam = new JumpPointParam(searchGrid, startGridPos, endGridPos);
var result = JumpPointFinder.FindPath(jpParam);
//result = JumpPointFinder.GetFullPath(result);
//var clmnIdx = 0;
//foreach (var row in MovableMatrix)
//{
// var rowIdx = 0;
// foreach (var b in row)
// {
// if (!b)
// {
// VisualClientHelper.Rect(new Point2D(clmnIdx * gridStep, rowIdx * gridStep),
// new Point2D((clmnIdx + 1) * gridStep, (rowIdx + 1) * gridStep), new VisualClientColor(0, 0, 1));
// }
// rowIdx++;
// }
// clmnIdx++;
//}
DrawPath(result, gridStep);
return(result);
}
public void FindPath_Trivial()
{
var grid = new StaticGrid(3, 3);
//grid.SetWalkableAll(true);
var param = new JumpPointParam(grid, new GridPos(0, 0), new GridPos(2, 2));
var path = JumpPointFinder.FindPath(param);
Assert.Contains(new GridPos(0, 0), path);
Assert.Contains(new GridPos(2, 2), path);
}
private void Load(string fileName)
{
if (!File.Exists(fileName))
{
Logger.Error("path file do not exit. {0}", fileName);
return;
}
FileStream fileStream = new FileStream(fileName, FileMode.Open, FileAccess.Read);
int nSeek = 0;
//先从文件头读取地图的长和宽
byte[] byteLen = new byte[Marshal.SizeOf(typeof(int))];
fileStream.Seek(nSeek, SeekOrigin.Begin);
fileStream.Read(byteLen, 0, byteLen.Length);
Height = System.BitConverter.ToInt32(byteLen, 0);
nSeek += byteLen.Length;
byte[] byteWid = new byte[Marshal.SizeOf(typeof(int))];
fileStream.Seek(nSeek, SeekOrigin.Begin);
fileStream.Read(byteWid, 0, byteWid.Length);
Width = System.BitConverter.ToInt32(byteWid, 0);
nSeek += byteWid.Length;
int nReadLen = Marshal.SizeOf(typeof(ObstacleItem));
byte[] read = new byte[nReadLen];
mSearchGrid = new StaticGrid(Width * 2 + 1, Height * 2 + 1);
byte[][] mat = new byte[Width * 2 + 1][];
for (int i = 0; i < Width * 2 + 1; i++)
{
mat[i] = new byte[Height * 2 + 1];
}
for (int i = 0; i < (Height * 2 + 1) * (Width * 2 + 1); ++i)
{
fileStream.Seek(nSeek, SeekOrigin.Begin);
fileStream.Read(read, 0, nReadLen);
nSeek += nReadLen;
ObstacleItem info = Byte2Struct(read);
mat[(int)(info.Fx * 2)][(int)(info.Fy * 2)] = (byte)info.Value;
}
mSearchGrid.Reset(mat);
mParam = new JumpPointParam(mSearchGrid, new GridPos(0, 0), new GridPos(0, 0), true, true, false,
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);
}
public StaticGrid(StaticGrid b)
: base(b) {
bool[][] tMatrix = new bool[b.width][];
for (int widthTrav = 0; widthTrav < b.width; widthTrav++) {
tMatrix[widthTrav] = new bool[b.height];
for (int heightTrav = 0; heightTrav < b.height; heightTrav++) {
if (b.IsWalkableAt(widthTrav, heightTrav))
tMatrix[widthTrav][heightTrav] = true;
else
tMatrix[widthTrav][heightTrav] = false;
}
}
this.m_nodes = buildNodes(b.width, b.height, tMatrix);
}
public StaticGrid(StaticGrid b)
: base(b)
{
var matrix = new bool[b.Width][];
for (var x = 0; x < b.Width; x++)
{
matrix[x] = new bool[b.Height];
for (var y = 0; y < b.Height; y++)
{
matrix[x][y] = b.IsWalkableAt(x, y);
}
}
Nodes = BuildNodes(b.Width, b.Height, matrix);
}
public static List <GridPos> GetPath(Point2D start, Point2D end)
{
BaseGrid searchGrid = new StaticGrid(gridSize, gridSize, MovableMatrix);
var startGridPos = GetGridPosByPoint2d(start, gridStep);
var endGridPos = GetGridPosByPoint2d(end, gridStep);
JumpPointParam jpParam = new JumpPointParam(searchGrid, startGridPos, endGridPos);
var result = JumpPointFinder.FindPath(jpParam);
result = JumpPointFinder.GetFullPath(result);
DrawPath(result, gridStep);
return(result);
}
public override BaseGrid Clone() {
int tWidth = width;
int tHeight = height;
Node[][] tNodes = this.m_nodes;
StaticGrid tNewGrid = new StaticGrid(tWidth, tHeight, null);
Node[][] tNewNodes = new Node[tWidth][];
for (int widthTrav = 0; widthTrav < tWidth; widthTrav++) {
tNewNodes[widthTrav] = new Node[tHeight];
for (int heightTrav = 0; heightTrav < tHeight; heightTrav++) {
tNewNodes[widthTrav][heightTrav] = new Node(widthTrav, heightTrav, tNodes[widthTrav][heightTrav].walkable);
}
}
tNewGrid.m_nodes = tNewNodes;
return tNewGrid;
}
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 override BaseGrid Clone()
{
var width = Width;
var height = Height;
var nodes = Nodes;
var newGrid = new StaticGrid(width, height);
var newNodes = new Node[width][];
for (var x = 0; x < width; x++)
{
newNodes[x] = new Node[height];
for (var y = 0; y < height; y++)
{
newNodes[x][y] = new Node(x, y, nodes[x][y].Walkable);
}
}
newGrid.Nodes = newNodes;
return(newGrid);
}
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 void Initialize()
{
var matrix = new bool[_map.Width, _map.Height];
var collisionBounds = _gameObjectManager.GameObjects.Where(x => x.CollisionEnabled).Select(z => z.CollisionBounds).ToList();
for (int y = 0; y < _map.Height; y++)
{
for (int x = 0; x < _map.Width; x++)
{
var tileCoordinate = new TileCoordinate(x, y);
if (_map.Tiles.TryGetValue(tileCoordinate, out var tile))
{
if (!tile.Traversable)
{
matrix[x, y] = false;
NotIncludedTiles.Add(new TileCoordinate(x, y));
continue;
}
}
var pos = tileCoordinate.ToActualVector2().ToPoint();
var rectangle = new Rectangle(pos.X, pos.Y, 32, 32);
if (collisionBounds.Any(rect => rect.Intersects(rectangle)))
{
matrix[x, y] = false;
NotIncludedTiles.Add(new TileCoordinate(x, y));
continue;
}
matrix[x, y] = true;
}
}
_staticGrid = new StaticGrid(_map.Width, _map.Height, matrix.ToJaggedArray());
}
public static bool Contains(this StaticGrid grid, Point point)
{
return(point.X < grid.width && point.Y < grid.height && point.X >= 0 && point.Y >= 0);
}
public void InitializeNavigationGrid()
{
bool[][] walkabilityMatrix = CreateWalkabilityMatrix();
_navigationGrid = new StaticGrid(_gridInfoProvider.XSize, _gridInfoProvider.YSize, walkabilityMatrix);
}
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 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);
}
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);
}
}
}
