OpenSim.Modules.PathFinding/src/PathFinder.cs

using OpenMetaverse;
using OpenSim.Region.Framework.Scenes;
using System;
using System.Collections.Generic;
using System.Drawing;
using System.Drawing.Imaging;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace OpenSim.Modules.PathFinding
{
    class PathFinder
    {
        private List<NodeInfo> m_nodes = new List<NodeInfo>();
        private UUID m_requestID = UUID.Zero;

        private Scene m_scene = null;

        public PathFinder(UUID requestID, Scene localScene)
        {
            m_requestID = requestID;

            m_scene = localScene;
        }

        public void preparePathFindingScene(bool terrain)
        {
            generateEmptryNodeGrid(new Vector2((int)m_scene.RegionInfo.RegionSizeX, (int)m_scene.RegionInfo.RegionSizeY));
            convertRegionToNodeList(terrain);
            saveDebugImageToDisc();
        }

        public void generatePath(Vector2 start, Vector2 target)
        {


        }

        private void convertRegionToNodeList(bool terrain)
        {
            if (terrain == true)
            {
                for (int X = 0; X < m_scene.RegionInfo.RegionSizeX; X++)
                {
                    for (int Y = 0; Y < m_scene.RegionInfo.RegionSizeY; Y++)
                    {
                        float baseheight = (float)m_scene.Heightmap[X, Y];

                        //Block the position then water is on this place.
                        if (baseheight <= m_scene.RegionInfo.RegionSettings.WaterHeight)
                            setNoteBlocked(new Vector2(X, Y), true);

                        //Allow all positions they have terrain over the water height.
                        if (baseheight > m_scene.RegionInfo.RegionSettings.WaterHeight)
                            setNoteBlocked(new Vector2(X, Y), false);
                    }
                }
            }

            Bitmap _objectData = new Bitmap((int)m_scene.RegionInfo.RegionSizeX, (int)m_scene.RegionInfo.RegionSizeY);
            PathFindingSceneGenerator.DrawObjectVolume(m_scene, ref _objectData);
            convertBitmapToNodeList(ref _objectData);
        }

        private void setNoteBlocked(Vector2 position, bool state)
        {
            NodeInfo _infoNode = m_nodes.Find(X => X.Position.X.Equals(position.X) && X.Position.Y.Equals(position.Y));

            if(_infoNode != null)
            {
                _infoNode.Blocked = state;
            }
            else
            {
                Console.WriteLine("setNoteBlocked: Cant find Node at " + position.ToString());
            }
        }

        private bool getNoteBlocked(Vector2 position)
        {
            NodeInfo _infoNode = m_nodes.Find(X => X.Position.X.Equals(position.X) && X.Position.Y.Equals(position.Y));

            if (_infoNode != null)
            {
                return _infoNode.Blocked;
            }
            else
            {
                Console.WriteLine("getNoteBlocked: Cant find Node at " + position.ToString());
            }

            return false;
        }

        private void generateEmptryNodeGrid(Vector2 size)
        {
            for (int X = 0; X <= size.X; X++)
            {
                for (int Y = 0; Y <= size.Y; Y++)
                {
                    m_nodes.Add(new NodeInfo(new Vector2(X, Y)));
                }
            }
        }

        private void convertBitmapToNodeList(ref Bitmap bitmap)
        {
            for (int X = 0; X < bitmap.Width; X++)
            {
                for (int Y = 0; Y < bitmap.Height; Y++)
                {
                    Color _pixelColor = bitmap.GetPixel(X, Y);

                    Console.WriteLine("convertBitmapToNodeList: X:"+X+" Y:"+Y+" COLOR:" + _pixelColor.R +";" + _pixelColor.G + ";" + _pixelColor.B);

                    bool blocking = true;

                    if (_pixelColor.R == 255)
                        blocking = false;

                    setNoteBlocked(new Vector2(X, Y), false);
                }
            }
        }

        private Bitmap convertNodeListToBitmap()
        {
            Bitmap _bitmap = new Bitmap((int)m_scene.RegionInfo.RegionSizeX, (int)m_scene.RegionInfo.RegionSizeY);

            for (int X = 0; X < _bitmap.Width; X++)
            {
                for (int Y = 0; Y < _bitmap.Height; Y++)
                {
                    bool _nodeState = getNoteBlocked(new Vector2(X, Y));

                    if(_nodeState == true)
                    {
                        _bitmap.SetPixel(X, Y, Color.Black);
                    }
                    else
                    {
                        _bitmap.SetPixel(X, Y, Color.White);
                    }
                }
            }

            return _bitmap;
        }

        private void saveDebugImageToDisc()
        {
            Bitmap _bitmap = convertNodeListToBitmap();
            _bitmap.Save(m_requestID + ".png", ImageFormat.Png);
        }
    }
}