diff options
Diffstat (limited to 'modules/bullet/space_bullet.cpp')
| -rw-r--r-- | modules/bullet/space_bullet.cpp | 505 |
1 files changed, 241 insertions, 264 deletions
diff --git a/modules/bullet/space_bullet.cpp b/modules/bullet/space_bullet.cpp index af7f511fab..d8c8cab17a 100644 --- a/modules/bullet/space_bullet.cpp +++ b/modules/bullet/space_bullet.cpp @@ -50,13 +50,10 @@ #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) { +int BulletPhysicsDirectSpaceState::intersect_point(const Vector3 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_mask) { if (p_result_max <= 0) return 0; @@ -71,15 +68,15 @@ int BulletPhysicsDirectSpaceState::intersect_point(const Vector3 &p_point, Shape // 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; + btResult.m_collisionFilterGroup = 0; + btResult.m_collisionFilterMask = p_collision_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) { +bool BulletPhysicsDirectSpaceState::intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_pick_ray) { btVector3 btVec_from; btVector3 btVec_to; @@ -89,8 +86,8 @@ bool BulletPhysicsDirectSpaceState::intersect_ray(const Vector3 &p_from, const V // 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_collisionFilterGroup = 0; + btResult.m_collisionFilterMask = p_collision_mask; btResult.m_pickRay = p_pick_ray; space->dynamicsWorld->rayTest(btVec_from, btVec_to, btResult); @@ -112,18 +109,19 @@ bool BulletPhysicsDirectSpaceState::intersect_ray(const Vector3 &p_from, const V } } -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) { +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_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); + btCollisionShape *btShape = shape->create_bt_shape(); + if (!btShape->isConvex()) { + bulletdelete(btShape); ERR_PRINTS("The shape is not a convex shape, then is not supported: shape type: " + itos(shape->get_type())); return 0; } + btConvexShape *btConvex = static_cast<btConvexShape *>(btShape); btVector3 scale_with_margin; G_TO_B(p_xform.basis.get_scale(), scale_with_margin); @@ -137,8 +135,8 @@ int BulletPhysicsDirectSpaceState::intersect_shape(const RID &p_shape, const Tra 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_collisionFilterGroup = 0; + btQuery.m_collisionFilterMask = p_collision_mask; btQuery.m_closestDistanceThreshold = p_margin; space->dynamicsWorld->contactTest(&collision_object, btQuery); @@ -147,15 +145,16 @@ int BulletPhysicsDirectSpaceState::intersect_shape(const RID &p_shape, const Tra 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) { +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_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); + btCollisionShape *btShape = shape->create_bt_shape(); + if (!btShape->isConvex()) { + bulletdelete(btShape); ERR_PRINTS("The shape is not a convex shape, then is not supported: shape type: " + itos(shape->get_type())); return 0; } + btConvexShape *bt_convex_shape = static_cast<btConvexShape *>(btShape); btVector3 bt_motion; G_TO_B(p_motion, bt_motion); @@ -171,22 +170,24 @@ bool BulletPhysicsDirectSpaceState::cast_motion(const RID &p_shape, const Transf 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; + btResult.m_collisionFilterGroup = 0; + btResult.m_collisionFilterMask = p_collision_mask; - space->dynamicsWorld->convexSweepTest(bt_convex_shape, bt_xform_from, bt_xform_to, btResult); + space->dynamicsWorld->convexSweepTest(bt_convex_shape, bt_xform_from, bt_xform_to, btResult, 0.002); 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; + if (r_info) { + 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()); + 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_shapeId; + } } bulletdelete(bt_convex_shape); @@ -194,18 +195,19 @@ bool BulletPhysicsDirectSpaceState::cast_motion(const RID &p_shape, const Transf } /// 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) { +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_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); + btCollisionShape *btShape = shape->create_bt_shape(); + if (!btShape->isConvex()) { + bulletdelete(btShape); ERR_PRINTS("The shape is not a convex shape, then is not supported: shape type: " + itos(shape->get_type())); return 0; } + btConvexShape *btConvex = static_cast<btConvexShape *>(btShape); btVector3 scale_with_margin; G_TO_B(p_shape_xform.basis.get_scale(), scale_with_margin); @@ -219,8 +221,8 @@ bool BulletPhysicsDirectSpaceState::collide_shape(RID p_shape, const Transform & 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_collisionFilterGroup = 0; + btQuery.m_collisionFilterMask = p_collision_mask; btQuery.m_closestDistanceThreshold = p_margin; space->dynamicsWorld->contactTest(&collision_object, btQuery); @@ -230,16 +232,17 @@ bool BulletPhysicsDirectSpaceState::collide_shape(RID p_shape, const Transform & 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) { +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_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); + btCollisionShape *btShape = shape->create_bt_shape(); + if (!btShape->isConvex()) { + bulletdelete(btShape); ERR_PRINTS("The shape is not a convex shape, then is not supported: shape type: " + itos(shape->get_type())); return 0; } + btConvexShape *btConvex = static_cast<btConvexShape *>(btShape); btVector3 scale_with_margin; G_TO_B(p_shape_xform.basis.get_scale() + Vector3(p_margin, p_margin, p_margin), scale_with_margin); @@ -253,8 +256,8 @@ bool BulletPhysicsDirectSpaceState::rest_info(RID p_shape, const Transform &p_sh 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_collisionFilterGroup = 0; + btQuery.m_collisionFilterMask = p_collision_mask; btQuery.m_closestDistanceThreshold = p_margin; space->dynamicsWorld->contactTest(&collision_object, btQuery); @@ -281,10 +284,6 @@ Vector3 BulletPhysicsDirectSpaceState::get_closest_point_to_object_volume(RID p_ 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; @@ -308,7 +307,7 @@ Vector3 BulletPhysicsDirectSpaceState::get_closest_point_to_object_volume(RID p_ input.m_transformB = body_transform * child_transform; btPointCollector result; - btGjkPairDetector gjk_pair_detector(&point_shape, convex_shape, &gjk_simplex_solver, &gjk_epa_pen_solver); + btGjkPairDetector gjk_pair_detector(&point_shape, convex_shape, space->gjk_simplex_solver, space->gjk_epa_pen_solver); gjk_pair_detector.getClosestPoints(input, result, 0); if (out_distance > result.m_distance) { @@ -468,6 +467,7 @@ void SpaceBullet::add_rigid_body(RigidBodyBullet *p_body) { 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()); + p_body->scratch_space_override_modificator(); } } @@ -558,13 +558,10 @@ btScalar calculateGodotCombinedRestitution(const btCollisionObject *body0, const } 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); + + gjk_epa_pen_solver = bulletnew(btGjkEpaPenetrationDepthSolver); + gjk_simplex_solver = bulletnew(btVoronoiSimplexSolver); + gjk_simplex_solver->setEqualVertexThreshold(0.f); void *world_mem; if (p_create_soft_world) { @@ -611,13 +608,6 @@ void SpaceBullet::create_empty_world(bool p_create_soft_world) { } 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 @@ -637,14 +627,13 @@ void SpaceBullet::destroy_world() { bulletdelete(dispatcher); bulletdelete(collisionConfiguration); bulletdelete(soft_body_world_info); + bulletdelete(gjk_simplex_solver); + bulletdelete(gjk_epa_pen_solver); } 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; @@ -701,7 +690,7 @@ void SpaceBullet::check_ghost_overlaps() { 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); + 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) { @@ -743,23 +732,11 @@ void SpaceBullet::check_body_collision() { 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()); + RigidBodyBullet *bodyA = static_cast<RigidBodyBullet *>(contactManifold->getBody0()->getUserPointer()); + RigidBodyBullet *bodyB = static_cast<RigidBodyBullet *>(contactManifold->getBody1()->getUserPointer()); if (CollisionObjectBullet::TYPE_RIGID_BODY == bodyA->getType() && CollisionObjectBullet::TYPE_RIGID_BODY == bodyB->getType()) { if (!bodyA->can_add_collision() && !bodyB->can_add_collision()) { @@ -784,13 +761,13 @@ void SpaceBullet::check_body_collision() { 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); + B_TO_G(pt.getPositionWorldOnB() - contactManifold->getBody1()->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); + B_TO_G(pt.getPositionWorldOnA() - contactManifold->getBody0()->getWorldTransform().getOrigin(), collisionLocalPosition); bodyB->add_collision_object(bodyA, collisionWorldPosition, collisionLocalPosition, normalOnB * -1, pt.m_index0, pt.m_index1); } @@ -807,7 +784,8 @@ void SpaceBullet::check_body_collision() { void SpaceBullet::update_gravity() { btVector3 btGravity; G_TO_B(gravityDirection * gravityMagnitude, btGravity); - dynamicsWorld->setGravity(btGravity); + //dynamicsWorld->setGravity(btGravity); + dynamicsWorld->setGravity(btVector3(0, 0, 0)); if (soft_body_world_info) { soft_body_world_info->m_gravity = btGravity; } @@ -817,7 +795,12 @@ void SpaceBullet::update_gravity() { /// I'm leaving this here just for future tests. /// Debug motion and normal vector drawing #define debug_test_motion 0 +#define PERFORM_INITIAL_UNSTACK 1 + #if debug_test_motion + +#include "scene/3d/immediate_geometry.h" + static ImmediateGeometry *motionVec(NULL); static ImmediateGeometry *normalLine(NULL); static Ref<SpatialMaterial> red_mat; @@ -825,10 +808,10 @@ 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) { +bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_from, const Vector3 &p_motion, PhysicsServer::MotionResult *r_result) { #if debug_test_motion - /// Yes I know this is not good, but I've used it as fast debugging. + /// Yes I know this is not good, but I've used it as fast debugging hack. /// I'm leaving it here just for speedup the other eventual debugs if (!normalLine) { motionVec = memnew(ImmediateGeometry); @@ -866,43 +849,21 @@ bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_f // } //} - btVector3 recover_initial_position; - recover_initial_position.setZero(); + btVector3 recover_initial_position(0, 0, 0); -/// 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; - // } - //} + { /// Phase one - multi shapes depenetration using margin +#if PERFORM_INITIAL_UNSTACK + if (recover_from_penetration(p_body, body_safe_position, recover_initial_position)) { - // Add recover position to "From" and "To" transforms - body_safe_position.getOrigin() += recover_initial_position; - } + // 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()); @@ -924,133 +885,59 @@ bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_f continue; } - btConvexShape *convex_shape_test(dynamic_cast<btConvexShape *>(p_body->get_bt_shape(shIndex))); - if (!convex_shape_test) { + if (!p_body->get_bt_shape(shIndex)->isConvex()) { // Skip no convex shape continue; } + btConvexShape *convex_shape_test(static_cast<btConvexShape *>(p_body->get_bt_shape(shIndex))); - btTransform shape_xform_from; - G_TO_B(p_body->get_shape_transform(shIndex), shape_xform_from); - //btTransform shape_xform_to(shape_xform_from); + btTransform shape_world_from; + G_TO_B(p_body->get_shape_transform(shIndex), shape_world_from); // Add local shape transform - shape_xform_from.getOrigin() += body_safe_position.getOrigin(); - shape_xform_from.getBasis() *= body_safe_position.getBasis(); + shape_world_from = body_safe_position * shape_world_from; - 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; + btTransform shape_world_to(shape_world_from); + shape_world_to.getOrigin() += recovered_motion; - GodotKinClosestConvexResultCallback btResult(shape_xform_from.getOrigin(), shape_xform_to.getOrigin(), p_body, IGNORE_AREAS_TRUE); + GodotKinClosestConvexResultCallback btResult(shape_world_from.getOrigin(), shape_world_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); + dynamicsWorld->convexSweepTest(convex_shape_test, shape_world_from, shape_world_to, btResult, 0.002); 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; + /// if another shape will hit something it means that has a deepest penetration respect the previous shape + recovered_motion *= btResult.m_closestHitFraction; } } } - bool hasHit = false; + bool hasPenetration = false; - { /// Phase three - contact test with margin + { /// Phase three - Recover + contact test with margin - btGhostObject *ghost = p_body->get_kinematic_utilities()->m_ghostObject; + RecoverResult r_recover_result; - 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(); + hasPenetration = recover_from_penetration(p_body, body_safe_position, recovered_motion, &r_recover_result); 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); + if (hasPenetration) { + const btRigidBody *btRigid = static_cast<const btRigidBody *>(r_recover_result.other_collision_object); 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 + B_TO_G(r_recover_result.pointWorld, r_result->collision_point); + B_TO_G(r_recover_result.pointNormalWorld, r_result->collision_normal); + B_TO_G(btRigid->getVelocityInLocalPoint(r_recover_result.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; + r_result->collider_shape = r_recover_result.other_compound_shape_index; + r_result->collision_local_shape = r_recover_result.local_shape_most_recovered; //{ /// Add manifold point to manage collisions // btPersistentManifold* manifold = dynamicsWorld->getDispatcher()->getNewManifold(p_body->getBtBody(), btRigid); @@ -1079,85 +966,175 @@ bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_f } } -EndExecution: - p_body->get_kinematic_utilities()->resetDefShape(); - return hasHit; + return hasPenetration; } -/// 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) { +struct RecoverPenetrationBroadPhaseCallback : public btBroadphaseAabbCallback { +private: + const btCollisionObject *self_collision_object; + uint32_t collision_layer; + uint32_t collision_mask; - bool penetration = false; - btPairCachingGhostObject *ghost = p_body->get_kinematic_utilities()->m_ghostObject; +public: + Vector<btCollisionObject *> result_collision_objects; - 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; +public: + RecoverPenetrationBroadPhaseCallback(const btCollisionObject *p_self_collision_object, uint32_t p_collision_layer, uint32_t p_collision_mask) + : self_collision_object(p_self_collision_object), + collision_layer(p_collision_layer), + collision_mask(p_collision_mask) {} + + virtual ~RecoverPenetrationBroadPhaseCallback() {} + + virtual bool process(const btBroadphaseProxy *proxy) { + + btCollisionObject *co = static_cast<btCollisionObject *>(proxy->m_clientObject); + if (co->getInternalType() <= btCollisionObject::CO_RIGID_BODY) { + if (self_collision_object != proxy->m_clientObject && GodotFilterCallback::test_collision_filters(collision_layer, collision_mask, proxy->m_collisionFilterGroup, proxy->m_collisionFilterMask)) { + result_collision_objects.push_back(co); + return true; + } } + return false; + } - btConvexShape *convexShape = kin_shape.shape; - btTransform shape_xform(kin_shape.transform); + void reset() { + result_collision_objects.empty(); + } +}; - // from local to world - shape_xform.getOrigin() += p_from.getOrigin(); - shape_xform.getBasis() *= p_from.getBasis(); +bool SpaceBullet::recover_from_penetration(RigidBodyBullet *p_body, const btTransform &p_body_position, btVector3 &r_recover_position, RecoverResult *r_recover_result) { - // Apply last recovery to avoid doubling the recovering - shape_xform.getOrigin() += out_recover_position; + RecoverPenetrationBroadPhaseCallback recover_broad_result(p_body->get_bt_collision_object(), p_body->get_collision_layer(), p_body->get_collision_mask()); - ghost->setCollisionShape(convexShape); - ghost->setWorldTransform(shape_xform); + btTransform body_shape_position; + btTransform body_shape_position_recovered; - btVector3 minAabb, maxAabb; - convexShape->getAabb(shape_xform, minAabb, maxAabb); - dynamicsWorld->getBroadphase()->setAabb(ghost->getBroadphaseHandle(), - minAabb, - maxAabb, - dynamicsWorld->getDispatcher()); + // Broad phase support + btVector3 minAabb, maxAabb; - dynamicsWorld->getDispatcher()->dispatchAllCollisionPairs(ghost->getOverlappingPairCache(), dynamicsWorld->getDispatchInfo(), dynamicsWorld->getDispatcher()); + bool penetration = false; - for (int i = 0; i < ghost->getOverlappingPairCache()->getNumOverlappingPairs(); ++i) { - p_body->get_kinematic_utilities()->m_manifoldArray.resize(0); + // For each shape + for (int kinIndex = p_body->get_kinematic_utilities()->shapes.size() - 1; 0 <= kinIndex; --kinIndex) { - btBroadphasePair *collisionPair = &ghost->getOverlappingPairCache()->getOverlappingPairArray()[i]; + recover_broad_result.reset(); - btCollisionObject *obj0 = static_cast<btCollisionObject *>(collisionPair->m_pProxy0->m_clientObject); - btCollisionObject *obj1 = static_cast<btCollisionObject *>(collisionPair->m_pProxy1->m_clientObject); + const RigidBodyBullet::KinematicShape &kin_shape(p_body->get_kinematic_utilities()->shapes[kinIndex]); + if (!kin_shape.is_active()) { + continue; + } + + body_shape_position = p_body_position * kin_shape.transform; + body_shape_position_recovered = body_shape_position; + body_shape_position_recovered.getOrigin() += r_recover_position; + + kin_shape.shape->getAabb(body_shape_position_recovered, minAabb, maxAabb); + dynamicsWorld->getBroadphase()->aabbTest(minAabb, maxAabb, recover_broad_result); - if ((obj0 && !obj0->hasContactResponse()) || (obj1 && !obj1->hasContactResponse())) + for (int i = recover_broad_result.result_collision_objects.size() - 1; 0 <= i; --i) { + btCollisionObject *otherObject = recover_broad_result.result_collision_objects[i]; + if (!p_body->get_bt_collision_object()->checkCollideWith(otherObject) || !otherObject->checkCollideWith(p_body->get_bt_collision_object())) continue; - // This is not required since the dispatched does all the job - //if (!needsCollision(obj0, obj1)) - // continue; + if (otherObject->getCollisionShape()->isCompound()) { - if (collisionPair->m_algorithm) - collisionPair->m_algorithm->getAllContactManifolds(p_body->get_kinematic_utilities()->m_manifoldArray); + // Each convex shape + btCompoundShape *cs = static_cast<btCompoundShape *>(otherObject->getCollisionShape()); + for (int x = cs->getNumChildShapes() - 1; 0 <= x; --x) { - for (int j = 0; j < p_body->get_kinematic_utilities()->m_manifoldArray.size(); ++j) { + if (cs->getChildShape(x)->isConvex()) { + if (RFP_convex_convex_test(kin_shape.shape, static_cast<const btConvexShape *>(cs->getChildShape(x)), otherObject, x, body_shape_position, otherObject->getWorldTransform() * cs->getChildTransform(x), r_recover_position, r_recover_result)) { - 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); + penetration = true; + } + } else { + if (RFP_convex_world_test(kin_shape.shape, cs->getChildShape(x), p_body->get_bt_collision_object(), otherObject, kinIndex, x, body_shape_position, otherObject->getWorldTransform() * cs->getChildTransform(x), r_recover_position, r_recover_result)) { - 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)); + penetration = true; + } } - //else { - // print_line("touching distance: " + rtos(dist)); - //} + } + } else if (otherObject->getCollisionShape()->isConvex()) { /// Execute GJK test against object shape + if (RFP_convex_convex_test(kin_shape.shape, static_cast<const btConvexShape *>(otherObject->getCollisionShape()), otherObject, 0, body_shape_position, otherObject->getWorldTransform(), r_recover_position, r_recover_result)) { + + penetration = true; + } + } else { + if (RFP_convex_world_test(kin_shape.shape, otherObject->getCollisionShape(), p_body->get_bt_collision_object(), otherObject, kinIndex, 0, body_shape_position, otherObject->getWorldTransform(), r_recover_position, r_recover_result)) { + + penetration = true; } } } } - p_body->get_kinematic_utilities()->resetDefShape(); return penetration; } + +bool SpaceBullet::RFP_convex_convex_test(const btConvexShape *p_shapeA, const btConvexShape *p_shapeB, btCollisionObject *p_objectB, int p_shapeId_B, const btTransform &p_transformA, const btTransform &p_transformB, btVector3 &r_recover_position, RecoverResult *r_recover_result) { + + // Initialize GJK input + btGjkPairDetector::ClosestPointInput gjk_input; + gjk_input.m_transformA = p_transformA; + gjk_input.m_transformA.getOrigin() += r_recover_position; + gjk_input.m_transformB = p_transformB; + + // Perform GJK test + btPointCollector result; + btGjkPairDetector gjk_pair_detector(p_shapeA, p_shapeB, gjk_simplex_solver, gjk_epa_pen_solver); + gjk_pair_detector.getClosestPoints(gjk_input, result, 0); + if (0 > result.m_distance) { + // Has penetration + r_recover_position += result.m_normalOnBInWorld * (result.m_distance * -1); + + if (r_recover_result) { + + r_recover_result->hasPenetration = true; + r_recover_result->other_collision_object = p_objectB; + r_recover_result->other_compound_shape_index = p_shapeId_B; + r_recover_result->penetration_distance = result.m_distance; + r_recover_result->pointNormalWorld = result.m_normalOnBInWorld; + r_recover_result->pointWorld = result.m_pointInWorld; + } + return true; + } + return false; +} + +bool SpaceBullet::RFP_convex_world_test(const btConvexShape *p_shapeA, const btCollisionShape *p_shapeB, btCollisionObject *p_objectA, btCollisionObject *p_objectB, int p_shapeId_A, int p_shapeId_B, const btTransform &p_transformA, const btTransform &p_transformB, btVector3 &r_recover_position, RecoverResult *r_recover_result) { + + /// Contact test + + btTransform p_recovered_transformA(p_transformA); + p_recovered_transformA.getOrigin() += r_recover_position; + + btCollisionObjectWrapper obA(NULL, p_shapeA, p_objectA, p_recovered_transformA, -1, p_shapeId_A); + btCollisionObjectWrapper obB(NULL, p_shapeB, p_objectB, p_transformB, -1, p_shapeId_B); + + btCollisionAlgorithm *algorithm = dispatcher->findAlgorithm(&obA, &obB, NULL, BT_CLOSEST_POINT_ALGORITHMS); + if (algorithm) { + GodotDeepPenetrationContactResultCallback contactPointResult(&obA, &obB); + //discrete collision detection query + algorithm->processCollision(&obA, &obB, dynamicsWorld->getDispatchInfo(), &contactPointResult); + + algorithm->~btCollisionAlgorithm(); + dispatcher->freeCollisionAlgorithm(algorithm); + + if (contactPointResult.hasHit()) { + r_recover_position += contactPointResult.m_pointNormalWorld * (contactPointResult.m_penetration_distance * -1); + + if (r_recover_result) { + + r_recover_result->hasPenetration = true; + r_recover_result->other_collision_object = p_objectB; + r_recover_result->other_compound_shape_index = p_shapeId_B; + r_recover_result->penetration_distance = contactPointResult.m_penetration_distance; + r_recover_result->pointNormalWorld = contactPointResult.m_pointNormalWorld; + r_recover_result->pointWorld = contactPointResult.m_pointWorld; + } + return true; + } + } + return false; +} |