double findTwoPointsPath(const vector<DjiEdge >& allAroundObsEdgesPointer, DjiPoint ptStart, DjiPoint ptEnd,
vector<DjiPoint>& path, bool isKeepStartEndPoint, bool isClearPath)
{
if (isSamePoint(ptStart, ptEnd))
{
printPoint("ptStart", ptStart);
printPoint("ptEnd", ptEnd);
printLog("ptStart is same ptEnd, in findTwoPointsPath");
return 0;
}
vector<vector<DjiPoint> > pathPtsSeq;
//求与障碍物边缘交点
for (int j = 0; j < (int)allAroundObsEdgesPointer.size(); j++)
{
addPathWihtAroundEdge(allAroundObsEdgesPointer[j], ptStart, ptEnd, pathPtsSeq);
}
//对pathPts 进行排序
sortPathsWithForwardVec(pathPtsSeq, ptEnd - ptStart);
vector<DjiPoint> pathWayPts;
pathWayPts.push_back(ptStart);//起点
for (int i = 0; i < (int)pathPtsSeq.size(); i++)
{
for (int j = 0; j < (int)pathPtsSeq[i].size(); j++)
{
pathWayPts.push_back(DjiPoint(pathPtsSeq[i][j].x, pathPtsSeq[i][j].y));
}
}
pathWayPts.push_back(ptEnd);//终点
if (pathWayPts.size() < 2)
printLog("pathWayPts.size() < 2, in findPathFromTwoPts", true);
double costDist= calPolylineDist(pathWayPts);
if (!isKeepStartEndPoint)
{
pathWayPts.pop_back();
pathWayPts.erase(pathWayPts.begin());
}
if (isClearPath)
{
path.clear();
}
if (pathWayPts.size() > 0)
{
path.insert(path.end(), pathWayPts.begin(), pathWayPts.end());
}
return costDist;
}
void PathInfo::updateNextPosition()
{
float x, y, z;
getStartPosition(x, y, z);
float startPos[3] = {y, z, x};
getEndPosition(x, y, z);
float endPos[3] = {y, z, x};
float* pathPoints = new float[MAX_PATH_LENGTH*3];
int pointCount = m_navMesh->findStraightPath(
startPos, // start position
endPos, // end position
m_pathPolyRefs, // current path
m_length, // lenth of current path
pathPoints, // [out] path corner points
0, // [out] flags
0, // [out] shortened path PATHFIND TODO: see if this is usable (IE, doesn't leave gaps in path)
MAX_PATH_LENGTH); // maximum number of points/polygons to use
// TODO: imitate PATHFIND_ITER code from RecastDemo so that terrain following is smoother
if(pointCount == 0)
{
delete [] pathPoints;
// only happens if pass bad data to findStraightPath or navmesh is broken
sLog.outDebug("%u's UpdateNextPosition failed: 0 length path", m_sourceObject->GetGUID());
shortcut();
return;
}
int ix, iy, iz;
if(pointCount == 1)
{
ix = 2; iy = 0; iz = 1;
}
else
{
ix = 5; iy = 3; iz = 4;
}
setNextPosition(pathPoints[ix], pathPoints[iy], pathPoints[iz]);
// if startPosition == nextPosition, NPC will never advance to destination
if(isSamePoint(m_startPosition, m_nextPosition) && !isSamePoint(m_nextPosition, m_endPosition))
setNextPosition(pathPoints[ix + 3], pathPoints[iy + 3], pathPoints[iz + 3]);
delete [] m_pathPoints;
m_pathPoints = pathPoints;
m_type = PathType(m_type | PATHFIND_NORMAL);
// if end position from findStraightPath isn't the
// place we want to go, there is likely no path
endPos[0] = pathPoints[(pointCount-1)*3+2];
endPos[1] = pathPoints[(pointCount-1)*3];
endPos[2] = pathPoints[(pointCount-1)*3+1];
if(!dtVequal(endPos, m_endPosition))
m_type = PathType(m_type | PATHFIND_NOPATH);
}
