godot/modules/bullet/space_bullet.cpp

/*************************************************************************/
/*  space_bullet.cpp                                                     */
/*  Author: AndreaCatania                                                */
/*************************************************************************/
/*                       This file is part of:                           */
/*                           GODOT ENGINE                                */
/*                    http://www.godotengine.org                         */
/*************************************************************************/
/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md)    */
/*                                                                       */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the       */
/* "Software"), to deal in the Software without restriction, including   */
/* without limitation the rights to use, copy, modify, merge, publish,   */
/* distribute, sublicense, and/or sell copies of the Software, and to    */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions:                                             */
/*                                                                       */
/* The above copyright notice and this permission notice shall be        */
/* included in all copies or substantial portions of the Software.       */
/*                                                                       */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
/*************************************************************************/

#include "space_bullet.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
#include "BulletCollision/CollisionDispatch/btGhostObject.h"
#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
#include "BulletCollision/NarrowPhaseCollision/btPointCollector.h"
#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
#include "BulletSoftBody/btSoftRigidDynamicsWorld.h"
#include "btBulletDynamicsCommon.h"
#include "bullet_physics_server.h"
#include "bullet_types_converter.h"
#include "bullet_utilities.h"
#include "constraint_bullet.h"
#include "godot_collision_configuration.h"
#include "godot_collision_dispatcher.h"
#include "rigid_body_bullet.h"
#include "servers/physics_server.h"
#include "soft_body_bullet.h"
#include "ustring.h"
#include <assert.h>

// test only
//#include "scene/3d/immediate_geometry.h"

BulletPhysicsDirectSpaceState::BulletPhysicsDirectSpaceState(SpaceBullet *p_space)
	: PhysicsDirectSpaceState(), space(p_space) {}

int BulletPhysicsDirectSpaceState::intersect_point(const Vector3 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_layer, uint32_t p_object_type_mask) {

	if (p_result_max <= 0)
		return 0;

	btVector3 bt_point;
	G_TO_B(p_point, bt_point);

	btSphereShape sphere_point(0.f);
	btCollisionObject collision_object_point;
	collision_object_point.setCollisionShape(&sphere_point);
	collision_object_point.setWorldTransform(btTransform(btQuaternion::getIdentity(), bt_point));

	// Setup query
	GodotAllContactResultCallback btResult(&collision_object_point, r_results, p_result_max, &p_exclude);
	btResult.m_collisionFilterGroup = p_collision_layer;
	btResult.m_collisionFilterMask = p_object_type_mask;
	space->dynamicsWorld->contactTest(&collision_object_point, btResult);

	// The results is already populated by GodotAllConvexResultCallback
	return btResult.m_count;
}

bool BulletPhysicsDirectSpaceState::intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const Set<RID> &p_exclude, uint32_t p_collision_layer, uint32_t p_object_type_mask, bool p_pick_ray) {

	btVector3 btVec_from;
	btVector3 btVec_to;

	G_TO_B(p_from, btVec_from);
	G_TO_B(p_to, btVec_to);

	// setup query
	GodotClosestRayResultCallback btResult(btVec_from, btVec_to, &p_exclude);
	btResult.m_collisionFilterGroup = p_collision_layer;
	btResult.m_collisionFilterMask = p_object_type_mask;
	btResult.m_pickRay = p_pick_ray;

	space->dynamicsWorld->rayTest(btVec_from, btVec_to, btResult);
	if (btResult.hasHit()) {
		B_TO_G(btResult.m_hitPointWorld, r_result.position);
		B_TO_G(btResult.m_hitNormalWorld.normalize(), r_result.normal);
		CollisionObjectBullet *gObj = static_cast<CollisionObjectBullet *>(btResult.m_collisionObject->getUserPointer());
		if (gObj) {
			r_result.shape = 0;
			r_result.rid = gObj->get_self();
			r_result.collider_id = gObj->get_instance_id();
			r_result.collider = 0 == r_result.collider_id ? NULL : ObjectDB::get_instance(r_result.collider_id);
		} else {
			WARN_PRINTS("The raycast performed has hit a collision object that is not part of Godot scene, please check it.");
		}
		return true;
	} else {
		return false;
	}
}

int BulletPhysicsDirectSpaceState::intersect_shape(const RID &p_shape, const Transform &p_xform, float p_margin, ShapeResult *p_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_layer, uint32_t p_object_type_mask) {
	if (p_result_max <= 0)
		return 0;

	ShapeBullet *shape = space->get_physics_server()->get_shape_owner()->get(p_shape);

	btConvexShape *btConvex = dynamic_cast<btConvexShape *>(shape->create_bt_shape());
	if (!btConvex) {
		bulletdelete(btConvex);
		ERR_PRINTS("The shape is not a convex shape, then is not supported: shape type: " + itos(shape->get_type()));
		return 0;
	}

	btVector3 scale_with_margin;
	G_TO_B(p_xform.basis.get_scale(), scale_with_margin);
	btConvex->setLocalScaling(scale_with_margin);

	btTransform bt_xform;
	G_TO_B(p_xform, bt_xform);

	btCollisionObject collision_object;
	collision_object.setCollisionShape(btConvex);
	collision_object.setWorldTransform(bt_xform);

	GodotAllContactResultCallback btQuery(&collision_object, p_results, p_result_max, &p_exclude);
	btQuery.m_collisionFilterGroup = p_collision_layer;
	btQuery.m_collisionFilterMask = p_object_type_mask;
	btQuery.m_closestDistanceThreshold = p_margin;
	space->dynamicsWorld->contactTest(&collision_object, btQuery);

	bulletdelete(btConvex);

	return btQuery.m_count;
}

