/// <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; } } } }
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; }
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; }