Support editing scenes of any large physical size

core/math/aabb.cpp

@@ -122,15 +122,15 @@ AABB AABB::intersection(const AABB &p_aabb) const {
 // The caller can therefore decide when INSIDE whether to use the
 // backtracked intersection, or use p_from as the intersection, and
 // carry on progressing without e.g. reflecting against the normal.
-bool AABB::find_intersects_ray(const Vector3 &p_from, const Vector3 &p_dir, bool &r_inside, Vector3 *r_intersection_point, Vector3 *r_normal) const {
+bool AABB::find_intersects_ray(const Vector3 &p_from, const Vector3 &p_dir, bool &r_inside, Vector3 *r_intersection_point, Vector3 *r_normal, real_t p_max_distance) const {
 #ifdef MATH_CHECKS
 	if (unlikely(size.x < 0 || size.y < 0 || size.z < 0)) {
 		ERR_PRINT("AABB size is negative, this is not supported. Use AABB.abs() to get an AABB with a positive size.");
 	}
 #endif
 	Vector3 end = position + size;
-	real_t tmin = -1e20;
-	real_t tmax = 1e20;
+	real_t tmin = -INFINITY;
+	real_t tmax = INFINITY;
 	int axis = 0;
 
 	// Make sure r_inside is always initialized,
@@ -159,7 +159,7 @@ bool AABB::find_intersects_ray(const Vector3 &p_from, const Vector3 &p_dir, bool
 				}
 				tmax = t2;
 			}
-			if (tmin > tmax) {
+			if (tmin > tmax || (p_max_distance > 0 && tmin > p_max_distance)) {
 				return false;
 			}
 		}
@@ -185,71 +185,6 @@ bool AABB::find_intersects_ray(const Vector3 &p_from, const Vector3 &p_dir, bool
 	return true;
 }
 
-bool AABB::intersects_segment(const Vector3 &p_from, const Vector3 &p_to, Vector3 *r_intersection_point, Vector3 *r_normal) const {
-#ifdef MATH_CHECKS
-	if (unlikely(size.x < 0 || size.y < 0 || size.z < 0)) {
-		ERR_PRINT("AABB size is negative, this is not supported. Use AABB.abs() to get an AABB with a positive size.");
-	}
-#endif
-	real_t min = 0, max = 1;
-	int axis = 0;
-	real_t sign = 0;
-
-	for (int i = 0; i < 3; i++) {
-		real_t seg_from = p_from[i];
-		real_t seg_to = p_to[i];
-		real_t box_begin = position[i];
-		real_t box_end = box_begin + size[i];
-		real_t cmin, cmax;
-		real_t csign;
-
-		if (seg_from < seg_to) {
-			if (seg_from > box_end || seg_to < box_begin) {
-				return false;
-			}
-			real_t length = seg_to - seg_from;
-			cmin = (seg_from < box_begin) ? ((box_begin - seg_from) / length) : 0;
-			cmax = (seg_to > box_end) ? ((box_end - seg_from) / length) : 1;
-			csign = -1.0;
-
-		} else {
-			if (seg_to > box_end || seg_from < box_begin) {
-				return false;
-			}
-			real_t length = seg_to - seg_from;
-			cmin = (seg_from > box_end) ? (box_end - seg_from) / length : 0;
-			cmax = (seg_to < box_begin) ? (box_begin - seg_from) / length : 1;
-			csign = 1.0;
-		}
-
-		if (cmin > min) {
-			min = cmin;
-			axis = i;
-			sign = csign;
-		}
-		if (cmax < max) {
-			max = cmax;
-		}
-		if (max < min) {
-			return false;
-		}
-	}
-
-	Vector3 rel = p_to - p_from;
-
-	if (r_normal) {
-		Vector3 normal;
-		normal[axis] = sign;
-		*r_normal = normal;
-	}
-
-	if (r_intersection_point) {
-		*r_intersection_point = p_from + rel * min;
-	}
-
-	return true;
-}
-
 bool AABB::intersects_plane(const Plane &p_plane) const {
 	Vector3 points[8] = {
 		Vector3(position.x, position.y, position.z),

core/math/aabb.h

@@ -73,12 +73,42 @@ struct [[nodiscard]] AABB {
 	AABB intersection(const AABB &p_aabb) const; ///get box where two intersect, empty if no intersection occurs
 	_FORCE_INLINE_ bool smits_intersect_ray(const Vector3 &p_from, const Vector3 &p_dir, real_t p_t0, real_t p_t1) const;
 
-	bool intersects_segment(const Vector3 &p_from, const Vector3 &p_to, Vector3 *r_intersection_point = nullptr, Vector3 *r_normal = nullptr) const;
+	bool intersects_segment(const Vector3 &p_from, const Vector3 &p_to, Vector3 *r_intersection_point = nullptr, Vector3 *r_normal = nullptr) const {
+		Vector3 segment = p_to - p_from;
+		real_t length = segment.length();
+		if (unlikely(length == 0)) { // p_from and p_to are strictly identical.
+			if (!has_point(p_from)) {
+				return false;
+			}
+			if (r_intersection_point != nullptr) {
+				*r_intersection_point = p_from;
+			}
+			if (r_normal != nullptr) {
+				*r_normal = Vector3();
+			}
+			return true;
+		}
+
+		bool inside = false;
+		bool inter = find_intersects_ray(p_from, segment / length, inside, r_intersection_point, r_normal, length);
+		if (inter && inside) {
+			// Revert intersection to p_from if the latter is located inside the box.
+			// This is because find_intersects_ray() returns the ray's entry point in the box *before* the starting point in this case.
+			// While this behavior may be relevant with infinite rays, it's not consistent with a segment intersection.
+			if (r_intersection_point != nullptr) {
+				*r_intersection_point = p_from;
+			}
+			if (r_normal != nullptr) {
+				*r_normal = Vector3();
+			}
+		}
+		return inter;
+	}
 	bool intersects_ray(const Vector3 &p_from, const Vector3 &p_dir) const {
 		bool inside;
 		return find_intersects_ray(p_from, p_dir, inside);
 	}
-	bool find_intersects_ray(const Vector3 &p_from, const Vector3 &p_dir, bool &r_inside, Vector3 *r_intersection_point = nullptr, Vector3 *r_normal = nullptr) const;
+	bool find_intersects_ray(const Vector3 &p_from, const Vector3 &p_dir, bool &r_inside, Vector3 *r_intersection_point = nullptr, Vector3 *r_normal = nullptr, real_t p_max_distance = 0) const;
 
 	_FORCE_INLINE_ bool intersects_convex_shape(const Plane *p_planes, int p_plane_count, const Vector3 *p_points, int p_point_count) const;
 	_FORCE_INLINE_ bool inside_convex_shape(const Plane *p_planes, int p_plane_count) const;

core/math/dynamic_bvh.h

@@ -310,7 +310,7 @@ class DynamicBVH {
 	template <typename QueryResult>
 	_FORCE_INLINE_ void convex_query(const Plane *p_planes, int p_plane_count, const Vector3 *p_points, int p_point_count, QueryResult &r_result);
 	template <typename QueryResult>
-	_FORCE_INLINE_ void ray_query(const Vector3 &p_from, const Vector3 &p_to, QueryResult &r_result);
+	_FORCE_INLINE_ void ray_query(const Vector3 &p_from, const Vector3 &p_dir, real_t p_length, QueryResult &r_result);
 
 	void set_index(uint32_t p_index);
 	uint32_t get_index() const;
@@ -416,13 +416,12 @@ void DynamicBVH::convex_query(const Plane *p_planes, int p_plane_count, const Ve
 	} while (depth > 0);
 }
 template <typename QueryResult>
-void DynamicBVH::ray_query(const Vector3 &p_from, const Vector3 &p_to, QueryResult &r_result) {
+void DynamicBVH::ray_query(const Vector3 &p_from, const Vector3 &p_dir, real_t p_length, QueryResult &r_result) {
 	if (!bvh_root) {
 		return;
 	}
 
-	Vector3 ray_dir = (p_to - p_from);
-	ray_dir.normalize();
+	Vector3 ray_dir = p_dir;
 
 	///what about division by zero? --> just set rayDirection[i] to INF/B3_LARGE_FLOAT
 	Vector3 inv_dir;
@@ -431,7 +430,7 @@ void DynamicBVH::ray_query(const Vector3 &p_from, const Vector3 &p_to, QueryResu
 	inv_dir[2] = ray_dir[2] == real_t(0.0) ? real_t(1e20) : real_t(1.0) / ray_dir[2];
 	unsigned int signs[3] = { inv_dir[0] < 0.0, inv_dir[1] < 0.0, inv_dir[2] < 0.0 };
 
-	real_t lambda_max = ray_dir.dot(p_to - p_from);
+	real_t lambda_max = p_length;
 
 	Vector3 bounds[2];
 

core/math/face3.cpp

@@ -107,8 +107,8 @@ int Face3::split_by_plane(const Plane &p_plane, Face3 p_res[3], bool p_is_point_
 	return polygons_created;
 }
 
-bool Face3::intersects_ray(const Vector3 &p_from, const Vector3 &p_dir, Vector3 *p_intersection) const {
-	return Geometry3D::ray_intersects_triangle(p_from, p_dir, vertex[0], vertex[1], vertex[2], p_intersection);
+bool Face3::intersects_ray(const Vector3 &p_from, const Vector3 &p_dir, Vector3 *p_intersection, real_t p_dist) const {
+	return Geometry3D::ray_intersects_triangle(p_from, p_dir, vertex[0], vertex[1], vertex[2], p_intersection, p_dist);
 }
 
 bool Face3::intersects_segment(const Vector3 &p_from, const Vector3 &p_dir, Vector3 *p_intersection) const {

core/math/face3.h

@@ -62,7 +62,7 @@ struct [[nodiscard]] Face3 {
 
 	Vector3 get_closest_point_to(const Vector3 &p_point) const;
 
-	bool intersects_ray(const Vector3 &p_from, const Vector3 &p_dir, Vector3 *p_intersection = nullptr) const;
+	bool intersects_ray(const Vector3 &p_from, const Vector3 &p_dir, Vector3 *p_intersection = nullptr, real_t p_dist = 0.0) const;
 	bool intersects_segment(const Vector3 &p_from, const Vector3 &p_dir, Vector3 *p_intersection = nullptr) const;
 
 	void get_support(const Vector3 &p_normal, const Transform3D &p_transform, Vector3 *p_vertices, int *p_count, int p_max) const;