bool BulletPhysicsDirectSpaceState::cast_motion(const RID &p_shape, const Transform &p_xform, const Vector3 &p_motion, float p_margin, float &p_closest_safe, float &p_closest_unsafe, const Set<RID> &p_exclude, uint32_t p_collision_layer, uint32_t p_object_type_mask, ShapeRestInfo *r_info) {
	ShapeBullet *shape = space->get_physics_server()->get_shape_owner()->get(p_shape);

	btConvexShape *bt_convex_shape = dynamic_cast<btConvexShape *>(shape->create_bt_shape());
	if (!bt_convex_shape) {
		bulletdelete(bt_convex_shape);
		ERR_PRINTS("The shape is not a convex shape, then is not supported: shape type: " + itos(shape->get_type()));
		return 0;
	}

	btVector3 bt_motion;
	G_TO_B(p_motion, bt_motion);

	btVector3 scale_with_margin;
	G_TO_B(p_xform.basis.get_scale() + Vector3(p_margin, p_margin, p_margin), scale_with_margin);
	bt_convex_shape->setLocalScaling(scale_with_margin);

	btTransform bt_xform_from;
	G_TO_B(p_xform, bt_xform_from);

	btTransform bt_xform_to(bt_xform_from);
	bt_xform_to.getOrigin() += bt_motion;

	GodotClosestConvexResultCallback btResult(bt_xform_from.getOrigin(), bt_xform_to.getOrigin(), &p_exclude);
	btResult.m_collisionFilterGroup = p_collision_layer;
	btResult.m_collisionFilterMask = p_object_type_mask;

	space->dynamicsWorld->convexSweepTest(bt_convex_shape, bt_xform_from, bt_xform_to, btResult);

	if (btResult.hasHit()) {
		if (btCollisionObject::CO_RIGID_BODY == btResult.m_hitCollisionObject->getInternalType()) {
			B_TO_G(static_cast<const btRigidBody *>(btResult.m_hitCollisionObject)->getVelocityInLocalPoint(btResult.m_hitPointWorld), r_info->linear_velocity);
		}
		CollisionObjectBullet *collision_object = static_cast<CollisionObjectBullet *>(btResult.m_hitCollisionObject->getUserPointer());
		p_closest_safe = p_closest_unsafe = btResult.m_closestHitFraction;
		B_TO_G(btResult.m_hitPointWorld, r_info->point);
		B_TO_G(btResult.m_hitNormalWorld, r_info->normal);
		r_info->rid = collision_object->get_self();
		r_info->collider_id = collision_object->get_instance_id();
		r_info->shape = btResult.m_shapePart;
	}

	bulletdelete(bt_convex_shape);
	return btResult.hasHit();
}

/// Returns the list of contacts pairs in this order: Local contact, other body contact
bool BulletPhysicsDirectSpaceState::collide_shape(RID p_shape, const Transform &p_shape_xform, float p_margin, Vector3 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude, uint32_t p_collision_layer, uint32_t p_object_type_mask) {
	if (p_result_max <= 0)
		return 0;

	ShapeBullet *shape = space->get_physics_server()->get_shape_owner()->get(p_shape);

	btConvexShape *btConvex = dynamic_cast<btConvexShape *>(shape->create_bt_shape());
	if (!btConvex) {
		bulletdelete(btConvex);
		ERR_PRINTS("The shape is not a convex shape, then is not supported: shape type: " + itos(shape->get_type()));
		return 0;
	}

	btVector3 scale_with_margin;
	G_TO_B(p_shape_xform.basis.get_scale(), scale_with_margin);
	btConvex->setLocalScaling(scale_with_margin);

	btTransform bt_xform;
	G_TO_B(p_shape_xform, bt_xform);

	btCollisionObject collision_object;
	collision_object.setCollisionShape(btConvex);
	collision_object.setWorldTransform(bt_xform);

	GodotContactPairContactResultCallback btQuery(&collision_object, r_results, p_result_max, &p_exclude);
	btQuery.m_collisionFilterGroup = p_collision_layer;
	btQuery.m_collisionFilterMask = p_object_type_mask;
	btQuery.m_closestDistanceThreshold = p_margin;
	space->dynamicsWorld->contactTest(&collision_object, btQuery);

	r_result_count = btQuery.m_count;
	bulletdelete(btConvex);

	return btQuery.m_count;
}

bool BulletPhysicsDirectSpaceState::rest_info(RID p_shape, const Transform &p_shape_xform, float p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude, uint32_t p_collision_layer, uint32_t p_object_type_mask) {

	ShapeBullet *shape = space->get_physics_server()->get_shape_owner()->get(p_shape);

	btConvexShape *btConvex = dynamic_cast<btConvexShape *>(shape->create_bt_shape());
	if (!btConvex) {
		bulletdelete(btConvex);
		ERR_PRINTS("The shape is not a convex shape, then is not supported: shape type: " + itos(shape->get_type()));
		return 0;
	}

	btVector3 scale_with_margin;
	G_TO_B(p_shape_xform.basis.get_scale() + Vector3(p_margin, p_margin, p_margin), scale_with_margin);
	btConvex->setLocalScaling(scale_with_margin);

	btTransform bt_xform;
	G_TO_B(p_shape_xform, bt_xform);

	btCollisionObject collision_object;
	collision_object.setCollisionShape(btConvex);
	collision_object.setWorldTransform(bt_xform);

	GodotRestInfoContactResultCallback btQuery(&collision_object, r_info, &p_exclude);
	btQuery.m_collisionFilterGroup = p_collision_layer;
	btQuery.m_collisionFilterMask = p_object_type_mask;
	btQuery.m_closestDistanceThreshold = p_margin;
	space->dynamicsWorld->contactTest(&collision_object, btQuery);

	bulletdelete(btConvex);

	if (btQuery.m_collided) {
		if (btCollisionObject::CO_RIGID_BODY == btQuery.m_rest_info_collision_object->getInternalType()) {
			B_TO_G(static_cast<const btRigidBody *>(btQuery.m_rest_info_collision_object)->getVelocityInLocalPoint(btQuery.m_rest_info_bt_point), r_info->linear_velocity);
		}
		B_TO_G(btQuery.m_rest_info_bt_point, r_info->point);
	}

	return btQuery.m_collided;
}

