Code:
dtNavMeshQuery* query = dtAllocNavMeshQuery();
dtStatus dtResult = query->init(navMesh, 1000000);
....
Vector3 extents = Vector3(5.f, 5.f, 5.f);
Vector3 closestPoint = Vector3(0.0f, 0.0f, 0.0f);
dtStatus dtResult = 0;
dtPolyRef startRef = 0;
dtResult = m_navMeshQuery->findNearestPoly((float*)&origin, (float*)&extents, &m_filter, &startRef, (float*)&closestPoint);
if (dtStatusFailed(dtResult) || startRef == INVALID_POLYREF)
{
printf("FAILED 1\n");
}
dtPolyRef endRef = 0;
dtResult = m_navMeshQuery->findNearestPoly((float*)&dest, (float*)&extents, &m_filter, &endRef, (float*)&closestPoint);
if (dtStatusFailed(dtResult) || endRef == INVALID_POLYREF)
{
printf("FAILED 2\n");
}
int prefixPolyLength = 0;
dtPolyRef* path = new dtPolyRef[20000];
dtResult = m_navMeshQuery->findPath(startRef, endRef, (float*)&origin, (float*)&dest, &m_filter, path, &prefixPolyLength, 20000);
if (dtStatusFailed(dtResult) || path == NULL)
{
printf("FAILED 3\n");
}
Vector3 pathPoints[20000];
BYTE pathFlags[20000];
dtPolyRef pathRef[20000];
unsigned int pointCount = 0;
dtResult = m_navMeshQuery->findStraightPath(
(float*)&origin,
(float*)&dest,
path,
prefixPolyLength,
(float*)pathPoints,
pathFlags,
pathRef,
(int*)&pointCount,
20000);
if (dtStatusFailed(dtResult) || !pointCount)
{
printf("FAILED 3\n");
}
vector<Vector3> __path;
for (int i = 0; i < pointCount; i++)
{
if (pathFlags[i] & DT_STRAIGHTPATH_OFFMESH_CONNECTION)
{
printf("FAILED 4\n");
}
else
{
static D3DXVECTOR3 last = D3DXVECTOR3(0,0,0);
D3DXVECTOR3 pos = D3DXVECTOR3(pathPoints[i].z, pathPoints[i].x, pathPoints[i].y);
D3DXVECTOR3 Target[2];
if (settings.m_Camera.CalcScreen(pos, Target[0]) && settings.m_Camera.CalcScreen(last, Target[1]))
{
CDraw::DrawLine(Target[0].x, Target[0].y, Target[1].x, Target[1].y, ImColor(255, 0, 255), 4);
}
last = D3DXVECTOR3(pathPoints[i].z, pathPoints[i].x, pathPoints[i].y);
__path.push_back(Vector3(pathPoints[i].x, pathPoints[i].y, pathPoints[i].x));
}
}