BVH broadphase creates objects with updated AABB to avoid extra checks

When set_static is called on a newly added object, the forced collision
check in BVH set_pairable was using an empty AABB, which caused
unnecessary collision checks at the origin, then a call to move was
checking again at the right position.

These changes ensure broadphase objects are added to the BVH tree with
proper AABB so collision checks are correctly done right away.

Octree & Basic broadphase trees are not affected by these changes.

servers/physics/broad_phase_basic.cpp

@@ -32,7 +32,7 @@
 #include "core/list.h"
 #include "core/print_string.h"
 
-BroadPhaseSW::ID BroadPhaseBasic::create(CollisionObjectSW *p_object, int p_subindex) {
+BroadPhaseSW::ID BroadPhaseBasic::create(CollisionObjectSW *p_object, int p_subindex, const AABB &p_aabb) {
 
 	ERR_FAIL_COND_V(p_object == NULL, 0);
 

servers/physics/broad_phase_basic.h

@@ -83,7 +83,7 @@ class BroadPhaseBasic : public BroadPhaseSW {
 
 public:
 	// 0 is an invalid ID
-	virtual ID create(CollisionObjectSW *p_object, int p_subindex = 0);
+	virtual ID create(CollisionObjectSW *p_object, int p_subindex = 0, const AABB &p_aabb = AABB());
 	virtual void move(ID p_id, const AABB &p_aabb);
 	virtual void set_static(ID p_id, bool p_static);
 	virtual void remove(ID p_id);

servers/physics/broad_phase_bvh.cpp

@@ -32,9 +32,9 @@
 #include "collision_object_sw.h"
 #include "core/project_settings.h"
 
-BroadPhaseSW::ID BroadPhaseBVH::create(CollisionObjectSW *p_object, int p_subindex) {
+BroadPhaseSW::ID BroadPhaseBVH::create(CollisionObjectSW *p_object, int p_subindex, const AABB &p_aabb) {
 
-	ID oid = bvh.create(p_object, AABB(), p_subindex, false, 1 << p_object->get_type(), 0);
+	ID oid = bvh.create(p_object, p_aabb, p_subindex, false, 1 << p_object->get_type(), 0);
 	return oid + 1;
 }
 

servers/physics/broad_phase_bvh.h

@@ -48,7 +48,7 @@ class BroadPhaseBVH : public BroadPhaseSW {
 
 public:
 	// 0 is an invalid ID
-	virtual ID create(CollisionObjectSW *p_object, int p_subindex = 0);
+	virtual ID create(CollisionObjectSW *p_object, int p_subindex = 0, const AABB &p_aabb = AABB());
 	virtual void move(ID p_id, const AABB &p_aabb);
 	virtual void set_static(ID p_id, bool p_static);
 	virtual void remove(ID p_id);

servers/physics/broad_phase_octree.cpp

@@ -31,7 +31,7 @@
 #include "broad_phase_octree.h"
 #include "collision_object_sw.h"
 
-BroadPhaseSW::ID BroadPhaseOctree::create(CollisionObjectSW *p_object, int p_subindex) {
+BroadPhaseSW::ID BroadPhaseOctree::create(CollisionObjectSW *p_object, int p_subindex, const AABB &p_aabb) {
 
 	ID oid = octree.create(p_object, AABB(), p_subindex, false, 1 << p_object->get_type(), 0);
 	return oid;

servers/physics/broad_phase_octree.h

@@ -48,7 +48,7 @@ class BroadPhaseOctree : public BroadPhaseSW {
 
 public:
 	// 0 is an invalid ID
-	virtual ID create(CollisionObjectSW *p_object, int p_subindex = 0);
+	virtual ID create(CollisionObjectSW *p_object, int p_subindex = 0, const AABB &p_aabb = AABB());
 	virtual void move(ID p_id, const AABB &p_aabb);
 	virtual void set_static(ID p_id, bool p_static);
 	virtual void remove(ID p_id);

servers/physics/broad_phase_sw.h

@@ -49,7 +49,7 @@ public:
 	typedef void (*UnpairCallback)(CollisionObjectSW *A, int p_subindex_A, CollisionObjectSW *B, int p_subindex_B, void *p_data, void *p_userdata);
 
 	// 0 is an invalid ID
-	virtual ID create(CollisionObjectSW *p_object_, int p_subindex = 0) = 0;
+	virtual ID create(CollisionObjectSW *p_object_, int p_subindex = 0, const AABB &p_aabb = AABB()) = 0;
 	virtual void move(ID p_id, const AABB &p_aabb) = 0;
 	virtual void set_static(ID p_id, bool p_static) = 0;
 	virtual void remove(ID p_id) = 0;

servers/physics/collision_object_sw.cpp

@@ -150,12 +150,7 @@ void CollisionObjectSW::_update_shapes() {
 		return;
 
 	for (int i = 0; i < shapes.size(); i++) {
-
 		Shape &s = shapes.write[i];
-		if (s.bpid == 0) {
-			s.bpid = space->get_broadphase()->create(this, i);
-			space->get_broadphase()->set_static(s.bpid, _static);
-		}
 
 		//not quite correct, should compute the next matrix..
 		AABB shape_aabb = s.shape->get_aabb();
@@ -167,6 +162,10 @@ void CollisionObjectSW::_update_shapes() {
 		Vector3 scale = xform.get_basis().get_scale();
 		s.area_cache = s.shape->get_area() * scale.x * scale.y * scale.z;
 
+		if (s.bpid == 0) {
+			s.bpid = space->get_broadphase()->create(this, i, s.aabb_cache);
+			space->get_broadphase()->set_static(s.bpid, _static);
+		}
 		space->get_broadphase()->move(s.bpid, s.aabb_cache);
 	}
 }
@@ -177,12 +176,7 @@ void CollisionObjectSW::_update_shapes_with_motion(const Vector3 &p_motion) {
 		return;
 
 	for (int i = 0; i < shapes.size(); i++) {
-
 		Shape &s = shapes.write[i];
-		if (s.bpid == 0) {
-			s.bpid = space->get_broadphase()->create(this, i);
-			space->get_broadphase()->set_static(s.bpid, _static);
-		}
 
 		//not quite correct, should compute the next matrix..
 		AABB shape_aabb = s.shape->get_aabb();
@@ -191,6 +185,10 @@ void CollisionObjectSW::_update_shapes_with_motion(const Vector3 &p_motion) {
 		shape_aabb = shape_aabb.merge(AABB(shape_aabb.position + p_motion, shape_aabb.size)); //use motion
 		s.aabb_cache = shape_aabb;
 
+		if (s.bpid == 0) {
+			s.bpid = space->get_broadphase()->create(this, i, s.aabb_cache);
+			space->get_broadphase()->set_static(s.bpid, _static);
+		}
 		space->get_broadphase()->move(s.bpid, shape_aabb);
 	}
 }