Vector3 BulletPhysicsDirectSpaceState::get_closest_point_to_object_volume(RID p_object, const Vector3 p_point) const {

	RigidCollisionObjectBullet *rigid_object = space->get_physics_server()->get_rigid_collisin_object(p_object);
	ERR_FAIL_COND_V(!rigid_object, Vector3());

	btVector3 out_closest_point(0, 0, 0);
	btScalar out_distance = 1e20;

	btVector3 bt_point;
	G_TO_B(p_point, bt_point);

	btGjkEpaPenetrationDepthSolver gjk_epa_pen_solver;
	btVoronoiSimplexSolver gjk_simplex_solver;
	gjk_simplex_solver.setEqualVertexThreshold(0.);

	btSphereShape point_shape(0.);

	btCollisionShape *shape;
	btConvexShape *convex_shape;
	btTransform child_transform;
	btTransform body_transform(rigid_object->get_bt_collision_object()->getWorldTransform());

	btGjkPairDetector::ClosestPointInput input;
	input.m_transformA.getBasis().setIdentity();
	input.m_transformA.setOrigin(bt_point);

	bool shapes_found = false;

	btCompoundShape *compound = rigid_object->get_compound_shape();
	for (int i = compound->getNumChildShapes() - 1; 0 <= i; --i) {
		shape = compound->getChildShape(i);
		if (shape->isConvex()) {
			child_transform = compound->getChildTransform(i);
			convex_shape = static_cast<btConvexShape *>(shape);

			input.m_transformB = body_transform * child_transform;

			btPointCollector result;
			btGjkPairDetector gjk_pair_detector(&point_shape, convex_shape, &gjk_simplex_solver, &gjk_epa_pen_solver);
			gjk_pair_detector.getClosestPoints(input, result, 0);

			if (out_distance > result.m_distance) {
				out_distance = result.m_distance;
				out_closest_point = result.m_pointInWorld;
			}
		}
		shapes_found = true;
	}

	if (shapes_found) {

		Vector3 out;
		B_TO_G(out_closest_point, out);
		return out;
	} else {

		// no shapes found, use distance to origin.
		return rigid_object->get_transform().get_origin();
	}
}

SpaceBullet::SpaceBullet(bool p_create_soft_world)
	: broadphase(NULL),
	  dispatcher(NULL),
	  solver(NULL),
	  collisionConfiguration(NULL),
	  dynamicsWorld(NULL),
	  soft_body_world_info(NULL),
	  ghostPairCallback(NULL),
	  godotFilterCallback(NULL),
	  gravityDirection(0, -1, 0),
	  gravityMagnitude(10),
	  contactDebugCount(0) {

	create_empty_world(p_create_soft_world);
	direct_access = memnew(BulletPhysicsDirectSpaceState(this));
}

SpaceBullet::~SpaceBullet() {
	memdelete(direct_access);
	destroy_world();
}

void SpaceBullet::flush_queries() {
	const btCollisionObjectArray &colObjArray = dynamicsWorld->getCollisionObjectArray();
	for (int i = colObjArray.size() - 1; 0 <= i; --i) {
		static_cast<CollisionObjectBullet *>(colObjArray[i]->getUserPointer())->dispatch_callbacks();
	}
}

void SpaceBullet::step(real_t p_delta_time) {
	dynamicsWorld->stepSimulation(p_delta_time, 0, 0);
}

void SpaceBullet::set_param(PhysicsServer::AreaParameter p_param, const Variant &p_value) {
	assert(dynamicsWorld);

	switch (p_param) {
		case PhysicsServer::AREA_PARAM_GRAVITY:
			gravityMagnitude = p_value;
			update_gravity();
			break;
		case PhysicsServer::AREA_PARAM_GRAVITY_VECTOR:
			gravityDirection = p_value;
			update_gravity();
			break;
		case PhysicsServer::AREA_PARAM_LINEAR_DAMP:
		case PhysicsServer::AREA_PARAM_ANGULAR_DAMP:
			break; // No damp
		case PhysicsServer::AREA_PARAM_PRIORITY:
			// Priority is always 0, the lower
			break;
		case PhysicsServer::AREA_PARAM_GRAVITY_IS_POINT:
		case PhysicsServer::AREA_PARAM_GRAVITY_DISTANCE_SCALE:
		case PhysicsServer::AREA_PARAM_GRAVITY_POINT_ATTENUATION:
			break;
		default:
			WARN_PRINTS("This set parameter (" + itos(p_param) + ") is ignored, the SpaceBullet doesn't support it.");
			break;
	}
}

Variant SpaceBullet::get_param(PhysicsServer::AreaParameter p_param) {
	switch (p_param) {
		case PhysicsServer::AREA_PARAM_GRAVITY:
			return gravityMagnitude;
		case PhysicsServer::AREA_PARAM_GRAVITY_VECTOR:
			return gravityDirection;
		case PhysicsServer::AREA_PARAM_LINEAR_DAMP:
		case PhysicsServer::AREA_PARAM_ANGULAR_DAMP:
			return 0; // No damp
		case PhysicsServer::AREA_PARAM_PRIORITY:
			return 0; // Priority is always 0, the lower
		case PhysicsServer::AREA_PARAM_GRAVITY_IS_POINT:
			return false;
		case PhysicsServer::AREA_PARAM_GRAVITY_DISTANCE_SCALE:
			return 0;
		case PhysicsServer::AREA_PARAM_GRAVITY_POINT_ATTENUATION:
			return 0;
		default:
			WARN_PRINTS("This get parameter (" + itos(p_param) + ") is ignored, the SpaceBullet doesn't support it.");
			return Variant();
	}
}

void SpaceBullet::set_param(PhysicsServer::SpaceParameter p_param, real_t p_value) {
	switch (p_param) {
		case PhysicsServer::SPACE_PARAM_CONTACT_RECYCLE_RADIUS:
		case PhysicsServer::SPACE_PARAM_CONTACT_MAX_SEPARATION:
		case PhysicsServer::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION:
		case PhysicsServer::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD:
		case PhysicsServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD:
		case PhysicsServer::SPACE_PARAM_BODY_TIME_TO_SLEEP:
		case PhysicsServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO:
		case PhysicsServer::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS:
		default:
			WARN_PRINTS("This set parameter (" + itos(p_param) + ") is ignored, the SpaceBullet doesn't support it.");
			break;
	}
}

real_t SpaceBullet::get_param(PhysicsServer::SpaceParameter p_param) {
	switch (p_param) {
		case PhysicsServer::SPACE_PARAM_CONTACT_RECYCLE_RADIUS:
		case PhysicsServer::SPACE_PARAM_CONTACT_MAX_SEPARATION:
		case PhysicsServer::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION:
		case PhysicsServer::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD:
		case PhysicsServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD:
		case PhysicsServer::SPACE_PARAM_BODY_TIME_TO_SLEEP:
		case PhysicsServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO:
		case PhysicsServer::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS:
		default:
			WARN_PRINTS("The SpaceBullet  doesn't support this get parameter (" + itos(p_param) + "), 0 is returned.");
			return 0.f;
	}
}

void SpaceBullet::add_area(AreaBullet *p_area) {
	areas.push_back(p_area);
	dynamicsWorld->addCollisionObject(p_area->get_bt_ghost(), p_area->get_collision_layer(), p_area->get_collision_mask());
}

void SpaceBullet::remove_area(AreaBullet *p_area) {
	areas.erase(p_area);
	dynamicsWorld->removeCollisionObject(p_area->get_bt_ghost());
}

