mirror of
https://github.com/godotengine/godot
synced 2024-09-05 20:09:26 +00:00
parent
46b6fb83ef
commit
f1b7b74d65
|
@ -340,16 +340,12 @@ Vector<Vector3> Navigation::get_simple_path(const Vector3 &p_start, const Vector
|
|||
};
|
||||
|
||||
Vector3 entry = Geometry::get_closest_point_to_segment(begin_poly->entry, edge);
|
||||
begin_poly->edges[i].C->distance = begin_poly->entry.distance_to(entry);
|
||||
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);
|
||||
|
||||
if (begin_poly->edges[i].C == end_poly) {
|
||||
found_route = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -370,28 +366,7 @@ Vector<Vector3> Navigation::get_simple_path(const Vector3 &p_start, const Vector
|
|||
|
||||
float cost = p->distance;
|
||||
#ifdef USE_ENTRY_POINT
|
||||
int es = p->edges.size();
|
||||
|
||||
float shortest_distance = 1e30;
|
||||
|
||||
for (int i = 0; i < es; i++) {
|
||||
Polygon::Edge &e = p->edges.write[i];
|
||||
|
||||
if (!e.C)
|
||||
continue;
|
||||
|
||||
Vector3 edge[2] = {
|
||||
_get_vertex(p->edges[i].point),
|
||||
_get_vertex(p->edges[(i + 1) % es].point)
|
||||
};
|
||||
|
||||
Vector3 edge_point = Geometry::get_closest_point_to_segment(p->entry, edge);
|
||||
float dist = p->entry.distance_to(edge_point);
|
||||
if (dist < shortest_distance)
|
||||
shortest_distance = dist;
|
||||
}
|
||||
|
||||
cost += shortest_distance;
|
||||
cost += p->entry.distance_to(end_point);
|
||||
#else
|
||||
cost += p->center.distance_to(end_point);
|
||||
#endif
|
||||
|
@ -404,6 +379,12 @@ Vector<Vector3> Navigation::get_simple_path(const Vector3 &p_start, const Vector
|
|||
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];
|
||||
|
@ -411,7 +392,17 @@ Vector<Vector3> Navigation::get_simple_path(const Vector3 &p_start, const Vector
|
|||
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?
|
||||
|
@ -420,25 +411,22 @@ Vector<Vector3> Navigation::get_simple_path(const Vector3 &p_start, const Vector
|
|||
|
||||
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);
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
|
@ -539,8 +527,12 @@ Vector<Vector3> Navigation::get_simple_path(const Vector3 &p_start, const Vector
|
|||
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)
|
||||
|
|
Loading…
Reference in a new issue