diff --git a/modules/bullet/btRayShape.cpp b/modules/bullet/btRayShape.cpp index 935d86daa66..b902d08eca7 100644 --- a/modules/bullet/btRayShape.cpp +++ b/modules/bullet/btRayShape.cpp @@ -79,7 +79,7 @@ void btRayShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVecto void btRayShape::getAabb(const btTransform &t, btVector3 &aabbMin, btVector3 &aabbMax) const { #define MARGIN_BROADPHASE 0.1 btVector3 localAabbMin(0, 0, 0); - btVector3 localAabbMax(m_shapeAxis * m_length); + btVector3 localAabbMax(m_shapeAxis * (m_cacheScaledLength + m_collisionMargin)); btTransformAabb(localAabbMin, localAabbMax, MARGIN_BROADPHASE, t, aabbMin, aabbMax); } @@ -97,8 +97,8 @@ void btRayShape::getPreferredPenetrationDirection(int index, btVector3 &penetrat void btRayShape::reload_cache() { - m_cacheScaledLength = m_length * m_localScaling[2] + m_collisionMargin; + m_cacheScaledLength = m_length * m_localScaling[2]; m_cacheSupportPoint.setIdentity(); - m_cacheSupportPoint.setOrigin(m_shapeAxis * m_cacheScaledLength); + m_cacheSupportPoint.setOrigin(m_shapeAxis * (m_cacheScaledLength + m_collisionMargin)); } diff --git a/modules/bullet/godot_ray_world_algorithm.cpp b/modules/bullet/godot_ray_world_algorithm.cpp index 449f625e177..cadc8dd59ef 100644 --- a/modules/bullet/godot_ray_world_algorithm.cpp +++ b/modules/bullet/godot_ray_world_algorithm.cpp @@ -35,8 +35,6 @@ #include -#define RAY_STABILITY_MARGIN 0.1 - /** @author AndreaCatania */ @@ -102,8 +100,8 @@ void GodotRayWorldAlgorithm::processCollision(const btCollisionObjectWrapper *bo btScalar depth(ray_shape->getScaledLength() * (btResult.m_closestHitFraction - 1)); - if (depth >= -RAY_STABILITY_MARGIN) - depth = 0; + if (depth >= -ray_shape->getMargin()) + depth *= 0.5; if (ray_shape->getSlipsOnSlope()) resultOut->addContactPoint(btResult.m_hitNormalWorld, btResult.m_hitPointWorld, depth); diff --git a/modules/bullet/godot_result_callbacks.cpp b/modules/bullet/godot_result_callbacks.cpp index 7babfcc1335..360950c4c75 100644 --- a/modules/bullet/godot_result_callbacks.cpp +++ b/modules/bullet/godot_result_callbacks.cpp @@ -333,14 +333,6 @@ void GodotDeepPenetrationContactResultCallback::addContactPoint(const btVector3 m_other_compound_shape_index = isSwapped ? m_index0 : m_index1; m_pointWorld = isSwapped ? (pointInWorldOnB + (normalOnBInWorld * depth)) : pointInWorldOnB; - const btCollisionObjectWrapper *bw0 = m_body0Wrap; - if (isSwapped) - bw0 = m_body1Wrap; - - if (bw0->getCollisionShape()->getShapeType() == CUSTOM_CONVEX_SHAPE_TYPE) { - m_pointNormalWorld = bw0->m_worldTransform.getBasis().transpose() * btVector3(0, 0, 1); - } else { - m_pointNormalWorld = isSwapped ? normalOnBInWorld * -1 : normalOnBInWorld; - } + m_pointNormalWorld = isSwapped ? normalOnBInWorld * -1 : normalOnBInWorld; } } diff --git a/modules/bullet/space_bullet.cpp b/modules/bullet/space_bullet.cpp index 6562b18b3c2..40db5f8a236 100644 --- a/modules/bullet/space_bullet.cpp +++ b/modules/bullet/space_bullet.cpp @@ -1224,6 +1224,21 @@ bool SpaceBullet::RFP_convex_world_test(const btConvexShape *p_shapeA, const btC return false; } +void SpaceBullet::convert_to_separation_result(PhysicsServer::SeparationResult *r_result, const SpaceBullet::RecoverResult &p_recover_result, int p_shape_id, const btCollisionObject *p_other_object) const { + + const btRigidBody *btRigid = static_cast(p_other_object); + CollisionObjectBullet *collisionObject = static_cast(p_other_object->getUserPointer()); + + r_result->collision_depth = p_recover_result.penetration_distance; + B_TO_G(p_recover_result.pointWorld, r_result->collision_point); + B_TO_G(p_recover_result.normal, r_result->collision_normal); + B_TO_G(btRigid->getVelocityInLocalPoint(p_recover_result.pointWorld - btRigid->getWorldTransform().getOrigin()), r_result->collider_velocity); + r_result->collision_local_shape = p_shape_id; + r_result->collider_id = collisionObject->get_instance_id(); + r_result->collider = collisionObject->get_self(); + r_result->collider_shape = p_recover_result.other_compound_shape_index; +} + int SpaceBullet::recover_from_penetration_ray(RigidBodyBullet *p_body, const btTransform &p_body_position, btScalar p_recover_movement_scale, bool p_infinite_inertia, int p_result_max, btVector3 &r_delta_recover_movement, PhysicsServer::SeparationResult *r_results) { RecoverPenetrationBroadPhaseCallback recover_broad_result(p_body->get_bt_collision_object(), p_body->get_collision_layer(), p_body->get_collision_mask()); @@ -1275,22 +1290,19 @@ int SpaceBullet::recover_from_penetration_ray(RigidBodyBullet *p_body, const btT btCompoundShape *cs = static_cast(otherObject->getCollisionShape()); for (int x = cs->getNumChildShapes() - 1; 0 <= x; --x) { - RecoverResult r_recover_result; - 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), p_recover_movement_scale, r_delta_recover_movement, &r_recover_result)) { + RecoverResult recover_result; + 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), p_recover_movement_scale, r_delta_recover_movement, &recover_result)) { - const btRigidBody *btRigid = static_cast(otherObject); - CollisionObjectBullet *collisionObject = static_cast(otherObject->getUserPointer()); - - r_results[ray_index].collision_depth = r_recover_result.penetration_distance; - B_TO_G(r_recover_result.pointWorld, r_results[ray_index].collision_point); - B_TO_G(r_recover_result.normal, r_results[ray_index].collision_normal); - B_TO_G(btRigid->getVelocityInLocalPoint(r_recover_result.pointWorld - btRigid->getWorldTransform().getOrigin()), r_results[ray_index].collider_velocity); - r_results[ray_index].collision_local_shape = kinIndex; - r_results[ray_index].collider_id = collisionObject->get_instance_id(); - r_results[ray_index].collider = collisionObject->get_self(); - r_results[ray_index].collider_shape = r_recover_result.other_compound_shape_index; + convert_to_separation_result(&r_results[ray_index], recover_result, kinIndex, otherObject); } } + } else { + + RecoverResult recover_result; + if (RFP_convex_world_test(kin_shape.shape, otherObject->getCollisionShape(), p_body->get_bt_collision_object(), otherObject, kinIndex, 0, body_shape_position, otherObject->getWorldTransform(), p_recover_movement_scale, r_delta_recover_movement, &recover_result)) { + + convert_to_separation_result(&r_results[ray_index], recover_result, kinIndex, otherObject); + } } } diff --git a/modules/bullet/space_bullet.h b/modules/bullet/space_bullet.h index 9f36c639822..7bf6a216b58 100644 --- a/modules/bullet/space_bullet.h +++ b/modules/bullet/space_bullet.h @@ -212,6 +212,7 @@ private: /// Using this we leave Bullet to select the best algorithm, For example GJK in case we have Convex Convex, or a Bullet accelerated algorithm bool 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, btScalar p_recover_movement_scale, btVector3 &r_delta_recover_movement, RecoverResult *r_recover_result = NULL); + void convert_to_separation_result(PhysicsServer::SeparationResult *r_result, const SpaceBullet::RecoverResult &p_recover_result, int p_shape_id, const btCollisionObject *p_other_object) const; int recover_from_penetration_ray(RigidBodyBullet *p_body, const btTransform &p_body_position, btScalar p_recover_movement_scale, bool p_infinite_inertia, int p_result_max, btVector3 &r_delta_recover_movement, PhysicsServer::SeparationResult *r_results); }; #endif