void SpaceBullet::reload_collision_filters(AreaBullet *p_area) {
	// This is necessary to change collision filter
	dynamicsWorld->removeCollisionObject(p_area->get_bt_ghost());
	dynamicsWorld->addCollisionObject(p_area->get_bt_ghost(), p_area->get_collision_layer(), p_area->get_collision_mask());
}

void SpaceBullet::add_rigid_body(RigidBodyBullet *p_body) {
	if (p_body->is_static()) {
		dynamicsWorld->addCollisionObject(p_body->get_bt_rigid_body(), p_body->get_collision_layer(), p_body->get_collision_mask());
	} else {
		dynamicsWorld->addRigidBody(p_body->get_bt_rigid_body(), p_body->get_collision_layer(), p_body->get_collision_mask());
	}
}

void SpaceBullet::remove_rigid_body(RigidBodyBullet *p_body) {
	if (p_body->is_static()) {
		dynamicsWorld->removeCollisionObject(p_body->get_bt_rigid_body());
	} else {
		dynamicsWorld->removeRigidBody(p_body->get_bt_rigid_body());
	}
}

void SpaceBullet::reload_collision_filters(RigidBodyBullet *p_body) {
	// This is necessary to change collision filter
	remove_rigid_body(p_body);
	add_rigid_body(p_body);
}

void SpaceBullet::add_soft_body(SoftBodyBullet *p_body) {
	if (is_using_soft_world()) {
		if (p_body->get_bt_soft_body()) {
			static_cast<btSoftRigidDynamicsWorld *>(dynamicsWorld)->addSoftBody(p_body->get_bt_soft_body(), p_body->get_collision_layer(), p_body->get_collision_mask());
		}
	} else {
		ERR_PRINT("This soft body can't be added to non soft world");
	}
}

void SpaceBullet::remove_soft_body(SoftBodyBullet *p_body) {
	if (is_using_soft_world()) {
		if (p_body->get_bt_soft_body()) {
			static_cast<btSoftRigidDynamicsWorld *>(dynamicsWorld)->removeSoftBody(p_body->get_bt_soft_body());
		}
	}
}

void SpaceBullet::reload_collision_filters(SoftBodyBullet *p_body) {
	// This is necessary to change collision filter
	remove_soft_body(p_body);
	add_soft_body(p_body);
}

void SpaceBullet::add_constraint(ConstraintBullet *p_constraint, bool disableCollisionsBetweenLinkedBodies) {
	p_constraint->set_space(this);
	dynamicsWorld->addConstraint(p_constraint->get_bt_constraint(), disableCollisionsBetweenLinkedBodies);
}

void SpaceBullet::remove_constraint(ConstraintBullet *p_constraint) {
	dynamicsWorld->removeConstraint(p_constraint->get_bt_constraint());
}

int SpaceBullet::get_num_collision_objects() const {
	return dynamicsWorld->getNumCollisionObjects();
}

void SpaceBullet::remove_all_collision_objects() {
	for (int i = dynamicsWorld->getNumCollisionObjects() - 1; 0 <= i; --i) {
		btCollisionObject *btObj = dynamicsWorld->getCollisionObjectArray()[i];
		CollisionObjectBullet *colObj = static_cast<CollisionObjectBullet *>(btObj->getUserPointer());
		colObj->set_space(NULL);
	}
}

void onBulletPreTickCallback(btDynamicsWorld *p_dynamicsWorld, btScalar timeStep) {
	static_cast<SpaceBullet *>(p_dynamicsWorld->getWorldUserInfo())->flush_queries();
}

void onBulletTickCallback(btDynamicsWorld *p_dynamicsWorld, btScalar timeStep) {

	// Notify all Collision objects the collision checker is started
	const btCollisionObjectArray &colObjArray = p_dynamicsWorld->getCollisionObjectArray();
	for (int i = colObjArray.size() - 1; 0 <= i; --i) {
		CollisionObjectBullet *colObj = static_cast<CollisionObjectBullet *>(colObjArray[i]->getUserPointer());
		assert(NULL != colObj);
		colObj->on_collision_checker_start();
	}

	SpaceBullet *sb = static_cast<SpaceBullet *>(p_dynamicsWorld->getWorldUserInfo());
	sb->check_ghost_overlaps();
	sb->check_body_collision();
}

BulletPhysicsDirectSpaceState *SpaceBullet::get_direct_state() {
	return direct_access;
}

btScalar calculateGodotCombinedRestitution(const btCollisionObject *body0, const btCollisionObject *body1) {
	return MAX(body0->getRestitution(), body1->getRestitution());
}

void SpaceBullet::create_empty_world(bool p_create_soft_world) {
	assert(NULL == broadphase);
	assert(NULL == dispatcher);
	assert(NULL == solver);
	assert(NULL == collisionConfiguration);
	assert(NULL == dynamicsWorld);
	assert(NULL == ghostPairCallback);
	assert(NULL == godotFilterCallback);

	void *world_mem;
	if (p_create_soft_world) {
		world_mem = malloc(sizeof(btSoftRigidDynamicsWorld));
	} else {
		world_mem = malloc(sizeof(btDiscreteDynamicsWorld));
	}

	if (p_create_soft_world) {
		collisionConfiguration = bulletnew(btSoftBodyRigidBodyCollisionConfiguration);
	} else {
		collisionConfiguration = bulletnew(GodotCollisionConfiguration(static_cast<btDiscreteDynamicsWorld *>(world_mem)));
	}

	dispatcher = bulletnew(GodotCollisionDispatcher(collisionConfiguration));
	broadphase = bulletnew(btDbvtBroadphase);
	solver = bulletnew(btSequentialImpulseConstraintSolver);

	if (p_create_soft_world) {
		dynamicsWorld = new (world_mem) btSoftRigidDynamicsWorld(dispatcher, broadphase, solver, collisionConfiguration);
		soft_body_world_info = bulletnew(btSoftBodyWorldInfo);
	} else {
		dynamicsWorld = new (world_mem) btDiscreteDynamicsWorld(dispatcher, broadphase, solver, collisionConfiguration);
	}

	ghostPairCallback = bulletnew(btGhostPairCallback);
	godotFilterCallback = bulletnew(GodotFilterCallback);
	gCalculateCombinedRestitutionCallback = &calculateGodotCombinedRestitution;

	dynamicsWorld->setWorldUserInfo(this);

	dynamicsWorld->setInternalTickCallback(onBulletPreTickCallback, this, true);
	dynamicsWorld->setInternalTickCallback(onBulletTickCallback, this, false);
	dynamicsWorld->getBroadphase()->getOverlappingPairCache()->setInternalGhostPairCallback(ghostPairCallback); // Setup ghost check
	dynamicsWorld->getPairCache()->setOverlapFilterCallback(godotFilterCallback);

	if (soft_body_world_info) {
		soft_body_world_info->m_broadphase = broadphase;
		soft_body_world_info->m_dispatcher = dispatcher;
		soft_body_world_info->m_sparsesdf.Initialize();
	}

	update_gravity();
}

