1 files changed, 48 insertions, 20 deletions

diff --git a/modules/bullet/rigid_body_bullet.cpp b/modules/bullet/rigid_body_bullet.cpp
index d9a77885b3..9dd04100ed 100644
--- a/modules/bullet/rigid_body_bullet.cpp
+++ b/modules/bullet/rigid_body_bullet.cpp
@@ -265,13 +265,14 @@ RigidBodyBullet::RigidBodyBullet() :
 		angularDamp(0),
 		can_sleep(true),
 		omit_forces_integration(false),
+		can_integrate_forces(false),
 		maxCollisionsDetection(0),
 		collisionsCount(0),
+		prev_collision_count(0),
 		maxAreasWhereIam(10),
 		areaWhereIamCount(0),
 		countGravityPointSpaces(0),
 		isScratchedSpaceOverrideModificator(false),
-		isTransformChanged(false),
 		previousActiveState(true),
 		force_integration_callback(NULL) {
 
@@ -279,9 +280,10 @@ RigidBodyBullet::RigidBodyBullet() :
 
 	// Initial properties
 	const btVector3 localInertia(0, 0, 0);
-	btRigidBody::btRigidBodyConstructionInfo cInfo(mass, godotMotionState, BulletPhysicsServer::get_empty_shape(), localInertia);
+	btRigidBody::btRigidBodyConstructionInfo cInfo(mass, godotMotionState, NULL, localInertia);
 
 	btBody = bulletnew(btRigidBody(cInfo));
+	reload_shapes();
 	setupBulletCollisionObject(btBody);
 
 	set_mode(PhysicsServer::BODY_MODE_RIGID);
@@ -292,6 +294,9 @@ RigidBodyBullet::RigidBodyBullet() :
 		areasWhereIam.write[i] = NULL;
 	}
 	btBody->setSleepingThresholds(0.2, 0.2);
+
+	prev_collision_traces = &collision_traces_1;
+	curr_collision_traces = &collision_traces_2;
 }
 
 RigidBodyBullet::~RigidBodyBullet() {
@@ -314,11 +319,9 @@ void RigidBodyBullet::destroy_kinematic_utilities() {
 	}
 }
 
