gaming/godot
gaming/godot
1
0
Fork 0
mirror of https://github.com/godotengine/godot synced 2024-09-05 20:09:26 +00:00
Code Issues Packages Projects Releases Wiki Activity

Fix navmesh not finding optimal paths

Browse source 
Addresses part of #17885
This commit is contained in:
Bojidar Marinov 2019-05-03 10:50:26 +03:00
parent 46b6fb83ef
commit f1b7b74d65
No known key found for this signature in database
GPG key ID: 4B0FD31949AD430D
1 changed files with 28 additions and 36 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

64
scene/3d/navigation.cpp
View file

@ -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)