void AvaMoveTo( int iTargetX, int iTargetY, std::function<void(bool)> fCallback = nullptr )
{
printf( "--AvaMoveTo %d,%d\n", iTargetX, iTargetY );
g_uiMoveTimeout = GetTickCount( );
g_fAvaMoveCallback = fCallback;
unsigned int uiMoveTime = g_uiMoveSpeed * ( (FAST_ABS(iTargetX-g_iAvaPosX) + FAST_ABS(iTargetY-g_iAvaPosY) > 1) ? 14 : 10 ) / 10;
g_uiMoveDoneTime = GetTickCount() + uiMoveTime;
RoPakMoveTo xPak;
xPak.pos.Set( iTargetX, iTargetY, 0 );
BotNetwork::SendPacket( 0x00a7, &xPak, sizeof(xPak) );
}
double get_line_distance_km(const double lat1, const double lon1,
const double lat2, const double lon2)
{
#define FAST_ABS(v) ((v) >= 0 ? (v) : -1 * (v))
#define DISTANCE_PER_LATITUDE 111.111
double lat_value;
double lng_distance;
double lat_distance;
lat_value = FAST_ABS(lat1) < FAST_ABS(lat2) ? lat1 : lat2;
lat_distance = FAST_ABS(lat1 - lat2) * DISTANCE_PER_LATITUDE;
lng_distance = FAST_ABS(lon1 - lon2) * DISTANCE_PER_LATITUDE *
cos(lat_value * 3.1415926 / 180.0);
return sqrt(lat_distance * lat_distance + lng_distance * lng_distance);
}
vector<PathNode> AStarFindPath::findPath(int startX, int startY, int endX, int endY, int* outNodeNum)
{
vector<PathNode> resultPath;
m_iStartX = startX; m_iStartY = startY;
m_iEndX = endX; m_iEndY = endY;
if (!canStartFind())
return resultPath;
ANode* pStartNode = m_pMapNodes + m_iStartY*m_iCol+m_iStartX;
m_pOpendHeap->push(pStartNode);
bool isFound = false;
while (!isFound)
{
ANode* pCurrNode = m_pOpendHeap->pop();
if (pCurrNode == nullptr)
break;
// 1.放入closed
m_arrClosedList.push_back(pCurrNode);
pCurrNode->m_iIndex = EAStarIndexType::Index_InClosed;
int expandX = -1;
int expandY = -1;
// 2.handler 8方向周边点
for (int i=0; i<DIR_NUM; ++i)
{
expandX = pCurrNode->m_iPosX + ARR_DIR_OFFSET[i][0];
expandY = pCurrNode->m_iPosY + ARR_DIR_OFFSET[i][1];
if (!canMove(expandX, expandY))
continue;
ANode* pCurExpandNode = m_pMapNodes + expandY*m_iCol + expandX;
if (pCurExpandNode->m_iIndex == EAStarIndexType::Index_InClosed)
{
continue;
}
else
{
int expandNodeG = pCurrNode->m_iG + calBaseCost(ARR_DIR_OFFSET[i][0], ARR_DIR_OFFSET[i][1]) + calAdditionalCost(expandX, expandY);
if (pCurExpandNode->m_iIndex == EAStarIndexType::Index_NotUse)
{
int h = (FAST_ABS(expandX-m_iEndX)+FAST_ABS(expandY-m_iEndY))*AStarCostRightAngle;
pCurExpandNode->m_iG = expandNodeG;
pCurExpandNode->m_iH = h;
pCurExpandNode->m_iF = expandNodeG+h;
pCurExpandNode->m_pParent = pCurrNode;
m_pOpendHeap->push(pCurExpandNode);
if (h == 0)
{
isFound = true;
break;
}
}
else
{
// 邻居节点在open list中
assert(pCurExpandNode->m_iIndex >= 0);
// 邻居在open中，则需要看邻居点的G与当前点的G+当前点到邻居点的距离（10、14）的大小
// 如果从此点走更近（即G更小），则将此点的父节点换成当前点
// 注意：对于同一个点，G小，则F必须小，因为H是相同的
// 当父节点改变后，open表中的最小点可能发生变化，需要再次排序得到最小点
if (pCurExpandNode->m_iG >= expandNodeG)
{
pCurExpandNode->m_iG = expandNodeG;
pCurExpandNode->m_iF = pCurExpandNode->m_iG + pCurExpandNode->m_iH;
pCurExpandNode->m_pParent = pCurrNode;
m_pOpendHeap->sortUp(pCurExpandNode->m_iIndex);
}
}
}
}
}
ANode* pResult = nullptr;
// isFound
if (!isFound)
{
m_eAstarError = EAStarError::Error_NotFound;
}
else
{
int num = 0;
// 首先定位到终点
pResult = m_pMapNodes + m_iEndY*m_iCol + m_iEndX;
assert(pResult != nullptr && pResult->m_pParent != nullptr);
do
{
resultPath.push_back({pResult->m_iPosX, pResult->m_iPosY});
pResult = pResult->m_pParent;
++num;
}while (pResult != nullptr);
assert(num >= 2);
*outNodeNum = num;
}
return resultPath;
}
