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;
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.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.distance_squared_to(apex_point) < CMP_EPSILON || 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].distance_to(apex_point) > CMP_EPSILON)
path.push_back(apex_point);
skip = true;
}
}
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;
} 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].distance_to(apex_point) > CMP_EPSILON)
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].distance_squared_to(begin_point) > CMP_EPSILON) {
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].distance_squared_to(end_point) > CMP_EPSILON) {
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>();
}
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>();
}