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