diff options
| -rw-r--r-- | core/math/aabb.cpp | 68 | ||||
| -rw-r--r-- | core/math/aabb.h | 9 | ||||
| -rw-r--r-- | tests/core/math/test_aabb.h | 61 |
3 files changed, 116 insertions, 22 deletions
diff --git a/core/math/aabb.cpp b/core/math/aabb.cpp index 76e9e74dea..7d1d7c5648 100644 --- a/core/math/aabb.cpp +++ b/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 depth_far = 1e20; + real_t tmin = -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]; - c2[i] = (end[i] - p_from[i]) / p_dir[i]; + real_t t1 = (position[i] - p_from[i]) / p_dir[i]; + real_t t2 = (end[i] - p_from[i]) / p_dir[i]; - if (c1[i] > c2[i]) { - SWAP(c1, c2); + if (t1 > t2) { + SWAP(t1, t2); } - if (c1[i] > depth_near) { - depth_near = c1[i]; + if (t1 >= tmin) { + tmin = t1; axis = i; } - if (c2[i] < depth_far) { - depth_far = c2[i]; + if (t2 < tmax) { + if (t2 < 0) { + return false; + } + tmax = t2; } - if ((depth_near > depth_far) || (depth_far < 0)) { + if (tmin > tmax) { return false; } } } - if (r_clip) { - *r_clip = c1; + // Did the ray start from inside the box? + // 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) { - *r_clip = p_from + rel * min; + if (r_intersection_point) { + *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(); diff --git a/core/math/aabb.h b/core/math/aabb.h index c2945a3ef1..9a74266ff7 100644 --- a/core/math/aabb.h +++ b/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; diff --git a/tests/core/math/test_aabb.h b/tests/core/math/test_aabb.h index b9f84cca24..dbc62bc248 100644 --- a/tests/core/math/test_aabb.h +++ b/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") { |