summaryrefslogtreecommitdiff
path: root/scene/3d/navigation.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'scene/3d/navigation.cpp')
-rw-r--r--scene/3d/navigation.cpp577
1 files changed, 268 insertions, 309 deletions
diff --git a/scene/3d/navigation.cpp b/scene/3d/navigation.cpp
index 5c8fe6bb67..093d217cb0 100644
--- a/scene/3d/navigation.cpp
+++ b/scene/3d/navigation.cpp
@@ -31,60 +31,58 @@
void Navigation::_navmesh_link(int p_id) {
ERR_FAIL_COND(!navmesh_map.has(p_id));
- NavMesh &nm=navmesh_map[p_id];
+ NavMesh &nm = navmesh_map[p_id];
ERR_FAIL_COND(nm.linked);
print_line("LINK");
- PoolVector<Vector3> vertices=nm.navmesh->get_vertices();
+ PoolVector<Vector3> vertices = nm.navmesh->get_vertices();
int len = vertices.size();
- if (len==0)
+ if (len == 0)
return;
- PoolVector<Vector3>::Read r=vertices.read();
+ PoolVector<Vector3>::Read r = vertices.read();
- for(int i=0;i<nm.navmesh->get_polygon_count();i++) {
+ 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;
+ 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;
+ int plen = poly.size();
+ const int *indices = poly.ptr();
+ bool valid = true;
p.edges.resize(plen);
Vector3 center;
- float sum=0;
+ float sum = 0;
- for(int j=0;j<plen;j++) {
+ for (int j = 0; j < plen; j++) {
int idx = indices[j];
- if (idx<0 || idx>=len) {
- valid=false;
+ 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;
+ 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));
+ 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;
+ p.clockwise = sum > 0;
if (!valid) {
nm.polygons.pop_back();
@@ -93,110 +91,106 @@ void Navigation::_navmesh_link(int p_id) {
}
p.center = center;
- if( plen != 0 ) {
+ if (plen != 0) {
p.center /= plen;
}
-
//connect
- for(int j=0;j<plen;j++) {
+ for (int j = 0; j < plen; j++) {
- int next = (j+1)%plen;
- EdgeKey ek(p.edges[j].point,p.edges[next].point);
+ int next = (j + 1) % plen;
+ EdgeKey ek(p.edges[j].point, p.edges[next].point);
- Map<EdgeKey,Connection>::Element *C=connections.find(ek);
+ 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;
+ c.A = &p;
+ c.A_edge = j;
+ c.B = NULL;
+ c.B_edge = -1;
+ connections[ek] = c;
} else {
- if (C->get().B!=NULL) {
+ if (C->get().B != NULL) {
ConnectionPending pending;
- pending.polygon=&p;
- pending.edge=j;
- p.edges[j].P=C->get().pending.push_back(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;
+ 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;
-
+ nm.linked = true;
}
-
void Navigation::_navmesh_unlink(int p_id) {
ERR_FAIL_COND(!navmesh_map.has(p_id));
- NavMesh &nm=navmesh_map[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()) {
-
+ for (List<Polygon>::Element *E = nm.polygons.front(); E; E = E->next()) {
- Polygon &p=E->get();
+ Polygon &p = E->get();
int ec = p.edges.size();
- Polygon::Edge *edges=p.edges.ptr();
+ Polygon::Edge *edges = p.edges.ptr();
- for(int i=0;i<ec;i++) {
- int next = (i+1)%ec;
+ 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);
+ 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;
+ 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;
+ 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()) {
+ if (C->get().A == &E->get()) {
- C->get().A=C->get().B;
- C->get().A_edge=C->get().B_edge;
+ C->get().A = C->get().B;
+ C->get().A_edge = C->get().B_edge;
}
- C->get().B=NULL;
- C->get().B_edge=-1;
+ 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;
+ 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 {
@@ -208,76 +202,68 @@ void Navigation::_navmesh_unlink(int p_id) {
nm.polygons.clear();
- nm.linked=false;
-
-
+ nm.linked = false;
}
-
-int Navigation::navmesh_create(const Ref<NavigationMesh>& p_mesh, const Transform& p_xform, Object *p_owner) {
+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;
+ 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){
+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)
+ NavMesh &nm = navmesh_map[p_id];
+ if (nm.xform == p_xform)
return; //bleh
_navmesh_unlink(p_id);
- nm.xform=p_xform;
+ nm.xform = p_xform;
_navmesh_link(p_id);
-
-
-
}
-void Navigation::navmesh_remove(int 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) {
+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];
+ Vector3 from = path[path.size() - 1];
- if (from.distance_to(p_to_point)<CMP_EPSILON)
+ 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())
+ 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) {
+ 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);
+ Vector3 b = _get_vertex(from_poly->edges[(pe + 1) % from_poly->edges.size()].point);
- from_poly=from_poly->edges[pe].C;
+ from_poly = from_poly->edges[pe].C;
ERR_FAIL_COND(!from_poly);
- if (a.distance_to(b)>CMP_EPSILON) {
+ 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) {
+ 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);
}
}
@@ -285,149 +271,138 @@ void Navigation::_clip_path(Vector<Vector3>& path, Polygon *from_poly, const Vec
}
}
-Vector<Vector3> Navigation::get_simple_path(const Vector3& p_start, const Vector3& p_end, bool p_optimize) {
+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;
+ Polygon *begin_poly = NULL;
+ Polygon *end_poly = NULL;
Vector3 begin_point;
Vector3 end_point;
- float begin_d=1e20;
- float end_d=1e20;
-
+ float begin_d = 1e20;
+ float end_d = 1e20;
- for (Map<int,NavMesh>::Element*E=navmesh_map.front();E;E=E->next()) {
+ 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()) {
+ 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++) {
+ 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));
+ 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;
+ 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;
+ if (dpoint < end_d) {
+ end_d = dpoint;
+ end_poly = &p;
+ end_point = spoint;
}
}
- p.prev_edge=-1;
+ p.prev_edge = -1;
}
}
-
if (!begin_poly || !end_poly) {
//print_line("No Path Path");
return Vector<Vector3>(); //no path
}
-
- if (begin_poly==end_poly) {
+ if (begin_poly == end_poly) {
Vector<Vector3> path;
path.resize(2);
- path[0]=begin_point;
- path[1]=end_point;
+ path[0] = begin_point;
+ path[1] = end_point;
//print_line("Direct Path");
return path;
}
+ bool found_route = false;
- bool found_route=false;
-
- List<Polygon*> open_list;
+ List<Polygon *> open_list;
- for(int i=0;i<begin_poly->edges.size();i++) {
+ 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);
+ 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;
+ if (begin_poly->edges[i].C == end_poly) {
+ found_route = true;
}
}
}
+ while (!found_route) {
- while(!found_route) {
-
- if (open_list.size()==0) {
+ 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;
+ 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();
+ 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);
+ float cost = p->distance;
+ cost += p->center.distance_to(end_point);
- if (cost<least_cost) {
+ if (cost < least_cost) {
- least_cost_poly=E;
- least_cost=cost;
+ least_cost_poly = E;
+ least_cost = cost;
}
}
-
- Polygon *p=least_cost_poly->get();
+ Polygon *p = least_cost_poly->get();
//open the neighbours for search
- for(int i=0;i<p->edges.size();i++) {
-
+ for (int i = 0; i < p->edges.size(); i++) {
- Polygon::Edge &e=p->edges[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) {
+ if (e.C->prev_edge != -1) {
//oh this was visited already, can we win the cost?
- if (e.C->distance>distance) {
+ if (e.C->distance > distance) {
- e.C->prev_edge=e.C_edge;
- 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;
+ e.C->prev_edge = e.C_edge;
+ e.C->distance = distance;
open_list.push_back(e.C);
- if (e.C==end_poly) {
+ if (e.C == end_poly) {
//oh my reached end! stop algorithm
- found_route=true;
+ found_route = true;
break;
-
}
-
}
}
@@ -444,174 +419,164 @@ Vector<Vector3> Navigation::get_simple_path(const Vector3& p_start, const Vector
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;
+ 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) {
+ while (p) {
Vector3 left;
Vector3 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) - (m_c)).cross((m_a) - (m_b)))
- if (p==begin_poly) {
- left=begin_point;
- right=begin_point;
+ 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();
+ 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);
+ SWAP(left, right);
}
}
- bool skip=false;
-
+ bool skip = false;
- if (CLOCK_TANGENT(apex_point,portal_left,left).dot(up) >= 0){
+ 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;
+ 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);
+ _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;
+ 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;
+ skip = true;
}
}
- if (!skip && CLOCK_TANGENT(apex_point,portal_right,right).dot(up) <= 0){
+ 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;
+ 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);
+ _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;
+ 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;
+ if (p != begin_poly)
+ p = p->edges[p->prev_edge].C;
else
- p=NULL;
-
+ p = NULL;
}
- if (path[path.size()-1]!=begin_point)
+ if (path[path.size() - 1] != begin_point)
path.push_back(begin_point);
path.invert();
-
-
-
} else {
//midpoints
- Polygon *p=end_poly;
+ Polygon *p = end_poly;
path.push_back(end_point);
- while(true) {
+ 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;
+ 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)
+ 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) {
-
+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;
+ bool use_collision = p_use_collision;
Vector3 closest_point;
- float closest_point_d=1e20;
- NavMesh *closest_navmesh=NULL;
+ float closest_point_d = 1e20;
+ NavMesh *closest_navmesh = NULL;
- for (Map<int,NavMesh>::Element*E=navmesh_map.front();E;E=E->next()) {
+ 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()) {
+ 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++) {
+ 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));
+ 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 (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;
+ 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++) {
+ for (int i = 0; i < p.edges.size(); i++) {
- Vector3 a,b;
+ 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);
+ 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) {
+ if (d < closest_point_d) {
- closest_point_d=d;
- closest_point=b;
- closest_navmesh=p.owner;
+ closest_point_d = d;
+ closest_point = b;
+ closest_navmesh = p.owner;
}
-
}
}
}
@@ -624,132 +589,126 @@ Vector3 Navigation::get_closest_point_to_segment(const Vector3& p_from,const Vec
return closest_point;
}
-Vector3 Navigation::get_closest_point(const Vector3& p_point) {
+Vector3 Navigation::get_closest_point(const Vector3 &p_point) {
Vector3 closest_point;
- float closest_point_d=1e20;
+ float closest_point_d = 1e20;
- for (Map<int,NavMesh>::Element*E=navmesh_map.front();E;E=E->next()) {
+ 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()) {
+ 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++) {
+ 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));
+ 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;
+ 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 Navigation::get_closest_point_normal(const Vector3 &p_point) {
Vector3 closest_point;
Vector3 closest_normal;
- float closest_point_d=1e20;
+ float closest_point_d = 1e20;
- for (Map<int,NavMesh>::Element*E=navmesh_map.front();E;E=E->next()) {
+ 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()) {
+ 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++) {
+ 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));
+ 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;
+ 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){
+Object *Navigation::get_closest_point_owner(const Vector3 &p_point) {
Vector3 closest_point;
- Object *owner=NULL;
- float closest_point_d=1e20;
+ Object *owner = NULL;
+ float closest_point_d = 1e20;
- for (Map<int,NavMesh>::Element*E=navmesh_map.front();E;E=E->next()) {
+ 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()) {
+ 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++) {
+ 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));
+ 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;
+ 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) {
+void Navigation::set_up_vector(const Vector3 &p_up) {
-
- up=p_up;
+ up = p_up;
}
-Vector3 Navigation::get_up_vector() const{
+Vector3 Navigation::get_up_vector() const {
return up;
}
-
void Navigation::_bind_methods() {
- ClassDB::bind_method(D_METHOD("navmesh_create","mesh:NavigationMesh","xform","owner"),&Navigation::navmesh_create,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("navmesh_create", "mesh:NavigationMesh", "xform", "owner"), &Navigation::navmesh_create, 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_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));
- ClassDB::bind_method(D_METHOD("get_closest_point","to_point"),&Navigation::get_closest_point);
- ClassDB::bind_method(D_METHOD("get_closest_point_normal","to_point"),&Navigation::get_closest_point_normal);
- ClassDB::bind_method(D_METHOD("get_closest_point_owner","to_point"),&Navigation::get_closest_point_owner);
+ 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));
+ ClassDB::bind_method(D_METHOD("get_closest_point", "to_point"), &Navigation::get_closest_point);
+ ClassDB::bind_method(D_METHOD("get_closest_point_normal", "to_point"), &Navigation::get_closest_point_normal);
+ ClassDB::bind_method(D_METHOD("get_closest_point_owner", "to_point"), &Navigation::get_closest_point_owner);
- 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_up_vector", "up"), &Navigation::set_up_vector);
+ ClassDB::bind_method(D_METHOD("get_up_vector"), &Navigation::get_up_vector);
- ADD_PROPERTY( PropertyInfo(Variant::VECTOR3,"up_vector"),"set_up_vector","get_up_vector");
+ ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "up_vector"), "set_up_vector", "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);
+ ERR_FAIL_COND(sizeof(Point) != 8);
+ cell_size = 0.01; //one centimeter
+ last_id = 1;
+ up = Vector3(0, 1, 0);
}
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-22 04:28:19 +0000