diff options
Diffstat (limited to 'scene/3d/navigation.cpp')
-rw-r--r-- | scene/3d/navigation.cpp | 706 |
1 files changed, 52 insertions, 654 deletions
diff --git a/scene/3d/navigation.cpp b/scene/3d/navigation.cpp index 4a82fe0080..8c543bc97f 100644 --- a/scene/3d/navigation.cpp +++ b/scene/3d/navigation.cpp @@ -30,669 +30,33 @@ #include "navigation.h" -#define USE_ENTRY_POINT +#include "servers/navigation_server.h" -void Navigation::_navmesh_link(int p_id) { +Vector<Vector3> Navigation::get_simple_path(const Vector3 &p_start, const Vector3 &p_end, bool p_optimize) const { - ERR_FAIL_COND(!navmesh_map.has(p_id)); - NavMesh &nm = navmesh_map[p_id]; - ERR_FAIL_COND(nm.linked); - ERR_FAIL_COND(nm.navmesh.is_null()); - - PoolVector<Vector3> vertices = nm.navmesh->get_vertices(); - int len = vertices.size(); - if (len == 0) - return; - - PoolVector<Vector3>::Read r = vertices.read(); - - for (int i = 0; i < nm.navmesh->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.navmesh->get_polygon(i); - int plen = poly.size(); - const int *indices = poly.ptr(); - bool valid = true; - p.edges.resize(plen); - - Vector3 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; - Vector3 ep = nm.xform.xform(r[idx]); - center += ep; - e.point = _get_point(ep); - p.edges.write[j] = e; - - if (j >= 2) { - Vector3 epa = nm.xform.xform(r[indices[j - 2]]); - Vector3 epb = nm.xform.xform(r[indices[j - 1]]); - - sum += up.dot((epb - epa).cross(ep - epa)); - } - } - - p.clockwise = sum > 0; - - if (!valid) { - nm.polygons.pop_back(); - ERR_CONTINUE(!valid); - } - - p.center = center; - if (plen != 0) { - p.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) { - - Connection c; - c.A = &p; - c.A_edge = j; - c.B = NULL; - c.B_edge = -1; - connections[ek] = c; - } else { - - 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; - } - - 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. - } - } - } - - nm.linked = true; + return NavigationServer::get_singleton()->map_get_path(map, p_start, p_end, p_optimize); } -void Navigation::_navmesh_unlink(int p_id) { - - ERR_FAIL_COND(!navmesh_map.has(p_id)); - NavMesh &nm = navmesh_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 - - 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 - } - } - } - - nm.polygons.clear(); - - nm.linked = false; +Vector3 Navigation::get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, bool p_use_collision) const { + return NavigationServer::get_singleton()->map_get_closest_point_to_segment(map, p_from, p_to, p_use_collision); } -int Navigation::navmesh_add(const Ref<NavigationMesh> &p_mesh, const Transform &p_xform, Object *p_owner) { - - int id = last_id++; - NavMesh nm; - nm.linked = false; - nm.navmesh = p_mesh; - nm.xform = p_xform; - nm.owner = p_owner; - navmesh_map[id] = nm; - - _navmesh_link(id); - - return id; +Vector3 Navigation::get_closest_point(const Vector3 &p_point) const { + return NavigationServer::get_singleton()->map_get_closest_point(map, p_point); } -void Navigation::navmesh_set_transform(int p_id, const Transform &p_xform) { - - ERR_FAIL_COND(!navmesh_map.has(p_id)); - NavMesh &nm = navmesh_map[p_id]; - if (nm.xform == p_xform) - return; //bleh - _navmesh_unlink(p_id); - nm.xform = p_xform; - _navmesh_link(p_id); +Vector3 Navigation::get_closest_point_normal(const Vector3 &p_point) const { + return NavigationServer::get_singleton()->map_get_closest_point_normal(map, p_point); } -void Navigation::navmesh_remove(int p_id) { - ERR_FAIL_COND_MSG(!navmesh_map.has(p_id), "Trying to remove nonexisting navmesh with id: " + itos(p_id)); - _navmesh_unlink(p_id); - navmesh_map.erase(p_id); -} - -void Navigation::_clip_path(Vector<Vector3> &path, Polygon *from_poly, const Vector3 &p_to_point, Polygon *p_to_poly) { - - Vector3 from = path[path.size() - 1]; - - if (from.distance_to(p_to_point) < CMP_EPSILON) - return; - Plane cut_plane; - cut_plane.normal = (from - p_to_point).cross(up); - if (cut_plane.normal == Vector3()) - return; - cut_plane.normal.normalize(); - cut_plane.d = cut_plane.normal.dot(from); - - while (from_poly != p_to_poly) { - - int pe = from_poly->prev_edge; - Vector3 a = _get_vertex(from_poly->edges[pe].point); - Vector3 b = _get_vertex(from_poly->edges[(pe + 1) % from_poly->edges.size()].point); - - from_poly = from_poly->edges[pe].C; - ERR_FAIL_COND(!from_poly); - - if (a.distance_to(b) > CMP_EPSILON) { - - Vector3 inters; - if (cut_plane.intersects_segment(a, b, &inters)) { - if (inters.distance_to(p_to_point) > CMP_EPSILON && inters.distance_to(path[path.size() - 1]) > CMP_EPSILON) { - path.push_back(inters); - } - } - } - } -} - -Vector<Vector3> Navigation::get_simple_path(const Vector3 &p_start, const Vector3 &p_end, bool p_optimize) { - - Polygon *begin_poly = NULL; - Polygon *end_poly = NULL; - Vector3 begin_point; - Vector3 end_point; - float begin_d = 1e20; - float end_d = 1e20; - - for (Map<int, NavMesh>::Element *E = navmesh_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++) { - - Face3 f(_get_vertex(p.edges[0].point), _get_vertex(p.edges[i - 1].point), _get_vertex(p.edges[i].point)); - Vector3 spoint = f.get_closest_point_to(p_start); - float dpoint = spoint.distance_to(p_start); - if (dpoint < begin_d) { - begin_d = dpoint; - begin_poly = &p; - begin_point = spoint; - } - - spoint = f.get_closest_point_to(p_end); - dpoint = spoint.distance_to(p_end); - if (dpoint < end_d) { - end_d = dpoint; - end_poly = &p; - end_point = spoint; - } - } - - p.prev_edge = -1; - } - } - - if (!begin_poly || !end_poly) { - - return Vector<Vector3>(); //no path - } - - if (begin_poly == end_poly) { - - Vector<Vector3> path; - path.resize(2); - path.write[0] = begin_point; - path.write[1] = end_point; - return path; - } - - bool found_route = false; - - List<Polygon *> open_list; - - 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 - 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_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); - } - } - - 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 - cost += p->entry.distance_to(end_point); -#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 - - 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]; - - 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? - - if (e.C->distance > distance) { - - 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); - } - } - - open_list.erase(least_cost_poly); - } - - if (found_route) { - - Vector<Vector3> path; - - if (p_optimize) { - //string pulling - - Polygon *apex_poly = end_poly; - Vector3 apex_point = end_point; - Vector3 portal_left = apex_point; - Vector3 portal_right = apex_point; - Polygon *left_poly = end_poly; - Polygon *right_poly = end_poly; - Polygon *p = end_poly; - path.push_back(end_point); - - while (p) { - - Vector3 left; - Vector3 right; - -#define CLOCK_TANGENT(m_a, m_b, m_c) (((m_a) - (m_c)).cross((m_a) - (m_b))) - - 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 (CLOCK_TANGENT(apex_point,left,(left+right)*0.5).dot(up) < 0){ - if (p->clockwise) { - SWAP(left, right); - } - } - - bool skip = false; - - if (CLOCK_TANGENT(apex_point, portal_left, left).dot(up) >= 0) { - //process - if (portal_left == apex_point || CLOCK_TANGENT(apex_point, left, portal_right).dot(up) > 0) { - left_poly = p; - portal_left = left; - } else { - - _clip_path(path, apex_poly, portal_right, right_poly); - - apex_point = portal_right; - p = right_poly; - left_poly = p; - apex_poly = p; - portal_left = apex_point; - portal_right = apex_point; - path.push_back(apex_point); - skip = true; - } - } - - if (!skip && CLOCK_TANGENT(apex_point, portal_right, right).dot(up) <= 0) { - //process - if (portal_right == apex_point || CLOCK_TANGENT(apex_point, right, portal_left).dot(up) < 0) { - right_poly = p; - portal_right = right; - } else { - - _clip_path(path, apex_poly, portal_left, left_poly); - - apex_point = portal_left; - p = left_poly; - right_poly = p; - apex_poly = p; - portal_right = apex_point; - portal_left = apex_point; - path.push_back(apex_point); - } - } - - if (p != begin_poly) - p = p->edges[p->prev_edge].C; - else - p = NULL; - } - - if (path[path.size() - 1] != begin_point) - path.push_back(begin_point); - - path.invert(); - - } else { - //midpoints - Polygon *p = end_poly; - - 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) - break; - } - - path.push_back(begin_point); - - path.invert(); - } - - return path; - } - - return Vector<Vector3>(); -} - -Vector3 Navigation::get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, const bool &p_use_collision) { - - bool use_collision = p_use_collision; - Vector3 closest_point; - float closest_point_d = 1e20; - - for (Map<int, NavMesh>::Element *E = navmesh_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++) { - - Face3 f(_get_vertex(p.edges[0].point), _get_vertex(p.edges[i - 1].point), _get_vertex(p.edges[i].point)); - Vector3 inters; - if (f.intersects_segment(p_from, p_to, &inters)) { - - if (!use_collision) { - closest_point = inters; - use_collision = true; - closest_point_d = p_from.distance_to(inters); - } else if (closest_point_d > inters.distance_to(p_from)) { - - closest_point = inters; - closest_point_d = p_from.distance_to(inters); - } - } - } - - if (!use_collision) { - - for (int i = 0; i < p.edges.size(); i++) { - - Vector3 a, b; - - Geometry::get_closest_points_between_segments(p_from, p_to, _get_vertex(p.edges[i].point), _get_vertex(p.edges[(i + 1) % p.edges.size()].point), a, b); - - float d = a.distance_to(b); - if (d < closest_point_d) { - - closest_point_d = d; - closest_point = b; - } - } - } - } - } - - return closest_point; -} - -Vector3 Navigation::get_closest_point(const Vector3 &p_point) { - - Vector3 closest_point; - float closest_point_d = 1e20; - - for (Map<int, NavMesh>::Element *E = navmesh_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++) { - - Face3 f(_get_vertex(p.edges[0].point), _get_vertex(p.edges[i - 1].point), _get_vertex(p.edges[i].point)); - Vector3 inters = f.get_closest_point_to(p_point); - float d = inters.distance_to(p_point); - if (d < closest_point_d) { - closest_point = inters; - closest_point_d = d; - } - } - } - } - - return closest_point; -} - -Vector3 Navigation::get_closest_point_normal(const Vector3 &p_point) { - - Vector3 closest_point; - Vector3 closest_normal; - float closest_point_d = 1e20; - - for (Map<int, NavMesh>::Element *E = navmesh_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++) { - - Face3 f(_get_vertex(p.edges[0].point), _get_vertex(p.edges[i - 1].point), _get_vertex(p.edges[i].point)); - Vector3 inters = f.get_closest_point_to(p_point); - float d = inters.distance_to(p_point); - if (d < closest_point_d) { - closest_point = inters; - closest_point_d = d; - closest_normal = f.get_plane().normal; - } - } - } - } - - return closest_normal; -} - -Object *Navigation::get_closest_point_owner(const Vector3 &p_point) { - - Vector3 closest_point; - Object *owner = NULL; - float closest_point_d = 1e20; - - for (Map<int, NavMesh>::Element *E = navmesh_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++) { - - Face3 f(_get_vertex(p.edges[0].point), _get_vertex(p.edges[i - 1].point), _get_vertex(p.edges[i].point)); - Vector3 inters = f.get_closest_point_to(p_point); - float d = inters.distance_to(p_point); - if (d < closest_point_d) { - closest_point = inters; - closest_point_d = d; - owner = E->get().owner; - } - } - } - } - - return owner; +RID Navigation::get_closest_point_owner(const Vector3 &p_point) const { + return NavigationServer::get_singleton()->map_get_closest_point_owner(map, p_point); } void Navigation::set_up_vector(const Vector3 &p_up) { up = p_up; + NavigationServer::get_singleton()->map_set_up(map, up); } Vector3 Navigation::get_up_vector() const { @@ -700,11 +64,19 @@ Vector3 Navigation::get_up_vector() const { return up; } +void Navigation::set_cell_size(float p_cell_size) { + cell_size = p_cell_size; + NavigationServer::get_singleton()->map_set_cell_size(map, cell_size); +} + +void Navigation::set_edge_connection_margin(float p_edge_connection_margin) { + edge_connection_margin = p_edge_connection_margin; + NavigationServer::get_singleton()->map_set_edge_connection_margin(map, edge_connection_margin); +} + void Navigation::_bind_methods() { - ClassDB::bind_method(D_METHOD("navmesh_add", "mesh", "xform", "owner"), &Navigation::navmesh_add, DEFVAL(Variant())); - ClassDB::bind_method(D_METHOD("navmesh_set_transform", "id", "xform"), &Navigation::navmesh_set_transform); - ClassDB::bind_method(D_METHOD("navmesh_remove", "id"), &Navigation::navmesh_remove); + ClassDB::bind_method(D_METHOD("get_rid"), &Navigation::get_rid); ClassDB::bind_method(D_METHOD("get_simple_path", "start", "end", "optimize"), &Navigation::get_simple_path, DEFVAL(true)); ClassDB::bind_method(D_METHOD("get_closest_point_to_segment", "start", "end", "use_collision"), &Navigation::get_closest_point_to_segment, DEFVAL(false)); @@ -715,13 +87,39 @@ void Navigation::_bind_methods() { ClassDB::bind_method(D_METHOD("set_up_vector", "up"), &Navigation::set_up_vector); ClassDB::bind_method(D_METHOD("get_up_vector"), &Navigation::get_up_vector); + ClassDB::bind_method(D_METHOD("set_cell_size", "cell_size"), &Navigation::set_cell_size); + ClassDB::bind_method(D_METHOD("get_cell_size"), &Navigation::get_cell_size); + + ClassDB::bind_method(D_METHOD("set_edge_connection_margin", "margin"), &Navigation::set_edge_connection_margin); + ClassDB::bind_method(D_METHOD("get_edge_connection_margin"), &Navigation::get_edge_connection_margin); + ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "up_vector"), "set_up_vector", "get_up_vector"); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "cell_size"), "set_cell_size", "get_cell_size"); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "edge_connection_margin"), "set_edge_connection_margin", "get_edge_connection_margin"); +} + +void Navigation::_notification(int p_what) { + switch (p_what) { + case NOTIFICATION_READY: { + NavigationServer::get_singleton()->map_set_active(map, true); + } break; + case NOTIFICATION_EXIT_TREE: { + + NavigationServer::get_singleton()->map_set_active(map, false); + } break; + } } Navigation::Navigation() { - ERR_FAIL_COND(sizeof(Point) != 8); - cell_size = 0.01; //one centimeter - last_id = 1; + map = NavigationServer::get_singleton()->map_create(); + + set_cell_size(0.3); + set_edge_connection_margin(5.0); // Five meters, depends alot on the agents radius + up = Vector3(0, 1, 0); } + +Navigation::~Navigation() { + NavigationServer::get_singleton()->free(map); +} |