diff --git a/core/math/bvh_debug.inc b/core/math/bvh_debug.inc
index 4a5df741aab..6ff5be54ff8 100644
--- a/core/math/bvh_debug.inc
+++ b/core/math/bvh_debug.inc
@@ -7,19 +7,19 @@ void _debug_recursive_print_tree(int p_tree_id) const {
 
 String _debug_aabb_to_string(const BVH_ABB &aabb) const {
 	String sz = "(";
-	sz += itos(-aabb.neg_min.x);
+	sz += itos(aabb.min.x);
 	sz += " ~ ";
-	sz += itos(aabb.max.x);
+	sz += itos(-aabb.neg_max.x);
 	sz += ") (";
 
-	sz += itos(-aabb.neg_min.y);
+	sz += itos(aabb.min.y);
 	sz += " ~ ";
-	sz += itos(aabb.max.y);
+	sz += itos(-aabb.neg_max.y);
 	sz += ") (";
 
-	sz += itos(-aabb.neg_min.z);
+	sz += itos(aabb.min.z);
 	sz += " ~ ";
-	sz += itos(aabb.max.z);
+	sz += itos(-aabb.neg_max.z);
 	sz += ") ";
 
 	Vector3 size = aabb.calculate_size();
@@ -41,14 +41,14 @@ void _debug_recursive_print_tree_node(uint32_t p_node_id, int depth = 0) const {
 	if (tnode.is_leaf()) {
 		sz += " L";
 		sz += itos(tnode.height) + " ";
-		const TLeaf *leaf = node_get_leaf(tnode);
+		const TLeaf &leaf = _node_get_leaf(tnode);
 
 		sz += "[";
-		for (int n = 0; n < leaf->num_items; n++) {
+		for (int n = 0; n < leaf.num_items; n++) {
 			if (n)
 				sz += ", ";
 			sz += "r";
-			sz += itos(leaf->get_item_ref_id(n));
+			sz += itos(leaf.get_item_ref_id(n));
 		}
 		sz += "]  ";
 	} else {
diff --git a/core/math/bvh_public.inc b/core/math/bvh_public.inc
index bb293fa0e3e..742c7589928 100644
--- a/core/math/bvh_public.inc
+++ b/core/math/bvh_public.inc
@@ -2,7 +2,7 @@ public:
 BVHHandle item_add(T *p_userdata, const AABB &p_aabb, int32_t p_subindex, bool p_pairable, uint32_t p_pairable_type, uint32_t p_pairable_mask, bool p_invisible = false) {
 #ifdef BVH_VERBOSE_TREE
 	VERBOSE_PRINT("\nitem_add BEFORE");
-	_recursive_print_tree();
+	_debug_recursive_print_tree(0);
 	VERBOSE_PRINT("\n");
 #endif
 
@@ -182,7 +182,7 @@ void item_remove(BVHHandle p_handle) {
 	//refit_all(_current_tree);
 
 #ifdef BVH_VERBOSE_TREE
-	_recursive_print_tree(tree_id);
+	_debug_recursive_print_tree(_current_tree);
 #endif
 }
 
@@ -281,6 +281,7 @@ void incremental_optimize() {
 	_logic_item_remove_and_reinsert(ref_id);
 
 #ifdef BVH_VERBOSE
+	/*
 	// memory use
 	int mem_refs = _refs.estimate_memory_use();
 	int mem_nodes = _nodes.estimate_memory_use();
@@ -292,7 +293,7 @@ void incremental_optimize() {
 	sz += "mem_leaves : " + itos(mem_leaves) + " ";
 	sz += ", num nodes : " + itos(_nodes.size());
 	print_line(sz);
-
+	*/
 #endif
 }
 
diff --git a/servers/physics/broad_phase_bvh.cpp b/servers/physics/broad_phase_bvh.cpp
index 8af2bae5797..9f09a71ff17 100644
--- a/servers/physics/broad_phase_bvh.cpp
+++ b/servers/physics/broad_phase_bvh.cpp
@@ -35,37 +35,37 @@
 BroadPhaseSW::ID BroadPhaseBVH::create(CollisionObjectSW *p_object, int p_subindex) {
 
 	ID oid = bvh.create(p_object, AABB(), p_subindex, false, 1 << p_object->get_type(), 0);
-	return oid;
+	return oid + 1;
 }
 
 void BroadPhaseBVH::move(ID p_id, const AABB &p_aabb) {
 
-	bvh.move(p_id, p_aabb);
+	bvh.move(p_id - 1, p_aabb);
 }
 
 void BroadPhaseBVH::set_static(ID p_id, bool p_static) {
 
-	CollisionObjectSW *it = bvh.get(p_id);
-	bvh.set_pairable(p_id, !p_static, 1 << it->get_type(), p_static ? 0 : 0xFFFFF); //pair everything, don't care 1?
+	CollisionObjectSW *it = bvh.get(p_id - 1);
+	bvh.set_pairable(p_id - 1, !p_static, 1 << it->get_type(), p_static ? 0 : 0xFFFFF); //pair everything, don't care 1?
 }
 void BroadPhaseBVH::remove(ID p_id) {
 
-	bvh.erase(p_id);
+	bvh.erase(p_id - 1);
 }
 
 CollisionObjectSW *BroadPhaseBVH::get_object(ID p_id) const {
 
-	CollisionObjectSW *it = bvh.get(p_id);
+	CollisionObjectSW *it = bvh.get(p_id - 1);
 	ERR_FAIL_COND_V(!it, NULL);
 	return it;
 }
 bool BroadPhaseBVH::is_static(ID p_id) const {
 
-	return !bvh.is_pairable(p_id);
+	return !bvh.is_pairable(p_id - 1);
 }
 int BroadPhaseBVH::get_subindex(ID p_id) const {
 
-	return bvh.get_subindex(p_id);
+	return bvh.get_subindex(p_id - 1);
 }
 
 int BroadPhaseBVH::cull_point(const Vector3 &p_point, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices) {