void SpaceBullet::destroy_world() {
	assert(NULL != broadphase);
	assert(NULL != dispatcher);
	assert(NULL != solver);
	assert(NULL != collisionConfiguration);
	assert(NULL != dynamicsWorld);
	assert(NULL != ghostPairCallback);
	assert(NULL != godotFilterCallback);

	/// The world elements (like: Collision Objects, Constraints, Shapes) are managed by godot

	dynamicsWorld->getBroadphase()->getOverlappingPairCache()->setInternalGhostPairCallback(NULL);
	dynamicsWorld->getPairCache()->setOverlapFilterCallback(NULL);

	bulletdelete(ghostPairCallback);
	bulletdelete(godotFilterCallback);

	// Deallocate world
	dynamicsWorld->~btDiscreteDynamicsWorld();
	free(dynamicsWorld);
	dynamicsWorld = NULL;

	bulletdelete(solver);
	bulletdelete(broadphase);
	bulletdelete(dispatcher);
	bulletdelete(collisionConfiguration);
	bulletdelete(soft_body_world_info);
}

void SpaceBullet::check_ghost_overlaps() {

	/// Algorith support variables
	btGjkEpaPenetrationDepthSolver gjk_epa_pen_solver;
	btVoronoiSimplexSolver gjk_simplex_solver;
	gjk_simplex_solver.setEqualVertexThreshold(0.f);
	btConvexShape *other_body_shape;
	btConvexShape *area_shape;
	btGjkPairDetector::ClosestPointInput gjk_input;
	AreaBullet *area;
	RigidCollisionObjectBullet *otherObject;
	int x(-1), i(-1), y(-1), z(-1), indexOverlap(-1);

	/// For each areas
	for (x = areas.size() - 1; 0 <= x; --x) {
		area = areas[x];

		if (!area->is_monitoring())
			continue;

		/// 1. Reset all states
		for (i = area->overlappingObjects.size() - 1; 0 <= i; --i) {
			AreaBullet::OverlappingObjectData &otherObj = area->overlappingObjects[i];
			// This check prevent the overwrite of ENTER state
			// if this function is called more times before dispatchCallbacks
			if (otherObj.state != AreaBullet::OVERLAP_STATE_ENTER) {
				otherObj.state = AreaBullet::OVERLAP_STATE_DIRTY;
			}
		}

		/// 2. Check all overlapping objects using GJK

		const btAlignedObjectArray<btCollisionObject *> ghostOverlaps = area->get_bt_ghost()->getOverlappingPairs();

		// For each overlapping
		for (i = ghostOverlaps.size() - 1; 0 <= i; --i) {

			if (!(ghostOverlaps[i]->getUserIndex() == CollisionObjectBullet::TYPE_RIGID_BODY || ghostOverlaps[i]->getUserIndex() == CollisionObjectBullet::TYPE_AREA))
				continue;

			otherObject = static_cast<RigidCollisionObjectBullet *>(ghostOverlaps[i]->getUserPointer());

			bool hasOverlap = false;

			// For each area shape
			for (y = area->get_compound_shape()->getNumChildShapes() - 1; 0 <= y; --y) {
				if (!area->get_compound_shape()->getChildShape(y)->isConvex())
					continue;

				gjk_input.m_transformA = area->get_transform__bullet() * area->get_compound_shape()->getChildTransform(y);
				area_shape = static_cast<btConvexShape *>(area->get_compound_shape()->getChildShape(y));

				// For each other object shape
				for (z = otherObject->get_compound_shape()->getNumChildShapes() - 1; 0 <= z; --z) {

					if (!otherObject->get_compound_shape()->getChildShape(z)->isConvex())
						continue;

					other_body_shape = static_cast<btConvexShape *>(otherObject->get_compound_shape()->getChildShape(z));
					gjk_input.m_transformB = otherObject->get_transform__bullet() * otherObject->get_compound_shape()->getChildTransform(z);

					btPointCollector result;
					btGjkPairDetector gjk_pair_detector(area_shape, other_body_shape, &gjk_simplex_solver, &gjk_epa_pen_solver);
					gjk_pair_detector.getClosestPoints(gjk_input, result, 0);

					if (0 >= result.m_distance) {
						hasOverlap = true;
						goto collision_found;
					}
				} // ~For each other object shape
			} // ~For each area shape

		collision_found:
			if (!hasOverlap)
				continue;

			indexOverlap = area->find_overlapping_object(otherObject);
			if (-1 == indexOverlap) {
				// Not found
				area->add_overlap(otherObject);
			} else {
				// Found
				area->put_overlap_as_inside(indexOverlap);
			}
		}

		/// 3. Remove not overlapping
		for (i = area->overlappingObjects.size() - 1; 0 <= i; --i) {
			// If the overlap has DIRTY state it means that it's no more overlapping
			if (area->overlappingObjects[i].state == AreaBullet::OVERLAP_STATE_DIRTY) {
				area->put_overlap_as_exit(i);
			}
		}
	}
}

