godotengine
/
godot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
godot/scene/2d/navigation2d.cpp

782 lines
20 KiB
C++
Raw Blame History

/*************************************************************************/
/* navigation2d.cpp */
/*************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/*************************************************************************/
/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */
/* Copyright (c) 2014-2019 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 "navigation2d.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);
DVector<Vector2> vertices = nm.navpoly->get_vertices();
int len = vertices.size();
if (len == 0)
return;
DVector<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[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);
continue;
}
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) {
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[j].P = C->get().pending.push_back(pending);
continue;
//print_line(String()+_get_vertex(ek.a)+" -> "+_get_vertex(ek.b));
}
C->get().B = &p;
C->get().B_edge = j;
C->get().A->edges[C->get().A_edge].C = &p;
C->get().A->edges[C->get().A_edge].C_edge = j;
p.edges[j].C = C->get().A;
p.edges[j].C_edge = C->get().A_edge;
//connection successful.
}
}
}
nm.linked = true;
}
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);
//print_line("UNLINK");
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.ptr();
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[C->get().B_edge].C = NULL;
C->get().B->edges[C->get().B_edge].C_edge = -1;
C->get().A->edges[C->get().A_edge].C = NULL;
C->get().A->edges[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[C->get().A_edge].C = cp.polygon;
C->get().A->edges[C->get().A_edge].C_edge = cp.edge;
cp.polygon->edges[cp.edge].C = C->get().A;
cp.polygon->edges[cp.edge].C_edge = C->get().A_edge;
cp.polygon->edges[cp.edge].P = NULL;
}
} else {
connections.erase(C);
//erase
}
}
}
nm.polygons.clear();
nm.linked = false;
}
int Navigation2D::navpoly_create(const Ref<NavigationPolygon> &p_mesh, const Matrix32 &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 Matrix32 &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::navpoly_remove(int p_id) {
ERR_FAIL_COND(!navpoly_map.has(p_id));
_navpoly_unlink(p_id);
navpoly_map.erase(p_id);
}
#if 0
void Navigation2D::_clip_path(Vector<Vector2>& path, Polygon *from_poly, const Vector2& p_to_point, Polygon* p_to_poly) {
Vector2 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==Vector2())
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;
Vector2 a = _get_vertex(from_poly->edges[pe].point);
Vector2 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) {
Vector2 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);
}
}
}
}
}
#endif
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[0] = begin_point;
path[1] = end_point;
//print_line("Direct Path");
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) {
// print_line("NOU OPEN LIST");
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;
cost += p->center.distance_to(end_point);
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[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 0
debug path
{
Polygon *p=end_poly;
int idx=0;
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;
String points;
for(int i=0;i<p->edges.size();i++) {
if (i>0)
points+=", ";
points+=_get_vertex(p->edges[i].point);
}
//print_line("poly "+itos(idx++)+" - "+points);
p = p->edges[prev].C;
if (p==begin_poly)
break;
}
}
#endif
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;
path.push_back(end_point);
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.distance_squared_to(apex_point) < CMP_EPSILON || CLOCK_TANGENT(apex_point, left, portal_right) > 0) {
left_poly = p;
portal_left = left;
//print_line("***ADVANCE LEFT");
} else {
apex_point = portal_right;
p = right_poly;
left_poly = p;
portal_left = apex_point;
portal_right = apex_point;
if (path[path.size() - 1].distance_to(apex_point) > CMP_EPSILON)
path.push_back(apex_point);
skip = true;
//print_line("addpoint left");
//print_line("***CLIP LEFT");
}
}
if (!skip && CLOCK_TANGENT(apex_point, portal_right, right) <= 0) {
//process
if (portal_right.distance_squared_to(apex_point) < CMP_EPSILON || CLOCK_TANGENT(apex_point, right, portal_left) < 0) {
right_poly = p;
portal_right = right;
//print_line("***ADVANCE RIGHT");
} else {
apex_point = portal_left;
p = left_poly;
right_poly = p;
portal_right = apex_point;
portal_left = apex_point;
if (path[path.size() - 1].distance_to(apex_point) > CMP_EPSILON)
path.push_back(apex_point);
//print_line("addpoint right");
//print_line("***CLIP RIGHT");
}
}
if (p != begin_poly)
p = p->edges[p->prev_edge].C;
else
p = NULL;
}
if (path[path.size() - 1].distance_to(begin_point) > CMP_EPSILON)
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;
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[path.size() - 1].distance_to(begin_point) > CMP_EPSILON)
path.push_back(begin_point);
path.invert();
}
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))) {
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() {
ObjectTypeDB::bind_method(_MD("navpoly_create", "mesh:NavigationPolygon", "xform", "owner"), &Navigation2D::navpoly_create, DEFVAL(Variant()));
ObjectTypeDB::bind_method(_MD("navpoly_set_transform", "id", "xform"), &Navigation2D::navpoly_set_transform);
ObjectTypeDB::bind_method(_MD("navpoly_remove", "id"), &Navigation2D::navpoly_remove);
ObjectTypeDB::bind_method(_MD("get_simple_path", "start", "end", "optimize"), &Navigation2D::get_simple_path, DEFVAL(true));
ObjectTypeDB::bind_method(_MD("get_closest_point", "to_point"), &Navigation2D::get_closest_point);
ObjectTypeDB::bind_method(_MD("get_closest_point_owner", "to_point"), &Navigation2D::get_closest_point_owner);
}
Navigation2D::Navigation2D() {
ERR_FAIL_COND(sizeof(Point) != 8);
cell_size = 1; // one pixel
last_id = 1;
}
Reference in New Issue View Git Blame Copy Permalink