Merge pull request #12713 from AndreaCatania/master

Rewritten kinematic system

modules/bullet/bullet_physics_server.cpp

@@ -635,6 +635,28 @@ float BulletPhysicsServer::body_get_param(RID p_body, BodyParameter p_param) con
 	return body->get_param(p_param);
 }
 
+void BulletPhysicsServer::body_set_kinematic_safe_margin(RID p_body, real_t p_margin) {
+	RigidBodyBullet *body = rigid_body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	if (body->get_kinematic_utilities()) {
+
+		body->get_kinematic_utilities()->setSafeMargin(p_margin);
+	}
+}
+
+real_t BulletPhysicsServer::body_get_kinematic_safe_margin(RID p_body) const {
+	RigidBodyBullet *body = rigid_body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body, 0);
+
+	if (body->get_kinematic_utilities()) {
+
+		return body->get_kinematic_utilities()->safe_margin;
+	}
+
+	return 0;
+}
+
 void BulletPhysicsServer::body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) {
 	RigidBodyBullet *body = rigid_body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
@@ -797,12 +819,12 @@ PhysicsDirectBodyState *BulletPhysicsServer::body_get_direct_state(RID p_body) {
 	return BulletPhysicsDirectBodyState::get_singleton(body);
 }
 
-bool BulletPhysicsServer::body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, float p_margin, MotionResult *r_result) {
+bool BulletPhysicsServer::body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, MotionResult *r_result) {
 	RigidBodyBullet *body = rigid_body_owner.get(p_body);
 	ERR_FAIL_COND_V(!body, false);
 	ERR_FAIL_COND_V(!body->get_space(), false);
 
-	return body->get_space()->test_body_motion(body, p_from, p_motion, p_margin, r_result);
+	return body->get_space()->test_body_motion(body, p_from, p_motion, r_result);
 }
 
 RID BulletPhysicsServer::soft_body_create(bool p_init_sleeping) {

modules/bullet/bullet_physics_server.h

@@ -210,6 +210,9 @@ class BulletPhysicsServer : public PhysicsServer {
 	virtual void body_set_param(RID p_body, BodyParameter p_param, float p_value);
 	virtual float body_get_param(RID p_body, BodyParameter p_param) const;
 
+	virtual void body_set_kinematic_safe_margin(RID p_body, real_t p_margin);
+	virtual real_t body_get_kinematic_safe_margin(RID p_body) const;
+
 	virtual void body_set_state(RID p_body, BodyState p_state, const Variant &p_variant);
 	virtual Variant body_get_state(RID p_body, BodyState p_state) const;
 
@@ -247,7 +250,7 @@ class BulletPhysicsServer : public PhysicsServer {
 	// this function only works on physics process, errors and returns null otherwise
 	virtual PhysicsDirectBodyState *body_get_direct_state(RID p_body);
 
-	virtual bool body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, float p_margin = 0.001, MotionResult *r_result = NULL);
+	virtual bool body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, MotionResult *r_result = NULL);
 
 	/* SOFT BODY API */
 

modules/bullet/collision_object_bullet.cpp

@@ -105,7 +105,7 @@ void CollisionObjectBullet::remove_collision_exception(const CollisionObjectBull
 }
 
 bool CollisionObjectBullet::has_collision_exception(const CollisionObjectBullet *p_otherCollisionObject) const {
-	return !bt_collision_object->checkCollideWithOverride(p_otherCollisionObject->bt_collision_object);
+	return !bt_collision_object->checkCollideWith(p_otherCollisionObject->bt_collision_object);
 }
 
 void CollisionObjectBullet::set_collision_enabled(bool p_enabled) {

modules/bullet/godot_result_callbacks.cpp

@@ -242,50 +242,3 @@ btScalar GodotRestInfoContactResultCallback::addSingleResult(btManifoldPoint &cp
 
 	return cp.getDistance();
 }
-
-bool GodotRecoverAndClosestContactResultCallback::needsCollision(btBroadphaseProxy *proxy0) const {
-	const bool needs = GodotFilterCallback::test_collision_filters(m_collisionFilterGroup, m_collisionFilterMask, proxy0->m_collisionFilterGroup, proxy0->m_collisionFilterMask);
-	if (needs) {
-		btCollisionObject *btObj = static_cast<btCollisionObject *>(proxy0->m_clientObject);
-		CollisionObjectBullet *gObj = static_cast<CollisionObjectBullet *>(btObj->getUserPointer());
-		if (gObj == m_self_object) {
-			return false;
-		} else {
-			if (m_ignore_areas && gObj->getType() == CollisionObjectBullet::TYPE_AREA) {
-				return false;
-			} else if (m_self_object->has_collision_exception(gObj)) {
-				return false;
-			}
-		}
-		return true;
-	} else {
-		return false;
-	}
-}
-
-btScalar GodotRecoverAndClosestContactResultCallback::addSingleResult(btManifoldPoint &cp, const btCollisionObjectWrapper *colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper *colObj1Wrap, int partId1, int index1) {
-
-	if (cp.getDistance() < -MAX_PENETRATION_DEPTH) {
-		if (m_most_penetrated_distance > cp.getDistance()) {
-			m_most_penetrated_distance = cp.getDistance();
-
-			// take other object
-			btScalar sign(1);
-			if (m_self_object == colObj0Wrap->getCollisionObject()->getUserPointer()) {
-				m_pointCollisionObject = colObj1Wrap->getCollisionObject();
-				m_other_compound_shape_index = cp.m_index1;
-			} else {
-				m_pointCollisionObject = colObj0Wrap->getCollisionObject();
-				sign = -1;
-				m_other_compound_shape_index = cp.m_index0;
-			}
-
-			m_pointNormalWorld = cp.m_normalWorldOnB * sign;
-			m_pointWorld = cp.getPositionWorldOnB();
-			m_penetration_distance = cp.getDistance();
-
-			m_recover_penetration -= cp.m_normalWorldOnB * sign * (cp.getDistance() + MAX_PENETRATION_DEPTH);
-		}
-	}
-	return 1;
-}

modules/bullet/godot_result_callbacks.h

@@ -36,8 +36,6 @@
 #include "btBulletDynamicsCommon.h"
 #include "servers/physics_server.h"
 
-#define MAX_PENETRATION_DEPTH 0.005
-
 class RigidBodyBullet;
 
 /// This class is required to implement custom collision behaviour in the broadphase
@@ -151,39 +149,4 @@ struct GodotRestInfoContactResultCallback : public btCollisionWorld::ContactResu
 	virtual btScalar addSingleResult(btManifoldPoint &cp, const btCollisionObjectWrapper *colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper *colObj1Wrap, int partId1, int index1);
 };
 
-struct GodotRecoverAndClosestContactResultCallback : public btCollisionWorld::ContactResultCallback {
-public:
-	btVector3 m_pointNormalWorld;
-	btVector3 m_pointWorld;
-	btScalar m_penetration_distance;
-	int m_other_compound_shape_index;
-	const btCollisionObject *m_pointCollisionObject;
-
-	const RigidBodyBullet *m_self_object;
-	bool m_ignore_areas;
-
-	btScalar m_most_penetrated_distance;
-	btVector3 m_recover_penetration;
-
-	GodotRecoverAndClosestContactResultCallback()
-		: m_pointCollisionObject(NULL), m_penetration_distance(0), m_other_compound_shape_index(0), m_self_object(NULL), m_ignore_areas(true), m_most_penetrated_distance(1e20), m_recover_penetration(0, 0, 0) {}
-
-	GodotRecoverAndClosestContactResultCallback(const RigidBodyBullet *p_self_object, bool p_ignore_areas)
-		: m_pointCollisionObject(NULL), m_penetration_distance(0), m_other_compound_shape_index(0), m_self_object(p_self_object), m_ignore_areas(p_ignore_areas), m_most_penetrated_distance(9999999999), m_recover_penetration(0, 0, 0) {}
-
-	void reset() {
-		m_pointCollisionObject = NULL;
-		m_most_penetrated_distance = 1e20;
-		m_recover_penetration.setZero();
-	}
-
-	bool hasHit() {
-		return m_pointCollisionObject;
-	}
-
-	virtual bool needsCollision(btBroadphaseProxy *proxy0) const;
-
-	virtual btScalar addSingleResult(btManifoldPoint &cp, const btCollisionObjectWrapper *colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper *colObj1Wrap, int partId1, int index1);
-};
-
 #endif // GODOT_RESULT_CALLBACKS_H

modules/bullet/rigid_body_bullet.cpp

@@ -177,31 +177,21 @@ PhysicsDirectSpaceState *BulletPhysicsDirectBodyState::get_space_state() {
 }
 
 RigidBodyBullet::KinematicUtilities::KinematicUtilities(RigidBodyBullet *p_owner)
-	: m_owner(p_owner), m_margin(0.01) // Godot default margin 0.001
-{
-	m_ghostObject = bulletnew(btPairCachingGhostObject);
-
-	int clearedCurrentFlags = m_ghostObject->getCollisionFlags();
-	clearedCurrentFlags &= ~(btCollisionObject::CF_KINEMATIC_OBJECT | btCollisionObject::CF_STATIC_OBJECT);
-
-	m_ghostObject->setCollisionFlags(clearedCurrentFlags | btCollisionObject::CF_KINEMATIC_OBJECT);
-	m_ghostObject->setUserPointer(p_owner);
-	m_ghostObject->setUserIndex(TYPE_KINEMATIC_GHOST_BODY);
-
-	resetDefShape();
+	: owner(p_owner),
+	  safe_margin(0.001) {
 }
 
 RigidBodyBullet::KinematicUtilities::~KinematicUtilities() {
-	just_delete_shapes(m_shapes.size()); // don't need to resize
-	bulletdelete(m_ghostObject);
+	just_delete_shapes(shapes.size()); // don't need to resize
 }
 
-void RigidBodyBullet::KinematicUtilities::resetDefShape() {
-	m_ghostObject->setCollisionShape(BulletPhysicsServer::get_empty_shape());
+void RigidBodyBullet::KinematicUtilities::setSafeMargin(btScalar p_margin) {
+	safe_margin = p_margin;
+	copyAllOwnerShapes();
 }
 
 void RigidBodyBullet::KinematicUtilities::copyAllOwnerShapes() {
-	const Vector<CollisionObjectBullet::ShapeWrapper> &shapes_wrappers(m_owner->get_shapes_wrappers());
+	const Vector<CollisionObjectBullet::ShapeWrapper> &shapes_wrappers(owner->get_shapes_wrappers());
 	const int shapes_count = shapes_wrappers.size();
 
 	just_delete_shapes(shapes_count);
@@ -213,35 +203,35 @@ void RigidBodyBullet::KinematicUtilities::copyAllOwnerShapes() {
 		if (!shape_wrapper->active) {
 			continue;
 		}
-		m_shapes[i].transform = shape_wrapper->transform;
+		shapes[i].transform = shape_wrapper->transform;
 
-		btConvexShape *&kin_shape_ref = m_shapes[i].shape;
+		btConvexShape *&kin_shape_ref = shapes[i].shape;
 
 		switch (shape_wrapper->shape->get_type()) {
 			case PhysicsServer::SHAPE_SPHERE: {
 				SphereShapeBullet *sphere = static_cast<SphereShapeBullet *>(shape_wrapper->shape);
-				kin_shape_ref = ShapeBullet::create_shape_sphere(sphere->get_radius() * m_owner->body_scale[0] + m_margin);
+				kin_shape_ref = ShapeBullet::create_shape_sphere(sphere->get_radius() * owner->body_scale[0] + safe_margin);
 				break;
 			}
 			case PhysicsServer::SHAPE_BOX: {
 				BoxShapeBullet *box = static_cast<BoxShapeBullet *>(shape_wrapper->shape);
-				kin_shape_ref = ShapeBullet::create_shape_box((box->get_half_extents() * m_owner->body_scale) + btVector3(m_margin, m_margin, m_margin));
+				kin_shape_ref = ShapeBullet::create_shape_box((box->get_half_extents() * owner->body_scale) + btVector3(safe_margin, safe_margin, safe_margin));
 				break;
 			}
 			case PhysicsServer::SHAPE_CAPSULE: {
 				CapsuleShapeBullet *capsule = static_cast<CapsuleShapeBullet *>(shape_wrapper->shape);
-				kin_shape_ref = ShapeBullet::create_shape_capsule(capsule->get_radius() * m_owner->body_scale[0] + m_margin, capsule->get_height() * m_owner->body_scale[1] + m_margin);
+				kin_shape_ref = ShapeBullet::create_shape_capsule(capsule->get_radius() * owner->body_scale[0] + safe_margin, capsule->get_height() * owner->body_scale[1] + safe_margin);
 				break;
 			}
 			case PhysicsServer::SHAPE_CONVEX_POLYGON: {
 				ConvexPolygonShapeBullet *godot_convex = static_cast<ConvexPolygonShapeBullet *>(shape_wrapper->shape);
 				kin_shape_ref = ShapeBullet::create_shape_convex(godot_convex->vertices);
-				kin_shape_ref->setLocalScaling(m_owner->body_scale + btVector3(m_margin, m_margin, m_margin));
+				kin_shape_ref->setLocalScaling(owner->body_scale + btVector3(safe_margin, safe_margin, safe_margin));
 				break;
 			}
 			case PhysicsServer::SHAPE_RAY: {
 				RayShapeBullet *godot_ray = static_cast<RayShapeBullet *>(shape_wrapper->shape);
-				kin_shape_ref = ShapeBullet::create_shape_ray(godot_ray->length * m_owner->body_scale[1] + m_margin);
+				kin_shape_ref = ShapeBullet::create_shape_ray(godot_ray->length * owner->body_scale[1] + safe_margin);
 				break;
 			}
 			default:
@@ -252,12 +242,12 @@ void RigidBodyBullet::KinematicUtilities::copyAllOwnerShapes() {
 }
 
 void RigidBodyBullet::KinematicUtilities::just_delete_shapes(int new_size) {
-	for (int i = m_shapes.size() - 1; 0 <= i; --i) {
-		if (m_shapes[i].shape) {
-			bulletdelete(m_shapes[i].shape);
+	for (int i = shapes.size() - 1; 0 <= i; --i) {
+		if (shapes[i].shape) {
+			bulletdelete(shapes[i].shape);
 		}
 	}
-	m_shapes.resize(new_size);
+	shapes.resize(new_size);
 }
 
 RigidBodyBullet::RigidBodyBullet()

modules/bullet/rigid_body_bullet.h

@@ -162,17 +162,15 @@ class RigidBodyBullet : public RigidCollisionObjectBullet {
 	};
 
 	struct KinematicUtilities {
-		RigidBodyBullet *m_owner;
-		btScalar m_margin;
-		btManifoldArray m_manifoldArray; ///keep track of the contact manifolds
-		class btPairCachingGhostObject *m_ghostObject;
-		Vector<KinematicShape> m_shapes;
+		RigidBodyBullet *owner;
+		btScalar safe_margin;
+		Vector<KinematicShape> shapes;
 
 		KinematicUtilities(RigidBodyBullet *p_owner);
 		~KinematicUtilities();
 
+		void setSafeMargin(btScalar p_margin);
 		/// Used to set the default shape to ghost
-		void resetDefShape();
 		void copyAllOwnerShapes();
 
 	private: