summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorRĂ©mi Verschelde <rverschelde@gmail.com>2019-05-03 12:20:39 +0200
committerGitHub <noreply@github.com>2019-05-03 12:20:39 +0200
commit913620a9b804b10a880f6274df1d8ef44c27cb54 (patch)
treef950e186fb5861c42e804b9a0900141f2b9cadce
parent184bd3f6e51b674cb53fa30ddbcfd5034d3d0347 (diff)
parentf1b7b74d656c9a6ad2546233af1834b15cca8c0d (diff)
Merge pull request #28640 from bojidar-bg/17885-navmesh-entry-broken
Fix navmesh not finding optimal paths
-rw-r--r--scene/3d/navigation.cpp64
1 files changed, 28 insertions, 36 deletions
diff --git a/scene/3d/navigation.cpp b/scene/3d/navigation.cpp
index 5a3c8223ff..612d91c6e1 100644
--- a/scene/3d/navigation.cpp
+++ b/scene/3d/navigation.cpp
@@ -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;
- }
}
}
@@ -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
@@ -404,6 +379,12 @@ 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];
@@ -411,7 +392,17 @@ Vector<Vector3> Navigation::get_simple_path(const Vector3 &p_start, const Vector
if (!e.C)
continue;
+#ifdef USE_ENTRY_POINT
+ 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?
@@ -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);
}
@@ -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)