Fix AABB Ray intersection - return inside

* Separates find_intersects from test_intersects for rays, and wraps the former.
* Changes parameter name to "r_intersection_point".
* Fixes broken old version which returned per axis t.
* Returns whether the ray origin is within the AABB.
* Returns intersection point when origin outside.
* Returns "backtracking" intersection point when inside.
* Returns sensible normal when inside.
* Returns valid results on borders.
* Returns robust results dealing with floating point error.

Co-authored-by: Claire Blackshaw <evilkimau@gmail.com>

core/math/aabb.cpp

@@ -117,55 +117,75 @@ AABB AABB::intersection(const AABB &p_aabb) const {
 	return AABB(min, max - min);
 }
 
-bool AABB::intersects_ray(const Vector3 &p_from, const Vector3 &p_dir, Vector3 *r_clip, Vector3 *r_normal) const {
+// Note that this routine returns the BACKTRACKED (i.e. behind the ray origin)
+// intersection point + normal if INSIDE the AABB.
+// 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 {
 #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 c1, c2;
 	Vector3 end = position + size;
-	real_t depth_near = -1e20;
+	real_t tmin = -1e20;
-	real_t depth_far = 1e20;
+	real_t tmax = 1e20;
 	int axis = 0;
 
+	// Make sure r_inside is always initialized,
+	// to prevent reading uninitialized data in the client code.
+	r_inside = false;
+
 	for (int i = 0; i < 3; i++) {
 		if (p_dir[i] == 0) {
 			if ((p_from[i] < position[i]) || (p_from[i] > end[i])) {
 				return false;
 			}
 		} else { // ray not parallel to planes in this direction
-			c1[i] = (position[i] - p_from[i]) / p_dir[i];
+			real_t t1 = (position[i] - p_from[i]) / p_dir[i];
-			c2[i] = (end[i] - p_from[i]) / p_dir[i];
+			real_t t2 = (end[i] - p_from[i]) / p_dir[i];
 
-			if (c1[i] > c2[i]) {
+			if (t1 > t2) {
-				SWAP(c1, c2);
+				SWAP(t1, t2);
 			}
-			if (c1[i] > depth_near) {
+			if (t1 >= tmin) {
-				depth_near = c1[i];
+				tmin = t1;
 				axis = i;
 			}
-			if (c2[i] < depth_far) {
+			if (t2 < tmax) {
-				depth_far = c2[i];
+				if (t2 < 0) {
+					return false;
+				}
+				tmax = t2;
 			}
-			if ((depth_near > depth_far) || (depth_far < 0)) {
+			if (tmin > tmax) {
 				return false;
 			}
 		}
 	}
 
-	if (r_clip) {
+	// Did the ray start from inside the box?
-		*r_clip = c1;
+	// In which case the intersection returned is the point of entry
+	// (behind the ray start) or the calling routine can use the ray origin as intersection point.
+	r_inside = tmin < 0;
+
+	if (r_intersection_point) {
+		*r_intersection_point = p_from + p_dir * tmin;
+
+		// Prevent float error by making sure the point is exactly
+		// on the AABB border on the relevant axis.
+		r_intersection_point->coord[axis] = (p_dir[axis] >= 0) ? position.coord[axis] : end.coord[axis];
 	}
 	if (r_normal) {
 		*r_normal = Vector3();
-		(*r_normal)[axis] = p_dir[axis] ? -1 : 1;
+		(*r_normal)[axis] = (p_dir[axis] >= 0) ? -1 : 1;
 	}
 
 	return true;
 }
 
-bool AABB::intersects_segment(const Vector3 &p_from, const Vector3 &p_to, Vector3 *r_clip, Vector3 *r_normal) const {
+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.");
@@ -223,8 +243,8 @@ bool AABB::intersects_segment(const Vector3 &p_from, const Vector3 &p_to, Vector
 		*r_normal = normal;
 	}
 
-	if (r_clip) {
+	if (r_intersection_point) {
-		*r_clip = p_from + rel * min;
+		*r_intersection_point = p_from + rel * min;
 	}
 
 	return true;
@@ -410,7 +430,15 @@ Variant AABB::intersects_segment_bind(const Vector3 &p_from, const Vector3 &p_to
 
 Variant AABB::intersects_ray_bind(const Vector3 &p_from, const Vector3 &p_dir) const {
 	Vector3 inters;
-	if (intersects_ray(p_from, p_dir, &inters)) {
+	bool inside = false;
+
+	if (find_intersects_ray(p_from, p_dir, inside, &inters)) {
+		// When inside the intersection point may be BEHIND the ray,
+		// so for general use we return the ray origin.
+		if (inside) {
+			return p_from;
+		}
+
 		return inters;
 	}
 	return Variant();

core/math/aabb.h

@@ -71,10 +71,15 @@ struct _NO_DISCARD_ AABB {
 	AABB merge(const AABB &p_with) const;
 	void merge_with(const AABB &p_aabb); ///merge with another AABB
 	AABB intersection(const AABB &p_aabb) const; ///get box where two intersect, empty if no intersection occurs
-	bool intersects_segment(const Vector3 &p_from, const Vector3 &p_to, Vector3 *r_clip = nullptr, Vector3 *r_normal = nullptr) const;
-	bool intersects_ray(const Vector3 &p_from, const Vector3 &p_dir, Vector3 *r_clip = nullptr, Vector3 *r_normal = nullptr) const;
 	_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_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;
+
 	_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;
 	bool intersects_plane(const Plane &p_plane) const;

tests/core/math/test_aabb.h

@@ -204,6 +204,67 @@ TEST_CASE("[AABB] Intersection") {
 	CHECK_MESSAGE(
 			!aabb_big.intersects_segment(Vector3(0, 300, 0), Vector3(0, 300, 0)),
 			"intersects_segment() should return the expected result with segment of length 0.");
+	CHECK_MESSAGE( // Simple ray intersection test.
+			aabb_big.intersects_ray(Vector3(-100, 3, 0), Vector3(1, 0, 0)),
+			"intersects_ray() should return true when ray points directly to AABB from outside.");
+	CHECK_MESSAGE( // Ray parallel to an edge.
+			!aabb_big.intersects_ray(Vector3(10, 10, 0), Vector3(0, 1, 0)),
+			"intersects_ray() should return false for ray parallel and outside of AABB.");
+	CHECK_MESSAGE( // Ray origin inside aabb.
+			aabb_big.intersects_ray(Vector3(1, 1, 1), Vector3(0, 1, 0)),
+			"intersects_ray() should return true for rays originating inside the AABB.");
+	CHECK_MESSAGE( // Ray pointing away from aabb.
+			!aabb_big.intersects_ray(Vector3(-10, 0, 0), Vector3(-1, 0, 0)),
+			"intersects_ray() should return false when ray points away from AABB.");
+	CHECK_MESSAGE( // Ray along a diagonal of aabb.
+			aabb_big.intersects_ray(Vector3(0, 0, 0), Vector3(1, 1, 1)),
+			"intersects_ray() should return true for rays along the AABB diagonal.");
+	CHECK_MESSAGE( // Ray originating at aabb edge.
+			aabb_big.intersects_ray(aabb_big.position, Vector3(-1, 0, 0)),
+			"intersects_ray() should return true for rays starting on AABB's edge.");
+	CHECK_MESSAGE( // Ray with zero direction inside.
+			aabb_big.intersects_ray(Vector3(-1, 3, -2), Vector3(0, 0, 0)),
+			"intersects_ray() should return true because its inside.");
+	CHECK_MESSAGE( // Ray with zero direction outside.
+			!aabb_big.intersects_ray(Vector3(-1000, 3, -2), Vector3(0, 0, 0)),
+			"intersects_ray() should return false for being outside.");
+
+	// Finding ray intersections.
+	const AABB aabb_simple = AABB(Vector3(), Vector3(1, 1, 1));
+	bool inside = false;
+	Vector3 intersection_point;
+	Vector3 intersection_normal;
+
+	// Borders.
+	aabb_simple.find_intersects_ray(Vector3(0.5, 0, 0.5), Vector3(0, 1, 0), inside, &intersection_point, &intersection_normal);
+	CHECK_MESSAGE(inside == false, "find_intersects_ray() should return outside on borders.");
+	CHECK_MESSAGE(intersection_point.is_equal_approx(Vector3(0.5, 0, 0.5)), "find_intersects_ray() border intersection point incorrect.");
+	CHECK_MESSAGE(intersection_normal.is_equal_approx(Vector3(0, -1, 0)), "find_intersects_ray() border intersection normal incorrect.");
+	aabb_simple.find_intersects_ray(Vector3(0.5, 1, 0.5), Vector3(0, -1, 0), inside, &intersection_point, &intersection_normal);
+	CHECK_MESSAGE(inside == false, "find_intersects_ray() should return outside on borders.");
+	CHECK_MESSAGE(intersection_point.is_equal_approx(Vector3(0.5, 1, 0.5)), "find_intersects_ray() border intersection point incorrect.");
+	CHECK_MESSAGE(intersection_normal.is_equal_approx(Vector3(0, 1, 0)), "find_intersects_ray() border intersection normal incorrect.");
+
+	// Inside.
+	aabb_simple.find_intersects_ray(Vector3(0.5, 0.1, 0.5), Vector3(0, 1, 0), inside, &intersection_point, &intersection_normal);
+	CHECK_MESSAGE(inside == true, "find_intersects_ray() should return inside when inside.");
+	CHECK_MESSAGE(intersection_point.is_equal_approx(Vector3(0.5, 0, 0.5)), "find_intersects_ray() inside backtracking intersection point incorrect.");
+	CHECK_MESSAGE(intersection_normal.is_equal_approx(Vector3(0, -1, 0)), "find_intersects_ray() inside intersection normal incorrect.");
+
+	// Zero sized AABB.
+	const AABB aabb_zero = AABB(Vector3(), Vector3(1, 0, 1));
+	aabb_zero.find_intersects_ray(Vector3(0.5, 0, 0.5), Vector3(0, 1, 0), inside, &intersection_point, &intersection_normal);
+	CHECK_MESSAGE(inside == false, "find_intersects_ray() should return outside on borders of zero sized AABB.");
+	CHECK_MESSAGE(intersection_point.is_equal_approx(Vector3(0.5, 0, 0.5)), "find_intersects_ray() border intersection point incorrect for zero sized AABB.");
+	CHECK_MESSAGE(intersection_normal.is_equal_approx(Vector3(0, -1, 0)), "find_intersects_ray() border intersection normal incorrect for zero sized AABB.");
+	aabb_zero.find_intersects_ray(Vector3(0.5, 0, 0.5), Vector3(0, -1, 0), inside, &intersection_point, &intersection_normal);
+	CHECK_MESSAGE(inside == false, "find_intersects_ray() should return outside on borders of zero sized AABB.");
+	CHECK_MESSAGE(intersection_point.is_equal_approx(Vector3(0.5, 0, 0.5)), "find_intersects_ray() border intersection point incorrect for zero sized AABB.");
+	CHECK_MESSAGE(intersection_normal.is_equal_approx(Vector3(0, 1, 0)), "find_intersects_ray() border intersection normal incorrect for zero sized AABB.");
+	aabb_zero.find_intersects_ray(Vector3(0.5, -1, 0.5), Vector3(0, 1, 0), inside, &intersection_point, &intersection_normal);
+	CHECK_MESSAGE(inside == false, "find_intersects_ray() should return outside on borders of zero sized AABB.");
+	CHECK_MESSAGE(intersection_point.is_equal_approx(Vector3(0.5, 0, 0.5)), "find_intersects_ray() border intersection point incorrect for zero sized AABB.");
+	CHECK_MESSAGE(intersection_normal.is_equal_approx(Vector3(0, -1, 0)), "find_intersects_ray() border intersection normal incorrect for zero sized AABB.");
 }
 
 TEST_CASE("[AABB] Merging") {