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

726 lines
16 KiB
C++
Raw Blame History

#include "navigation.h"
void Navigation::_navmesh_link(int p_id) {
ERR_FAIL_COND(!navmesh_map.has(p_id));
NavMesh &nm=navmesh_map[p_id];
ERR_FAIL_COND(nm.linked);
print_line("LINK");
DVector<Vector3> vertices=nm.navmesh->get_vertices();
int len = vertices.size();
if (len==0)
return;
DVector<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[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);
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));
}
//ERR_CONTINUE(C->get().B!=NULL); //wut
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 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);
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 Navigation::navmesh_create(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;
}
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);
}
void Navigation::navmesh_remove(int p_id){
ERR_FAIL_COND(!navmesh_map.has(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) {
//print_line("No Path Path");
return Vector<Vector3>(); //no path
}
if (begin_poly==end_poly) {
Vector<Vector3> 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;
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;
begin_poly->edges[i].C->distance=begin_poly->center.distance_to(begin_poly->edges[i].C->center);
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
for(int i=0;i<p->edges.size();i++) {
Polygon::Edge &e=p->edges[i];
if (!e.C)
continue;
float distance = p->center.distance_to(e.C->center) + p->distance;
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;
}
} else {
//add to open neighbours
e.C->prev_edge=e.C_edge;
e.C->distance=distance;
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<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;
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;
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;
NavMesh *closest_navmesh=NULL;
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);
closest_navmesh=p.owner;
} else if (closest_point_d > inters.distance_to(p_from)){
closest_point=inters;
closest_point_d=p_from.distance_to(inters);
closest_navmesh=p.owner;
}
}
}
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;
closest_navmesh=p.owner;
}
}
}
}
}
if (closest_navmesh && closest_navmesh->owner) {
//print_line("navmesh is: "+closest_navmesh->owner->cast_to<Node>()->get_name());
}
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;
}
void Navigation::set_up_vector(const Vector3& p_up) {
up=p_up;
}
Vector3 Navigation::get_up_vector() const{
return up;
}
void Navigation::_bind_methods() {
ObjectTypeDB::bind_method(_MD("navmesh_create","mesh:NavigationMesh","xform","owner"),&Navigation::navmesh_create,DEFVAL(Variant()));
ObjectTypeDB::bind_method(_MD("navmesh_set_transform","id","xform"),&Navigation::navmesh_set_transform);
ObjectTypeDB::bind_method(_MD("navmesh_remove","id"),&Navigation::navmesh_remove);
ObjectTypeDB::bind_method(_MD("get_simple_path","start","end","optimize"),&Navigation::get_simple_path,DEFVAL(true));
ObjectTypeDB::bind_method(_MD("get_closest_point_to_segment","start","end","use_collision"),&Navigation::get_closest_point_to_segment,DEFVAL(false));
ObjectTypeDB::bind_method(_MD("get_closest_point","to_point"),&Navigation::get_closest_point);
ObjectTypeDB::bind_method(_MD("get_closest_point_normal","to_point"),&Navigation::get_closest_point_normal);
ObjectTypeDB::bind_method(_MD("get_closest_point_owner","to_point"),&Navigation::get_closest_point_owner);
ObjectTypeDB::bind_method(_MD("set_up_vector","up"),&Navigation::set_up_vector);
ObjectTypeDB::bind_method(_MD("get_up_vector"),&Navigation::get_up_vector);
ADD_PROPERTY( PropertyInfo(Variant::VECTOR3,"up_vector"),_SCS("set_up_vector"),_SCS("get_up_vector"));
}
Navigation::Navigation() {
ERR_FAIL_COND( sizeof(Point)!=8 );
cell_size=0.01; //one centimeter
last_id=1;
up=Vector3(0,1,0);
}
Reference in New Issue View Git Blame Copy Permalink