summaryrefslogtreecommitdiff
path: root/core
diff options
context:
space:
mode:
Diffstat (limited to 'core')
-rw-r--r--core/math/aabb.h20
-rw-r--r--core/math/triangle_mesh.cpp216
-rw-r--r--core/math/triangle_mesh.h2
3 files changed, 238 insertions, 0 deletions
diff --git a/core/math/aabb.h b/core/math/aabb.h
index 39b8f403e7..cdb8eb48a3 100644
--- a/core/math/aabb.h
+++ b/core/math/aabb.h
@@ -76,6 +76,7 @@ public:
_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 inside_convex_shape(const Plane *p_planes, int p_plane_count) const;
bool intersects_plane(const Plane &p_plane) const;
_FORCE_INLINE_ bool has_point(const Vector3 &p_point) const;
@@ -207,6 +208,25 @@ bool AABB::intersects_convex_shape(const Plane *p_planes, int p_plane_count) con
return true;
}
+bool AABB::inside_convex_shape(const Plane *p_planes, int p_plane_count) const {
+
+ Vector3 half_extents = size * 0.5;
+ Vector3 ofs = position + half_extents;
+
+ for (int i = 0; i < p_plane_count; i++) {
+ const Plane &p = p_planes[i];
+ Vector3 point(
+ (p.normal.x < 0) ? -half_extents.x : half_extents.x,
+ (p.normal.y < 0) ? -half_extents.y : half_extents.y,
+ (p.normal.z < 0) ? -half_extents.z : half_extents.z);
+ point += ofs;
+ if (p.is_point_over(point))
+ return false;
+ }
+
+ return true;
+}
+
bool AABB::has_point(const Vector3 &p_point) const {
if (p_point.x < position.x)
diff --git a/core/math/triangle_mesh.cpp b/core/math/triangle_mesh.cpp
index 0dd1cbf1c3..5475f733c3 100644
--- a/core/math/triangle_mesh.cpp
+++ b/core/math/triangle_mesh.cpp
@@ -510,6 +510,222 @@ 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 {
+ uint32_t *stack = (uint32_t *)alloca(sizeof(int) * max_depth);
+
+ //p_fully_inside = true;
+
+ enum {
+ TEST_AABB_BIT = 0,
+ VISIT_LEFT_BIT = 1,
+ VISIT_RIGHT_BIT = 2,
+ VISIT_DONE_BIT = 3,
+ VISITED_BIT_SHIFT = 29,
+ NODE_IDX_MASK = (1 << VISITED_BIT_SHIFT) - 1,
+ VISITED_BIT_MASK = ~NODE_IDX_MASK,
+
+ };
+
+ int level = 0;
+
+ PoolVector<Triangle>::Read trianglesr = triangles.read();
+ PoolVector<Vector3>::Read verticesr = vertices.read();
+ PoolVector<BVH>::Read bvhr = bvh.read();
+
+ const Triangle *triangleptr = trianglesr.ptr();
+ const Vector3 *vertexptr = verticesr.ptr();
+ int pos = bvh.size() - 1;
+ const BVH *bvhptr = bvhr.ptr();
+
+ stack[0] = pos;
+ while (true) {
+
+ uint32_t node = stack[level] & NODE_IDX_MASK;
+ const BVH &b = bvhptr[node];
+ bool done = false;
+
+ switch (stack[level] >> VISITED_BIT_SHIFT) {
+ case TEST_AABB_BIT: {
+
+ bool valid = b.aabb.intersects_convex_shape(p_planes, p_plane_count);
+ if (!valid) {
+
+ stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
+
+ } else {
+
+ if (b.face_index >= 0) {
+
+ const Triangle &s = triangleptr[b.face_index];
+
+ for (int j = 0; j < 3; ++j) {
+ const Vector3 &point = vertexptr[s.indices[j]];
+ const Vector3 &next_point = vertexptr[s.indices[(j + 1) % 3]];
+ Vector3 res;
+ bool over = true;
+ for (int i = 0; i < p_plane_count; i++) {
+ const Plane &p = p_planes[i];
+
+ if (p.intersects_segment(point, next_point, &res)) {
+ bool inisde = true;
+ for (int k = 0; k < p_plane_count; k++) {
+ if (k == i) continue;
+ const Plane &pp = p_planes[k];
+ if (pp.is_point_over(res)) {
+ inisde = false;
+ break;
+ }
+ }
+ if (inisde) return true;
+ }
+
+ if (p.is_point_over(point)) {
+ over = false;
+ break;
+ }
+ }
+ if (over) return true;
+ }
+
+ stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
+
+ } else {
+
+ stack[level] = (VISIT_LEFT_BIT << VISITED_BIT_SHIFT) | node;
+ }
+ }
+ continue;
+ }
+ case VISIT_LEFT_BIT: {
+
+ stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node;
+ stack[level + 1] = b.left | TEST_AABB_BIT;
+ level++;
+ continue;
+ }
+ case VISIT_RIGHT_BIT: {
+
+ stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
+ stack[level + 1] = b.right | TEST_AABB_BIT;
+ level++;
+ continue;
+ }
+ case VISIT_DONE_BIT: {
+
+ if (level == 0) {
+ done = true;
+ break;
+ } else
+ level--;
+ continue;
+ }
+ }
+
+ if (done)
+ break;
+ }
+
+ return false;
+}
+
+bool TriangleMesh::inside_convex_shape(const Plane *p_planes, int p_plane_count, Vector3 p_scale) const {
+ uint32_t *stack = (uint32_t *)alloca(sizeof(int) * max_depth);
+
+ enum {
+ TEST_AABB_BIT = 0,
+ VISIT_LEFT_BIT = 1,
+ VISIT_RIGHT_BIT = 2,
+ VISIT_DONE_BIT = 3,
+ VISITED_BIT_SHIFT = 29,
+ NODE_IDX_MASK = (1 << VISITED_BIT_SHIFT) - 1,
+ VISITED_BIT_MASK = ~NODE_IDX_MASK,
+
+ };
+
+ int level = 0;
+
+ PoolVector<Triangle>::Read trianglesr = triangles.read();
+ PoolVector<Vector3>::Read verticesr = vertices.read();
+ PoolVector<BVH>::Read bvhr = bvh.read();
+
+ Transform scale(Basis().scaled(p_scale));
+
+ const Triangle *triangleptr = trianglesr.ptr();
+ const Vector3 *vertexptr = verticesr.ptr();
+ int pos = bvh.size() - 1;
+ const BVH *bvhptr = bvhr.ptr();
+
+ stack[0] = pos;
+ while (true) {
+
+ uint32_t node = stack[level] & NODE_IDX_MASK;
+ const BVH &b = bvhptr[node];
+ bool done = false;
+
+ switch (stack[level] >> VISITED_BIT_SHIFT) {
+ case TEST_AABB_BIT: {
+
+ bool intersects = scale.xform(b.aabb).intersects_convex_shape(p_planes, p_plane_count);
+ if (!intersects) return false;
+
+ bool inside = scale.xform(b.aabb).inside_convex_shape(p_planes, p_plane_count);
+ if (inside) {
+
+ stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
+
+ } else {
+
+ if (b.face_index >= 0) {
+ const Triangle &s = triangleptr[b.face_index];
+ for (int j = 0; j < 3; ++j) {
+ Vector3 point = scale.xform(vertexptr[s.indices[j]]);
+ for (int i = 0; i < p_plane_count; i++) {
+ const Plane &p = p_planes[i];
+ if (p.is_point_over(point)) return false;
+ }
+ }
+
+ stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
+
+ } else {
+
+ stack[level] = (VISIT_LEFT_BIT << VISITED_BIT_SHIFT) | node;
+ }
+ }
+ continue;
+ }
+ case VISIT_LEFT_BIT: {
+
+ stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node;
+ stack[level + 1] = b.left | TEST_AABB_BIT;
+ level++;
+ continue;
+ }
+ case VISIT_RIGHT_BIT: {
+
+ stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
+ stack[level + 1] = b.right | TEST_AABB_BIT;
+ level++;
+ continue;
+ }
+ case VISIT_DONE_BIT: {
+
+ if (level == 0) {
+ done = true;
+ break;
+ } else
+ level--;
+ continue;
+ }
+ }
+
+ if (done)
+ break;
+ }
+
+ return true;
+}
+
bool TriangleMesh::is_valid() const {
return valid;
diff --git a/core/math/triangle_mesh.h b/core/math/triangle_mesh.h
index 78de7ae7ee..bf793fc50f 100644
--- a/core/math/triangle_mesh.h
+++ b/core/math/triangle_mesh.h
@@ -89,6 +89,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;
Vector3 get_area_normal(const AABB &p_aabb) const;
PoolVector<Face3> get_faces() const;
generated by cgit v1.2.3 (git 2.25.1) at 2025-02-06 19:02:51 +0000