Beispiel #1
0
        /// <summary>
        /// コンストラクタ
        /// </summary>
        /// <param name="areaMap"></param>
        public ActiveDetour(LocationPresumption.GridMap areaMap, double[] lrfData)
        {
            // GridMapの1/5 [100mm間隔から、500mmに変換]
            hexMap = new HexPixelMap(areaMap.W / dotScale, areaMap.H / dotScale);

            // GridMap[100mm間隔]からhexMap[500mm間隔]に変換
            for (int iH = 0; iH < areaMap.H / dotScale; iH++)
            {
                for (int iW = 0; iW < areaMap.W / dotScale; iW++)
                {
                    bool bFree = true;

                    // 500mm間隔の中に1つでも通行不可ならば、通行できない
                    for (int nH = 0; nH < dotScale; nH++)
                    {
                        for (int nW = 0; nW < dotScale; nW++)
                        {
                            if (areaMap[iW * dotScale + nW, iH *dotScale + nH] != LocationPresumption.Grid.Free)
                            {
                                bFree = false;
                            }
                        }
                    }

                    if (!bFree)
                    {
                        // 障害物あり
                        hexMap[iW, iH].powVal = 2.0;
                    }
                }
            }

            // LRFから現在の障害物をマップに落とし込む
            {
                double lrfScale = URG_LRF.getScale();   // スケール変換値
                double rPI      = Math.PI / 180.0;
                double mmToPix  = 1.0 / 500.0;

                // LRFの値を描画
                for (int i = 0; i < lrfData.Length; i++)
                {
                    double val = lrfData[i] * lrfScale;
                    double rad = (-i + (270.0 / 2.0) - 90) * rPI;

                    // LRFは右下から左回り
                    int iX = (int)(val * Math.Cos(rad) * mmToPix);
                    int iY = (int)(val * Math.Sin(rad) * mmToPix);

                    // 障害物あり
                    if (iX >= 0 && iX < hexMap.W &&
                        iY >= 0 && iY < hexMap.H)
                    {
                        hexMap[iX, iY].powVal = 2.0;
                    }
                }
            }
        }
        /// <summary>
        /// コンストラクタ
        /// </summary>
        /// <param name="areaMap"></param>
        public ActiveDetour(LocationPresumption.GridMap areaMap, double[] lrfData )
        {
            // GridMapの1/5 [100mm間隔から、500mmに変換]
            hexMap = new HexPixelMap(areaMap.W / dotScale, areaMap.H / dotScale);

            // GridMap[100mm間隔]からhexMap[500mm間隔]に変換
            for (int iH = 0; iH < areaMap.H / dotScale; iH++)
            {
                for (int iW = 0; iW < areaMap.W / dotScale; iW++)
                {
                    bool bFree = true;

                    // 500mm間隔の中に1つでも通行不可ならば、通行できない
                    for (int nH = 0; nH < dotScale; nH++)
                    {
                        for (int nW = 0; nW < dotScale; nW++)
                        {
                            if (areaMap[iW * dotScale + nW, iH * dotScale + nH] != LocationPresumption.Grid.Free)
                            {
                                bFree = false;
                            }
                        }
                    }

                    if (!bFree)
                    {
                        // 障害物あり
                        hexMap[iW, iH].powVal = 2.0;
                    }
                }
            }

            // LRFから現在の障害物をマップに落とし込む
            {
                double lrfScale = URG_LRF.getScale();   // スケール変換値
                double rPI = Math.PI / 180.0;
                double mmToPix = 1.0 / 500.0;

                // LRFの値を描画
                for (int i = 0; i < lrfData.Length; i++)
                {
                    double val = lrfData[i] * lrfScale;
                    double rad = (-i + (270.0/2.0) - 90) * rPI;

                    // LRFは右下から左回り
                    int iX = (int)(val * Math.Cos(rad) * mmToPix);
                    int iY = (int)(val * Math.Sin(rad) * mmToPix);

                    // 障害物あり
                    if (iX >= 0 && iX < hexMap.W &&
                        iY >= 0 && iY < hexMap.H)
                    {
                        hexMap[iX, iY].powVal = 2.0;
                    }
                }
            }
        }
Beispiel #3
0
            public ANodeMgr(HexPixelMap layer, int xgoal, int ygoal, bool allowdiag = true)
            {
                _layer     = layer;
                _allowdiag = allowdiag;

                _openList = new List <ANode>();
                _pool     = new Dictionary <int, ANode>();

                _xgoal = xgoal;
                _ygoal = ygoal;
            }
Beispiel #4
0
            public ANodeMgr(HexPixelMap layer, int xgoal, int ygoal, bool allowdiag = true)
            {
                _layer = layer;
                _allowdiag = allowdiag;

                _openList = new List<ANode>();
                _pool = new Dictionary<int, ANode>();

                _xgoal = xgoal;
                _ygoal = ygoal;
            }