std::vector<Vector2> NavMesh::FindStraightPath(const Vertices& vertices, const std::vector<NavTriangle *> &path) { std::vector<Vector2> straightPath; if (path.empty()) { return straightPath; } std::vector<Vector2> portalsLeft; std::vector<Vector2> portalsRight; portalsLeft.push_back(path[path.size()-1]->position); portalsRight.push_back(path[path.size()-1]->position); for (int i=(int)path.size()-1; i>=1; i--) { NavTriangle* current = path[i]; NavTriangle* next = path[i-1]; int forwardIndex = current->FindNeighborIndex(next); portalsRight.push_back(vertices[current->corners[forwardIndex]]); portalsLeft.push_back(forwardIndex<2 ? vertices[current->corners[forwardIndex + 1]] : vertices[current->corners[0]]); } portalsLeft.push_back(path[0]->position); portalsRight.push_back(path[0]->position); Vector2 portalApex = portalsLeft[0]; Vector2 portalLeft = portalsLeft[0]; Vector2 portalRight = portalsRight[0]; int apexIndex = 0, leftIndex = 0, rightIndex = 0; straightPath.push_back(portalApex); for (int i=1; i<portalsLeft.size(); i++) { Vector2 left = portalsLeft[i]; Vector2 right = portalsRight[i]; // Update right vertex. if (triangleArea(portalApex, portalRight, right) <= 0.0f) { if (portalApex.EqualEpsilon(portalRight) || triangleArea(portalApex, portalLeft, right) > 0.0f) { // Tighten the funnel. portalRight = right; rightIndex = i; } else { // Right over left, insert left to path and restart scan from portal left point. straightPath.push_back(portalLeft); // Make current left the new apex. portalApex = portalLeft; apexIndex = leftIndex; // Reset portal portalLeft = portalApex; portalRight = portalApex; leftIndex = apexIndex; rightIndex = apexIndex; // Restart scan i = apexIndex; continue; } } // Update left vertex. if (triangleArea(portalApex, portalLeft, left) >= 0.0f) { if (portalApex.EqualEpsilon(portalLeft) || triangleArea(portalApex, portalRight, left) < 0.0f) { portalLeft = left; // Tighten the funnel. leftIndex = i; } else { // Left over right, insert right to path and restart scan from portal right point. straightPath.push_back(portalRight); // Make current right the new apex. portalApex = portalRight; apexIndex = rightIndex; // Reset portal portalLeft = portalApex; portalRight = portalApex; leftIndex = apexIndex; rightIndex = apexIndex; // Restart scan i = apexIndex; continue; } } } straightPath.push_back(path[0]->position); /* const float radius = 1.0f; std::vector<Vector2> offsetedPath; offsetedPath.push_back(straightPath[0]); for (int i=1; i<straightPath.size()-1; i++) { Vector2 forward = straightPath[i] - straightPath[i-1]; Vector2 backwards = straightPath[i] - straightPath[i+1]; forward.Normalize(); backwards.Normalize(); Vector2 normal = forward + backwards; normal.Normalize(); offsetedPath.push_back(straightPath[i]+normal * radius); } offsetedPath.push_back(straightPath[straightPath.size() - 1]); */ return straightPath; }