Exemplo n.º 1
0
 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;
 }
Exemplo n.º 2
0
        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]);
                    }
                }
            }
        }
Exemplo n.º 3
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);
        }
Exemplo n.º 4
0
        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);
        }
Exemplo n.º 5
0
        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);
        }
Exemplo n.º 6
0
        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);
        }
Exemplo n.º 7
0
    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);
        }
    }
Exemplo n.º 8
0
    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);
        }
    }
Exemplo n.º 9
0
        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);
                }
            }
        }
Exemplo n.º 11
0
        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);
        }