|
@@ -627,24 +627,20 @@ Vector3 NavMap::get_closest_point_to_segment(const Vector3 &p_from, const Vector
|
|
closest_point_d = d;
|
|
closest_point_d = d;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
- }
|
|
|
|
-
|
|
|
|
- if (use_collision == false) {
|
|
|
|
- for (size_t point_id = 0; point_id < p.points.size(); point_id += 1) {
|
|
|
|
- Vector3 a, b;
|
|
|
|
-
|
|
|
|
- Geometry3D::get_closest_points_between_segments(
|
|
|
|
- p_from,
|
|
|
|
- p_to,
|
|
|
|
- p.points[point_id].pos,
|
|
|
|
- p.points[(point_id + 1) % p.points.size()].pos,
|
|
|
|
- a,
|
|
|
|
- b);
|
|
|
|
|
|
+ // If segment does not itersect face, check the distance from segment's endpoints.
|
|
|
|
+ else if (!use_collision) {
|
|
|
|
+ const Vector3 p_from_closest = f.get_closest_point_to(p_from);
|
|
|
|
+ const real_t d_p_from = p_from.distance_to(p_from_closest);
|
|
|
|
+ if (closest_point_d > d_p_from) {
|
|
|
|
+ closest_point = p_from_closest;
|
|
|
|
+ closest_point_d = d_p_from;
|
|
|
|
+ }
|
|
|
|
|
|
- const real_t d = a.distance_to(b);
|
|
|
|
- if (d < closest_point_d) {
|
|
|
|
- closest_point_d = d;
|
|
|
|
- closest_point = b;
|
|
|
|
|
|
+ const Vector3 p_to_closest = f.get_closest_point_to(p_to);
|
|
|
|
+ const real_t d_p_to = p_to.distance_to(p_to_closest);
|
|
|
|
+ if (closest_point_d > d_p_to) {
|
|
|
|
+ closest_point = p_to_closest;
|
|
|
|
+ closest_point_d = d_p_to;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|