-void RigidBodyBullet::main_shape_resetted() {
-	if (get_main_shape())
-		btBody->setCollisionShape(get_main_shape());
-	else
-		btBody->setCollisionShape(BulletPhysicsServer::get_empty_shape());
+void RigidBodyBullet::main_shape_changed() {
+	CRASH_COND(!get_main_shape())
+	btBody->setCollisionShape(get_main_shape());
 	set_continuous_collision_detection(is_continuous_collision_detection_enabled()); // Reset
 }
 
@@ -333,7 +336,7 @@ void RigidBodyBullet::reload_body() {
 void RigidBodyBullet::set_space(SpaceBullet *p_space) {
 	// Clear the old space if there is one
 	if (space) {
-		isTransformChanged = false;
+		can_integrate_forces = false;
 
 		// Remove all eventual constraints
 		assert_no_constraints();
@@ -350,8 +353,8 @@ void RigidBodyBullet::set_space(SpaceBullet *p_space) {
 }
 
 void RigidBodyBullet::dispatch_callbacks() {
-	/// The check isTransformChanged is necessary in order to call integrated forces only when the first transform is sent
-	if ((btBody->isKinematicObject() || btBody->isActive() || previousActiveState != btBody->isActive()) && force_integration_callback && isTransformChanged) {
+	/// The check isFirstTransformChanged is necessary in order to call integrated forces only when the first transform is sent
+	if ((btBody->isKinematicObject() || btBody->isActive() || previousActiveState != btBody->isActive()) && force_integration_callback && can_integrate_forces) {
 
 		if (omit_forces_integration)
 			btBody->clearForces();
@@ -400,10 +403,6 @@ void RigidBodyBullet::set_force_integration_callback(ObjectID p_id, const String
 	}
 }
 
-void RigidBodyBullet::scratch() {
-	isTransformChanged = true;
-}
-
 void RigidBodyBullet::scratch_space_override_modificator() {
 	isScratchedSpaceOverrideModificator = true;
 }
@@ -415,7 +414,19 @@ void RigidBodyBullet::on_collision_filters_change() {
 }
 
 void RigidBodyBullet::on_collision_checker_start() {
+
+	prev_collision_count = collisionsCount;
 	collisionsCount = 0;
+
+	// Swap array
+	Vector<RigidBodyBullet *> *s = prev_collision_traces;
+	prev_collision_traces = curr_collision_traces;
+	curr_collision_traces = s;
+}
+
+void RigidBodyBullet::on_collision_checker_end() {
+	// Always true if active and not a static or kinematic body
+	isTransformChanged = btBody->isActive() && !btBody->isStaticOrKinematicObject();
 }
 
 bool RigidBodyBullet::add_collision_object(RigidBodyBullet *p_otherObject, const Vector3 &p_hitWorldLocation, const Vector3 &p_hitLocalLocation, const Vector3 &p_hitNormal, const float &p_appliedImpulse, int p_other_shape_index, int p_local_shape_index) {
@@ -433,10 +444,20 @@ bool RigidBodyBullet::add_collision_object(RigidBodyBullet *p_otherObject, const
 	cd.other_object_shape = p_other_shape_index;
 	cd.local_shape = p_local_shape_index;
 
+	curr_collision_traces->write[collisionsCount] = p_otherObject;
+
 	++collisionsCount;
 	return true;
 }
 
+bool RigidBodyBullet::was_colliding(RigidBodyBullet *p_other_object) {
+	for (int i = prev_collision_count - 1; 0 <= i; --i) {
+		if ((*prev_collision_traces)[i] == p_other_object)
+			return true;
+	}
+	return false;
+}
+
 void RigidBodyBullet::assert_no_constraints() {
 	if (btBody->getNumConstraintRefs()) {
 		WARN_PRINT("A body with a joints is destroyed. Please check the implementation in order to destroy the joint before the body.");
@@ -520,7 +541,7 @@ real_t RigidBodyBullet::get_param(PhysicsServer::BodyParameter p_param) const {
 
 void RigidBodyBullet::set_mode(PhysicsServer::BodyMode p_mode) {
 	// This is necessary to block force_integration untile next move
-	isTransformChanged = false;
+	can_integrate_forces = false;
 	destroy_kinematic_utilities();
 	// The mode change is relevant to its mass
 	switch (p_mode) {
@@ -777,8 +798,7 @@ void RigidBodyBullet::set_transform__bullet(const btTransform &p_global_transfor
 		// The kinematic use MotionState class
 		godotMotionState->moveBody(p_global_transform);
 	}
-	btBody->setWorldTransform(p_global_transform);
-	scratch();
+	CollisionObjectBullet::set_transform__bullet(p_global_transform);
 }
 
 const btTransform &RigidBodyBullet::get_transform__bullet() const {
@@ -791,14 +811,17 @@ const btTransform &RigidBodyBullet::get_transform__bullet() const {
 	}
 }
 
-void RigidBodyBullet::on_shapes_changed() {
-	RigidCollisionObjectBullet::on_shapes_changed();
+void RigidBodyBullet::reload_shapes() {
+	RigidCollisionObjectBullet::reload_shapes();
 
 	const btScalar invMass = btBody->getInvMass();
 	const btScalar mass = invMass == 0 ? 0 : 1 / invMass;
 
 	if (mainShape) {
-		btVector3 inertia;
+		// inertia initialised zero here because some of bullet's collision
+		// shapes incorrectly do not set the vector in calculateLocalIntertia.
+		// Arbitrary zero is preferable to undefined behaviour.
+		btVector3 inertia(0, 0, 0);
 		mainShape->calculateLocalInertia(mass, inertia);
 		btBody->setMassProps(mass, inertia);
 	}
@@ -987,6 +1010,11 @@ void RigidBodyBullet::reload_kinematic_shapes() {
 	kinematic_utilities->copyAllOwnerShapes();
 }
 
+void RigidBodyBullet::notify_transform_changed() {
+	RigidCollisionObjectBullet::notify_transform_changed();
+	can_integrate_forces = true;
+}
+
 void RigidBodyBullet::_internal_set_mass(real_t p_mass) {
 
 	btVector3 localInertia(0, 0, 0);