Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Fix navmesh not finding optimal paths #28640

Merged
merged 1 commit into from
May 3, 2019
Merged

Fix navmesh not finding optimal paths #28640

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
64 changes: 28 additions & 36 deletions scene/3d/navigation.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -340,16 +340,12 @@ Vector<Vector3> Navigation::get_simple_path(const Vector3 &p_start, const Vector
};

Vector3 entry = Geometry::get_closest_point_to_segment(begin_poly->entry, edge);
begin_poly->edges[i].C->distance = begin_poly->entry.distance_to(entry);
begin_poly->edges[i].C->distance = begin_point.distance_to(entry);
begin_poly->edges[i].C->entry = entry;
#else
begin_poly->edges[i].C->distance = begin_poly->center.distance_to(begin_poly->edges[i].C->center);
#endif
open_list.push_back(begin_poly->edges[i].C);

if (begin_poly->edges[i].C == end_poly) {
found_route = true;
}
}
}

Expand All @@ -370,28 +366,7 @@ Vector<Vector3> Navigation::get_simple_path(const Vector3 &p_start, const Vector

float cost = p->distance;
#ifdef USE_ENTRY_POINT
int es = p->edges.size();

float shortest_distance = 1e30;

for (int i = 0; i < es; i++) {
Polygon::Edge &e = p->edges.write[i];

if (!e.C)
continue;

Vector3 edge[2] = {
_get_vertex(p->edges[i].point),
_get_vertex(p->edges[(i + 1) % es].point)
};

Vector3 edge_point = Geometry::get_closest_point_to_segment(p->entry, edge);
float dist = p->entry.distance_to(edge_point);
if (dist < shortest_distance)
shortest_distance = dist;
}

cost += shortest_distance;
cost += p->entry.distance_to(end_point);
#else
cost += p->center.distance_to(end_point);
#endif
Expand All @@ -404,14 +379,30 @@ Vector<Vector3> Navigation::get_simple_path(const Vector3 &p_start, const Vector
Polygon *p = least_cost_poly->get();
//open the neighbours for search

if (p == end_poly) {
//oh my reached end! stop algorithm
found_route = true;
break;
}

for (int i = 0; i < p->edges.size(); i++) {

Polygon::Edge &e = p->edges.write[i];

if (!e.C)
continue;

#ifdef USE_ENTRY_POINT
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I wonder what's the point of this define since it can't be toggled conditionally. Sounds like we're maintaining dead code in the #else branches.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It was added in #22047, so I decided to not touch it yet.

Vector3 edge[2] = {
_get_vertex(p->edges[i].point),
_get_vertex(p->edges[(i + 1) % p->edges.size()].point)
};

Vector3 entry = Geometry::get_closest_point_to_segment(p->entry, edge);
float distance = p->entry.distance_to(entry) + p->distance;
#else
float distance = p->center.distance_to(e.C->center) + p->distance;
#endif

if (e.C->prev_edge != -1) {
//oh this was visited already, can we win the cost?
Expand All @@ -420,25 +411,22 @@ Vector<Vector3> Navigation::get_simple_path(const Vector3 &p_start, const Vector

e.C->prev_edge = e.C_edge;
e.C->distance = distance;
#ifdef USE_ENTRY_POINT
e.C->entry = entry;
#endif
}
} else {
//add to open neighbours

e.C->prev_edge = e.C_edge;
e.C->distance = distance;
#ifdef USE_ENTRY_POINT
e.C->entry = entry;
#endif
open_list.push_back(e.C);

if (e.C == end_poly) {
//oh my reached end! stop algorithm
found_route = true;
break;
}
}
}

if (found_route)
break;

open_list.erase(least_cost_poly);
}

Expand Down Expand Up @@ -539,8 +527,12 @@ Vector<Vector3> Navigation::get_simple_path(const Vector3 &p_start, const Vector
path.push_back(end_point);
while (true) {
int prev = p->prev_edge;
#ifdef USE_ENTRY_POINT
Vector3 point = p->entry;
#else
int prev_n = (p->prev_edge + 1) % p->edges.size();
Vector3 point = (_get_vertex(p->edges[prev].point) + _get_vertex(p->edges[prev_n].point)) * 0.5;
#endif
path.push_back(point);
p = p->edges[prev].C;
if (p == begin_poly)
Expand Down