void SpaceBullet::check_body_collision() {
#ifdef DEBUG_ENABLED
	reset_debug_contact_count();
#endif

	const int numManifolds = dynamicsWorld->getDispatcher()->getNumManifolds();
	for (int i = 0; i < numManifolds; ++i) {
		btPersistentManifold *contactManifold = dynamicsWorld->getDispatcher()->getManifoldByIndexInternal(i);
		const btCollisionObject *obA = contactManifold->getBody0();
		const btCollisionObject *obB = contactManifold->getBody1();

		if (btCollisionObject::CO_RIGID_BODY != obA->getInternalType() || btCollisionObject::CO_RIGID_BODY != obB->getInternalType()) {
			// This checks is required to be sure the ghost object is skipped
			// The ghost object "getUserPointer" return the BodyBullet owner so this check is required
			continue;
		}

		// Asserts all Godot objects are assigned
		assert(NULL != obA->getUserPointer());
		assert(NULL != obB->getUserPointer());

		// I know this static cast is a bit risky. But I'm checking its type just after it.
		// This allow me to avoid a lot of other cast and checks
		RigidBodyBullet *bodyA = static_cast<RigidBodyBullet *>(obA->getUserPointer());
		RigidBodyBullet *bodyB = static_cast<RigidBodyBullet *>(obB->getUserPointer());

		if (CollisionObjectBullet::TYPE_RIGID_BODY == bodyA->getType() && CollisionObjectBullet::TYPE_RIGID_BODY == bodyB->getType()) {
			if (!bodyA->can_add_collision() && !bodyB->can_add_collision()) {
				continue;
			}

			const int numContacts = contactManifold->getNumContacts();
#define REPORT_ALL_CONTACTS 0
#if REPORT_ALL_CONTACTS
			for (int j = 0; j < numContacts; j++) {
				btManifoldPoint &pt = contactManifold->getContactPoint(j);
#else
			// Since I don't need report all contacts for these objects, I'll report only the first
			if (numContacts) {
				btManifoldPoint &pt = contactManifold->getContactPoint(0);
#endif
				Vector3 collisionWorldPosition;
				Vector3 collisionLocalPosition;
				Vector3 normalOnB;
				B_TO_G(pt.m_normalWorldOnB, normalOnB);

				if (bodyA->can_add_collision()) {
					B_TO_G(pt.getPositionWorldOnB(), collisionWorldPosition);
					/// pt.m_localPointB Doesn't report the exact point in local space
					B_TO_G(pt.getPositionWorldOnB() - obB->getWorldTransform().getOrigin(), collisionLocalPosition);
					bodyA->add_collision_object(bodyB, collisionWorldPosition, collisionLocalPosition, normalOnB, pt.m_index1, pt.m_index0);
				}
				if (bodyB->can_add_collision()) {
					B_TO_G(pt.getPositionWorldOnA(), collisionWorldPosition);
					/// pt.m_localPointA Doesn't report the exact point in local space
					B_TO_G(pt.getPositionWorldOnA() - obA->getWorldTransform().getOrigin(), collisionLocalPosition);
					bodyB->add_collision_object(bodyA, collisionWorldPosition, collisionLocalPosition, normalOnB * -1, pt.m_index0, pt.m_index1);
				}

#ifdef DEBUG_ENABLED
				if (is_debugging_contacts()) {
					add_debug_contact(collisionWorldPosition);
				}
#endif
			}
		}
	}
}

void SpaceBullet::update_gravity() {
	btVector3 btGravity;
	G_TO_B(gravityDirection * gravityMagnitude, btGravity);
	dynamicsWorld->setGravity(btGravity);
	if (soft_body_world_info) {
		soft_body_world_info->m_gravity = btGravity;
	}
}

/// IMPORTANT: Please don't turn it ON this is not managed correctly!!
/// I'm leaving this here just for future tests.
/// Debug motion and normal vector drawing
#define debug_test_motion 0
#if debug_test_motion
static ImmediateGeometry *motionVec(NULL);
static ImmediateGeometry *normalLine(NULL);
static Ref<SpatialMaterial> red_mat;
static Ref<SpatialMaterial> blue_mat;
#endif

#define IGNORE_AREAS_TRUE true
bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_from, const Vector3 &p_motion, real_t p_margin, PhysicsServer::MotionResult *r_result) {

#if debug_test_motion
	/// Yes I know this is not good, but I've used it as fast debugging.
	/// I'm leaving it here just for speedup the other eventual debugs
	if (!normalLine) {
		motionVec = memnew(ImmediateGeometry);
		normalLine = memnew(ImmediateGeometry);
		SceneTree::get_singleton()->get_current_scene()->add_child(motionVec);
		SceneTree::get_singleton()->get_current_scene()->add_child(normalLine);

		red_mat = Ref<SpatialMaterial>(memnew(SpatialMaterial));
		red_mat->set_flag(SpatialMaterial::FLAG_UNSHADED, true);
		red_mat->set_line_width(20.0);
		red_mat->set_feature(SpatialMaterial::FEATURE_TRANSPARENT, true);
		red_mat->set_flag(SpatialMaterial::FLAG_ALBEDO_FROM_VERTEX_COLOR, true);
		red_mat->set_flag(SpatialMaterial::FLAG_SRGB_VERTEX_COLOR, true);
		red_mat->set_albedo(Color(1, 0, 0, 1));
		motionVec->set_material_override(red_mat);

		blue_mat = Ref<SpatialMaterial>(memnew(SpatialMaterial));
		blue_mat->set_flag(SpatialMaterial::FLAG_UNSHADED, true);
		blue_mat->set_line_width(20.0);
		blue_mat->set_feature(SpatialMaterial::FEATURE_TRANSPARENT, true);
		blue_mat->set_flag(SpatialMaterial::FLAG_ALBEDO_FROM_VERTEX_COLOR, true);
		blue_mat->set_flag(SpatialMaterial::FLAG_SRGB_VERTEX_COLOR, true);
		blue_mat->set_albedo(Color(0, 0, 1, 1));
		normalLine->set_material_override(blue_mat);
	}
#endif

	///// Release all generated manifolds
	//{
	//    if(p_body->get_kinematic_utilities()){
	//        for(int i= p_body->get_kinematic_utilities()->m_generatedManifold.size()-1; 0<=i; --i){
	//            dispatcher->releaseManifold( p_body->get_kinematic_utilities()->m_generatedManifold[i] );
	//        }
	//        p_body->get_kinematic_utilities()->m_generatedManifold.clear();
	//    }
	//}

	btVector3 recover_initial_position;
	recover_initial_position.setZero();

/// I'm performing the unstack at the end of movement so I'm sure the player is unstacked even after the movement.
///  I've removed the initial unstack because this is useful just for the first tick since after the first
///  the real unstack is performed at the end of process.
/// However I'm leaving here the old code.
///  Note: It has a bug when two shapes touches something simultaneously the body is moved too much away (I'm not fixing it for the reason written above).
#define INITIAL_UNSTACK 0
#if !INITIAL_UNSTACK
	btTransform body_safe_position;
	G_TO_B(p_from, body_safe_position);
//btTransform body_unsafe_positino;
//G_TO_B(p_from, body_unsafe_positino);
#else
	btTransform body_safe_position;
	btTransform body_unsafe_positino;
	{ /// Phase one - multi shapes depenetration using margin
		G_TO_B(p_from, body_safe_position);
		G_TO_B(p_from, body_unsafe_positino);

		// MAX_PENETRATION_DEPTH Is useful have the ghost a bit penetrated so I can detect the floor easily
		recover_from_penetration(p_body, body_safe_position, MAX_PENETRATION_DEPTH, /* p_depenetration_speed */ 1, recover_initial_position);

		/// Not required if I put p_depenetration_speed = 1
		//for(int t = 0; t<4; ++t){
		//    if(!recover_from_penetration(p_body, body_safe_position, MAX_PENETRATION_DEPTH, /* p_depenetration_speed */0.2, recover_initial_position)){
		//       break;
		//    }
		//}

		// Add recover position to "From" and "To" transforms
		body_safe_position.getOrigin() += recover_initial_position;
	}
#endif

	int shape_most_recovered(-1);
	btVector3 recovered_motion;
	G_TO_B(p_motion, recovered_motion);
	const int shape_count(p_body->get_shape_count());

	{ /// phase two - sweep test, from a secure position without margin

#if debug_test_motion
		Vector3 sup_line;
		B_TO_G(body_safe_position.getOrigin(), sup_line);
		motionVec->clear();
		motionVec->begin(Mesh::PRIMITIVE_LINES, NULL);
		motionVec->add_vertex(sup_line);
		motionVec->add_vertex(sup_line + p_motion * 10);
		motionVec->end();
#endif

		for (int shIndex = 0; shIndex < shape_count; ++shIndex) {
			if (p_body->is_shape_disabled(shIndex)) {
				continue;
			}

			btConvexShape *convex_shape_test(dynamic_cast<btConvexShape *>(p_body->get_bt_shape(shIndex)));
			if (!convex_shape_test) {
				// Skip no convex shape
				continue;
			}

			btTransform shape_xform_from;
			G_TO_B(p_body->get_shape_transform(shIndex), shape_xform_from);
			//btTransform shape_xform_to(shape_xform_from);

			// Add local shape transform
			shape_xform_from.getOrigin() += body_safe_position.getOrigin();
			shape_xform_from.getBasis() *= body_safe_position.getBasis();

			btTransform shape_xform_to(shape_xform_from);
			//shape_xform_to.getOrigin() += body_unsafe_positino.getOrigin();
			//shape_xform_to.getBasis() *= body_unsafe_positino.getBasis();
			shape_xform_to.getOrigin() += recovered_motion;

			GodotKinClosestConvexResultCallback btResult(shape_xform_from.getOrigin(), shape_xform_to.getOrigin(), p_body, IGNORE_AREAS_TRUE);
			btResult.m_collisionFilterGroup = p_body->get_collision_layer();
			btResult.m_collisionFilterMask = p_body->get_collision_mask();

			dynamicsWorld->convexSweepTest(convex_shape_test, shape_xform_from, shape_xform_to, btResult);

			if (btResult.hasHit()) {
				//recovered_motion *= btResult.m_closestHitFraction;
				/// Since for each sweep test I fix the motion of new shapes in base the recover result,
				/// if another shape will hit something it means that has a deepest recovering respect the previous shape
				shape_most_recovered = shIndex;
			}
		}
	}

	bool hasHit = false;

	{ /// Phase three - contact test with margin

		btGhostObject *ghost = p_body->get_kinematic_utilities()->m_ghostObject;

		GodotRecoverAndClosestContactResultCallback result_callabck;

		if (false && 0 <= shape_most_recovered) {
			result_callabck.m_self_object = p_body;
			result_callabck.m_ignore_areas = IGNORE_AREAS_TRUE;
			result_callabck.m_collisionFilterGroup = p_body->get_collision_layer();
			result_callabck.m_collisionFilterMask = p_body->get_collision_mask();

			const RigidBodyBullet::KinematicShape &kin(p_body->get_kinematic_utilities()->m_shapes[shape_most_recovered]);
			ghost->setCollisionShape(kin.shape);
			ghost->setWorldTransform(body_safe_position);

			ghost->getWorldTransform().getOrigin() += recovered_motion;
			ghost->getWorldTransform().getOrigin() += kin.transform.getOrigin();
			ghost->getWorldTransform().getBasis() *= kin.transform.getBasis();

			dynamicsWorld->contactTest(ghost, result_callabck);

			recovered_motion += result_callabck.m_recover_penetration; // Required to avoid all kind of penetration

		} else {
			// The sweep result does not return a penetrated shape, so I've to check all shapes
			// Then return the most penetrated shape

			GodotRecoverAndClosestContactResultCallback iter_result_callabck(p_body, IGNORE_AREAS_TRUE);
			iter_result_callabck.m_collisionFilterGroup = p_body->get_collision_layer();
			iter_result_callabck.m_collisionFilterMask = p_body->get_collision_mask();

			btScalar max_penetration(99999999999);
			for (int i = 0; i < shape_count; ++i) {

				const RigidBodyBullet::KinematicShape &kin(p_body->get_kinematic_utilities()->m_shapes[i]);
				if (!kin.is_active()) {
					continue;
				}

				// reset callback each function
				iter_result_callabck.reset();

				ghost->setCollisionShape(kin.shape);
				ghost->setWorldTransform(body_safe_position);
				ghost->getWorldTransform().getOrigin() += recovered_motion;
				ghost->getWorldTransform().getOrigin() += kin.transform.getOrigin();
				ghost->getWorldTransform().getBasis() *= kin.transform.getBasis();

				dynamicsWorld->contactTest(ghost, iter_result_callabck);

				if (iter_result_callabck.hasHit()) {
					if (max_penetration > iter_result_callabck.m_penetration_distance) {
						max_penetration = iter_result_callabck.m_penetration_distance;
						shape_most_recovered = i;
						// This is more penetrated
						result_callabck.m_pointCollisionObject = iter_result_callabck.m_pointCollisionObject;
						result_callabck.m_pointNormalWorld = iter_result_callabck.m_pointNormalWorld;
						result_callabck.m_pointWorld = iter_result_callabck.m_pointWorld;
						result_callabck.m_penetration_distance = iter_result_callabck.m_penetration_distance;
						result_callabck.m_other_compound_shape_index = iter_result_callabck.m_other_compound_shape_index;

						recovered_motion += iter_result_callabck.m_recover_penetration; // Required to avoid all kind of penetration
					}
				}
			}
		}

		hasHit = result_callabck.hasHit();

		if (r_result) {

			B_TO_G(recovered_motion + recover_initial_position, r_result->motion);

			if (hasHit) {

				if (btCollisionObject::CO_RIGID_BODY != result_callabck.m_pointCollisionObject->getInternalType()) {
					ERR_PRINT("The collision is not against a rigid body. Please check what's going on.");
					goto EndExecution;
				}
				const btRigidBody *btRigid = static_cast<const btRigidBody *>(result_callabck.m_pointCollisionObject);
				CollisionObjectBullet *collisionObject = static_cast<CollisionObjectBullet *>(btRigid->getUserPointer());

				r_result->remainder = p_motion - r_result->motion; // is the remaining movements
				B_TO_G(result_callabck.m_pointWorld, r_result->collision_point);
				B_TO_G(result_callabck.m_pointNormalWorld, r_result->collision_normal);
				B_TO_G(btRigid->getVelocityInLocalPoint(result_callabck.m_pointWorld - btRigid->getWorldTransform().getOrigin()), r_result->collider_velocity); // It calculates velocity at point and assign it using special function Bullet_to_Godot
				r_result->collider = collisionObject->get_self();
				r_result->collider_id = collisionObject->get_instance_id();
				r_result->collider_shape = result_callabck.m_other_compound_shape_index;
				r_result->collision_local_shape = shape_most_recovered;

//{ /// Add manifold point to manage collisions
//    btPersistentManifold* manifold = dynamicsWorld->getDispatcher()->getNewManifold(p_body->getBtBody(), btRigid);
//    btManifoldPoint manifoldPoint(result_callabck.m_pointWorld, result_callabck.m_pointWorld, result_callabck.m_pointNormalWorld, result_callabck.m_penetration_distance);
//    manifoldPoint.m_index0 = r_result->collision_local_shape;
//    manifoldPoint.m_index1 = r_result->collider_shape;
//    manifold->addManifoldPoint(manifoldPoint);
//    p_body->get_kinematic_utilities()->m_generatedManifold.push_back(manifold);
//}

#if debug_test_motion
				Vector3 sup_line2;
				B_TO_G(recovered_motion, sup_line2);
				//Vector3 sup_pos;
				//B_TO_G( pt.getPositionWorldOnB(), sup_pos);
				normalLine->clear();
				normalLine->begin(Mesh::PRIMITIVE_LINES, NULL);
				normalLine->add_vertex(r_result->collision_point);
				normalLine->add_vertex(r_result->collision_point + r_result->collision_normal * 10);
				normalLine->end();
#endif

			} else {
				r_result->remainder = Vector3();
			}
		}
	}

EndExecution:
	p_body->get_kinematic_utilities()->resetDefShape();
	return hasHit;
}

