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