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