///  Note: It has a bug when two shapes touches something simultaneously the body is moved too much away
/// (I'm not fixing it because I don't use it).
bool SpaceBullet::recover_from_penetration(RigidBodyBullet *p_body, const btTransform &p_from, btScalar p_maxPenetrationDepth, btScalar p_depenetration_speed, btVector3 &out_recover_position) {

	bool penetration = false;
	btPairCachingGhostObject *ghost = p_body->get_kinematic_utilities()->m_ghostObject;

	for (int kinIndex = p_body->get_kinematic_utilities()->m_shapes.size() - 1; 0 <= kinIndex; --kinIndex) {
		const RigidBodyBullet::KinematicShape &kin_shape(p_body->get_kinematic_utilities()->m_shapes[kinIndex]);
		if (!kin_shape.is_active()) {
			continue;
		}

		btConvexShape *convexShape = kin_shape.shape;
		btTransform shape_xform(kin_shape.transform);

		// from local to world
		shape_xform.getOrigin() += p_from.getOrigin();
		shape_xform.getBasis() *= p_from.getBasis();

		// Apply last recovery to avoid doubling the recovering
		shape_xform.getOrigin() += out_recover_position;

		ghost->setCollisionShape(convexShape);
		ghost->setWorldTransform(shape_xform);

		btVector3 minAabb, maxAabb;
		convexShape->getAabb(shape_xform, minAabb, maxAabb);
		dynamicsWorld->getBroadphase()->setAabb(ghost->getBroadphaseHandle(),
				minAabb,
				maxAabb,
				dynamicsWorld->getDispatcher());

		dynamicsWorld->getDispatcher()->dispatchAllCollisionPairs(ghost->getOverlappingPairCache(), dynamicsWorld->getDispatchInfo(), dynamicsWorld->getDispatcher());

		for (int i = 0; i < ghost->getOverlappingPairCache()->getNumOverlappingPairs(); ++i) {
			p_body->get_kinematic_utilities()->m_manifoldArray.resize(0);

			btBroadphasePair *collisionPair = &ghost->getOverlappingPairCache()->getOverlappingPairArray()[i];

			btCollisionObject *obj0 = static_cast<btCollisionObject *>(collisionPair->m_pProxy0->m_clientObject);
			btCollisionObject *obj1 = static_cast<btCollisionObject *>(collisionPair->m_pProxy1->m_clientObject);

			if ((obj0 && !obj0->hasContactResponse()) || (obj1 && !obj1->hasContactResponse()))
				continue;

			// This is not required since the dispatched does all the job
			//if (!needsCollision(obj0, obj1))
			//    continue;

			if (collisionPair->m_algorithm)
				collisionPair->m_algorithm->getAllContactManifolds(p_body->get_kinematic_utilities()->m_manifoldArray);

			for (int j = 0; j < p_body->get_kinematic_utilities()->m_manifoldArray.size(); ++j) {

				btPersistentManifold *manifold = p_body->get_kinematic_utilities()->m_manifoldArray[j];
				btScalar directionSign = manifold->getBody0() == ghost ? btScalar(-1.0) : btScalar(1.0);
				for (int p = 0; p < manifold->getNumContacts(); ++p) {
					const btManifoldPoint &pt = manifold->getContactPoint(p);

					btScalar dist = pt.getDistance();
					if (dist < -p_maxPenetrationDepth) {
						penetration = true;
						out_recover_position += pt.m_normalWorldOnB * directionSign * (dist + p_maxPenetrationDepth) * p_depenetration_speed;
						//print_line("penetrate distance: " + rtos(dist));
					}
					//else {
					//    print_line("touching distance: " + rtos(dist));
					//}
				}
			}
		}
	}

	p_body->get_kinematic_utilities()->resetDefShape();
	return penetration;
}