godotengine
/
godot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #22047 from DualMatrix/better_nav_2D 

Better heuristic for the shortest path algorithm for navigation2D
This commit is contained in:
Rémi Verschelde 2018-09-20 22:31:22 +02:00 committed by GitHub
parent 6d46e553f9 0b5c694b74
commit 34f8441434
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 65 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
scene/2d/navigation2d.cpp
View File

@ -388,10 +388,34 @@ Vector<Vector2> Navigation2D::get_simple_path(const Vector2 &p_start, const Vect
Polygon *p = E->get();
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;
Vector2 edge[2] = {
_get_vertex(p->edges[i].point),
_get_vertex(p->edges[(i + 1) % es].point)
};
Vector2 edge_point = Geometry::get_closest_point_to_segment_2d(p->entry, edge);
float dist = p->entry.distance_to(edge_point);
if (dist < shortest_distance)
shortest_distance = dist;
}
cost += shortest_distance;
#else
cost += p->center.distance_to(end_point);
#endif
if (cost < least_cost) {
least_cost_poly = E;
least_cost = cost;
}

40
scene/3d/navigation.cpp
View File

@ -30,6 +30,8 @@
#include "navigation.h"
#define USE_ENTRY_POINT
void Navigation::_navmesh_link(int p_id) {
ERR_FAIL_COND(!navmesh_map.has(p_id));
@ -331,7 +333,18 @@ Vector<Vector3> Navigation::get_simple_path(const Vector3 &p_start, const Vector
if (begin_poly->edges[i].C) {
begin_poly->edges[i].C->prev_edge = begin_poly->edges[i].C_edge;
#ifdef USE_ENTRY_POINT
Vector3 edge[2] = {
_get_vertex(begin_poly->edges[i].point),
_get_vertex(begin_poly->edges[(i + 1) % begin_poly->edges.size()].point)
};
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->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) {
@ -356,10 +369,33 @@ Vector<Vector3> Navigation::get_simple_path(const Vector3 &p_start, const Vector
Polygon *p = E->get();
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;
#else
cost += p->center.distance_to(end_point);
#endif
if (cost < least_cost) {
least_cost_poly = E;
least_cost = cost;
}

1
scene/3d/navigation.h
View File

@ -94,6 +94,7 @@ class Navigation : public Spatial {
Vector<Edge> edges;
Vector3 center;
Vector3 entry;
float distance;
int prev_edge;