summaryrefslogtreecommitdiff
path: root/scene/2d
diff options
context:
space:
mode:
Diffstat (limited to 'scene/2d')
-rw-r--r--scene/2d/navigation_2d.cpp714
-rw-r--r--scene/2d/navigation_2d.h137
-rw-r--r--scene/2d/navigation_agent_2d.cpp341
-rw-r--r--scene/2d/navigation_agent_2d.h150
-rw-r--r--scene/2d/navigation_obstacle_2d.cpp154
-rw-r--r--scene/2d/navigation_obstacle_2d.h71
-rw-r--r--scene/2d/navigation_polygon.cpp90
-rw-r--r--scene/2d/navigation_polygon.h12
-rw-r--r--scene/2d/tile_map.cpp16
-rw-r--r--scene/2d/tile_map.h2
10 files changed, 843 insertions, 844 deletions
diff --git a/scene/2d/navigation_2d.cpp b/scene/2d/navigation_2d.cpp
index 9209cc5074..de01d97ad9 100644
--- a/scene/2d/navigation_2d.cpp
+++ b/scene/2d/navigation_2d.cpp
@@ -29,710 +29,52 @@
/*************************************************************************/
#include "navigation_2d.h"
+#include "servers/navigation_2d_server.h"
-#define USE_ENTRY_POINT
-
-void Navigation2D::_navpoly_link(int p_id) {
-
- ERR_FAIL_COND(!navpoly_map.has(p_id));
- NavMesh &nm = navpoly_map[p_id];
- ERR_FAIL_COND(nm.linked);
-
- PoolVector<Vector2> vertices = nm.navpoly->get_vertices();
- int len = vertices.size();
- if (len == 0)
- return;
-
- PoolVector<Vector2>::Read r = vertices.read();
-
- for (int i = 0; i < nm.navpoly->get_polygon_count(); i++) {
-
- //build
-
- List<Polygon>::Element *P = nm.polygons.push_back(Polygon());
- Polygon &p = P->get();
- p.owner = &nm;
-
- Vector<int> poly = nm.navpoly->get_polygon(i);
- int plen = poly.size();
- const int *indices = poly.ptr();
- bool valid = true;
- p.edges.resize(plen);
-
- Vector2 center;
- float sum = 0;
-
- for (int j = 0; j < plen; j++) {
-
- int idx = indices[j];
- if (idx < 0 || idx >= len) {
- valid = false;
- break;
- }
-
- Polygon::Edge e;
- Vector2 ep = nm.xform.xform(r[idx]);
- center += ep;
- e.point = _get_point(ep);
- p.edges.write[j] = e;
-
- int idxn = indices[(j + 1) % plen];
- if (idxn < 0 || idxn >= len) {
- valid = false;
- break;
- }
-
- Vector2 epn = nm.xform.xform(r[idxn]);
-
- sum += (epn.x - ep.x) * (epn.y + ep.y);
- }
-
- p.clockwise = sum > 0;
-
- if (!valid) {
- nm.polygons.pop_back();
- ERR_CONTINUE(!valid);
- }
-
- p.center = center / plen;
-
- //connect
-
- for (int j = 0; j < plen; j++) {
-
- int next = (j + 1) % plen;
- EdgeKey ek(p.edges[j].point, p.edges[next].point);
-
- Map<EdgeKey, Connection>::Element *C = connections.find(ek);
- if (!C) {
+void Navigation2D::_bind_methods() {
+ ClassDB::bind_method(D_METHOD("get_rid"), &Navigation2D::get_rid);
- Connection c;
- c.A = &p;
- c.A_edge = j;
- c.B = NULL;
- c.B_edge = -1;
- connections[ek] = c;
- } else {
+ ClassDB::bind_method(D_METHOD("get_simple_path", "start", "end", "optimize"), &Navigation2D::get_simple_path, DEFVAL(true));
- if (C->get().B != NULL) {
- ConnectionPending pending;
- pending.polygon = &p;
- pending.edge = j;
- p.edges.write[j].P = C->get().pending.push_back(pending);
- continue;
- }
+ ClassDB::bind_method(D_METHOD("set_cell_size", "cell_size"), &Navigation2D::set_cell_size);
+ ClassDB::bind_method(D_METHOD("get_cell_size"), &Navigation2D::get_cell_size);
- C->get().B = &p;
- C->get().B_edge = j;
- C->get().A->edges.write[C->get().A_edge].C = &p;
- C->get().A->edges.write[C->get().A_edge].C_edge = j;
- p.edges.write[j].C = C->get().A;
- p.edges.write[j].C_edge = C->get().A_edge;
- //connection successful.
- }
- }
- }
+ ClassDB::bind_method(D_METHOD("set_edge_connection_margin", "margin"), &Navigation2D::set_edge_connection_margin);
+ ClassDB::bind_method(D_METHOD("get_edge_connection_margin"), &Navigation2D::get_edge_connection_margin);
- nm.linked = true;
+ ADD_PROPERTY(PropertyInfo(Variant::REAL, "cell_size"), "set_cell_size", "get_cell_size");
+ ADD_PROPERTY(PropertyInfo(Variant::REAL, "edge_connection_margin"), "set_edge_connection_margin", "get_edge_connection_margin");
}
-void Navigation2D::_navpoly_unlink(int p_id) {
-
- ERR_FAIL_COND(!navpoly_map.has(p_id));
- NavMesh &nm = navpoly_map[p_id];
- ERR_FAIL_COND(!nm.linked);
-
- for (List<Polygon>::Element *E = nm.polygons.front(); E; E = E->next()) {
-
- Polygon &p = E->get();
-
- int ec = p.edges.size();
- Polygon::Edge *edges = p.edges.ptrw();
-
- for (int i = 0; i < ec; i++) {
- int next = (i + 1) % ec;
-
- EdgeKey ek(edges[i].point, edges[next].point);
- Map<EdgeKey, Connection>::Element *C = connections.find(ek);
- ERR_CONTINUE(!C);
-
- if (edges[i].P) {
- C->get().pending.erase(edges[i].P);
- edges[i].P = NULL;
-
- } else if (C->get().B) {
- //disconnect
+void Navigation2D::_notification(int p_what) {
+ switch (p_what) {
+ case NOTIFICATION_READY: {
+ Navigation2DServer::get_singleton()->map_set_active(map, true);
+ } break;
+ case NOTIFICATION_EXIT_TREE: {
- C->get().B->edges.write[C->get().B_edge].C = NULL;
- C->get().B->edges.write[C->get().B_edge].C_edge = -1;
- C->get().A->edges.write[C->get().A_edge].C = NULL;
- C->get().A->edges.write[C->get().A_edge].C_edge = -1;
-
- if (C->get().A == &E->get()) {
-
- C->get().A = C->get().B;
- C->get().A_edge = C->get().B_edge;
- }
- C->get().B = NULL;
- C->get().B_edge = -1;
-
- if (C->get().pending.size()) {
- //reconnect if something is pending
- ConnectionPending cp = C->get().pending.front()->get();
- C->get().pending.pop_front();
-
- C->get().B = cp.polygon;
- C->get().B_edge = cp.edge;
- C->get().A->edges.write[C->get().A_edge].C = cp.polygon;
- C->get().A->edges.write[C->get().A_edge].C_edge = cp.edge;
- cp.polygon->edges.write[cp.edge].C = C->get().A;
- cp.polygon->edges.write[cp.edge].C_edge = C->get().A_edge;
- cp.polygon->edges.write[cp.edge].P = NULL;
- }
-
- } else {
- connections.erase(C);
- //erase
- }
- }
+ Navigation2DServer::get_singleton()->map_set_active(map, false);
+ } break;
}
-
- nm.polygons.clear();
-
- nm.linked = false;
-}
-
-int Navigation2D::navpoly_add(const Ref<NavigationPolygon> &p_mesh, const Transform2D &p_xform, Object *p_owner) {
-
- int id = last_id++;
- NavMesh nm;
- nm.linked = false;
- nm.navpoly = p_mesh;
- nm.xform = p_xform;
- nm.owner = p_owner;
- navpoly_map[id] = nm;
-
- _navpoly_link(id);
-
- return id;
}
-void Navigation2D::navpoly_set_transform(int p_id, const Transform2D &p_xform) {
-
- ERR_FAIL_COND(!navpoly_map.has(p_id));
- NavMesh &nm = navpoly_map[p_id];
- if (nm.xform == p_xform)
- return; //bleh
- _navpoly_unlink(p_id);
- nm.xform = p_xform;
- _navpoly_link(p_id);
+void Navigation2D::set_cell_size(float p_cell_size) {
+ cell_size = p_cell_size;
+ Navigation2DServer::get_singleton()->map_set_cell_size(map, cell_size);
}
-void Navigation2D::navpoly_remove(int p_id) {
- ERR_FAIL_COND(!navpoly_map.has(p_id));
- _navpoly_unlink(p_id);
- navpoly_map.erase(p_id);
+void Navigation2D::set_edge_connection_margin(float p_edge_connection_margin) {
+ edge_connection_margin = p_edge_connection_margin;
+ Navigation2DServer::get_singleton()->map_set_edge_connection_margin(map, edge_connection_margin);
}
Vector<Vector2> Navigation2D::get_simple_path(const Vector2 &p_start, const Vector2 &p_end, bool p_optimize) {
-
- Polygon *begin_poly = NULL;
- Polygon *end_poly = NULL;
- Vector2 begin_point;
- Vector2 end_point;
- float begin_d = 1e20;
- float end_d = 1e20;
-
- //look for point inside triangle
-
- for (Map<int, NavMesh>::Element *E = navpoly_map.front(); E; E = E->next()) {
-
- if (!E->get().linked)
- continue;
- for (List<Polygon>::Element *F = E->get().polygons.front(); F; F = F->next()) {
-
- Polygon &p = F->get();
- if (begin_d || end_d) {
- for (int i = 2; i < p.edges.size(); i++) {
-
- if (begin_d > 0) {
-
- if (Geometry::is_point_in_triangle(p_start, _get_vertex(p.edges[0].point), _get_vertex(p.edges[i - 1].point), _get_vertex(p.edges[i].point))) {
-
- begin_poly = &p;
- begin_point = p_start;
- begin_d = 0;
- if (end_d == 0)
- break;
- }
- }
-
- if (end_d > 0) {
-
- if (Geometry::is_point_in_triangle(p_end, _get_vertex(p.edges[0].point), _get_vertex(p.edges[i - 1].point), _get_vertex(p.edges[i].point))) {
-
- end_poly = &p;
- end_point = p_end;
- end_d = 0;
- if (begin_d == 0)
- break;
- }
- }
- }
- }
-
- p.prev_edge = -1;
- }
- }
-
- //start or end not inside triangle.. look for closest segment :|
- if (begin_d || end_d) {
- for (Map<int, NavMesh>::Element *E = navpoly_map.front(); E; E = E->next()) {
-
- if (!E->get().linked)
- continue;
- for (List<Polygon>::Element *F = E->get().polygons.front(); F; F = F->next()) {
-
- Polygon &p = F->get();
- int es = p.edges.size();
- for (int i = 0; i < es; i++) {
-
- Vector2 edge[2] = {
- _get_vertex(p.edges[i].point),
- _get_vertex(p.edges[(i + 1) % es].point)
- };
-
- if (begin_d > 0) {
- Vector2 spoint = Geometry::get_closest_point_to_segment_2d(p_start, edge);
- float d = spoint.distance_to(p_start);
- if (d < begin_d) {
- begin_poly = &p;
- begin_point = spoint;
- begin_d = d;
- }
- }
-
- if (end_d > 0) {
- Vector2 spoint = Geometry::get_closest_point_to_segment_2d(p_end, edge);
- float d = spoint.distance_to(p_end);
- if (d < end_d) {
- end_poly = &p;
- end_point = spoint;
- end_d = d;
- }
- }
- }
- }
- }
- }
-
- if (!begin_poly || !end_poly) {
-
- return Vector<Vector2>(); //no path
- }
-
- if (begin_poly == end_poly) {
-
- Vector<Vector2> path;
- path.resize(2);
- path.write[0] = begin_point;
- path.write[1] = end_point;
- return path;
- }
-
- bool found_route = false;
-
- List<Polygon *> open_list;
-
- begin_poly->entry = p_start;
-
- for (int i = 0; i < begin_poly->edges.size(); i++) {
-
- if (begin_poly->edges[i].C) {
-
- begin_poly->edges[i].C->prev_edge = begin_poly->edges[i].C_edge;
-#ifdef USE_ENTRY_POINT
- Vector2 edge[2] = {
- _get_vertex(begin_poly->edges[i].point),
- _get_vertex(begin_poly->edges[(i + 1) % begin_poly->edges.size()].point)
- };
-
- Vector2 entry = Geometry::get_closest_point_to_segment_2d(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) {
- found_route = true;
- }
- }
- }
-
- while (!found_route) {
-
- if (open_list.size() == 0) {
- break;
- }
- //check open list
-
- List<Polygon *>::Element *least_cost_poly = NULL;
- float least_cost = 1e30;
-
- //this could be faster (cache previous results)
- for (List<Polygon *>::Element *E = open_list.front(); E; E = E->next()) {
-
- 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;
- }
- }
-
- Polygon *p = least_cost_poly->get();
- //open the neighbours for search
- int es = p->edges.size();
-
- for (int i = 0; i < es; i++) {
-
- Polygon::Edge &e = p->edges.write[i];
-
- if (!e.C)
- continue;
-
-#ifdef USE_ENTRY_POINT
- Vector2 edge[2] = {
- _get_vertex(p->edges[i].point),
- _get_vertex(p->edges[(i + 1) % es].point)
- };
-
- Vector2 edge_entry = Geometry::get_closest_point_to_segment_2d(p->entry, edge);
- float distance = p->entry.distance_to(edge_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?
-
- if (e.C->distance > distance) {
-
- e.C->prev_edge = e.C_edge;
- e.C->distance = distance;
-#ifdef USE_ENTRY_POINT
- e.C->entry = edge_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 = edge_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);
- }
-
- if (found_route) {
-
- Vector<Vector2> path;
-
- if (p_optimize) {
- //string pulling
-
- Vector2 apex_point = end_point;
- Vector2 portal_left = apex_point;
- Vector2 portal_right = apex_point;
- Polygon *left_poly = end_poly;
- Polygon *right_poly = end_poly;
- Polygon *p = end_poly;
-
- while (p) {
-
- Vector2 left;
- Vector2 right;
-
-//#define CLOCK_TANGENT(m_a,m_b,m_c) ( ((m_a)-(m_c)).cross((m_a)-(m_b)) )
-#define CLOCK_TANGENT(m_a, m_b, m_c) ((((m_a).x - (m_c).x) * ((m_b).y - (m_c).y) - ((m_b).x - (m_c).x) * ((m_a).y - (m_c).y)))
-
- if (p == begin_poly) {
- left = begin_point;
- right = begin_point;
- } else {
- int prev = p->prev_edge;
- int prev_n = (p->prev_edge + 1) % p->edges.size();
- left = _get_vertex(p->edges[prev].point);
- right = _get_vertex(p->edges[prev_n].point);
-
- if (p->clockwise) {
- SWAP(left, right);
- }
- /*if (CLOCK_TANGENT(apex_point,left,(left+right)*0.5) < 0){
- SWAP(left,right);
- }*/
- }
-
- bool skip = false;
-
- /*
- print_line("-----\nAPEX: "+(apex_point-end_point));
- print_line("LEFT:");
- print_line("\tPortal: "+(portal_left-end_point));
- print_line("\tPoint: "+(left-end_point));
- print_line("\tLeft Tangent: "+rtos(CLOCK_TANGENT(apex_point,portal_left,left)));
- print_line("\tLeft Distance: "+rtos(portal_left.distance_squared_to(apex_point)));
- print_line("\tLeft Test: "+rtos(CLOCK_TANGENT(apex_point,left,portal_right)));
- print_line("RIGHT:");
- print_line("\tPortal: "+(portal_right-end_point));
- print_line("\tPoint: "+(right-end_point));
- print_line("\tRight Tangent: "+rtos(CLOCK_TANGENT(apex_point,portal_right,right)));
- print_line("\tRight Distance: "+rtos(portal_right.distance_squared_to(apex_point)));
- print_line("\tRight Test: "+rtos(CLOCK_TANGENT(apex_point,right,portal_left)));
- */
-
- if (CLOCK_TANGENT(apex_point, portal_left, left) >= 0) {
- //process
- if (portal_left.is_equal_approx(apex_point) || CLOCK_TANGENT(apex_point, left, portal_right) > 0) {
- left_poly = p;
- portal_left = left;
- } else {
-
- apex_point = portal_right;
- p = right_poly;
- left_poly = p;
- portal_left = apex_point;
- portal_right = apex_point;
- if (!path.size() || !path[path.size() - 1].is_equal_approx(apex_point))
- path.push_back(apex_point);
- skip = true;
- }
- }
-
- if (!skip && CLOCK_TANGENT(apex_point, portal_right, right) <= 0) {
- //process
- if (portal_right.is_equal_approx(apex_point) || CLOCK_TANGENT(apex_point, right, portal_left) < 0) {
- right_poly = p;
- portal_right = right;
- } else {
-
- apex_point = portal_left;
- p = left_poly;
- right_poly = p;
- portal_right = apex_point;
- portal_left = apex_point;
- if (!path.size() || !path[path.size() - 1].is_equal_approx(apex_point))
- path.push_back(apex_point);
- }
- }
-
- if (p != begin_poly)
- p = p->edges[p->prev_edge].C;
- else
- p = NULL;
- }
-
- } else {
- //midpoints
- Polygon *p = end_poly;
-
- while (true) {
- int prev = p->prev_edge;
- int prev_n = (p->prev_edge + 1) % p->edges.size();
- Vector2 point = (_get_vertex(p->edges[prev].point) + _get_vertex(p->edges[prev_n].point)) * 0.5;
- path.push_back(point);
- p = p->edges[prev].C;
- if (p == begin_poly)
- break;
- }
- }
-
- if (!path.size() || !path[path.size() - 1].is_equal_approx(begin_point)) {
- path.push_back(begin_point); // Add the begin point
- } else {
- path.write[path.size() - 1] = begin_point; // Replace first midpoint by the exact begin point
- }
-
- path.invert();
-
- if (path.size() <= 1 || !path[path.size() - 1].is_equal_approx(end_point)) {
- path.push_back(end_point); // Add the end point
- } else {
- path.write[path.size() - 1] = end_point; // Replace last midpoint by the exact end point
- }
-
- return path;
- }
-
- return Vector<Vector2>();
-}
-
-Vector2 Navigation2D::get_closest_point(const Vector2 &p_point) {
-
- Vector2 closest_point = Vector2();
- float closest_point_d = 1e20;
-
- for (Map<int, NavMesh>::Element *E = navpoly_map.front(); E; E = E->next()) {
-
- if (!E->get().linked)
- continue;
- for (List<Polygon>::Element *F = E->get().polygons.front(); F; F = F->next()) {
-
- Polygon &p = F->get();
- for (int i = 2; i < p.edges.size(); i++) {
-
- if (Geometry::is_point_in_triangle(p_point, _get_vertex(p.edges[0].point), _get_vertex(p.edges[i - 1].point), _get_vertex(p.edges[i].point))) {
-
- return p_point; //inside triangle, nothing else to discuss
- }
- }
- }
- }
-
- for (Map<int, NavMesh>::Element *E = navpoly_map.front(); E; E = E->next()) {
-
- if (!E->get().linked)
- continue;
- for (List<Polygon>::Element *F = E->get().polygons.front(); F; F = F->next()) {
-
- Polygon &p = F->get();
- int es = p.edges.size();
- for (int i = 0; i < es; i++) {
-
- Vector2 edge[2] = {
- _get_vertex(p.edges[i].point),
- _get_vertex(p.edges[(i + 1) % es].point)
- };
-
- Vector2 spoint = Geometry::get_closest_point_to_segment_2d(p_point, edge);
- float d = spoint.distance_squared_to(p_point);
- if (d < closest_point_d) {
-
- closest_point = spoint;
- closest_point_d = d;
- }
- }
- }
- }
-
- return closest_point;
-}
-
-Object *Navigation2D::get_closest_point_owner(const Vector2 &p_point) {
-
- Object *owner = NULL;
- Vector2 closest_point = Vector2();
- float closest_point_d = 1e20;
-
- for (Map<int, NavMesh>::Element *E = navpoly_map.front(); E; E = E->next()) {
-
- if (!E->get().linked)
- continue;
- for (List<Polygon>::Element *F = E->get().polygons.front(); F; F = F->next()) {
-
- Polygon &p = F->get();
- for (int i = 2; i < p.edges.size(); i++) {
-
- if (Geometry::is_point_in_triangle(p_point, _get_vertex(p.edges[0].point), _get_vertex(p.edges[i - 1].point), _get_vertex(p.edges[i].point))) {
-
- return E->get().owner;
- }
- }
- }
- }
-
- for (Map<int, NavMesh>::Element *E = navpoly_map.front(); E; E = E->next()) {
-
- if (!E->get().linked)
- continue;
- for (List<Polygon>::Element *F = E->get().polygons.front(); F; F = F->next()) {
-
- Polygon &p = F->get();
- int es = p.edges.size();
- for (int i = 0; i < es; i++) {
-
- Vector2 edge[2] = {
- _get_vertex(p.edges[i].point),
- _get_vertex(p.edges[(i + 1) % es].point)
- };
-
- Vector2 spoint = Geometry::get_closest_point_to_segment_2d(p_point, edge);
- float d = spoint.distance_squared_to(p_point);
- if (d < closest_point_d) {
-
- closest_point = spoint;
- closest_point_d = d;
- owner = E->get().owner;
- }
- }
- }
- }
-
- return owner;
-}
-
-void Navigation2D::_bind_methods() {
-
- ClassDB::bind_method(D_METHOD("navpoly_add", "mesh", "xform", "owner"), &Navigation2D::navpoly_add, DEFVAL(Variant()));
- ClassDB::bind_method(D_METHOD("navpoly_set_transform", "id", "xform"), &Navigation2D::navpoly_set_transform);
- ClassDB::bind_method(D_METHOD("navpoly_remove", "id"), &Navigation2D::navpoly_remove);
-
- ClassDB::bind_method(D_METHOD("get_simple_path", "start", "end", "optimize"), &Navigation2D::get_simple_path, DEFVAL(true));
- ClassDB::bind_method(D_METHOD("get_closest_point", "to_point"), &Navigation2D::get_closest_point);
- ClassDB::bind_method(D_METHOD("get_closest_point_owner", "to_point"), &Navigation2D::get_closest_point_owner);
+ return Navigation2DServer::get_singleton()->map_get_path(map, p_start, p_end, p_optimize);
}
Navigation2D::Navigation2D() {
- ERR_FAIL_COND(sizeof(Point) != 8);
- cell_size = 1; // one pixel
- last_id = 1;
+ map = Navigation2DServer::get_singleton()->map_create();
+ set_cell_size(10); // Ten pixels
+ set_edge_connection_margin(100);
}
diff --git a/scene/2d/navigation_2d.h b/scene/2d/navigation_2d.h
index 41b9aea35a..08642a5489 100644
--- a/scene/2d/navigation_2d.h
+++ b/scene/2d/navigation_2d.h
@@ -38,135 +38,30 @@ class Navigation2D : public Node2D {
GDCLASS(Navigation2D, Node2D);
- union Point {
+ RID map;
+ real_t cell_size;
+ real_t edge_connection_margin;
- struct {
- int64_t x : 32;
- int64_t y : 32;
- };
-
- uint64_t key;
- bool operator<(const Point &p_key) const { return key < p_key.key; }
- };
-
- struct EdgeKey {
-
- Point a;
- Point b;
-
- bool operator<(const EdgeKey &p_key) const {
- return (a.key == p_key.a.key) ? (b.key < p_key.b.key) : (a.key < p_key.a.key);
- };
-
- EdgeKey(const Point &p_a = Point(), const Point &p_b = Point()) :
- a(p_a),
- b(p_b) {
- if (a.key > b.key) {
- SWAP(a, b);
- }
- }
- };
-
- struct NavMesh;
- struct Polygon;
-
- struct ConnectionPending {
-
- Polygon *polygon;
- int edge;
- };
-
- struct Polygon {
-
- struct Edge {
- Point point;
- Polygon *C; //connection
- int C_edge;
- List<ConnectionPending>::Element *P;
- Edge() {
- C = NULL;
- C_edge = -1;
- P = NULL;
- }
- };
-
- Vector<Edge> edges;
-
- Vector2 center;
- Vector2 entry;
-
- float distance;
- int prev_edge;
-
- bool clockwise;
-
- NavMesh *owner;
- };
-
- struct Connection {
-
- Polygon *A;
- int A_edge;
- Polygon *B;
- int B_edge;
-
- List<ConnectionPending> pending;
-
- Connection() {
- A = NULL;
- B = NULL;
- A_edge = -1;
- B_edge = -1;
- }
- };
-
- Map<EdgeKey, Connection> connections;
-
- struct NavMesh {
-
- Object *owner;
- Transform2D xform;
- bool linked;
- Ref<NavigationPolygon> navpoly;
- List<Polygon> polygons;
- };
-
- _FORCE_INLINE_ Point _get_point(const Vector2 &p_pos) const {
-
- int x = int(Math::floor(p_pos.x / cell_size));
- int y = int(Math::floor(p_pos.y / cell_size));
+protected:
+ static void _bind_methods();
+ void _notification(int p_what);
- Point p;
- p.key = 0;
- p.x = x;
- p.y = y;
- return p;
+public:
+ RID get_rid() const {
+ return map;
}
- _FORCE_INLINE_ Vector2 _get_vertex(const Point &p_point) const {
-
- return Vector2(p_point.x, p_point.y) * cell_size;
+ void set_cell_size(float p_cell_size);
+ float get_cell_size() const {
+ return cell_size;
}
- void _navpoly_link(int p_id);
- void _navpoly_unlink(int p_id);
-
- float cell_size;
- Map<int, NavMesh> navpoly_map;
- int last_id;
-
-protected:
- static void _bind_methods();
-
-public:
- //API should be as dynamic as possible
- int navpoly_add(const Ref<NavigationPolygon> &p_mesh, const Transform2D &p_xform, Object *p_owner = NULL);
- void navpoly_set_transform(int p_id, const Transform2D &p_xform);
- void navpoly_remove(int p_id);
+ void set_edge_connection_margin(float p_edge_connection_margin);
+ float get_edge_connection_margin() const {
+ return edge_connection_margin;
+ }
Vector<Vector2> get_simple_path(const Vector2 &p_start, const Vector2 &p_end, bool p_optimize = true);
- Vector2 get_closest_point(const Vector2 &p_point);
- Object *get_closest_point_owner(const Vector2 &p_point);
Navigation2D();
};
diff --git a/scene/2d/navigation_agent_2d.cpp b/scene/2d/navigation_agent_2d.cpp
new file mode 100644
index 0000000000..fe4d3b322f
--- /dev/null
+++ b/scene/2d/navigation_agent_2d.cpp
@@ -0,0 +1,341 @@
+/*************************************************************************/
+/* navigation_agent_2d.cpp */
+/*************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/*************************************************************************/
+/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md) */
+/* */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the */
+/* "Software"), to deal in the Software without restriction, including */
+/* without limitation the rights to use, copy, modify, merge, publish, */
+/* distribute, sublicense, and/or sell copies of the Software, and to */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions: */
+/* */
+/* The above copyright notice and this permission notice shall be */
+/* included in all copies or substantial portions of the Software. */
+/* */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
+/*************************************************************************/
+
+#include "navigation_agent_2d.h"
+
+#include "core/engine.h"
+#include "scene/2d/navigation_2d.h"
+#include "servers/navigation_2d_server.h"
+
+void NavigationAgent2D::_bind_methods() {
+
+ ClassDB::bind_method(D_METHOD("set_target_desired_distance", "desired_distance"), &NavigationAgent2D::set_target_desired_distance);
+ ClassDB::bind_method(D_METHOD("get_target_desired_distance"), &NavigationAgent2D::get_target_desired_distance);
+
+ ClassDB::bind_method(D_METHOD("set_radius", "radius"), &NavigationAgent2D::set_radius);
+ ClassDB::bind_method(D_METHOD("get_radius"), &NavigationAgent2D::get_radius);
+
+ ClassDB::bind_method(D_METHOD("set_navigation", "navigation"), &NavigationAgent2D::set_navigation_node);
+ ClassDB::bind_method(D_METHOD("get_navigation"), &NavigationAgent2D::get_navigation_node);
+
+ ClassDB::bind_method(D_METHOD("set_neighbor_dist", "neighbor_dist"), &NavigationAgent2D::set_neighbor_dist);
+ ClassDB::bind_method(D_METHOD("get_neighbor_dist"), &NavigationAgent2D::get_neighbor_dist);
+
+ ClassDB::bind_method(D_METHOD("set_max_neighbors", "max_neighbors"), &NavigationAgent2D::set_max_neighbors);
+ ClassDB::bind_method(D_METHOD("get_max_neighbors"), &NavigationAgent2D::get_max_neighbors);
+
+ ClassDB::bind_method(D_METHOD("set_time_horizon", "time_horizon"), &NavigationAgent2D::set_time_horizon);
+ ClassDB::bind_method(D_METHOD("get_time_horizon"), &NavigationAgent2D::get_time_horizon);
+
+ ClassDB::bind_method(D_METHOD("set_max_speed", "max_speed"), &NavigationAgent2D::set_max_speed);
+ ClassDB::bind_method(D_METHOD("get_max_speed"), &NavigationAgent2D::get_max_speed);
+
+ ClassDB::bind_method(D_METHOD("set_path_max_distance", "max_speed"), &NavigationAgent2D::set_path_max_distance);
+ ClassDB::bind_method(D_METHOD("get_path_max_distance"), &NavigationAgent2D::get_path_max_distance);
+
+ ClassDB::bind_method(D_METHOD("set_target_location", "location"), &NavigationAgent2D::set_target_location);
+ ClassDB::bind_method(D_METHOD("get_target_location"), &NavigationAgent2D::get_target_location);
+ ClassDB::bind_method(D_METHOD("get_next_location"), &NavigationAgent2D::get_next_location);
+ ClassDB::bind_method(D_METHOD("distance_to_target"), &NavigationAgent2D::distance_to_target);
+ ClassDB::bind_method(D_METHOD("set_velocity", "velocity"), &NavigationAgent2D::set_velocity);
+ ClassDB::bind_method(D_METHOD("get_nav_path"), &NavigationAgent2D::get_nav_path);
+ ClassDB::bind_method(D_METHOD("get_nav_path_index"), &NavigationAgent2D::get_nav_path_index);
+ ClassDB::bind_method(D_METHOD("is_target_reached"), &NavigationAgent2D::is_target_reached);
+ ClassDB::bind_method(D_METHOD("is_target_reachable"), &NavigationAgent2D::is_target_reachable);
+ ClassDB::bind_method(D_METHOD("is_navigation_finished"), &NavigationAgent2D::is_navigation_finished);
+ ClassDB::bind_method(D_METHOD("get_final_location"), &NavigationAgent2D::get_final_location);
+
+ ClassDB::bind_method(D_METHOD("_avoidance_done", "new_velocity"), &NavigationAgent2D::_avoidance_done);
+
+ ADD_PROPERTY(PropertyInfo(Variant::REAL, "target_desired_distance", PROPERTY_HINT_RANGE, "0.1,100,0.01"), "set_target_desired_distance", "get_target_desired_distance");
+ ADD_PROPERTY(PropertyInfo(Variant::REAL, "radius", PROPERTY_HINT_RANGE, "0.1,500,0.01"), "set_radius", "get_radius");
+ ADD_PROPERTY(PropertyInfo(Variant::REAL, "neighbor_dist", PROPERTY_HINT_RANGE, "0.1,100000,0.01"), "set_neighbor_dist", "get_neighbor_dist");
+ ADD_PROPERTY(PropertyInfo(Variant::INT, "max_neighbors", PROPERTY_HINT_RANGE, "1,10000,1"), "set_max_neighbors", "get_max_neighbors");
+ ADD_PROPERTY(PropertyInfo(Variant::REAL, "time_horizon", PROPERTY_HINT_RANGE, "0.1,10000,0.01"), "set_time_horizon", "get_time_horizon");
+ ADD_PROPERTY(PropertyInfo(Variant::REAL, "max_speed", PROPERTY_HINT_RANGE, "0.1,100000,0.01"), "set_max_speed", "get_max_speed");
+ ADD_PROPERTY(PropertyInfo(Variant::REAL, "path_max_distance", PROPERTY_HINT_RANGE, "10,100,1"), "set_path_max_distance", "get_path_max_distance");
+
+ ADD_SIGNAL(MethodInfo("path_changed"));
+ ADD_SIGNAL(MethodInfo("target_reached"));
+ ADD_SIGNAL(MethodInfo("navigation_finished"));
+ ADD_SIGNAL(MethodInfo("velocity_computed", PropertyInfo(Variant::VECTOR3, "safe_velocity")));
+}
+
+void NavigationAgent2D::_notification(int p_what) {
+ switch (p_what) {
+ case NOTIFICATION_READY: {
+
+ agent_parent = Object::cast_to<Node2D>(get_parent());
+
+ Navigation2DServer::get_singleton()->agent_set_callback(agent, this, "_avoidance_done");
+
+ // Search the navigation node and set it
+ {
+ Navigation2D *nav = NULL;
+ Node *p = get_parent();
+ while (p != NULL) {
+ nav = Object::cast_to<Navigation2D>(p);
+ if (nav != NULL)
+ p = NULL;
+ else
+ p = p->get_parent();
+ }
+
+ set_navigation(nav);
+ }
+
+ set_physics_process_internal(true);
+ } break;
+ case NOTIFICATION_EXIT_TREE: {
+ agent_parent = NULL;
+ set_navigation(NULL);
+ set_physics_process_internal(false);
+ } break;
+ case NOTIFICATION_INTERNAL_PHYSICS_PROCESS: {
+ if (agent_parent) {
+
+ Navigation2DServer::get_singleton()->agent_set_position(agent, agent_parent->get_global_transform().get_origin());
+ if (!target_reached) {
+ if (distance_to_target() < target_desired_distance) {
+ emit_signal("target_reached");
+ target_reached = true;
+ }
+ }
+ }
+ } break;
+ }
+}
+
+NavigationAgent2D::NavigationAgent2D() :
+ agent_parent(NULL),
+ navigation(NULL),
+ agent(RID()),
+ target_desired_distance(1.0),
+ path_max_distance(3.0),
+ velocity_submitted(false),
+ target_reached(false),
+ navigation_finished(true) {
+ agent = Navigation2DServer::get_singleton()->agent_create();
+ set_neighbor_dist(500.0);
+ set_max_neighbors(10);
+ set_time_horizon(20.0);
+ set_radius(10.0);
+ set_max_speed(200.0);
+}
+
+NavigationAgent2D::~NavigationAgent2D() {
+ Navigation2DServer::get_singleton()->free(agent);
+ agent = RID(); // Pointless
+}
+
+void NavigationAgent2D::set_navigation(Navigation2D *p_nav) {
+ if (navigation == p_nav)
+ return; // Pointless
+
+ navigation = p_nav;
+ Navigation2DServer::get_singleton()->agent_set_map(agent, navigation == NULL ? RID() : navigation->get_rid());
+}
+
+void NavigationAgent2D::set_navigation_node(Node *p_nav) {
+ Navigation2D *nav = Object::cast_to<Navigation2D>(p_nav);
+ ERR_FAIL_COND(nav == NULL);
+ set_navigation(nav);
+}
+
+Node *NavigationAgent2D::get_navigation_node() const {
+ return Object::cast_to<Node>(navigation);
+}
+
+void NavigationAgent2D::set_target_desired_distance(real_t p_dd) {
+ target_desired_distance = p_dd;
+}
+
+void NavigationAgent2D::set_radius(real_t p_radius) {
+ radius = p_radius;
+ Navigation2DServer::get_singleton()->agent_set_radius(agent, radius);
+}
+
+void NavigationAgent2D::set_neighbor_dist(real_t p_dist) {
+ neighbor_dist = p_dist;
+ Navigation2DServer::get_singleton()->agent_set_neighbor_dist(agent, neighbor_dist);
+}
+
+void NavigationAgent2D::set_max_neighbors(int p_count) {
+ max_neighbors = p_count;
+ Navigation2DServer::get_singleton()->agent_set_max_neighbors(agent, max_neighbors);
+}
+
+void NavigationAgent2D::set_time_horizon(real_t p_time) {
+ time_horizon = p_time;
+ Navigation2DServer::get_singleton()->agent_set_time_horizon(agent, time_horizon);
+}
+
+void NavigationAgent2D::set_max_speed(real_t p_max_speed) {
+ max_speed = p_max_speed;
+ Navigation2DServer::get_singleton()->agent_set_max_speed(agent, max_speed);
+}
+
+void NavigationAgent2D::set_path_max_distance(real_t p_pmd) {
+ path_max_distance = p_pmd;
+}
+
+real_t NavigationAgent2D::get_path_max_distance() {
+ return path_max_distance;
+}
+
+void NavigationAgent2D::set_target_location(Vector2 p_location) {
+ target_location = p_location;
+ navigation_path.clear();
+ target_reached = false;
+ navigation_finished = false;
+}
+
+Vector2 NavigationAgent2D::get_target_location() const {
+ return target_location;
+}
+
+Vector2 NavigationAgent2D::get_next_location() {
+ update_navigation();
+ if (navigation_path.size() == 0) {
+ ERR_FAIL_COND_V(agent_parent == NULL, Vector2());
+ return agent_parent->get_global_transform().get_origin();
+ } else {
+ return navigation_path[nav_path_index];
+ }
+}
+
+real_t NavigationAgent2D::distance_to_target() const {
+ ERR_FAIL_COND_V(agent_parent == NULL, 0.0);
+ return agent_parent->get_global_transform().get_origin().distance_to(target_location);
+}
+
+bool NavigationAgent2D::is_target_reached() const {
+ return target_reached;
+}
+
+bool NavigationAgent2D::is_target_reachable() {
+ return target_desired_distance >= get_final_location().distance_to(target_location);
+}
+
+bool NavigationAgent2D::is_navigation_finished() {
+ update_navigation();
+ return navigation_finished;
+}
+
+Vector2 NavigationAgent2D::get_final_location() {
+ update_navigation();
+ if (navigation_path.size() == 0) {
+ return Vector2();
+ }
+ return navigation_path[navigation_path.size() - 1];
+}
+
+void NavigationAgent2D::set_velocity(Vector2 p_velocity) {
+ target_velocity = p_velocity;
+ Navigation2DServer::get_singleton()->agent_set_target_velocity(agent, target_velocity);
+ Navigation2DServer::get_singleton()->agent_set_velocity(agent, prev_safe_velocity);
+ velocity_submitted = true;
+}
+
+void NavigationAgent2D::_avoidance_done(Vector3 p_new_velocity) {
+ const Vector2 velocity = Vector2(p_new_velocity.x, p_new_velocity.z);
+ prev_safe_velocity = velocity;
+
+ if (!velocity_submitted) {
+ target_velocity = Vector2();
+ return;
+ }
+ velocity_submitted = false;
+
+ emit_signal("velocity_computed", velocity);
+}
+
+String NavigationAgent2D::get_configuration_warning() const {
+ if (!Object::cast_to<Node2D>(get_parent())) {
+ return TTR("The NavigationAgent2D can be used only under a Node2D node");
+ }
+
+ return String();
+}
+
+void NavigationAgent2D::update_navigation() {
+
+ if (agent_parent == NULL) return;
+ if (navigation == NULL) return;
+ if (update_frame_id == Engine::get_singleton()->get_physics_frames()) return;
+
+ update_frame_id = Engine::get_singleton()->get_physics_frames();
+
+ Vector2 o = agent_parent->get_global_transform().get_origin();
+
+ bool reload_path = false;
+
+ if (Navigation2DServer::get_singleton()->agent_is_map_changed(agent)) {
+ reload_path = true;
+ } else if (navigation_path.size() == 0) {
+ reload_path = true;
+ } else {
+ // Check if too far from the navigation path
+ if (nav_path_index > 0) {
+ Vector2 segment[2];
+ segment[0] = navigation_path[nav_path_index - 1];
+ segment[1] = navigation_path[nav_path_index];
+ Vector2 p = Geometry::get_closest_point_to_segment_2d(o, segment);
+ if (o.distance_to(p) >= path_max_distance) {
+ // To faraway, reload path
+ reload_path = true;
+ }
+ }
+ }
+
+ if (reload_path) {
+ navigation_path = Navigation2DServer::get_singleton()->map_get_path(navigation->get_rid(), o, target_location, true);
+ navigation_finished = false;
+ nav_path_index = 0;
+ emit_signal("path_changed");
+ }
+
+ if (navigation_path.size() == 0)
+ return;
+
+ // Check if we can advance the navigation path
+ if (navigation_finished == false) {
+ // Advances to the next far away location.
+ while (o.distance_to(navigation_path[nav_path_index]) < target_desired_distance) {
+ nav_path_index += 1;
+ if (nav_path_index == navigation_path.size()) {
+ nav_path_index -= 1;
+ navigation_finished = true;
+ emit_signal("navigation_finished");
+ break;
+ }
+ }
+ }
+}
diff --git a/scene/2d/navigation_agent_2d.h b/scene/2d/navigation_agent_2d.h
new file mode 100644
index 0000000000..60527091ce
--- /dev/null
+++ b/scene/2d/navigation_agent_2d.h
@@ -0,0 +1,150 @@
+/*************************************************************************/
+/* navigation_agent_2d.h */
+/*************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/*************************************************************************/
+/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md) */
+/* */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the */
+/* "Software"), to deal in the Software without restriction, including */
+/* without limitation the rights to use, copy, modify, merge, publish, */
+/* distribute, sublicense, and/or sell copies of the Software, and to */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions: */
+/* */
+/* The above copyright notice and this permission notice shall be */
+/* included in all copies or substantial portions of the Software. */
+/* */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
+/*************************************************************************/
+
+#ifndef NAVIGATION_AGENT_2D_H
+#define NAVIGATION_AGENT_2D_H
+
+#include "core/vector.h"
+#include "scene/main/node.h"
+
+class Node2D;
+class Navigation2D;
+
+class NavigationAgent2D : public Node {
+ GDCLASS(NavigationAgent2D, Node);
+
+ Node2D *agent_parent;
+ Navigation2D *navigation;
+
+ RID agent;
+
+ real_t target_desired_distance;
+ real_t radius;
+ real_t neighbor_dist;
+ int max_neighbors;
+ real_t time_horizon;
+ real_t max_speed;
+
+ real_t path_max_distance;
+
+ Vector2 target_location;
+ Vector<Vector2> navigation_path;
+ int nav_path_index;
+ bool velocity_submitted;
+ Vector2 prev_safe_velocity;
+ /// The submitted target velocity
+ Vector2 target_velocity;
+ bool target_reached;
+ bool navigation_finished;
+ // No initialized on purpose
+ uint32_t update_frame_id;
+
+protected:
+ static void _bind_methods();
+ void _notification(int p_what);
+
+public:
+ NavigationAgent2D();
+ virtual ~NavigationAgent2D();
+
+ void set_navigation(Navigation2D *p_nav);
+ const Navigation2D *get_navigation() const {
+ return navigation;
+ }
+
+ void set_navigation_node(Node *p_nav);
+ Node *get_navigation_node() const;
+
+ RID get_rid() const {
+ return agent;
+ }
+
+ void set_target_desired_distance(real_t p_dd);
+ real_t get_target_desired_distance() const {
+ return target_desired_distance;
+ }
+
+ void set_radius(real_t p_radius);
+ real_t get_radius() const {
+ return radius;
+ }
+
+ void set_neighbor_dist(real_t p_dist);
+ real_t get_neighbor_dist() const {
+ return neighbor_dist;
+ }
+
+ void set_max_neighbors(int p_count);
+ int get_max_neighbors() const {
+ return max_neighbors;
+ }
+
+ void set_time_horizon(real_t p_time);
+ real_t get_time_horizon() const {
+ return time_horizon;
+ }
+
+ void set_max_speed(real_t p_max_speed);
+ real_t get_max_speed() const {
+ return max_speed;
+ }
+
+ void set_path_max_distance(real_t p_pmd);
+ real_t get_path_max_distance();
+
+ void set_target_location(Vector2 p_location);
+ Vector2 get_target_location() const;
+
+ Vector2 get_next_location();
+
+ Vector<Vector2> get_nav_path() const {
+ return navigation_path;
+ }
+
+ int get_nav_path_index() const {
+ return nav_path_index;
+ }
+
+ real_t distance_to_target() const;
+ bool is_target_reached() const;
+ bool is_target_reachable();
+ bool is_navigation_finished();
+ Vector2 get_final_location();
+
+ void set_velocity(Vector2 p_velocity);
+ void _avoidance_done(Vector3 p_new_velocity);
+
+ virtual String get_configuration_warning() const;
+
+private:
+ void update_navigation();
+};
+
+#endif
diff --git a/scene/2d/navigation_obstacle_2d.cpp b/scene/2d/navigation_obstacle_2d.cpp
new file mode 100644
index 0000000000..0740b616b6
--- /dev/null
+++ b/scene/2d/navigation_obstacle_2d.cpp
@@ -0,0 +1,154 @@
+/*************************************************************************/
+/* navigation_obstacle.cpp */
+/*************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/*************************************************************************/
+/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md) */
+/* */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the */
+/* "Software"), to deal in the Software without restriction, including */
+/* without limitation the rights to use, copy, modify, merge, publish, */
+/* distribute, sublicense, and/or sell copies of the Software, and to */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions: */
+/* */
+/* The above copyright notice and this permission notice shall be */
+/* included in all copies or substantial portions of the Software. */
+/* */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
+/*************************************************************************/
+
+#include "navigation_obstacle_2d.h"
+
+#include "scene/2d/collision_shape_2d.h"
+#include "scene/2d/navigation_2d.h"
+#include "scene/2d/physics_body_2d.h"
+#include "servers/navigation_2d_server.h"
+
+void NavigationObstacle2D::_bind_methods() {
+
+ ClassDB::bind_method(D_METHOD("set_navigation", "navigation"), &NavigationObstacle2D::set_navigation_node);
+ ClassDB::bind_method(D_METHOD("get_navigation"), &NavigationObstacle2D::get_navigation_node);
+}
+
+void NavigationObstacle2D::_notification(int p_what) {
+ switch (p_what) {
+ case NOTIFICATION_READY: {
+
+ update_agent_shape();
+
+ // Search the navigation node and set it
+ {
+ Navigation2D *nav = NULL;
+ Node *p = get_parent();
+ while (p != NULL) {
+ nav = Object::cast_to<Navigation2D>(p);
+ if (nav != NULL)
+ p = NULL;
+ else
+ p = p->get_parent();
+ }
+
+ set_navigation(nav);
+ }
+
+ set_physics_process_internal(true);
+ } break;
+ case NOTIFICATION_EXIT_TREE: {
+ set_navigation(NULL);
+ set_physics_process_internal(false);
+ } break;
+ case NOTIFICATION_INTERNAL_PHYSICS_PROCESS: {
+ Node2D *node = Object::cast_to<Node2D>(get_parent());
+ if (node) {
+ Navigation2DServer::get_singleton()->agent_set_position(agent, node->get_global_transform().get_origin());
+ }
+
+ } break;
+ }
+}
+
+NavigationObstacle2D::NavigationObstacle2D() :
+ navigation(NULL),
+ agent(RID()) {
+ agent = Navigation2DServer::get_singleton()->agent_create();
+}
+
+NavigationObstacle2D::~NavigationObstacle2D() {
+ Navigation2DServer::get_singleton()->free(agent);
+ agent = RID(); // Pointless
+}
+
+void NavigationObstacle2D::set_navigation(Navigation2D *p_nav) {
+ if (navigation == p_nav)
+ return; // Pointless
+
+ navigation = p_nav;
+ Navigation2DServer::get_singleton()->agent_set_map(agent, navigation == NULL ? RID() : navigation->get_rid());
+}
+
+void NavigationObstacle2D::set_navigation_node(Node *p_nav) {
+ Navigation2D *nav = Object::cast_to<Navigation2D>(p_nav);
+ ERR_FAIL_COND(nav == NULL);
+ set_navigation(nav);
+}
+
+Node *NavigationObstacle2D::get_navigation_node() const {
+ return Object::cast_to<Node>(navigation);
+}
+
+String NavigationObstacle2D::get_configuration_warning() const {
+ if (!Object::cast_to<Node2D>(get_parent())) {
+ return TTR("The NavigationObstacle2D only serves to provide collision avoidance to a Node2D object.");
+ }
+
+ return String();
+}
+
+void NavigationObstacle2D::update_agent_shape() {
+ Node *node = get_parent();
+
+ // Estimate the radius of this physics body
+ real_t radius = 0.0;
+ for (int i(0); i < node->get_child_count(); i++) {
+ // For each collision shape
+ CollisionShape2D *cs = Object::cast_to<CollisionShape2D>(node->get_child(i));
+ if (cs) {
+ // Take the distance between the Body center to the shape center
+ real_t r = cs->get_transform().get_origin().length();
+ if (cs->get_shape().is_valid()) {
+ // and add the enclosing shape radius
+ r += cs->get_shape()->get_enclosing_radius();
+ }
+ Size2 s = cs->get_global_transform().get_scale();
+ r *= MAX(s.x, s.y);
+ // Takes the biggest radius
+ radius = MAX(radius, r);
+ }
+ }
+ Node2D *node_2d = Object::cast_to<Node2D>(node);
+ if (node_2d) {
+ Vector2 s = node_2d->get_global_transform().get_scale();
+ radius *= MAX(s.x, s.y);
+ }
+
+ if (radius == 0.0)
+ radius = 1.0; // Never a 0 radius
+
+ // Initialize the Agent as an object
+ Navigation2DServer::get_singleton()->agent_set_neighbor_dist(agent, 0.0);
+ Navigation2DServer::get_singleton()->agent_set_max_neighbors(agent, 0);
+ Navigation2DServer::get_singleton()->agent_set_time_horizon(agent, 0.0);
+ Navigation2DServer::get_singleton()->agent_set_radius(agent, radius);
+ Navigation2DServer::get_singleton()->agent_set_max_speed(agent, 0.0);
+}
diff --git a/scene/2d/navigation_obstacle_2d.h b/scene/2d/navigation_obstacle_2d.h
new file mode 100644
index 0000000000..f098255799
--- /dev/null
+++ b/scene/2d/navigation_obstacle_2d.h
@@ -0,0 +1,71 @@
+/*************************************************************************/
+/* navigation_obstacle.h */
+/*************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/*************************************************************************/
+/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md) */
+/* */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the */
+/* "Software"), to deal in the Software without restriction, including */
+/* without limitation the rights to use, copy, modify, merge, publish, */
+/* distribute, sublicense, and/or sell copies of the Software, and to */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions: */
+/* */
+/* The above copyright notice and this permission notice shall be */
+/* included in all copies or substantial portions of the Software. */
+/* */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
+/*************************************************************************/
+
+#ifndef NAVIGATION_OBSTACLE_2D_H
+#define NAVIGATION_OBSTACLE_2D_H
+
+#include "scene/main/node.h"
+
+class Navigation2D;
+
+class NavigationObstacle2D : public Node {
+ GDCLASS(NavigationObstacle2D, Node);
+
+ Navigation2D *navigation;
+
+ RID agent;
+
+protected:
+ static void _bind_methods();
+ void _notification(int p_what);
+
+public:
+ NavigationObstacle2D();
+ virtual ~NavigationObstacle2D();
+
+ void set_navigation(Navigation2D *p_nav);
+ const Navigation2D *get_navigation() const {
+ return navigation;
+ }
+
+ void set_navigation_node(Node *p_nav);
+ Node *get_navigation_node() const;
+
+ RID get_rid() const {
+ return agent;
+ }
+
+ virtual String get_configuration_warning() const;
+
+private:
+ void update_agent_shape();
+};
+
+#endif
diff --git a/scene/2d/navigation_polygon.cpp b/scene/2d/navigation_polygon.cpp
index 883fb37668..c40cdb4720 100644
--- a/scene/2d/navigation_polygon.cpp
+++ b/scene/2d/navigation_polygon.cpp
@@ -32,7 +32,9 @@
#include "core/core_string_names.h"
#include "core/engine.h"
+#include "core/os/mutex.h"
#include "navigation_2d.h"
+#include "servers/navigation_2d_server.h"
#include "thirdparty/misc/triangulator.h"
@@ -80,6 +82,9 @@ bool NavigationPolygon::_edit_is_selected_on_click(const Point2 &p_point, double
void NavigationPolygon::set_vertices(const PoolVector<Vector2> &p_vertices) {
+ navmesh_generation->lock();
+ navmesh.unref();
+ navmesh_generation->unlock();
vertices = p_vertices;
rect_cache_dirty = true;
}
@@ -91,6 +96,9 @@ PoolVector<Vector2> NavigationPolygon::get_vertices() const {
void NavigationPolygon::_set_polygons(const Array &p_array) {
+ navmesh_generation->lock();
+ navmesh.unref();
+ navmesh_generation->unlock();
polygons.resize(p_array.size());
for (int i = 0; i < p_array.size(); i++) {
polygons.write[i].indices = p_array[i];
@@ -133,6 +141,9 @@ void NavigationPolygon::add_polygon(const Vector<int> &p_polygon) {
Polygon polygon;
polygon.indices = p_polygon;
polygons.push_back(polygon);
+ navmesh_generation->lock();
+ navmesh.unref();
+ navmesh_generation->unlock();
}
void NavigationPolygon::add_outline_at_index(const PoolVector<Vector2> &p_outline, int p_index) {
@@ -153,6 +164,34 @@ Vector<int> NavigationPolygon::get_polygon(int p_idx) {
void NavigationPolygon::clear_polygons() {
polygons.clear();
+ navmesh_generation->lock();
+ navmesh.unref();
+ navmesh_generation->unlock();
+}
+
+Ref<NavigationMesh> NavigationPolygon::get_mesh() {
+ navmesh_generation->lock();
+ if (navmesh.is_null()) {
+ navmesh.instance();
+ PoolVector<Vector3> verts;
+ {
+ verts.resize(get_vertices().size());
+ PoolVector<Vector3>::Write w = verts.write();
+
+ PoolVector<Vector2>::Read r = get_vertices().read();
+
+ for (int i(0); i < get_vertices().size(); i++) {
+ w[i] = Vector3(r[i].x, 0.0, r[i].y);
+ }
+ }
+ navmesh->set_vertices(verts);
+
+ for (int i(0); i < get_polygon_count(); i++) {
+ navmesh->add_polygon(get_polygon(i));
+ }
+ }
+ navmesh_generation->unlock();
+ return navmesh;
}
void NavigationPolygon::add_outline(const PoolVector<Vector2> &p_outline) {
@@ -191,6 +230,9 @@ void NavigationPolygon::clear_outlines() {
}
void NavigationPolygon::make_polygons_from_outlines() {
+ navmesh_generation->lock();
+ navmesh.unref();
+ navmesh_generation->unlock();
List<TriangulatorPoly> in_poly, out_poly;
Vector2 outside_point(-1e10, -1e10);
@@ -320,7 +362,9 @@ void NavigationPolygon::_bind_methods() {
}
NavigationPolygon::NavigationPolygon() :
- rect_cache_dirty(true) {
+ rect_cache_dirty(true),
+ navmesh_generation(NULL) {
+ navmesh_generation = Mutex::create();
}
void NavigationPolygonInstance::set_enabled(bool p_enabled) {
@@ -334,18 +378,12 @@ void NavigationPolygonInstance::set_enabled(bool p_enabled) {
if (!enabled) {
- if (nav_id != -1) {
- navigation->navpoly_remove(nav_id);
- nav_id = -1;
- }
+ Navigation2DServer::get_singleton()->region_set_map(region, RID());
} else {
if (navigation) {
- if (navpoly.is_valid()) {
-
- nav_id = navigation->navpoly_add(navpoly, get_relative_transform_to_parent(navigation), this);
- }
+ Navigation2DServer::get_singleton()->region_set_map(region, navigation->get_rid());
}
}
@@ -382,9 +420,9 @@ void NavigationPolygonInstance::_notification(int p_what) {
navigation = Object::cast_to<Navigation2D>(c);
if (navigation) {
- if (enabled && navpoly.is_valid()) {
+ if (enabled) {
- nav_id = navigation->navpoly_add(navpoly, get_relative_transform_to_parent(navigation), this);
+ Navigation2DServer::get_singleton()->region_set_map(region, navigation->get_rid());
}
break;
}
@@ -395,19 +433,14 @@ void NavigationPolygonInstance::_notification(int p_what) {
} break;
case NOTIFICATION_TRANSFORM_CHANGED: {
- if (navigation && nav_id != -1) {
- navigation->navpoly_set_transform(nav_id, get_relative_transform_to_parent(navigation));
- }
+ Navigation2DServer::get_singleton()->region_set_transform(region, get_global_transform());
} break;
case NOTIFICATION_EXIT_TREE: {
if (navigation) {
- if (nav_id != -1) {
- navigation->navpoly_remove(nav_id);
- nav_id = -1;
- }
+ Navigation2DServer::get_singleton()->region_set_map(region, RID());
}
navigation = NULL;
} break;
@@ -466,24 +499,18 @@ void NavigationPolygonInstance::set_navigation_polygon(const Ref<NavigationPolyg
return;
}
- if (navigation && nav_id != -1) {
- navigation->navpoly_remove(nav_id);
- nav_id = -1;
- }
-
if (navpoly.is_valid()) {
navpoly->disconnect(CoreStringNames::get_singleton()->changed, this, "_navpoly_changed");
}
+
navpoly = p_navpoly;
+ Navigation2DServer::get_singleton()->region_set_navpoly(region, p_navpoly);
+
if (navpoly.is_valid()) {
navpoly->connect(CoreStringNames::get_singleton()->changed, this, "_navpoly_changed");
}
_navpoly_changed();
- if (navigation && navpoly.is_valid() && enabled) {
- nav_id = navigation->navpoly_add(navpoly, get_relative_transform_to_parent(navigation), this);
- }
-
_change_notify("navpoly");
update_configuration_warning();
}
@@ -536,8 +563,13 @@ void NavigationPolygonInstance::_bind_methods() {
NavigationPolygonInstance::NavigationPolygonInstance() {
- navigation = NULL;
- nav_id = -1;
enabled = true;
set_notify_transform(true);
+ region = Navigation2DServer::get_singleton()->region_create();
+
+ navigation = NULL;
+}
+
+NavigationPolygonInstance::~NavigationPolygonInstance() {
+ Navigation2DServer::get_singleton()->free(region);
}
diff --git a/scene/2d/navigation_polygon.h b/scene/2d/navigation_polygon.h
index cbc1711a32..92cfc8f5e6 100644
--- a/scene/2d/navigation_polygon.h
+++ b/scene/2d/navigation_polygon.h
@@ -32,6 +32,9 @@
#define NAVIGATION_POLYGON_H
#include "scene/2d/node_2d.h"
+#include "scene/resources/navigation_mesh.h"
+
+class Mutex;
class NavigationPolygon : public Resource {
@@ -47,6 +50,10 @@ class NavigationPolygon : public Resource {
mutable Rect2 item_rect;
mutable bool rect_cache_dirty;
+ Mutex *navmesh_generation;
+ // Navigation mesh
+ Ref<NavigationMesh> navmesh;
+
protected:
static void _bind_methods();
@@ -81,6 +88,8 @@ public:
Vector<int> get_polygon(int p_idx);
void clear_polygons();
+ Ref<NavigationMesh> get_mesh();
+
NavigationPolygon();
};
@@ -91,7 +100,7 @@ class NavigationPolygonInstance : public Node2D {
GDCLASS(NavigationPolygonInstance, Node2D);
bool enabled;
- int nav_id;
+ RID region;
Navigation2D *navigation;
Ref<NavigationPolygon> navpoly;
@@ -116,6 +125,7 @@ public:
String get_configuration_warning() const;
NavigationPolygonInstance();
+ ~NavigationPolygonInstance();
};
#endif // NAVIGATIONPOLYGON_H
diff --git a/scene/2d/tile_map.cpp b/scene/2d/tile_map.cpp
index b6db025d44..d8b880e571 100644
--- a/scene/2d/tile_map.cpp
+++ b/scene/2d/tile_map.cpp
@@ -35,6 +35,7 @@
#include "core/method_bind_ext.gen.inc"
#include "core/os/os.h"
#include "scene/2d/area_2d.h"
+#include "servers/navigation_2d_server.h"
#include "servers/physics_2d_server.h"
int TileMap::_get_quadrant_size() const {
@@ -86,7 +87,7 @@ void TileMap::_notification(int p_what) {
if (navigation) {
for (Map<PosKey, Quadrant::NavPoly>::Element *F = q.navpoly_ids.front(); F; F = F->next()) {
- navigation->navpoly_remove(F->get().id);
+ Navigation2DServer::get_singleton()->region_set_map(F->get().region, RID());
}
q.navpoly_ids.clear();
}
@@ -163,7 +164,7 @@ void TileMap::_update_quadrant_transform() {
if (navigation) {
for (Map<PosKey, Quadrant::NavPoly>::Element *F = q.navpoly_ids.front(); F; F = F->next()) {
- navigation->navpoly_set_transform(F->get().id, nav_rel * F->get().xform);
+ Navigation2DServer::get_singleton()->region_set_transform(F->get().region, nav_rel * F->get().xform);
}
}
@@ -377,7 +378,7 @@ void TileMap::update_dirty_quadrants() {
if (navigation) {
for (Map<PosKey, Quadrant::NavPoly>::Element *E = q.navpoly_ids.front(); E; E = E->next()) {
- navigation->navpoly_remove(E->get().id);
+ Navigation2DServer::get_singleton()->region_set_map(E->get().region, RID());
}
q.navpoly_ids.clear();
}
@@ -611,10 +612,13 @@ void TileMap::update_dirty_quadrants() {
xform.set_origin(offset.floor() + q.pos);
_fix_cell_transform(xform, c, npoly_ofs, s);
- int pid = navigation->navpoly_add(navpoly, nav_rel * xform);
+ RID region = Navigation2DServer::get_singleton()->region_create();
+ Navigation2DServer::get_singleton()->region_set_map(region, navigation->get_rid());
+ Navigation2DServer::get_singleton()->region_set_transform(region, nav_rel * xform);
+ Navigation2DServer::get_singleton()->region_set_navpoly(region, navpoly);
Quadrant::NavPoly np;
- np.id = pid;
+ np.region = region;
np.xform = xform;
q.navpoly_ids[E->key()] = np;
@@ -809,7 +813,7 @@ void TileMap::_erase_quadrant(Map<PosKey, Quadrant>::Element *Q) {
if (navigation) {
for (Map<PosKey, Quadrant::NavPoly>::Element *E = q.navpoly_ids.front(); E; E = E->next()) {
- navigation->navpoly_remove(E->get().id);
+ Navigation2DServer::get_singleton()->region_set_map(E->get().region, RID());
}
q.navpoly_ids.clear();
}
diff --git a/scene/2d/tile_map.h b/scene/2d/tile_map.h
index 0875d197eb..d5ef7fc818 100644
--- a/scene/2d/tile_map.h
+++ b/scene/2d/tile_map.h
@@ -139,7 +139,7 @@ private:
SelfList<Quadrant> dirty_list;
struct NavPoly {
- int id;
+ RID region;
Transform2D xform;
};
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-23 14:37:19 +0000