asgardius/virtualx-engine
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

NavMap Fix polygons being treated like triangle strips instead of triangle fans

Browse source
This commit is contained in:
kleonc 2022-02-13 16:07:01 +01:00 committed by Pawel Lampe
parent 9bb6f9576d
commit ff06d0978a
3 changed files with 33 additions and 69 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

94
modules/navigation/nav_map.cpp
View file

@ -97,12 +97,9 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
for (size_t i(0); i < polygons.size(); i++) { for (size_t i(0); i < polygons.size(); i++) {
const gd::Polygon &p = polygons[i]; const gd::Polygon &p = polygons[i];
// For each point cast a face and check the distance between the origin/destination // For each face check the distance between the origin/destination
for (size_t point_id = 0; point_id < p.points.size(); point_id++) { for (size_t point_id = 2; point_id < p.points.size(); point_id++) {
const Vector3 p1 = p.points[point_id].pos; const Face3 face(p.points[0].pos, p.points[point_id - 1].pos, p.points[point_id].pos);
const Vector3 p2 = p.points[(point_id + 1) % p.points.size()].pos;
const Vector3 p3 = p.points[(point_id + 2) % p.points.size()].pos;
const Face3 face(p1, p2, p3);
Vector3 spoint = face.get_closest_point_to(p_origin); Vector3 spoint = face.get_closest_point_to(p_origin);
float dpoint = spoint.distance_to(p_origin); float dpoint = spoint.distance_to(p_origin);
@ -229,7 +226,7 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
end_poly = reachable_end; end_poly = reachable_end;
end_d = 1e20; end_d = 1e20;
for (size_t point_id = 2; point_id < end_poly->points.size(); point_id++) { for (size_t point_id = 2; point_id < end_poly->points.size(); point_id++) {
Face3 f(end_poly->points[point_id - 2].pos, end_poly->points[point_id - 1].pos, end_poly->points[point_id].pos); Face3 f(end_poly->points[0].pos, end_poly->points[point_id - 1].pos, end_poly->points[point_id].pos);
Vector3 spoint = f.get_closest_point_to(p_destination); Vector3 spoint = f.get_closest_point_to(p_destination);
float dpoint = spoint.distance_to(p_destination); float dpoint = spoint.distance_to(p_destination);
if (dpoint < end_d) { if (dpoint < end_d) {
@ -405,13 +402,12 @@ Vector3 NavMap::get_closest_point_to_segment(const Vector3 &p_from, const Vector
Vector3 closest_point; Vector3 closest_point;
real_t closest_point_d = 1e20; real_t closest_point_d = 1e20;
// Find the initial poly and the end poly on this map.
for (size_t i(0); i < polygons.size(); i++) { for (size_t i(0); i < polygons.size(); i++) {
const gd::Polygon &p = polygons[i]; const gd::Polygon &p = polygons[i];
// For each point cast a face and check the distance to the segment // For each face check the distance to the segment
for (size_t point_id = 2; point_id < p.points.size(); point_id += 1) { for (size_t point_id = 2; point_id < p.points.size(); point_id += 1) {
const Face3 f(p.points[point_id - 2].pos, p.points[point_id - 1].pos, p.points[point_id].pos); const Face3 f(p.points[0].pos, p.points[point_id - 1].pos, p.points[point_id].pos);
Vector3 inters; Vector3 inters;
if (f.intersects_segment(p_from, p_to, &inters)) { if (f.intersects_segment(p_from, p_to, &inters)) {
const real_t d = closest_point_d = p_from.distance_to(inters); const real_t d = closest_point_d = p_from.distance_to(inters);
@ -451,82 +447,42 @@ Vector3 NavMap::get_closest_point_to_segment(const Vector3 &p_from, const Vector
} }
Vector3 NavMap::get_closest_point(const Vector3 &p_point) const { Vector3 NavMap::get_closest_point(const Vector3 &p_point) const {
// TODO this is really not optimal, please redesign the API to directly return all this data gd::ClosestPointQueryResult cp = get_closest_point_info(p_point);
return cp.point;
Vector3 closest_point;
real_t closest_point_d = 1e20;
// Find the initial poly and the end poly on this map.
for (size_t i(0); i < polygons.size(); i++) {
const gd::Polygon &p = polygons[i];
// For each point cast a face and check the distance to the point
for (size_t point_id = 2; point_id < p.points.size(); point_id += 1) {
const Face3 f(p.points[point_id - 2].pos, p.points[point_id - 1].pos, p.points[point_id].pos);
const Vector3 inters = f.get_closest_point_to(p_point);
const real_t d = inters.distance_to(p_point);
if (d < closest_point_d) {
closest_point = inters;
closest_point_d = d;
}
}
}
return closest_point;
} }
Vector3 NavMap::get_closest_point_normal(const Vector3 &p_point) const { Vector3 NavMap::get_closest_point_normal(const Vector3 &p_point) const {
// TODO this is really not optimal, please redesign the API to directly return all this data gd::ClosestPointQueryResult cp = get_closest_point_info(p_point);
return cp.normal;
Vector3 closest_point;
Vector3 closest_point_normal;
real_t closest_point_d = 1e20;
// Find the initial poly and the end poly on this map.
for (size_t i(0); i < polygons.size(); i++) {
const gd::Polygon &p = polygons[i];
// For each point cast a face and check the distance to the point
for (size_t point_id = 2; point_id < p.points.size(); point_id += 1) {
const Face3 f(p.points[point_id - 2].pos, p.points[point_id - 1].pos, p.points[point_id].pos);
const Vector3 inters = f.get_closest_point_to(p_point);
const real_t d = inters.distance_to(p_point);
if (d < closest_point_d) {
closest_point = inters;
closest_point_normal = f.get_plane().normal;
closest_point_d = d;
}
}
}
return closest_point_normal;
} }
RID NavMap::get_closest_point_owner(const Vector3 &p_point) const { RID NavMap::get_closest_point_owner(const Vector3 &p_point) const {
// TODO this is really not optimal, please redesign the API to directly return all this data gd::ClosestPointQueryResult cp = get_closest_point_info(p_point);
return cp.owner;
}
Vector3 closest_point; gd::ClosestPointQueryResult NavMap::get_closest_point_info(const Vector3 &p_point) const {
RID closest_point_owner; gd::ClosestPointQueryResult result;
real_t closest_point_d = 1e20; real_t closest_point_ds = 1e20;
// Find the initial poly and the end poly on this map.
for (size_t i(0); i < polygons.size(); i++) { for (size_t i(0); i < polygons.size(); i++) {
const gd::Polygon &p = polygons[i]; const gd::Polygon &p = polygons[i];
// For each point cast a face and check the distance to the point // For each face check the distance to the point
for (size_t point_id = 2; point_id < p.points.size(); point_id += 1) { for (size_t point_id = 2; point_id < p.points.size(); point_id += 1) {
const Face3 f(p.points[point_id - 2].pos, p.points[point_id - 1].pos, p.points[point_id].pos); const Face3 f(p.points[0].pos, p.points[point_id - 1].pos, p.points[point_id].pos);
const Vector3 inters = f.get_closest_point_to(p_point); const Vector3 inters = f.get_closest_point_to(p_point);
const real_t d = inters.distance_to(p_point); const real_t ds = inters.distance_squared_to(p_point);
if (d < closest_point_d) { if (ds < closest_point_ds) {
closest_point = inters; result.point = inters;
closest_point_owner = p.owner->get_self(); result.normal = f.get_plane().normal;
closest_point_d = d; result.owner = p.owner->get_self();
closest_point_ds = ds;
} }
} }
} }
return closest_point_owner; return result;
} }
void NavMap::add_region(NavRegion *p_region) { void NavMap::add_region(NavRegion *p_region) {

1
modules/navigation/nav_map.h
View file

@ -112,6 +112,7 @@ public:
Vector3 get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const; Vector3 get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const;
Vector3 get_closest_point(const Vector3 &p_point) const; Vector3 get_closest_point(const Vector3 &p_point) const;
Vector3 get_closest_point_normal(const Vector3 &p_point) const; Vector3 get_closest_point_normal(const Vector3 &p_point) const;
gd::ClosestPointQueryResult get_closest_point_info(const Vector3 &p_point) const;
RID get_closest_point_owner(const Vector3 &p_point) const; RID get_closest_point_owner(const Vector3 &p_point) const;
void add_region(NavRegion *p_region); void add_region(NavRegion *p_region);

7
modules/navigation/nav_utils.h
View file

@ -161,6 +161,13 @@ struct FreeEdge {
Vector3 edge_dir; Vector3 edge_dir;
float edge_len_squared; float edge_len_squared;
}; };
struct ClosestPointQueryResult {
Vector3 point;
Vector3 normal;
RID owner;
};
} // namespace gd } // namespace gd
#endif // NAV_UTILS_H #endif // NAV_UTILS_H