asgardius/virtualx-engine
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Better heuristic for the shortest path algorithm for navigation2D and navigation.

Browse source 
Better heuristic for the shortest path algorithm for navigation2D and navigation.
It now will use the shortest distance to the polygon as cost instead of the distance to the center.
This commit is contained in:
DualMatrix 2018-09-13 21:11:33 +02:00
parent 8704b77876
commit 0b5c694b74
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;