@woosh/meep-engine 2.90.0 → 2.91.1

package/src/core/geom/2d/bvh/BVH2D.js

@@ -0,0 +1,1143 @@
+import { assert } from "../../../assert.js";
+import { UINT32_MAX } from "../../../binary/UINT32_MAX.js";
+import { array_copy } from "../../../collection/array/array_copy.js";
+import { typed_array_copy } from "../../../collection/array/typed/typed_array_copy.js";
+import { max2 } from "../../../math/max2.js";
+import { min2 } from "../../../math/min2.js";
+import { aabb2_compute_area } from "../aabb/aabb2_compute_area.js";
+
+
+export const COLUMN_PARENT = 4;
+export const COLUMN_CHILD_1 = 5;
+export const COLUMN_CHILD_2 = 6;
+export const COLUMN_HEIGHT = 7;
+
+/**
+ * A non-leaf node have both CHILD_1 and CHILD_2 set, when CHILD_1 is not set - it's a leaf node
+ * So we can utilize space of CHILD_2 to store USER_DATA, hence there is overlap in schema
+ * @readonly
+ * @type {number}
+ */
+export const COLUMN_USER_DATA = COLUMN_CHILD_2;
+
+/**
+ *
+ * @type {number}
+ */
+export const NULL_NODE = UINT32_MAX;
+
+/**
+ * @readonly
+ * @type {number}
+ */
+const CAPACITY_GROW_MULTIPLIER = 1.2;
+
+/**
+ * @readonly
+ * @type {number}
+ */
+const CAPACITY_GROW_MIN_STEP = 64;
+
+/**
+ * How many words are used for a single NODE in the tree
+ * One "word" is 4 bytes for the sake of alignment
+ * @readonly
+ * @type {number}
+ */
+export const ELEMENT_WORD_COUNT = 8;
+
+/**
+ * How many nodes can be stored in the newly constructed tree before allocation needs to take place
+ * @readonly
+ * @type {number}
+ */
+const INITIAL_CAPACITY = 128;
+
+
+/**
+ * Tree can not contain more than this number of nodes
+ * @type {number}
+ */
+const NODE_CAPACITY_LIMIT = Math.floor(UINT32_MAX / (ELEMENT_WORD_COUNT * 4));
+
+/**
+ * 2D Bounding Volume Hierarchy implementation.
+ * Based on BVH (3D) implementation
+ * @class
+ */
+export class BVH2D {
+
+    /**
+     *
+     * @type {ArrayBuffer}
+     * @private
+     */
+    __data_buffer = new ArrayBuffer(INITIAL_CAPACITY * ELEMENT_WORD_COUNT * 4);
+
+    /**
+     *
+     * @type {Float32Array}
+     * @private
+     */
+    __data_float32 = new Float32Array(this.__data_buffer);
+
+    /**
+     *
+     * @type {Uint32Array}
+     * @private
+     */
+    __data_uint32 = new Uint32Array(this.__data_buffer);
+
+    /**
+     * How many nodes are currently reserved, this will grow automatically through {@link #allocate_node} method usage
+     * @type {number}
+     * @private
+     */
+    __capacity = INITIAL_CAPACITY;
+
+    /**
+     * Number of used nodes. These are either live nodes, or node sitting in the {@link #__free} pool
+     * @type {number}
+     * @private
+     */
+    __size = 0;
+
+    /**
+     * Indices of released nodes. Nodes are pulled from here first if available, before the whole tree gets resized
+     * @type {number[]}
+     * @private
+     */
+    __free = [];
+
+    /**
+     * Pointer into __free array that's used as a stack, so this pointer represents top of the stack
+     * @type {number}
+     * @private
+     */
+    __free_pointer = 0;
+
+    /**
+     * Root node of the hierarchy
+     * @type {number}
+     * @private
+     */
+    __root = NULL_NODE;
+
+    /**
+     *
+     * @returns {number}
+     */
+    get root() {
+        return this.__root;
+    }
+
+    /**
+     *
+     * @returns {number}
+     */
+    get node_capacity() {
+        return this.__capacity;
+    }
+
+    /**
+     *
+     * @param {number} v
+     */
+    set node_capacity(v) {
+        if (this.__size > v) {
+            throw new Error(`Can't shrink capacity to ${v}, because it's below occupancy(${this.__size}).`);
+        }
+
+        this.__set_capacity(v);
+    }
+
+    __grow_capacity() {
+        if (this.__capacity >= NODE_CAPACITY_LIMIT) {
+            throw new Error('Can not grow capacity, already at maximum platform limit');
+        }
+
+        let new_capacity = Math.ceil(max2(
+            this.__capacity * CAPACITY_GROW_MULTIPLIER,
+            this.__capacity + CAPACITY_GROW_MIN_STEP
+        ));
+
+        if (new_capacity > NODE_CAPACITY_LIMIT) {
+            // can not grow as much as we'd like, but we can still grow up to the limit
+            new_capacity = NODE_CAPACITY_LIMIT;
+        }
+
+        this.__set_capacity(new_capacity);
+    }
+
+    /**
+     *
+     * @param {number} new_capacity in number of nodes
+     * @private
+     */
+    __set_capacity(new_capacity) {
+        assert.isNonNegativeInteger(new_capacity, 'new_capacity');
+
+        if (this.__capacity === new_capacity) {
+            // already at the exact desired capacity
+            return;
+        }
+
+        const old_data_uint32 = this.__data_uint32;
+
+        const new_data_buffer = new ArrayBuffer(new_capacity * ELEMENT_WORD_COUNT * 4);
+
+        this.__data_buffer = new_data_buffer;
+
+        this.__data_float32 = new Float32Array(new_data_buffer);
+        this.__data_uint32 = new Uint32Array(new_data_buffer);
+
+        // copy old data into new buffer
+        typed_array_copy(old_data_uint32, this.__data_uint32);
+
+        this.__capacity = new_capacity;
+    }
+
+    /**
+     * Trim allocated memory region to only contain allocated nodes
+     */
+    trim() {
+        if (this.__capacity > this.__size) {
+            this.__set_capacity(this.__size);
+        }
+    }
+
+    /**
+     *
+     * @returns {number}
+     */
+    allocate_node() {
+        let result;
+        const free_stack_top = this.__free_pointer;
+
+        if (free_stack_top > 0) {
+
+            // nodes in the free pool
+
+            const free_index = free_stack_top - 1;
+
+            result = this.__free[free_index];
+
+            this.__free_pointer = free_index;
+
+        } else {
+
+            result = this.__size;
+
+            if (result >= this.__capacity) {
+                // grow capacity
+                // console.log(`#GROW`);
+                this.__grow_capacity();
+            }
+
+            this.__size++;
+        }
+
+        // initialize node
+
+        const address = ELEMENT_WORD_COUNT * result;
+
+        const float32 = this.__data_float32;
+
+        // write AABB
+        float32[address] = Number.POSITIVE_INFINITY;
+        float32[address + 1] = Number.POSITIVE_INFINITY;
+        float32[address + 2] = Number.NEGATIVE_INFINITY;
+        float32[address + 3] = Number.NEGATIVE_INFINITY;
+
+        const uint32 = this.__data_uint32;
+
+        uint32[address + COLUMN_PARENT] = NULL_NODE; // parent
+        uint32[address + COLUMN_CHILD_1] = NULL_NODE; // child-1
+        uint32[address + COLUMN_CHILD_2] = NULL_NODE; // child-2 / user-data
+        uint32[address + COLUMN_HEIGHT] = 0; // height
+
+        // console.log(`#ALLOCATED: ${result}`);
+
+        return result;
+    }
+
+    /**
+     * Release memory used by the node back into the pool
+     * NOTE: Make sure that the node is not "live" (not attached to the hierarchy), otherwise this operation may corrupt the tree
+     * @param {number} id
+     */
+    release_node(id) {
+        assert.isNonNegativeInteger(id, 'id');
+        // no reset required as that's handled in the allocation method
+        this.__free[this.__free_pointer++] = id;
+    }
+
+    /**
+     *
+     * @param {number} id
+     * @returns {boolean}
+     */
+    node_is_leaf(id) {
+        assert.isNonNegativeInteger(id, 'id');
+        const child_1 = this.__data_uint32[ELEMENT_WORD_COUNT * id + COLUMN_CHILD_1];
+
+        return child_1 === NULL_NODE;
+    }
+
+    /**
+     *
+     * @param {number} id
+     * @returns {number}
+     */
+    node_get_user_data(id) {
+        assert.isNonNegativeInteger(id, 'id');
+        return this.__data_uint32[ELEMENT_WORD_COUNT * id + COLUMN_USER_DATA];
+    }
+
+    /**
+     *
+     * @param {number} id
+     * @param {number} value
+     */
+    node_set_user_data(id, value) {
+        assert.isNonNegativeInteger(id, 'id');
+        assert.isNonNegativeInteger(value, 'value');
+
+        this.__data_uint32[ELEMENT_WORD_COUNT * id + COLUMN_USER_DATA] = value;
+    }
+
+    /**
+     *
+     * @param {number} id
+     * @returns {number}
+     */
+    node_get_child1(id) {
+        assert.isNonNegativeInteger(id, 'id');
+        return this.__data_uint32[ELEMENT_WORD_COUNT * id + COLUMN_CHILD_1];
+    }
+
+    /**
+     *
+     * @param {number} node
+     * @param {number} child1
+     */
+    node_set_child1(node, child1) {
+        this.__data_uint32[ELEMENT_WORD_COUNT * node + COLUMN_CHILD_1] = child1;
+    }
+
+    /**
+     *
+     * @param {number} id
+     * @returns {number}
+     */
+    node_get_child2(id) {
+        assert.isNonNegativeInteger(id, 'id');
+        return this.__data_uint32[ELEMENT_WORD_COUNT * id + COLUMN_CHILD_2];
+    }
+
+    /**
+     *
+     * @param {number} node
+     * @param {number} child2
+     */
+    node_set_child2(node, child2) {
+        this.__data_uint32[ELEMENT_WORD_COUNT * node + COLUMN_CHILD_2] = child2;
+    }
+
+    /**
+     *
+     * @param {number} id
+     * @returns {number}
+     */
+    node_get_parent(id) {
+        assert.isNonNegativeInteger(id, 'id');
+        return this.__data_uint32[ELEMENT_WORD_COUNT * id + COLUMN_PARENT];
+    }
+
+    /**
+     *
+     * @param {number} node
+     * @param {number} parent
+     */
+    node_set_parent(node, parent) {
+        this.__data_uint32[ELEMENT_WORD_COUNT * node + COLUMN_PARENT] = parent;
+    }
+
+
+    /**
+     *
+     * @param {number} id
+     * @returns {number}
+     */
+    node_get_height(id) {
+        assert.isNonNegativeInteger(id, 'id');
+        return this.__data_uint32[ELEMENT_WORD_COUNT * id + COLUMN_HEIGHT];
+    }
+
+    /**
+     *
+     * @param {number} id
+     * @param {number} height
+     */
+    node_set_height(id, height) {
+        assert.isNonNegativeInteger(id, 'id');
+        this.__data_uint32[ELEMENT_WORD_COUNT * id + COLUMN_HEIGHT] = height;
+    }
+
+    /**
+     *
+     * @param {number} id
+     * @param {number[]|Float32Array} result
+     */
+    node_get_aabb(id, result) {
+        assert.isNonNegativeInteger(id, 'id');
+
+        const address = ELEMENT_WORD_COUNT * id;
+        const float32 = this.__data_float32;
+
+        result[0] = float32[address];
+        result[1] = float32[address + 1];
+
+        result[2] = float32[address + 2];
+        result[3] = float32[address + 3];
+    }
+
+    /**
+     *
+     * @param {number} id
+     * @param {number[]|ArrayLike<number>} aabb
+     */
+    node_set_aabb(id, aabb) {
+        assert.isNonNegativeInteger(id, 'id');
+
+        assert.notNaN(aabb[0], 'aabb[0] x0');
+        assert.notNaN(aabb[1], 'aabb[1] y0');
+        assert.notNaN(aabb[2], 'aabb[3] x1');
+        assert.notNaN(aabb[3], 'aabb[4] y1');
+
+        const address = ELEMENT_WORD_COUNT * id;
+        const float32 = this.__data_float32;
+
+        float32[address] = aabb[0];
+        float32[address + 1] = aabb[1];
+
+        float32[address + 2] = aabb[2];
+        float32[address + 3] = aabb[3];
+    }
+
+    /**
+     *
+     * @param {number} id
+     * @param {number[]} aabb
+     */
+    node_move_aabb(id, aabb) {
+        // TODO only refit for small changes, and re-insert for large changes
+        this.node_set_aabb(id, aabb);
+
+        const parent = this.__data_uint32[ELEMENT_WORD_COUNT * id + COLUMN_PARENT];
+
+        if (parent !== NULL_NODE) {
+            this.bubble_up_refit(parent);
+        }
+    }
+
+    /**
+     *
+     * @param {number} id
+     * @param {number} x0
+     * @param {number} y0
+     * @param {number} z0
+     * @param {number} x1
+     * @param {number} y1
+     * @param {number} z1
+     */
+    node_set_aabb_primitive(
+        id,
+        x0, y0,
+        x1, y1
+    ) {
+        assert.isNonNegativeInteger(id, 'id');
+
+        const address = ELEMENT_WORD_COUNT * id;
+        const float32 = this.__data_float32;
+
+        float32[address] = x0;
+        float32[address + 1] = y0;
+
+        float32[address + 2] = x1;
+        float32[address + 3] = y1;
+    }
+
+    /**
+     *
+     * @param {number} id
+     * @returns {number}
+     */
+    node_get_surface_area(id) {
+        assert.isNonNegativeInteger(id, 'id');
+
+        const address = ELEMENT_WORD_COUNT * id;
+        const float32 = this.__data_float32;
+
+        const x0 = float32[address];
+        const y0 = float32[address + 1];
+
+        const x1 = float32[address + 2];
+        const y1 = float32[address + 3];
+
+        return aabb2_compute_area(
+            x0, y0,
+            x1, y1
+        );
+    }
+
+    /**
+     *
+     * @param {number} index_a
+     * @param {number} index_b
+     * @returns {number}
+     */
+    node_get_combined_surface_area(index_a, index_b) {
+        const address_a = ELEMENT_WORD_COUNT * index_a;
+        const address_b = ELEMENT_WORD_COUNT * index_b;
+
+        const float32 = this.__data_float32;
+
+        const a_x0 = float32[address_a];
+        const b_x0 = float32[address_b];
+        const x0 = min2(a_x0, b_x0);
+
+        const a_y0 = float32[address_a + 1];
+        const b_y0 = float32[address_b + 1];
+        const y0 = min2(a_y0, b_y0);
+
+        const a_x1 = float32[address_a + 2];
+        const b_x1 = float32[address_b + 2];
+        const x1 = max2(a_x1, b_x1);
+
+        const a_y1 = float32[address_a + 3];
+        const b_y1 = float32[address_b + 3];
+        const y1 = max2(a_y1, b_y1);
+
+
+        return aabb2_compute_area(
+            x0, y0,
+            x1, y1,
+        );
+    }
+
+    /**
+     *
+     * @param {number} destination
+     * @param {number} index_a
+     * @param {number} index_b
+     */
+    node_set_combined_aabb(destination, index_a, index_b) {
+        const address_a = ELEMENT_WORD_COUNT * index_a;
+        const address_b = ELEMENT_WORD_COUNT * index_b;
+
+        const float32 = this.__data_float32;
+
+        const a_x0 = float32[address_a];
+        const b_x0 = float32[address_b];
+        const x0 = min2(a_x0, b_x0);
+
+        const a_y0 = float32[address_a + 1];
+        const b_y0 = float32[address_b + 1];
+        const y0 = min2(a_y0, b_y0);
+
+        const a_x1 = float32[address_a + 2];
+        const b_x1 = float32[address_b + 2];
+        const x1 = max2(a_x1, b_x1);
+
+        const a_y1 = float32[address_a + 3];
+        const b_y1 = float32[address_b + 3];
+        const y1 = max2(a_y1, b_y1);
+
+        const address_destination = destination * ELEMENT_WORD_COUNT;
+
+        float32[address_destination] = x0;
+        float32[address_destination + 1] = y0;
+
+        float32[address_destination + 2] = x1;
+        float32[address_destination + 3] = y1;
+    }
+
+    /**
+     *
+     * @param {number} leaf
+     * @returns {void}
+     */
+    insert_leaf(leaf) {
+        assert.isNonNegativeInteger(leaf, 'leaf');
+        assert.equal(this.node_is_leaf(leaf), true, 'not is not a leaf');
+
+        let uint32 = this.__data_uint32;
+
+        if (this.__root === NULL_NODE) {
+            // special case - no root, set this node as a root
+            this.__root = leaf;
+
+            uint32[leaf * ELEMENT_WORD_COUNT + COLUMN_PARENT] = NULL_NODE;
+
+            return;
+        }
+
+        // Find the best sibling for this node
+        let index = this.__root;
+
+        while (this.node_is_leaf(index) === false) {
+            const node_address = index * ELEMENT_WORD_COUNT;
+
+            const child1 = uint32[node_address + COLUMN_CHILD_1];
+            const child2 = uint32[node_address + COLUMN_CHILD_2];
+
+            const area = this.node_get_surface_area(index);
+
+            const combinedArea = this.node_get_combined_surface_area(index, leaf);
+
+            // Cost of creating a new parent for this node and the new leaf
+            const cost = 2.0 * combinedArea;
+
+            // Minimum cost of pushing the leaf further down the tree
+            const inheritanceCost = 2.0 * (combinedArea - area);
+
+            // Cost of descending into child1
+            let cost1;
+            if (this.node_is_leaf(child1)) {
+                cost1 = this.node_get_combined_surface_area(leaf, child1) + inheritanceCost;
+            } else {
+
+                const oldArea = this.node_get_surface_area(child1);
+                const newArea = this.node_get_combined_surface_area(leaf, child1);
+                cost1 = (newArea - oldArea) + inheritanceCost;
+            }
+
+            // Cost of descending into child2
+            let cost2;
+            if (this.node_is_leaf(child2)) {
+                cost2 = this.node_get_combined_surface_area(leaf, child2) + inheritanceCost;
+            } else {
+                const oldArea = this.node_get_surface_area(child2);
+                const newArea = this.node_get_combined_surface_area(leaf, child2);
+                cost2 = newArea - oldArea + inheritanceCost;
+            }
+
+            // Descend according to the minimum cost.
+            if (cost < cost1 && cost < cost2) {
+                break;
+            }
+
+            // Descend
+            if (cost1 < cost2) {
+                index = child1;
+            } else {
+                index = child2;
+            }
+        }
+
+        const sibling = index;
+
+        // Create a new parent.
+        const oldParent = uint32[sibling * ELEMENT_WORD_COUNT + COLUMN_PARENT];
+        const newParent = this.allocate_node();
+
+        uint32 = this.__data_uint32; // reference can be invalidated after allocation, re-bind
+
+        uint32[newParent * ELEMENT_WORD_COUNT + COLUMN_PARENT] = oldParent;
+        this.node_set_combined_aabb(newParent, leaf, sibling);
+        uint32[newParent * ELEMENT_WORD_COUNT + COLUMN_HEIGHT] = uint32[sibling * ELEMENT_WORD_COUNT + COLUMN_HEIGHT] + 1;
+
+        if (oldParent !== NULL_NODE) {
+            // The sibling was not the root.
+            if (uint32[oldParent * ELEMENT_WORD_COUNT + COLUMN_CHILD_1] === sibling) {
+                uint32[oldParent * ELEMENT_WORD_COUNT + COLUMN_CHILD_1] = newParent;
+            } else {
+                assert.equal(uint32[oldParent * ELEMENT_WORD_COUNT + COLUMN_CHILD_2], sibling);
+
+                uint32[oldParent * ELEMENT_WORD_COUNT + COLUMN_CHILD_2] = newParent;
+            }
+
+        } else {
+            // The sibling was the root.
+            this.__root = newParent;
+        }
+
+        uint32[newParent * ELEMENT_WORD_COUNT + COLUMN_CHILD_1] = sibling;
+        uint32[newParent * ELEMENT_WORD_COUNT + COLUMN_CHILD_2] = leaf;
+
+        uint32[sibling * ELEMENT_WORD_COUNT + COLUMN_PARENT] = newParent;
+        uint32[leaf * ELEMENT_WORD_COUNT + COLUMN_PARENT] = newParent;
+
+        // Walk back up the tree fixing heights and AABBs
+        this.bubble_up_update(newParent);
+
+    }
+
+    /**
+     * refit and update nodes up the tree. Only updates bounds
+     * NOTE: Does not update "height"
+     * @param {number} parent
+     * @private
+     */
+    bubble_up_refit(parent) {
+        let index = parent;
+
+        const uint32 = this.__data_uint32;
+
+        do {
+            index = this.balance(index);
+
+            const address = index * ELEMENT_WORD_COUNT;
+
+            const child1 = uint32[address + COLUMN_CHILD_1];
+            const child2 = uint32[address + COLUMN_CHILD_2];
+
+            assert.notEqual(child1, NULL_NODE, 'child1 is null');
+            assert.notEqual(child2, NULL_NODE, 'child2 is null');
+
+            assert.notEqual(child1, index, 'child1 is equal to parent');
+            assert.notEqual(child2, index, 'child2 is equal to parent');
+
+            this.node_set_combined_aabb(index, child1, child2);
+
+            index = uint32[address + COLUMN_PARENT];
+        } while (index !== NULL_NODE)
+    }
+
+    /**
+     * refit and update nodes up the tree
+     * @param {number} parent
+     * @private
+     */
+    bubble_up_update(parent) {
+        let index = parent;
+
+        const uint32 = this.__data_uint32;
+
+        while (index !== NULL_NODE) {
+            index = this.balance(index);
+
+            const node_address = index * ELEMENT_WORD_COUNT;
+
+            const child1 = uint32[node_address + COLUMN_CHILD_1];
+            const child2 = uint32[node_address + COLUMN_CHILD_2];
+
+            assert.notEqual(child1, NULL_NODE, 'child1 is null');
+            assert.notEqual(child2, NULL_NODE, 'child2 is null');
+
+            assert.notEqual(child1, index, 'child1 is equal to parent');
+            assert.notEqual(child2, index, 'child2 is equal to parent');
+
+            uint32[node_address + COLUMN_HEIGHT] = 1 + max2(
+                uint32[child1 * ELEMENT_WORD_COUNT + COLUMN_HEIGHT],
+                uint32[child2 * ELEMENT_WORD_COUNT + COLUMN_HEIGHT],
+            );
+
+            this.node_set_combined_aabb(index, child1, child2);
+
+            index = uint32[node_address + COLUMN_PARENT];
+        }
+    }
+
+    /**
+     * NOTE: Leaf node is not released, make sure to call {@link #release_node} separately when you no longer need the leaf node
+     * @param {number} leaf
+     * @returns {void}
+     */
+    remove_leaf(leaf) {
+        assert.isNonNegativeInteger(leaf, 'leaf');
+
+        if (leaf === this.__root) {
+            this.__root = NULL_NODE;
+            return;
+        }
+
+        const uint32 = this.__data_uint32;
+
+        const parent = uint32[leaf * ELEMENT_WORD_COUNT + COLUMN_PARENT];
+        const grandParent = uint32[parent * ELEMENT_WORD_COUNT + COLUMN_PARENT];
+
+        let sibling;
+
+        const parent_child1 = uint32[parent * ELEMENT_WORD_COUNT + COLUMN_CHILD_1];
+
+        if (parent_child1 === leaf) {
+            sibling = uint32[parent * ELEMENT_WORD_COUNT + COLUMN_CHILD_2];
+        } else {
+            sibling = parent_child1;
+        }
+
+        if (grandParent !== NULL_NODE) {
+            // Destroy parent and connect sibling to grandParent.
+            const grand_parent_child1 = uint32[grandParent * ELEMENT_WORD_COUNT + COLUMN_CHILD_1];
+            if (grand_parent_child1 === parent) {
+                uint32[grandParent * ELEMENT_WORD_COUNT + COLUMN_CHILD_1] = sibling;
+            } else {
+                uint32[grandParent * ELEMENT_WORD_COUNT + COLUMN_CHILD_2] = sibling;
+            }
+            uint32[sibling * ELEMENT_WORD_COUNT + COLUMN_PARENT] = grandParent;
+
+            this.release_node(parent);
+
+            // Adjust ancestor bounds.
+            this.bubble_up_update(grandParent);
+
+        } else {
+            this.__root = sibling;
+            uint32[sibling * ELEMENT_WORD_COUNT + COLUMN_PARENT] = NULL_NODE;
+            this.release_node(parent);
+        }
+    }
+
+    /**
+     * Perform a left or right rotation if node A is imbalanced.
+     * Returns the new root index.
+     * @param {number} iA
+     * @returns {number}
+     * @private
+     */
+    balance(iA) {
+        assert.notEqual(iA, NULL_NODE, 'input is a null node');
+
+        //b2TreeNode* A = m_nodes + iA;
+        const uint32 = this.__data_uint32;
+
+        if (this.node_is_leaf(iA) || uint32[iA * ELEMENT_WORD_COUNT + COLUMN_HEIGHT] < 2) {
+            return iA;
+        }
+
+        const iB = uint32[iA * ELEMENT_WORD_COUNT + COLUMN_CHILD_1];
+        const iC = uint32[iA * ELEMENT_WORD_COUNT + COLUMN_CHILD_2];
+
+        assert.notEqual(iA, iB, 'child1 equal to parent');
+        assert.notEqual(iA, iB, 'child2 equal to parent');
+
+        assert.greaterThanOrEqual(iB, 0);
+        assert.lessThan(iB, this.node_capacity);
+        assert.greaterThanOrEqual(iC, 0);
+        assert.lessThan(iC, this.node_capacity);
+
+        //b2TreeNode* B = m_nodes + iB;
+        //b2TreeNode* C = m_nodes + iC;
+
+        const balance = uint32[iC * ELEMENT_WORD_COUNT + COLUMN_HEIGHT] - uint32[iB * ELEMENT_WORD_COUNT + COLUMN_HEIGHT];
+
+        // Rotate C up
+        if (balance > 1) {
+            const iF = uint32[iC * ELEMENT_WORD_COUNT + COLUMN_CHILD_1];
+            const iG = uint32[iC * ELEMENT_WORD_COUNT + COLUMN_CHILD_2];
+
+            assert.notEqual(iC, iF, 'child1 equal to parent');
+            assert.notEqual(iC, iG, 'child2 equal to parent');
+
+            // b2TreeNode* F = m_nodes + iF;
+            // b2TreeNode* G = m_nodes + iG;
+
+            assert.greaterThanOrEqual(iF, 0);
+            assert.lessThan(iF, this.node_capacity);
+            assert.greaterThanOrEqual(iG, 0);
+            assert.lessThan(iG, this.node_capacity);
+
+            // Swap A and C
+            uint32[iC * ELEMENT_WORD_COUNT + COLUMN_CHILD_1] = iA;
+
+            const a_parent = uint32[iA * ELEMENT_WORD_COUNT + COLUMN_PARENT];
+
+            uint32[iC * ELEMENT_WORD_COUNT + COLUMN_PARENT] = a_parent;
+            uint32[iA * ELEMENT_WORD_COUNT + COLUMN_PARENT] = iC;
+
+            // A's old parent should point to C
+            if (a_parent !== NULL_NODE) {
+                assert.notEqual(a_parent, iC);
+                if (uint32[a_parent * ELEMENT_WORD_COUNT + COLUMN_CHILD_1] === iA) {
+                    uint32[a_parent * ELEMENT_WORD_COUNT + COLUMN_CHILD_1] = iC;
+                } else {
+                    //b2Assert(m_nodes[C->parent].child2 == iA);
+                    uint32[a_parent * ELEMENT_WORD_COUNT + COLUMN_CHILD_2] = iC;
+                }
+            } else {
+                this.__root = iC;
+            }
+
+            // Rotate
+            if (uint32[iF * ELEMENT_WORD_COUNT + COLUMN_HEIGHT] > uint32[iG * ELEMENT_WORD_COUNT + COLUMN_HEIGHT]) {
+
+                uint32[iC * ELEMENT_WORD_COUNT + COLUMN_CHILD_2] = iF;
+                uint32[iA * ELEMENT_WORD_COUNT + COLUMN_CHILD_2] = iG;
+
+                uint32[iG * ELEMENT_WORD_COUNT + COLUMN_PARENT] = iA;
+
+                this.node_set_combined_aabb(iA, iB, iG);
+                this.node_set_combined_aabb(iC, iA, iF);
+
+                uint32[iA * ELEMENT_WORD_COUNT + COLUMN_HEIGHT] = 1 + max2(
+                    uint32[iB * ELEMENT_WORD_COUNT + COLUMN_HEIGHT],
+                    uint32[iG * ELEMENT_WORD_COUNT + COLUMN_HEIGHT]
+                );
+
+                uint32[iC * ELEMENT_WORD_COUNT + COLUMN_HEIGHT] = 1 + max2(
+                    uint32[iA * ELEMENT_WORD_COUNT + COLUMN_HEIGHT],
+                    uint32[iF * ELEMENT_WORD_COUNT + COLUMN_HEIGHT]
+                );
+
+            } else {
+                uint32[iC * ELEMENT_WORD_COUNT + COLUMN_CHILD_2] = iG;
+                uint32[iA * ELEMENT_WORD_COUNT + COLUMN_CHILD_2] = iF;
+
+                uint32[iF * ELEMENT_WORD_COUNT + COLUMN_PARENT] = iA;
+
+                this.node_set_combined_aabb(iA, iB, iF);
+                this.node_set_combined_aabb(iC, iA, iG);
+
+                uint32[iA * ELEMENT_WORD_COUNT + COLUMN_HEIGHT] = 1 + max2(
+                    uint32[iB * ELEMENT_WORD_COUNT + COLUMN_HEIGHT],
+                    uint32[iF * ELEMENT_WORD_COUNT + COLUMN_HEIGHT]
+                );
+
+                uint32[iC * ELEMENT_WORD_COUNT + COLUMN_HEIGHT] = 1 + max2(
+                    uint32[iA * ELEMENT_WORD_COUNT + COLUMN_HEIGHT],
+                    uint32[iG * ELEMENT_WORD_COUNT + COLUMN_HEIGHT]
+                );
+
+            }
+
+            assert.notEqual(iC, uint32[iC * ELEMENT_WORD_COUNT + COLUMN_CHILD_1]);
+            assert.notEqual(iC, uint32[iC * ELEMENT_WORD_COUNT + COLUMN_CHILD_2]);
+
+            return iC;
+        }
+
+        // Rotate B up
+        if (balance < -1) {
+            const iD = uint32[iB * ELEMENT_WORD_COUNT + COLUMN_CHILD_1];
+            const iE = uint32[iB * ELEMENT_WORD_COUNT + COLUMN_CHILD_2];
+
+            assert.notEqual(iB, iD, 'child1 equal to parent');
+            assert.notEqual(iB, iE, 'child2 equal to parent');
+
+            assert.greaterThanOrEqual(iD, 0);
+            assert.lessThan(iD, this.node_capacity);
+            assert.greaterThanOrEqual(iE, 0);
+            assert.lessThan(iE, this.node_capacity);
+
+            // Swap A and B
+            uint32[iB * ELEMENT_WORD_COUNT + COLUMN_CHILD_1] = iA;
+            const a_parent = uint32[iA * ELEMENT_WORD_COUNT + COLUMN_PARENT];
+            uint32[iB * ELEMENT_WORD_COUNT + COLUMN_PARENT] = a_parent;
+            uint32[iA * ELEMENT_WORD_COUNT + COLUMN_PARENT] = iB;
+
+            // A's old parent should point to B
+            if (a_parent !== NULL_NODE) {
+                if (uint32[a_parent * ELEMENT_WORD_COUNT + COLUMN_CHILD_1] === iA) {
+                    uint32[a_parent * ELEMENT_WORD_COUNT + COLUMN_CHILD_1] = iB;
+                } else {
+                    //b2Assert(m_nodes[B->parent].child2 == iA);
+                    assert.equal(uint32[uint32[iB * ELEMENT_WORD_COUNT + COLUMN_PARENT] * ELEMENT_WORD_COUNT + COLUMN_CHILD_2], iA);
+
+                    uint32[a_parent * ELEMENT_WORD_COUNT + COLUMN_CHILD_2] = iB;
+                }
+            } else {
+                this.__root = iB;
+            }
+
+            // Rotate
+            if (uint32[iD * ELEMENT_WORD_COUNT + COLUMN_HEIGHT] > uint32[iE * ELEMENT_WORD_COUNT + COLUMN_HEIGHT]) {
+
+                uint32[iB * ELEMENT_WORD_COUNT + COLUMN_CHILD_2] = iD;
+                uint32[iA * ELEMENT_WORD_COUNT + COLUMN_CHILD_1] = iE;
+                uint32[iE * ELEMENT_WORD_COUNT + COLUMN_PARENT] = iA;
+
+                this.node_set_combined_aabb(iA, iC, iE);
+                this.node_set_combined_aabb(iB, iA, iD);
+
+                uint32[iA * ELEMENT_WORD_COUNT + COLUMN_HEIGHT] = 1 + max2(
+                    uint32[iC * ELEMENT_WORD_COUNT + COLUMN_HEIGHT],
+                    uint32[iE * ELEMENT_WORD_COUNT + COLUMN_HEIGHT]
+                );
+
+                uint32[iB * ELEMENT_WORD_COUNT + COLUMN_HEIGHT] = 1 + max2(
+                    uint32[iA * ELEMENT_WORD_COUNT + COLUMN_HEIGHT],
+                    uint32[iD * ELEMENT_WORD_COUNT + COLUMN_HEIGHT]
+                );
+
+            } else {
+
+                uint32[iB * ELEMENT_WORD_COUNT + COLUMN_CHILD_2] = iE;
+                uint32[iA * ELEMENT_WORD_COUNT + COLUMN_CHILD_1] = iD;
+                uint32[iD * ELEMENT_WORD_COUNT + COLUMN_PARENT] = iA;
+
+                this.node_set_combined_aabb(iA, iC, iD);
+                this.node_set_combined_aabb(iB, iA, iE);
+
+                uint32[iA * ELEMENT_WORD_COUNT + COLUMN_HEIGHT] = 1 + max2(
+                    uint32[iC * ELEMENT_WORD_COUNT + COLUMN_HEIGHT],
+                    uint32[iD * ELEMENT_WORD_COUNT + COLUMN_HEIGHT]
+                );
+
+                uint32[iB * ELEMENT_WORD_COUNT + COLUMN_HEIGHT] = 1 + max2(
+                    uint32[iA * ELEMENT_WORD_COUNT + COLUMN_HEIGHT],
+                    uint32[iE * ELEMENT_WORD_COUNT + COLUMN_HEIGHT]
+                );
+
+            }
+
+            assert.notEqual(iB, uint32[iB * ELEMENT_WORD_COUNT + COLUMN_CHILD_1]);
+            assert.notEqual(iB, uint32[iB * ELEMENT_WORD_COUNT + COLUMN_CHILD_2]);
+
+            return iB;
+        }
+
+        assert.notEqual(iA, uint32[iA * ELEMENT_WORD_COUNT + COLUMN_CHILD_1]);
+        assert.notEqual(iA, uint32[iA * ELEMENT_WORD_COUNT + COLUMN_CHILD_2]);
+
+        // no rotation
+        return iA;
+    }
+
+    /**
+     * Release all nodes, this essentially resets the tree to empty state
+     * NOTE: For performance reasons, released memory is not reset, this means that attempting to access cleared nodes' memory will yield garbage data
+     */
+    release_all() {
+        this.__root = NULL_NODE;
+        this.__size = 0;
+        this.__free_pointer = 0;
+    }
+
+    /**
+     *
+     * @param {function(node:number, tree:BVH):void} callback
+     * @param {*} [ctx]
+     */
+    traverse(callback, ctx) {
+        let cursor = 0;
+        const stack = [];
+
+        const root = this.__root;
+
+        if (root !== NULL_NODE) {
+            stack[cursor++] = root;
+        }
+
+        const uint32 = this.__data_uint32;
+
+        while (cursor > 0) {
+            cursor--;
+
+            const node = stack[cursor];
+
+            callback.call(ctx, node, this);
+
+            const node_address = node * ELEMENT_WORD_COUNT;
+
+            const child1 = uint32[node_address + COLUMN_CHILD_1];
+            const child2 = uint32[node_address + COLUMN_CHILD_2];
+
+            if (child1 !== NULL_NODE) {
+                stack[cursor++] = child2;
+                stack[cursor++] = child1;
+            }
+
+        }
+    }
+
+    /**
+     *
+     * @param {number[]} destination
+     * @param {number} destination_offset
+     * @returns {number}
+     */
+    collect_nodes_all(destination, destination_offset) {
+
+        let i = destination_offset;
+
+        this.traverse(n => {
+            destination[i++] = n;
+        });
+
+        return i - destination_offset;
+    }
+
+    /**
+     * Update parent and child links of a given node to point to a new location, useful for re-locating nodes
+     * @param {number} node node to update
+     * @param {number} destination Where updated links should point to
+     * @private
+     */
+    __move_node_links(node, destination) {
+
+        const uint32 = this.__data_uint32;
+
+        const source_address = node * ELEMENT_WORD_COUNT;
+
+        // update children of a
+        const child1 = uint32[source_address + COLUMN_CHILD_1];
+        const child2 = uint32[source_address + COLUMN_CHILD_2];
+
+        if (child1 !== NULL_NODE) {
+
+            uint32[child1 * ELEMENT_WORD_COUNT + COLUMN_PARENT] = destination;
+            uint32[child2 * ELEMENT_WORD_COUNT + COLUMN_PARENT] = destination;
+
+        }
+
+        // update parent of a
+        const parent = uint32[source_address + COLUMN_PARENT];
+
+        if (parent !== NULL_NODE) {
+            const parent_child1 = uint32[parent * ELEMENT_WORD_COUNT + COLUMN_CHILD_1];
+
+            if (parent_child1 === node) {
+                uint32[parent * ELEMENT_WORD_COUNT + COLUMN_CHILD_1] = destination;
+            } else {
+                uint32[parent * ELEMENT_WORD_COUNT + COLUMN_CHILD_2] = destination;
+            }
+        }
+    }
+
+    /**
+     * Swap two nodes in memory
+     * @param {number} a
+     * @param {number} b
+     * @returns {boolean}
+     */
+    swap_nodes(a, b) {
+        // console.log(`swap ${a} - ${b}`)
+
+        const uint32 = this.__data_uint32;
+
+        const address_a = a * ELEMENT_WORD_COUNT;
+        const address_b = b * ELEMENT_WORD_COUNT;
+
+
+        if (uint32[address_a + COLUMN_PARENT] === b) {
+            // attempting to swap direct parent/child, this is unsupported
+            return false;
+        }
+        if (uint32[address_b + COLUMN_PARENT] === a) {
+            // attempting to swap direct parent/child, this is unsupported
+            return false;
+        }
+
+        this.__move_node_links(a, b);
+        this.__move_node_links(b, a);
+
+        // copy A to temp buffer
+        array_copy(uint32, address_a, this.__free, this.__free_pointer, ELEMENT_WORD_COUNT);
+
+        // write data
+        array_copy(uint32, address_b, uint32, address_a, ELEMENT_WORD_COUNT);
+        array_copy(this.__free, this.__free_pointer, uint32, address_b, ELEMENT_WORD_COUNT);
+
+        // update root as necessary
+        if (this.__root === a) {
+            this.__root = b;
+        } else if (this.__root === b) {
+            this.__root = a;
+        }
+
+        return true;
+    }
+}
+