code: /*************************************************************************/
  /*  dynamic_bvh.cpp                                                      */
  /*************************************************************************/
  /*                       This file is part of:                           */
  /*                           GODOT ENGINE                                */
  /*                      https://godotengine.org                          */
  /*************************************************************************/
  /* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur.                 */
  /* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md).   */
  /*                                                                       */
  /* Permission is hereby granted, free of charge, to any person obtaining */
  /* a copy of this software and associated documentation files (the       */
  /* "Software"), to deal in the Software without restriction, including   */
  /* without limitation the rights to use, copy, modify, merge, publish,   */
  /* distribute, sublicense, and/or sell copies of the Software, and to    */
  /* permit persons to whom the Software is furnished to do so, subject to */
  /* the following conditions:                                             */
  /*                                                                       */
  /* The above copyright notice and this permission notice shall be        */
  /* included in all copies or substantial portions of the Software.       */
  /*                                                                       */
  /* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
  /* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
  /* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
  /* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
  /* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
  /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
  /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
  /*************************************************************************/
  
  #include "dynamic_bvh.h"
  
  void DynamicBVH::_delete_node(Node *p_node) {
  	node_allocator.free(p_node);
  }
  
  void DynamicBVH::_recurse_delete_node(Node *p_node) {
  	if (!p_node->is_leaf()) {
  		_recurse_delete_node(p_node->childs[0]);
  		_recurse_delete_node(p_node->childs[1]);
  	}
  	if (p_node == bvh_root) {
  		bvh_root = nullptr;
  	}
  	_delete_node(p_node);
  }
  
  DynamicBVH::Node *DynamicBVH::_create_node(Node *p_parent, void *p_data) {
  	Node *node = node_allocator.alloc();
  	node->parent = p_parent;
  	node->data = p_data;
  	return (node);
  }
  
  DynamicBVH::Node *DynamicBVH::_create_node_with_volume(Node *p_parent, const Volume &p_volume, void *p_data) {
  	Node *node = _create_node(p_parent, p_data);
  	node->volume = p_volume;
  	return node;
  }
  
  void DynamicBVH::_insert_leaf(Node *p_root, Node *p_leaf) {
  	if (!bvh_root) {
  		bvh_root = p_leaf;
  		p_leaf->parent = 0;
  	} else {
  		if (!p_root->is_leaf()) {
  			do {
  				p_root = p_root->childs[p_leaf->volume.select_by_proximity(
  						p_root->childs[0]->volume,
  						p_root->childs[1]->volume)];
  			} while (!p_root->is_leaf());
  		}
  		Node *prev = p_root->parent;
  		Node *node = _create_node_with_volume(prev, p_leaf->volume.merge(p_root->volume), 0);
  		if (prev) {
  			prev->childs[p_root->get_index_in_parent()] = node;
  			node->childs[0] = p_root;
  			p_root->parent = node;
  			node->childs[1] = p_leaf;
  			p_leaf->parent = node;
  			do {
  				if (!prev->volume.contains(node->volume)) {
  					prev->volume = prev->childs[0]->volume.merge(prev->childs[1]->volume);
  				} else {
  					break;
  				}
  				node = prev;
  			} while (0 != (prev = node->parent));
  		} else {
  			node->childs[0] = p_root;
  			p_root->parent = node;
  			node->childs[1] = p_leaf;
  			p_leaf->parent = node;
  			bvh_root = node;
  		}
  	}
  }
  
  DynamicBVH::Node *DynamicBVH::_remove_leaf(Node *leaf) {
  	if (leaf == bvh_root) {
  		bvh_root = 0;
  		return (0);
  	} else {
  		Node *parent = leaf->parent;
  		Node *prev = parent->parent;
  		Node *sibling = parent->childs[1 - leaf->get_index_in_parent()];
  		if (prev) {
  			prev->childs[parent->get_index_in_parent()] = sibling;
  			sibling->parent = prev;
  			_delete_node(parent);
  			while (prev) {
  				const Volume pb = prev->volume;
  				prev->volume = prev->childs[0]->volume.merge(prev->childs[1]->volume);
  				if (pb.is_not_equal_to(prev->volume)) {
  					prev = prev->parent;
  				} else
  					break;
  			}
  			return (prev ? prev : bvh_root);
  		} else {
  			bvh_root = sibling;
  			sibling->parent = 0;
  			_delete_node(parent);
  			return (bvh_root);
  		}
  	}
  }
  
  void DynamicBVH::_fetch_leaves(Node *p_root, LocalVector<Node *> &r_leaves, int p_depth) {
  	if (p_root->is_internal() && p_depth) {
  		_fetch_leaves(p_root->childs[0], r_leaves, p_depth - 1);
  		_fetch_leaves(p_root->childs[1], r_leaves, p_depth - 1);
  		_delete_node(p_root);
  	} else {
  		r_leaves.push_back(p_root);
  	}
  }
  
  // Partitions leaves such that leaves[0, n) are on the
  // left of axis, and leaves[n, count) are on the right
  // of axis. returns N.
  int DynamicBVH::_split(Node **leaves, int p_count, const Vector3 &p_org, const Vector3 &p_axis) {
  	int begin = 0;
  	int end = p_count;
  	for (;;) {
  		while (begin != end && leaves[begin]->is_left_of_axis(p_org, p_axis)) {
  			++begin;
  		}
  
  		if (begin == end) {
  			break;
  		}
  
  		while (begin != end && !leaves[end - 1]->is_left_of_axis(p_org, p_axis)) {
  			--end;
  		}
  
  		if (begin == end) {
  			break;
  		}
  
  		// swap out of place nodes
  		--end;
  		Node *temp = leaves[begin];
  		leaves[begin] = leaves[end];
  		leaves[end] = temp;
  		++begin;
  	}
  
  	return begin;
  }
  
  DynamicBVH::Volume DynamicBVH::_bounds(Node **leaves, int p_count) {
  	Volume volume = leaves[0]->volume;
  	for (int i = 1, ni = p_count; i < ni; ++i) {
  		volume = volume.merge(leaves[i]->volume);
  	}
  	return (volume);
  }
  
  void DynamicBVH::_bottom_up(Node **leaves, int p_count) {
  	while (p_count > 1) {
  		real_t minsize = Math_INF;
  		int minidx[2] = { -1, -1 };
  		for (int i = 0; i < p_count; ++i) {
  			for (int j = i + 1; j < p_count; ++j) {
  				const real_t sz = leaves[i]->volume.merge(leaves[j]->volume).get_size();
  				if (sz < minsize) {
  					minsize = sz;
  					minidx[0] = i;
  					minidx[1] = j;
  				}
  			}
  		}
  		Node *n[] = { leaves[minidx[0]], leaves[minidx[1]] };
  		Node *p = _create_node_with_volume(nullptr, n[0]->volume.merge(n[1]->volume), nullptr);
  		p->childs[0] = n[0];
  		p->childs[1] = n[1];
  		n[0]->parent = p;
  		n[1]->parent = p;
  		leaves[minidx[0]] = p;
  		leaves[minidx[1]] = leaves[p_count - 1];
  		--p_count;
  	}
  }
  
  DynamicBVH::Node *DynamicBVH::_top_down(Node **leaves, int p_count, int p_bu_threshold) {
  	static const Vector3 axis[] = { Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1) };
  
  	ERR_FAIL_COND_V(p_bu_threshold <= 1, nullptr);
  	if (p_count > 1) {
  		if (p_count > p_bu_threshold) {
  			const Volume vol = _bounds(leaves, p_count);
  			const Vector3 org = vol.get_center();
  			int partition;
  			int bestaxis = -1;
  			int bestmidp = p_count;
  			int splitcount[3][2] = { { 0, 0 }, { 0, 0 }, { 0, 0 } };
  			int i;
  			for (i = 0; i < p_count; ++i) {
  				const Vector3 x = leaves[i]->volume.get_center() - org;
  				for (int j = 0; j < 3; ++j) {
  					++splitcount[j][x.dot(axis[j]) > 0 ? 1 : 0];
  				}
  			}
  			for (i = 0; i < 3; ++i) {
  				if ((splitcount[i][0] > 0) && (splitcount[i][1] > 0)) {
  					const int midp = (int)Math::abs(real_t(splitcount[i][0] - splitcount[i][1]));
  					if (midp < bestmidp) {
  						bestaxis = i;
  						bestmidp = midp;
  					}
  				}
  			}
  			if (bestaxis >= 0) {
  				partition = _split(leaves, p_count, org, axis[bestaxis]);
  				ERR_FAIL_COND_V(partition == 0 || partition == p_count, nullptr);
  			} else {
  				partition = p_count / 2 + 1;
  			}
  
  			Node *node = _create_node_with_volume(nullptr, vol, nullptr);
  			node->childs[0] = _top_down(&leaves[0], partition, p_bu_threshold);
  			node->childs[1] = _top_down(&leaves[partition], p_count - partition, p_bu_threshold);
  			node->childs[0]->parent = node;
  			node->childs[1]->parent = node;
  			return (node);
  		} else {
  			_bottom_up(leaves, p_count);
  			return (leaves[0]);
  		}
  	}
  	return (leaves[0]);
  }
  
  DynamicBVH::Node *DynamicBVH::_node_sort(Node *n, Node *&r) {
  	Node *p = n->parent;
  	ERR_FAIL_COND_V(!n->is_internal(), nullptr);
  	if (p > n) {
  		const int i = n->get_index_in_parent();
  		const int j = 1 - i;
  		Node *s = p->childs[j];
  		Node *q = p->parent;
  		ERR_FAIL_COND_V(n != p->childs[i], nullptr);
  		if (q)
  			q->childs[p->get_index_in_parent()] = n;
  		else
  			r = n;
  		s->parent = n;
  		p->parent = n;
  		n->parent = q;
  		p->childs[0] = n->childs[0];
  		p->childs[1] = n->childs[1];
  		n->childs[0]->parent = p;
  		n->childs[1]->parent = p;
  		n->childs[i] = p;
  		n->childs[j] = s;
  		SWAP(p->volume, n->volume);
  		return (p);
  	}
  	return (n);
  }
  
  void DynamicBVH::clear() {
  	if (bvh_root) {
  		_recurse_delete_node(bvh_root);
  	}
  	lkhd = -1;
  	opath = 0;
  }
  
  void DynamicBVH::optimize_bottom_up() {
  	if (bvh_root) {
  		LocalVector<Node *> leaves;
  		_fetch_leaves(bvh_root, leaves);
  		_bottom_up(&leaves[0], leaves.size());
  		bvh_root = leaves[0];
  	}
  }
  
  void DynamicBVH::optimize_top_down(int bu_threshold) {
  	if (bvh_root) {
  		LocalVector<Node *> leaves;
  		_fetch_leaves(bvh_root, leaves);
  		bvh_root = _top_down(&leaves[0], leaves.size(), bu_threshold);
  	}
  }
  
  void DynamicBVH::optimize_incremental(int passes) {
  	if (passes < 0)
  		passes = total_leaves;
  	if (bvh_root && (passes > 0)) {
  		do {
  			Node *node = bvh_root;
  			unsigned bit = 0;
  			while (node->is_internal()) {
  				node = _node_sort(node, bvh_root)->childs[(opath >> bit) & 1];
  				bit = (bit + 1) & (sizeof(unsigned) * 8 - 1);
  			}
  			_update(node);
  			++opath;
  		} while (--passes);
  	}
  }
  
  DynamicBVH::ID DynamicBVH::insert(const AABB &p_box, void *p_userdata) {
  	Volume volume;
  	volume.min = p_box.position;
  	volume.max = p_box.position + p_box.size;
  
  	Node *leaf = _create_node_with_volume(nullptr, volume, p_userdata);
  	_insert_leaf(bvh_root, leaf);
  	++total_leaves;
  
  	ID id;
  	id.node = leaf;
  
  	return id;
  }
  
  void DynamicBVH::_update(Node *leaf, int lookahead) {
  	Node *root = _remove_leaf(leaf);
  	if (root) {
  		if (lookahead >= 0) {
  			for (int i = 0; (i < lookahead) && root->parent; ++i) {
  				root = root->parent;
  			}
  		} else
  			root = bvh_root;
  	}
  	_insert_leaf(root, leaf);
  }
  
  bool DynamicBVH::update(const ID &p_id, const AABB &p_box) {
  	ERR_FAIL_COND_V(!p_id.is_valid(), false);
  	Node *leaf = p_id.node;
  
  	Volume volume;
  	volume.min = p_box.position;
  	volume.max = p_box.position + p_box.size;
  
  	if (leaf->volume.min.is_equal_approx(volume.min) && leaf->volume.max.is_equal_approx(volume.max)) {
  		// noop
  		return false;
  	}
  
  	Node *base = _remove_leaf(leaf);
  	if (base) {
  		if (lkhd >= 0) {
  			for (int i = 0; (i < lkhd) && base->parent; ++i) {
  				base = base->parent;
  			}
  		} else
  			base = bvh_root;
  	}
  	leaf->volume = volume;
  	_insert_leaf(base, leaf);
  	return true;
  }
  
  void DynamicBVH::remove(const ID &p_id) {
  	ERR_FAIL_COND(!p_id.is_valid());
  	Node *leaf = p_id.node;
  	_remove_leaf(leaf);
  	_delete_node(leaf);
  	--total_leaves;
  }
  
  void DynamicBVH::_extract_leaves(Node *p_node, List<ID> *r_elements) {
  	if (p_node->is_internal()) {
  		_extract_leaves(p_node->childs[0], r_elements);
  		_extract_leaves(p_node->childs[1], r_elements);
  	} else {
  		ID id;
  		id.node = p_node;
  		r_elements->push_back(id);
  	}
  }
  
  void DynamicBVH::set_index(uint32_t p_index) {
  	ERR_FAIL_COND(bvh_root != nullptr);
  	index = p_index;
  }
  
  uint32_t DynamicBVH::get_index() const {
  	return index;
  }
  
  void DynamicBVH::get_elements(List<ID> *r_elements) {
  	if (bvh_root) {
  		_extract_leaves(bvh_root, r_elements);
  	}
  }
  
  int DynamicBVH::get_leaf_count() const {
  	return total_leaves;
  }
  int DynamicBVH::get_max_depth() const {
  	if (bvh_root) {
  		int depth = 1;
  		int max_depth = 0;
  		bvh_root->get_max_depth(depth, max_depth);
  		return max_depth;
  	} else {
  		return 0;
  	}
  }
  
  DynamicBVH::~DynamicBVH() {
  	clear();
  }
  