1/**************************************************************************/
2/* dynamic_bvh.h */
3/**************************************************************************/
4/* This file is part of: */
5/* GODOT ENGINE */
6/* https://godotengine.org */
7/**************************************************************************/
8/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
9/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
10/* */
11/* Permission is hereby granted, free of charge, to any person obtaining */
12/* a copy of this software and associated documentation files (the */
13/* "Software"), to deal in the Software without restriction, including */
14/* without limitation the rights to use, copy, modify, merge, publish, */
15/* distribute, sublicense, and/or sell copies of the Software, and to */
16/* permit persons to whom the Software is furnished to do so, subject to */
17/* the following conditions: */
18/* */
19/* The above copyright notice and this permission notice shall be */
20/* included in all copies or substantial portions of the Software. */
21/* */
22/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
23/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
24/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
25/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
26/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
27/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
28/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
29/**************************************************************************/
30
31#ifndef DYNAMIC_BVH_H
32#define DYNAMIC_BVH_H
33
34#include "core/math/aabb.h"
35#include "core/templates/list.h"
36#include "core/templates/local_vector.h"
37#include "core/templates/paged_allocator.h"
38#include "core/typedefs.h"
39
40// Based on bullet Dbvh
41
42/*
43Bullet Continuous Collision Detection and Physics Library
44Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org
45
46This software is provided 'as-is', without any express or implied warranty.
47In no event will the authors be held liable for any damages arising from the use of this software.
48Permission is granted to anyone to use this software for any purpose,
49including commercial applications, and to alter it and redistribute it freely,
50subject to the following restrictions:
51
521. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
532. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
543. This notice may not be removed or altered from any source distribution.
55*/
56
57///DynamicBVH implementation by Nathanael Presson
58// The DynamicBVH class implements a fast dynamic bounding volume tree based on axis aligned bounding boxes (aabb tree).
59
60class DynamicBVH {
61 struct Node;
62
63public:
64 struct ID {
65 Node *node = nullptr;
66
67 public:
68 _FORCE_INLINE_ bool is_valid() const { return node != nullptr; }
69 };
70
71private:
72 struct Volume {
73 Vector3 min, max;
74
75 _FORCE_INLINE_ Vector3 get_center() const { return ((min + max) / 2); }
76 _FORCE_INLINE_ Vector3 get_length() const { return (max - min); }
77
78 _FORCE_INLINE_ bool contains(const Volume &a) const {
79 return ((min.x <= a.min.x) &&
80 (min.y <= a.min.y) &&
81 (min.z <= a.min.z) &&
82 (max.x >= a.max.x) &&
83 (max.y >= a.max.y) &&
84 (max.z >= a.max.z));
85 }
86
87 _FORCE_INLINE_ Volume merge(const Volume &b) const {
88 Volume r;
89 for (int i = 0; i < 3; ++i) {
90 if (min[i] < b.min[i]) {
91 r.min[i] = min[i];
92 } else {
93 r.min[i] = b.min[i];
94 }
95 if (max[i] > b.max[i]) {
96 r.max[i] = max[i];
97 } else {
98 r.max[i] = b.max[i];
99 }
100 }
101 return r;
102 }
103
104 _FORCE_INLINE_ real_t get_size() const {
105 const Vector3 edges = get_length();
106 return (edges.x * edges.y * edges.z +
107 edges.x + edges.y + edges.z);
108 }
109
110 _FORCE_INLINE_ bool is_not_equal_to(const Volume &b) const {
111 return ((min.x != b.min.x) ||
112 (min.y != b.min.y) ||
113 (min.z != b.min.z) ||
114 (max.x != b.max.x) ||
115 (max.y != b.max.y) ||
116 (max.z != b.max.z));
117 }
118
119 _FORCE_INLINE_ real_t get_proximity_to(const Volume &b) const {
120 const Vector3 d = (min + max) - (b.min + b.max);
121 return (Math::abs(d.x) + Math::abs(d.y) + Math::abs(d.z));
122 }
123
124 _FORCE_INLINE_ int select_by_proximity(const Volume &a, const Volume &b) const {
125 return (get_proximity_to(a) < get_proximity_to(b) ? 0 : 1);
126 }
127
128 //
129 _FORCE_INLINE_ bool intersects(const Volume &b) const {
130 return ((min.x <= b.max.x) &&
131 (max.x >= b.min.x) &&
132 (min.y <= b.max.y) &&
133 (max.y >= b.min.y) &&
134 (min.z <= b.max.z) &&
135 (max.z >= b.min.z));
136 }
137
138 _FORCE_INLINE_ bool intersects_convex(const Plane *p_planes, int p_plane_count, const Vector3 *p_points, int p_point_count) const {
139 Vector3 half_extents = (max - min) * 0.5;
140 Vector3 ofs = min + half_extents;
141
142 for (int i = 0; i < p_plane_count; i++) {
143 const Plane &p = p_planes[i];
144 Vector3 point(
145 (p.normal.x > 0) ? -half_extents.x : half_extents.x,
146 (p.normal.y > 0) ? -half_extents.y : half_extents.y,
147 (p.normal.z > 0) ? -half_extents.z : half_extents.z);
148 point += ofs;
149 if (p.is_point_over(point)) {
150 return false;
151 }
152 }
153
154 // Make sure all points in the shape aren't fully separated from the AABB on
155 // each axis.
156 int bad_point_counts_positive[3] = { 0 };
157 int bad_point_counts_negative[3] = { 0 };
158
159 for (int k = 0; k < 3; k++) {
160 for (int i = 0; i < p_point_count; i++) {
161 if (p_points[i].coord[k] > ofs.coord[k] + half_extents.coord[k]) {
162 bad_point_counts_positive[k]++;
163 }
164 if (p_points[i].coord[k] < ofs.coord[k] - half_extents.coord[k]) {
165 bad_point_counts_negative[k]++;
166 }
167 }
168
169 if (bad_point_counts_negative[k] == p_point_count) {
170 return false;
171 }
172 if (bad_point_counts_positive[k] == p_point_count) {
173 return false;
174 }
175 }
176
177 return true;
178 }
179 };
180
181 struct Node {
182 Volume volume;
183 Node *parent = nullptr;
184 union {
185 Node *children[2];
186 void *data;
187 };
188
189 _FORCE_INLINE_ bool is_leaf() const { return children[1] == nullptr; }
190 _FORCE_INLINE_ bool is_internal() const { return (!is_leaf()); }
191
192 _FORCE_INLINE_ int get_index_in_parent() const {
193 ERR_FAIL_NULL_V(parent, 0);
194 return (parent->children[1] == this) ? 1 : 0;
195 }
196 void get_max_depth(int depth, int &maxdepth) {
197 if (is_internal()) {
198 children[0]->get_max_depth(depth + 1, maxdepth);
199 children[1]->get_max_depth(depth + 1, maxdepth);
200 } else {
201 maxdepth = MAX(maxdepth, depth);
202 }
203 }
204
205 //
206 int count_leaves() const {
207 if (is_internal()) {
208 return children[0]->count_leaves() + children[1]->count_leaves();
209 } else {
210 return (1);
211 }
212 }
213
214 bool is_left_of_axis(const Vector3 &org, const Vector3 &axis) const {
215 return axis.dot(volume.get_center() - org) <= 0;
216 }
217
218 Node() {
219 children[0] = nullptr;
220 children[1] = nullptr;
221 }
222 };
223
224 PagedAllocator<Node> node_allocator;
225 // Fields
226 Node *bvh_root = nullptr;
227 int lkhd = -1;
228 int total_leaves = 0;
229 uint32_t opath = 0;
230 uint32_t index = 0;
231
232 enum {
233 ALLOCA_STACK_SIZE = 128
234 };
235
236 _FORCE_INLINE_ void _delete_node(Node *p_node);
237 void _recurse_delete_node(Node *p_node);
238 _FORCE_INLINE_ Node *_create_node(Node *p_parent, void *p_data);
239 _FORCE_INLINE_ DynamicBVH::Node *_create_node_with_volume(Node *p_parent, const Volume &p_volume, void *p_data);
240 _FORCE_INLINE_ void _insert_leaf(Node *p_root, Node *p_leaf);
241 _FORCE_INLINE_ Node *_remove_leaf(Node *leaf);
242 void _fetch_leaves(Node *p_root, LocalVector<Node *> &r_leaves, int p_depth = -1);
243 static int _split(Node **leaves, int p_count, const Vector3 &p_org, const Vector3 &p_axis);
244 static Volume _bounds(Node **leaves, int p_count);
245 void _bottom_up(Node **leaves, int p_count);
246 Node *_top_down(Node **leaves, int p_count, int p_bu_threshold);
247 Node *_node_sort(Node *n, Node *&r);
248
249 _FORCE_INLINE_ void _update(Node *leaf, int lookahead = -1);
250
251 void _extract_leaves(Node *p_node, List<ID> *r_elements);
252
253 _FORCE_INLINE_ bool _ray_aabb(const Vector3 &rayFrom, const Vector3 &rayInvDirection, const unsigned int raySign[3], const Vector3 bounds[2], real_t &tmin, real_t lambda_min, real_t lambda_max) {
254 real_t tmax, tymin, tymax, tzmin, tzmax;
255 tmin = (bounds[raySign[0]].x - rayFrom.x) * rayInvDirection.x;
256 tmax = (bounds[1 - raySign[0]].x - rayFrom.x) * rayInvDirection.x;
257 tymin = (bounds[raySign[1]].y - rayFrom.y) * rayInvDirection.y;
258 tymax = (bounds[1 - raySign[1]].y - rayFrom.y) * rayInvDirection.y;
259
260 if ((tmin > tymax) || (tymin > tmax)) {
261 return false;
262 }
263
264 if (tymin > tmin) {
265 tmin = tymin;
266 }
267
268 if (tymax < tmax) {
269 tmax = tymax;
270 }
271
272 tzmin = (bounds[raySign[2]].z - rayFrom.z) * rayInvDirection.z;
273 tzmax = (bounds[1 - raySign[2]].z - rayFrom.z) * rayInvDirection.z;
274
275 if ((tmin > tzmax) || (tzmin > tmax)) {
276 return false;
277 }
278 if (tzmin > tmin) {
279 tmin = tzmin;
280 }
281 if (tzmax < tmax) {
282 tmax = tzmax;
283 }
284 return ((tmin < lambda_max) && (tmax > lambda_min));
285 }
286
287public:
288 // Methods
289 void clear();
290 bool is_empty() const { return (nullptr == bvh_root); }
291 void optimize_bottom_up();
292 void optimize_top_down(int bu_threshold = 128);
293 void optimize_incremental(int passes);
294 ID insert(const AABB &p_box, void *p_userdata);
295 bool update(const ID &p_id, const AABB &p_box);
296 void remove(const ID &p_id);
297 void get_elements(List<ID> *r_elements);
298
299 int get_leaf_count() const;
300 int get_max_depth() const;
301
302 /* Discouraged, but works as a reference on how it must be used */
303 struct DefaultQueryResult {
304 virtual bool operator()(void *p_data) = 0; //return true whether you want to continue the query
305 virtual ~DefaultQueryResult() {}
306 };
307
308 template <class QueryResult>
309 _FORCE_INLINE_ void aabb_query(const AABB &p_aabb, QueryResult &r_result);
310 template <class QueryResult>
311 _FORCE_INLINE_ void convex_query(const Plane *p_planes, int p_plane_count, const Vector3 *p_points, int p_point_count, QueryResult &r_result);
312 template <class QueryResult>
313 _FORCE_INLINE_ void ray_query(const Vector3 &p_from, const Vector3 &p_to, QueryResult &r_result);
314
315 void set_index(uint32_t p_index);
316 uint32_t get_index() const;
317
318 ~DynamicBVH();
319};
320
321template <class QueryResult>
322void DynamicBVH::aabb_query(const AABB &p_box, QueryResult &r_result) {
323 if (!bvh_root) {
324 return;
325 }
326
327 Volume volume;
328 volume.min = p_box.position;
329 volume.max = p_box.position + p_box.size;
330
331 const Node **stack = (const Node **)alloca(ALLOCA_STACK_SIZE * sizeof(const Node *));
332 stack[0] = bvh_root;
333 int32_t depth = 1;
334 int32_t threshold = ALLOCA_STACK_SIZE - 2;
335
336 LocalVector<const Node *> aux_stack; //only used in rare occasions when you run out of alloca memory because tree is too unbalanced. Should correct itself over time.
337
338 do {
339 depth--;
340 const Node *n = stack[depth];
341 if (n->volume.intersects(volume)) {
342 if (n->is_internal()) {
343 if (depth > threshold) {
344 if (aux_stack.is_empty()) {
345 aux_stack.resize(ALLOCA_STACK_SIZE * 2);
346 memcpy(aux_stack.ptr(), stack, ALLOCA_STACK_SIZE * sizeof(const Node *));
347 } else {
348 aux_stack.resize(aux_stack.size() * 2);
349 }
350 stack = aux_stack.ptr();
351 threshold = aux_stack.size() - 2;
352 }
353 stack[depth++] = n->children[0];
354 stack[depth++] = n->children[1];
355 } else {
356 if (r_result(n->data)) {
357 return;
358 }
359 }
360 }
361 } while (depth > 0);
362}
363
364template <class QueryResult>
365void DynamicBVH::convex_query(const Plane *p_planes, int p_plane_count, const Vector3 *p_points, int p_point_count, QueryResult &r_result) {
366 if (!bvh_root) {
367 return;
368 }
369
370 //generate a volume anyway to improve pre-testing
371 Volume volume;
372 for (int i = 0; i < p_point_count; i++) {
373 if (i == 0) {
374 volume.min = p_points[0];
375 volume.max = p_points[0];
376 } else {
377 volume.min.x = MIN(volume.min.x, p_points[i].x);
378 volume.min.y = MIN(volume.min.y, p_points[i].y);
379 volume.min.z = MIN(volume.min.z, p_points[i].z);
380
381 volume.max.x = MAX(volume.max.x, p_points[i].x);
382 volume.max.y = MAX(volume.max.y, p_points[i].y);
383 volume.max.z = MAX(volume.max.z, p_points[i].z);
384 }
385 }
386
387 const Node **stack = (const Node **)alloca(ALLOCA_STACK_SIZE * sizeof(const Node *));
388 stack[0] = bvh_root;
389 int32_t depth = 1;
390 int32_t threshold = ALLOCA_STACK_SIZE - 2;
391
392 LocalVector<const Node *> aux_stack; //only used in rare occasions when you run out of alloca memory because tree is too unbalanced. Should correct itself over time.
393
394 do {
395 depth--;
396 const Node *n = stack[depth];
397 if (n->volume.intersects(volume) && n->volume.intersects_convex(p_planes, p_plane_count, p_points, p_point_count)) {
398 if (n->is_internal()) {
399 if (depth > threshold) {
400 if (aux_stack.is_empty()) {
401 aux_stack.resize(ALLOCA_STACK_SIZE * 2);
402 memcpy(aux_stack.ptr(), stack, ALLOCA_STACK_SIZE * sizeof(const Node *));
403 } else {
404 aux_stack.resize(aux_stack.size() * 2);
405 }
406 stack = aux_stack.ptr();
407 threshold = aux_stack.size() - 2;
408 }
409 stack[depth++] = n->children[0];
410 stack[depth++] = n->children[1];
411 } else {
412 if (r_result(n->data)) {
413 return;
414 }
415 }
416 }
417 } while (depth > 0);
418}
419template <class QueryResult>
420void DynamicBVH::ray_query(const Vector3 &p_from, const Vector3 &p_to, QueryResult &r_result) {
421 if (!bvh_root) {
422 return;
423 }
424
425 Vector3 ray_dir = (p_to - p_from);
426 ray_dir.normalize();
427
428 ///what about division by zero? --> just set rayDirection[i] to INF/B3_LARGE_FLOAT
429 Vector3 inv_dir;
430 inv_dir[0] = ray_dir[0] == real_t(0.0) ? real_t(1e20) : real_t(1.0) / ray_dir[0];
431 inv_dir[1] = ray_dir[1] == real_t(0.0) ? real_t(1e20) : real_t(1.0) / ray_dir[1];
432 inv_dir[2] = ray_dir[2] == real_t(0.0) ? real_t(1e20) : real_t(1.0) / ray_dir[2];
433 unsigned int signs[3] = { inv_dir[0] < 0.0, inv_dir[1] < 0.0, inv_dir[2] < 0.0 };
434
435 real_t lambda_max = ray_dir.dot(p_to - p_from);
436
437 Vector3 bounds[2];
438
439 const Node **stack = (const Node **)alloca(ALLOCA_STACK_SIZE * sizeof(const Node *));
440 stack[0] = bvh_root;
441 int32_t depth = 1;
442 int32_t threshold = ALLOCA_STACK_SIZE - 2;
443
444 LocalVector<const Node *> aux_stack; //only used in rare occasions when you run out of alloca memory because tree is too unbalanced. Should correct itself over time.
445
446 do {
447 depth--;
448 const Node *node = stack[depth];
449 bounds[0] = node->volume.min;
450 bounds[1] = node->volume.max;
451 real_t tmin = 1.f, lambda_min = 0.f;
452 unsigned int result1 = false;
453 result1 = _ray_aabb(p_from, inv_dir, signs, bounds, tmin, lambda_min, lambda_max);
454 if (result1) {
455 if (node->is_internal()) {
456 if (depth > threshold) {
457 if (aux_stack.is_empty()) {
458 aux_stack.resize(ALLOCA_STACK_SIZE * 2);
459 memcpy(aux_stack.ptr(), stack, ALLOCA_STACK_SIZE * sizeof(const Node *));
460 } else {
461 aux_stack.resize(aux_stack.size() * 2);
462 }
463 stack = aux_stack.ptr();
464 threshold = aux_stack.size() - 2;
465 }
466 stack[depth++] = node->children[0];
467 stack[depth++] = node->children[1];
468 } else {
469 if (r_result(node->data)) {
470 return;
471 }
472 }
473 }
474 } while (depth > 0);
475}
476
477#endif // DYNAMIC_BVH_H
478