1
2// for slow incremental optimization, we will periodically remove each
3// item from the tree and reinsert, to give it a chance to find a better position
4void _logic_item_remove_and_reinsert(uint32_t p_ref_id) {
5 // get the reference
6 ItemRef &ref = _refs[p_ref_id];
7
8 // no need to optimize inactive items
9 if (!ref.is_active()) {
10 return;
11 }
12
13 // special case of debug draw
14 if (ref.item_id == BVHCommon::INVALID) {
15 return;
16 }
17
18 BVH_ASSERT(ref.tnode_id != BVHCommon::INVALID);
19
20 // some overlay elaborate way to find out which tree the node is in!
21 BVHHandle temp_handle;
22 temp_handle.set_id(p_ref_id);
23 uint32_t tree_id = _handle_get_tree_id(temp_handle);
24
25 // remove and reinsert
26 BVHABB_CLASS abb;
27 node_remove_item(p_ref_id, tree_id, &abb);
28
29 // we must choose where to add to tree
30 ref.tnode_id = _logic_choose_item_add_node(_root_node_id[tree_id], abb);
31 _node_add_item(ref.tnode_id, p_ref_id, abb);
32
33 refit_upward_and_balance(ref.tnode_id, tree_id);
34}
35
36// from randy gaul balance function
37BVHABB_CLASS _logic_abb_merge(const BVHABB_CLASS &a, const BVHABB_CLASS &b) {
38 BVHABB_CLASS c = a;
39 c.merge(b);
40 return c;
41}
42
43//--------------------------------------------------------------------------------------------------
44/**
45 * @file q3DynamicAABBTree.h
46 * @author Randy Gaul
47 * @date 10/10/2014
48 * Copyright (c) 2014 Randy Gaul http://www.randygaul.net
49 * This software is provided 'as-is', without any express or implied
50 * warranty. In no event will the authors be held liable for any damages
51 * arising from the use of this software.
52 * Permission is granted to anyone to use this software for any purpose,
53 * including commercial applications, and to alter it and redistribute it
54 * freely, subject to the following restrictions:
55 * 1. The origin of this software must not be misrepresented; you must not
56 * claim that you wrote the original software. If you use this software
57 * in a product, an acknowledgment in the product documentation would be
58 * appreciated but is not required.
59 * 2. Altered source versions must be plainly marked as such, and must not
60 * be misrepresented as being the original software.
61 * 3. This notice may not be removed or altered from any source distribution.
62 */
63//--------------------------------------------------------------------------------------------------
64
65// This function is based on the 'Balance' function from Randy Gaul's qu3e
66// https://github.com/RandyGaul/qu3e
67// It is MODIFIED from qu3e version.
68// This is the only function used (and _logic_abb_merge helper function).
69int32_t _logic_balance(int32_t iA, uint32_t p_tree_id) {
70 //return iA; // uncomment this to bypass balance
71
72 TNode *A = &_nodes[iA];
73
74 if (A->is_leaf() || A->height == 1) {
75 return iA;
76 }
77
78 /* A
79 * / \
80 * B C
81 * / \ / \
82 * D E F G
83 */
84
85 CRASH_COND(A->num_children != 2);
86 int32_t iB = A->children[0];
87 int32_t iC = A->children[1];
88 TNode *B = &_nodes[iB];
89 TNode *C = &_nodes[iC];
90
91 int32_t balance = C->height - B->height;
92
93 // C is higher, promote C
94 if (balance > 1) {
95 int32_t iF = C->children[0];
96 int32_t iG = C->children[1];
97 TNode *F = &_nodes[iF];
98 TNode *G = &_nodes[iG];
99
100 // grandParent point to C
101 if (A->parent_id != BVHCommon::INVALID) {
102 if (_nodes[A->parent_id].children[0] == iA) {
103 _nodes[A->parent_id].children[0] = iC;
104
105 } else {
106 _nodes[A->parent_id].children[1] = iC;
107 }
108 } else {
109 // check this .. seems dodgy
110 change_root_node(iC, p_tree_id);
111 }
112
113 // Swap A and C
114 C->children[0] = iA;
115 C->parent_id = A->parent_id;
116 A->parent_id = iC;
117
118 // Finish rotation
119 if (F->height > G->height) {
120 C->children[1] = iF;
121 A->children[1] = iG;
122 G->parent_id = iA;
123 A->aabb = _logic_abb_merge(B->aabb, G->aabb);
124 C->aabb = _logic_abb_merge(A->aabb, F->aabb);
125
126 A->height = 1 + MAX(B->height, G->height);
127 C->height = 1 + MAX(A->height, F->height);
128 }
129
130 else {
131 C->children[1] = iG;
132 A->children[1] = iF;
133 F->parent_id = iA;
134 A->aabb = _logic_abb_merge(B->aabb, F->aabb);
135 C->aabb = _logic_abb_merge(A->aabb, G->aabb);
136
137 A->height = 1 + MAX(B->height, F->height);
138 C->height = 1 + MAX(A->height, G->height);
139 }
140
141 return iC;
142 }
143
144 // B is higher, promote B
145 else if (balance < -1) {
146 int32_t iD = B->children[0];
147 int32_t iE = B->children[1];
148 TNode *D = &_nodes[iD];
149 TNode *E = &_nodes[iE];
150
151 // grandParent point to B
152 if (A->parent_id != BVHCommon::INVALID) {
153 if (_nodes[A->parent_id].children[0] == iA) {
154 _nodes[A->parent_id].children[0] = iB;
155 } else {
156 _nodes[A->parent_id].children[1] = iB;
157 }
158 }
159
160 else {
161 // check this .. seems dodgy
162 change_root_node(iB, p_tree_id);
163 }
164
165 // Swap A and B
166 B->children[1] = iA;
167 B->parent_id = A->parent_id;
168 A->parent_id = iB;
169
170 // Finish rotation
171 if (D->height > E->height) {
172 B->children[0] = iD;
173 A->children[0] = iE;
174 E->parent_id = iA;
175 A->aabb = _logic_abb_merge(C->aabb, E->aabb);
176 B->aabb = _logic_abb_merge(A->aabb, D->aabb);
177
178 A->height = 1 + MAX(C->height, E->height);
179 B->height = 1 + MAX(A->height, D->height);
180 }
181
182 else {
183 B->children[0] = iE;
184 A->children[0] = iD;
185 D->parent_id = iA;
186 A->aabb = _logic_abb_merge(C->aabb, D->aabb);
187 B->aabb = _logic_abb_merge(A->aabb, E->aabb);
188
189 A->height = 1 + MAX(C->height, D->height);
190 B->height = 1 + MAX(A->height, E->height);
191 }
192
193 return iB;
194 }
195
196 return iA;
197}
198
199// either choose an existing node to add item to, or create a new node and return this
200uint32_t _logic_choose_item_add_node(uint32_t p_node_id, const BVHABB_CLASS &p_aabb) {
201 while (true) {
202 BVH_ASSERT(p_node_id != BVHCommon::INVALID);
203 TNode &tnode = _nodes[p_node_id];
204
205 if (tnode.is_leaf()) {
206 // if a leaf, and non full, use this to add to
207 if (!node_is_leaf_full(tnode)) {
208 return p_node_id;
209 }
210
211 // else split the leaf, and use one of the children to add to
212 return split_leaf(p_node_id, p_aabb);
213 }
214
215 // this should not happen???
216 // is still happening, need to debug and find circumstances. Is not that serious
217 // but would be nice to prevent. I think it only happens with the root node.
218 if (tnode.num_children == 1) {
219 WARN_PRINT_ONCE("BVH::recursive_choose_item_add_node, node with 1 child, recovering");
220 p_node_id = tnode.children[0];
221 } else {
222 BVH_ASSERT(tnode.num_children == 2);
223 TNode &childA = _nodes[tnode.children[0]];
224 TNode &childB = _nodes[tnode.children[1]];
225 int which = p_aabb.select_by_proximity(childA.aabb, childB.aabb);
226
227 p_node_id = tnode.children[which];
228 }
229 }
230}
231