diff --git a/core/math/aabb.h b/core/math/aabb.h index d9d50c71393..235a678fa00 100644 --- a/core/math/aabb.h +++ b/core/math/aabb.h @@ -76,7 +76,7 @@ public: bool intersects_ray(const Vector3 &p_from, const Vector3 &p_dir, Vector3 *r_clip = NULL, Vector3 *r_normal = NULL) const; _FORCE_INLINE_ bool smits_intersect_ray(const Vector3 &p_from, const Vector3 &p_dir, real_t t0, real_t t1) const; - _FORCE_INLINE_ bool intersects_convex_shape(const Plane *p_planes, int p_plane_count) 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; @@ -190,7 +190,7 @@ Vector3 AABB::get_endpoint(int p_point) const { ERR_FAIL_V(Vector3()); } -bool AABB::intersects_convex_shape(const Plane *p_planes, int p_plane_count) const { +bool AABB::intersects_convex_shape(const Plane *p_planes, int p_plane_count, const Vector3 *p_points, int p_point_count) const { Vector3 half_extents = size * 0.5; Vector3 ofs = position + half_extents; @@ -206,6 +206,30 @@ bool AABB::intersects_convex_shape(const Plane *p_planes, int p_plane_count) con return false; } + // Make sure all points in the shape aren't fully separated from the AABB on + // each axis. + int bad_point_counts_positive[3] = { 0 }; + int bad_point_counts_negative[3] = { 0 }; + + for (int k = 0; k < 3; k++) { + + for (int i = 0; i < p_point_count; i++) { + if (p_points[i].coord[k] > ofs.coord[k] + half_extents.coord[k]) { + bad_point_counts_positive[k]++; + } + if (p_points[i].coord[k] < ofs.coord[k] - half_extents.coord[k]) { + bad_point_counts_negative[k]++; + } + } + + if (bad_point_counts_negative[k] == p_point_count) { + return false; + } + if (bad_point_counts_positive[k] == p_point_count) { + return false; + } + } + return true; } diff --git a/core/math/geometry.cpp b/core/math/geometry.cpp index 7eb48290a88..8151f1682f7 100644 --- a/core/math/geometry.cpp +++ b/core/math/geometry.cpp @@ -1185,3 +1185,42 @@ Vector > Geometry::_polypath_offset(const Vector &p_polyp } return polypaths; } + +Vector Geometry::compute_convex_mesh_points(const Plane *p_planes, int p_plane_count) { + + Vector points; + + // Iterate through every unique combination of any three planes. + for (int i = p_plane_count - 1; i >= 0; i--) { + for (int j = i - 1; j >= 0; j--) { + for (int k = j - 1; k >= 0; k--) { + + // Find the point where these planes all cross over (if they + // do at all). + Vector3 convex_shape_point; + if (p_planes[i].intersect_3(p_planes[j], p_planes[k], &convex_shape_point)) { + + // See if any *other* plane excludes this point because it's + // on the wrong side. + bool excluded = false; + for (int n = 0; n < p_plane_count; n++) { + if (n != i && n != j && n != k) { + real_t dp = p_planes[n].normal.dot(convex_shape_point); + if (dp - p_planes[n].d > CMP_EPSILON) { + excluded = true; + break; + } + } + } + + // Only add the point if it passed all tests. + if (!excluded) { + points.push_back(convex_shape_point); + } + } + } + } + } + + return points; +} diff --git a/core/math/geometry.h b/core/math/geometry.h index b9193242bcb..2d990ed3ab0 100644 --- a/core/math/geometry.h +++ b/core/math/geometry.h @@ -1014,6 +1014,8 @@ public: static void make_atlas(const Vector &p_rects, Vector &r_result, Size2i &r_size); + static Vector compute_convex_mesh_points(const Plane *p_planes, int p_plane_count); + private: static Vector > _polypaths_do_operation(PolyBooleanOperation p_op, const Vector &p_polypath_a, const Vector &p_polypath_b, bool is_a_open = false); static Vector > _polypath_offset(const Vector &p_polypath, real_t p_delta, PolyJoinType p_join_type, PolyEndType p_end_type); diff --git a/core/math/octree.h b/core/math/octree.h index 5478bdaf771..24f23b5e745 100644 --- a/core/math/octree.h +++ b/core/math/octree.h @@ -34,6 +34,7 @@ #include "core/list.h" #include "core/map.h" #include "core/math/aabb.h" +#include "core/math/geometry.h" #include "core/math/vector3.h" #include "core/print_string.h" #include "core/variant.h" @@ -341,6 +342,8 @@ private: const Plane *planes; int plane_count; + const Vector3 *points; + int point_count; T **result_array; int *result_idx; int result_max; @@ -1017,8 +1020,7 @@ void Octree::_cull_convex(Octant *p_octant, _CullConvexData *p continue; e->last_pass = pass; - if (e->aabb.intersects_convex_shape(p_cull->planes, p_cull->plane_count)) { - + if (e->aabb.intersects_convex_shape(p_cull->planes, p_cull->plane_count, p_cull->points, p_cull->point_count)) { if (*p_cull->result_idx < p_cull->result_max) { p_cull->result_array[*p_cull->result_idx] = e->userdata; (*p_cull->result_idx)++; @@ -1043,7 +1045,7 @@ void Octree::_cull_convex(Octant *p_octant, _CullConvexData *p continue; e->last_pass = pass; - if (e->aabb.intersects_convex_shape(p_cull->planes, p_cull->plane_count)) { + if (e->aabb.intersects_convex_shape(p_cull->planes, p_cull->plane_count, p_cull->points, p_cull->point_count)) { if (*p_cull->result_idx < p_cull->result_max) { @@ -1059,7 +1061,7 @@ void Octree::_cull_convex(Octant *p_octant, _CullConvexData *p for (int i = 0; i < 8; i++) { - if (p_octant->children[i] && p_octant->children[i]->aabb.intersects_convex_shape(p_cull->planes, p_cull->plane_count)) { + if (p_octant->children[i] && p_octant->children[i]->aabb.intersects_convex_shape(p_cull->planes, p_cull->plane_count, p_cull->points, p_cull->point_count)) { _cull_convex(p_octant->children[i], p_cull); } } @@ -1291,11 +1293,15 @@ int Octree::cull_convex(const Vector &p_convex, T **p_r if (!root) return 0; + Vector convex_points = Geometry::compute_convex_mesh_points(&p_convex[0], p_convex.size()); + int result_count = 0; pass++; _CullConvexData cdata; cdata.planes = &p_convex[0]; cdata.plane_count = p_convex.size(); + cdata.points = &convex_points[0]; + cdata.point_count = convex_points.size(); cdata.result_array = p_result_array; cdata.result_max = p_result_max; cdata.result_idx = &result_count; diff --git a/core/math/triangle_mesh.cpp b/core/math/triangle_mesh.cpp index 53d4ea0a966..13540c16171 100644 --- a/core/math/triangle_mesh.cpp +++ b/core/math/triangle_mesh.cpp @@ -511,7 +511,7 @@ bool TriangleMesh::intersect_ray(const Vector3 &p_begin, const Vector3 &p_dir, V return inters; } -bool TriangleMesh::intersect_convex_shape(const Plane *p_planes, int p_plane_count) const { +bool TriangleMesh::intersect_convex_shape(const Plane *p_planes, int p_plane_count, const Vector3 *p_points, int p_point_count) const { uint32_t *stack = (uint32_t *)alloca(sizeof(int) * max_depth); //p_fully_inside = true; @@ -548,7 +548,7 @@ bool TriangleMesh::intersect_convex_shape(const Plane *p_planes, int p_plane_cou switch (stack[level] >> VISITED_BIT_SHIFT) { case TEST_AABB_BIT: { - bool valid = b.aabb.intersects_convex_shape(p_planes, p_plane_count); + bool valid = b.aabb.intersects_convex_shape(p_planes, p_plane_count, p_points, p_point_count); if (!valid) { stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; @@ -629,7 +629,7 @@ bool TriangleMesh::intersect_convex_shape(const Plane *p_planes, int p_plane_cou return false; } -bool TriangleMesh::inside_convex_shape(const Plane *p_planes, int p_plane_count, Vector3 p_scale) const { +bool TriangleMesh::inside_convex_shape(const Plane *p_planes, int p_plane_count, const Vector3 *p_points, int p_point_count, Vector3 p_scale) const { uint32_t *stack = (uint32_t *)alloca(sizeof(int) * max_depth); enum { @@ -666,7 +666,7 @@ bool TriangleMesh::inside_convex_shape(const Plane *p_planes, int p_plane_count, switch (stack[level] >> VISITED_BIT_SHIFT) { case TEST_AABB_BIT: { - bool intersects = scale.xform(b.aabb).intersects_convex_shape(p_planes, p_plane_count); + bool intersects = scale.xform(b.aabb).intersects_convex_shape(p_planes, p_plane_count, p_points, p_point_count); if (!intersects) return false; bool inside = scale.xform(b.aabb).inside_convex_shape(p_planes, p_plane_count); diff --git a/core/math/triangle_mesh.h b/core/math/triangle_mesh.h index 575a78b0b52..eef7089bbb4 100644 --- a/core/math/triangle_mesh.h +++ b/core/math/triangle_mesh.h @@ -90,8 +90,8 @@ public: bool is_valid() const; bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal) const; bool intersect_ray(const Vector3 &p_begin, const Vector3 &p_dir, Vector3 &r_point, Vector3 &r_normal) const; - bool intersect_convex_shape(const Plane *p_planes, int p_plane_count) const; - bool inside_convex_shape(const Plane *p_planes, int p_plane_count, Vector3 p_scale = Vector3(1, 1, 1)) const; + bool intersect_convex_shape(const Plane *p_planes, int p_plane_count, const Vector3 *p_points, int p_point_count) const; + bool inside_convex_shape(const Plane *p_planes, int p_plane_count, const Vector3 *p_points, int p_point_count, Vector3 p_scale = Vector3(1, 1, 1)) const; Vector3 get_area_normal(const AABB &p_aabb) const; PoolVector get_faces() const; diff --git a/editor/plugins/spatial_editor_plugin.cpp b/editor/plugins/spatial_editor_plugin.cpp index eeb52bb53e1..af5b2d7b0c7 100644 --- a/editor/plugins/spatial_editor_plugin.cpp +++ b/editor/plugins/spatial_editor_plugin.cpp @@ -669,17 +669,13 @@ void SpatialEditorViewport::_select_region() { } } - if (!orthogonal) { - Plane near(cam_pos, -_get_camera_normal()); - near.d -= get_znear(); + Plane near(cam_pos, -_get_camera_normal()); + near.d -= get_znear(); + frustum.push_back(near); - frustum.push_back(near); - - Plane far = -near; - far.d += get_zfar(); - - frustum.push_back(far); - } + Plane far = -near; + far.d += get_zfar(); + frustum.push_back(far); Vector instances = VisualServer::get_singleton()->instances_cull_convex(frustum, get_tree()->get_root()->get_world()->get_scenario()); Vector selected; diff --git a/editor/spatial_editor_gizmos.cpp b/editor/spatial_editor_gizmos.cpp index c94b0eeab0f..9ea8b88c7cc 100644 --- a/editor/spatial_editor_gizmos.cpp +++ b/editor/spatial_editor_gizmos.cpp @@ -476,11 +476,13 @@ bool EditorSpatialGizmo::intersect_frustum(const Camera *p_camera, const Vector< Vector transformed_frustum; - for (int i = 0; i < 4; i++) { + for (int i = 0; i < p_frustum.size(); i++) { transformed_frustum.push_back(it.xform(p_frustum[i])); } - if (collision_mesh->inside_convex_shape(transformed_frustum.ptr(), transformed_frustum.size(), mesh_scale)) { + Vector convex_points = Geometry::compute_convex_mesh_points(p_frustum.ptr(), p_frustum.size()); + + if (collision_mesh->inside_convex_shape(transformed_frustum.ptr(), transformed_frustum.size(), convex_points.ptr(), convex_points.size(), mesh_scale)) { return true; } }