diff options
| -rw-r--r-- | scene/2d/navigation2d.cpp | 28 | ||||
| -rw-r--r-- | scene/3d/navigation.cpp | 40 | ||||
| -rw-r--r-- | scene/3d/navigation.h | 1 |
3 files changed, 65 insertions, 4 deletions
diff --git a/scene/2d/navigation2d.cpp b/scene/2d/navigation2d.cpp index 9eec8e6cc3..1b789bab9d 100644 --- a/scene/2d/navigation2d.cpp +++ b/scene/2d/navigation2d.cpp @@ -388,10 +388,34 @@ Vector<Vector2> Navigation2D::get_simple_path(const Vector2 &p_start, const Vect Polygon *p = E->get(); float cost = p->distance; - cost += p->center.distance_to(end_point); - if (cost < least_cost) { +#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; } diff --git a/scene/3d/navigation.cpp b/scene/3d/navigation.cpp index 8d84d2408c..54f74c2df3 100644 --- a/scene/3d/navigation.cpp +++ b/scene/3d/navigation.cpp @@ -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; - cost += p->center.distance_to(end_point); +#ifdef USE_ENTRY_POINT + int es = p->edges.size(); - if (cost < least_cost) { + 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; } diff --git a/scene/3d/navigation.h b/scene/3d/navigation.h index 5a501039c8..8f200997cd 100644 --- a/scene/3d/navigation.h +++ b/scene/3d/navigation.h @@ -94,6 +94,7 @@ class Navigation : public Spatial { Vector<Edge> edges; Vector3 center; + Vector3 entry; float distance; int prev_edge; |