diff --git a/modules/bullet/space_bullet.cpp b/modules/bullet/space_bullet.cpp index 7d337bc4d0cf..cef54f2de004 100644 --- a/modules/bullet/space_bullet.cpp +++ b/modules/bullet/space_bullet.cpp @@ -890,108 +890,44 @@ void SpaceBullet::update_gravity() { } } -/// 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 - -#define RECOVERING_MOVEMENT_SCALE 0.4 -#define RECOVERING_MOVEMENT_CYCLES 4 - -#if debug_test_motion - -#include "scene/3d/immediate_geometry.h" - -static ImmediateGeometry3D *motionVec(nullptr); -static ImmediateGeometry3D *normalLine(nullptr); -static Ref red_mat; -static Ref blue_mat; -#endif - -bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, PhysicsServer3D::MotionResult *r_result, bool p_exclude_raycast_shapes) { -#if debug_test_motion - /// 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(ImmediateGeometry3D); - normalLine = memnew(ImmediateGeometry3D); - SceneTree::get_singleton()->get_current_scene()->add_child(motionVec); - SceneTree::get_singleton()->get_current_scene()->add_child(normalLine); - - motionVec->set_as_top_level(true); - normalLine->set_as_top_level(true); - - red_mat = Ref(memnew(StandardMaterial3D)); - red_mat->set_shading_mode(StandardMaterial3D::SHADING_MODE_UNSHADED); - red_mat->set_line_width(20.0); - red_mat->set_transparency(StandardMaterial3D::TRANSPARENCY_ALPHA); - red_mat->set_flag(StandardMaterial3D::FLAG_ALBEDO_FROM_VERTEX_COLOR, true); - red_mat->set_flag(StandardMaterial3D::FLAG_SRGB_VERTEX_COLOR, true); - red_mat->set_albedo(Color(1, 0, 0, 1)); - motionVec->set_material_override(red_mat); - - blue_mat = Ref(memnew(StandardMaterial3D)); - blue_mat->set_shading_mode(StandardMaterial3D::SHADING_MODE_UNSHADED); - blue_mat->set_line_width(20.0); - blue_mat->set_transparency(StandardMaterial3D::TRANSPARENCY_ALPHA); - blue_mat->set_flag(StandardMaterial3D::FLAG_ALBEDO_FROM_VERTEX_COLOR, true); - blue_mat->set_flag(StandardMaterial3D::FLAG_SRGB_VERTEX_COLOR, true); - blue_mat->set_albedo(Color(0, 0, 1, 1)); - normalLine->set_material_override(blue_mat); - } -#endif - +bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, PhysicsServer3D::MotionResult *r_motion_result, bool p_exclude_raycast_shapes) { + bool is_colliding = false; + btVector3 motion; + G_TO_B(p_motion, motion); btTransform body_transform; G_TO_B(p_from, body_transform); UNSCALE_BT_BASIS(body_transform); - btVector3 initial_recover_motion(0, 0, 0); - { /// Phase one - multi shapes depenetration using margin - for (int t(RECOVERING_MOVEMENT_CYCLES); 0 < t; --t) { - if (!recover_from_penetration(p_body, body_transform, RECOVERING_MOVEMENT_SCALE, p_infinite_inertia, initial_recover_motion)) { - break; - } - } - // Add recover movement in order to make it safe - body_transform.getOrigin() += initial_recover_motion; + // Step 1: Free body if stuck + btVector3 recover_motion(0, 0, 0); + { + KinematicCollisionResult collision_result; + kinematic_collision_test(p_body, body_transform, p_infinite_inertia, recover_motion, &collision_result); + // Move body to a safe place. + body_transform.getOrigin() += recover_motion; } - btVector3 motion; - G_TO_B(p_motion, motion); + // Step 2: Test motion. { - // Phase two - sweep test, from a secure position without margin - const int shape_count(p_body->get_shape_count()); -#if debug_test_motion - Vector3 sup_line; - B_TO_G(body_safe_position.getOrigin(), sup_line); - motionVec->clear(); - motionVec->begin(Mesh::PRIMITIVE_LINES, nullptr); - 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)) { + for (int shape_idx = 0; shape_idx < shape_count; ++shape_idx) { + if (p_body->is_shape_disabled(shape_idx)) { continue; } - if (!p_body->get_bt_shape(shIndex)->isConvex()) { - // Skip no convex shape + // Must be a convex shape. + if (!p_body->get_bt_shape(shape_idx)->isConvex()) { continue; } - if (p_exclude_raycast_shapes && p_body->get_bt_shape(shIndex)->getShapeType() == CUSTOM_CONVEX_SHAPE_TYPE) { - // Skip rayshape in order to implement custom separation process + // Skip ray shapes if specified. + if (p_exclude_raycast_shapes && p_body->get_bt_shape(shape_idx)->getShapeType() == CUSTOM_CONVEX_SHAPE_TYPE) { continue; } - btConvexShape *convex_shape_test(static_cast(p_body->get_bt_shape(shIndex))); - - btTransform shape_world_from = body_transform * p_body->get_kinematic_utilities()->shapes[shIndex].transform; - + btConvexShape *convex_shape(static_cast(p_body->get_bt_shape(shape_idx))); + btTransform shape_world_from = body_transform * p_body->get_kinematic_utilities()->shapes[shape_idx].transform; btTransform shape_world_to(shape_world_from); shape_world_to.getOrigin() += motion; @@ -1004,90 +940,95 @@ bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_f btResult.m_collisionFilterGroup = p_body->get_collision_layer(); btResult.m_collisionFilterMask = p_body->get_collision_mask(); - dynamicsWorld->convexSweepTest(convex_shape_test, shape_world_from, shape_world_to, btResult, dynamicsWorld->getDispatchInfo().m_allowedCcdPenetration); + dynamicsWorld->convexSweepTest(convex_shape, shape_world_from, shape_world_to, btResult); if (btResult.hasHit()) { - /// 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 penetration respect the previous shape + is_colliding = true; + + // Extract available collision information. + // This is required, for shapes that aren't detected in the kinematic collision test. + if (r_motion_result) { + const btRigidBody *btRigid = static_cast(btResult.m_hitCollisionObject); + CollisionObjectBullet *collisionObject = static_cast(btRigid->getUserPointer()); + + B_TO_G(btResult.m_hitPointWorld, r_motion_result->collision_point); + B_TO_G(btResult.m_hitNormalWorld, r_motion_result->collision_normal); + B_TO_G(btRigid->getVelocityInLocalPoint(btResult.m_hitPointWorld - btRigid->getWorldTransform().getOrigin()), r_motion_result->collider_velocity); + r_motion_result->collider = collisionObject->get_self(); + r_motion_result->collider_id = collisionObject->get_instance_id(); + r_motion_result->collision_local_shape = shape_idx; + } + + // Adjust the motion so only closer shapes will also hit. motion *= btResult.m_closestHitFraction; } } - - body_transform.getOrigin() += motion; } - bool has_penetration = false; - - { /// Phase three - contact test with margin - - btVector3 __rec(0, 0, 0); - RecoverResult r_recover_result; - - has_penetration = recover_from_penetration(p_body, body_transform, 1, p_infinite_inertia, __rec, &r_recover_result); - - // Parse results - if (r_result) { - B_TO_G(motion + initial_recover_motion + __rec, r_result->motion); + if (r_motion_result) { + if (is_colliding) { + btVector3 discard_motion(0, 0, 0); + btTransform collision_transform = body_transform; + if (motion.fuzzyZero()) { + btVector3 original_motion; + G_TO_B(p_motion, original_motion); + collision_transform.getOrigin() += original_motion * 0.1f; + } else { + collision_transform.getOrigin() += motion; + } - if (has_penetration) { - const btRigidBody *btRigid = static_cast(r_recover_result.other_collision_object); + KinematicCollisionResult collision_result; + if (kinematic_collision_test(p_body, collision_transform, p_infinite_inertia, discard_motion, &collision_result)) { + // Extract collision information. + const btRigidBody *btRigid = static_cast(collision_result.other_collision_object); CollisionObjectBullet *collisionObject = static_cast(btRigid->getUserPointer()); - B_TO_G(motion, r_result->remainder); // is the remaining movements - r_result->remainder = p_motion - r_result->remainder; - - B_TO_G(r_recover_result.pointWorld, r_result->collision_point); - B_TO_G(r_recover_result.normal, 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 = r_recover_result.other_compound_shape_index; - r_result->collision_local_shape = r_recover_result.local_shape_most_recovered; - -#if debug_test_motion - Vector3 sup_line2; - B_TO_G(motion, sup_line2); - normalLine->clear(); - normalLine->begin(Mesh::PRIMITIVE_LINES, nullptr); - 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(); + B_TO_G(collision_result.pointWorld, r_motion_result->collision_point); + B_TO_G(collision_result.normal, r_motion_result->collision_normal); + B_TO_G(btRigid->getVelocityInLocalPoint(collision_result.pointWorld - btRigid->getWorldTransform().getOrigin()), r_motion_result->collider_velocity); + r_motion_result->collider = collisionObject->get_self(); + r_motion_result->collider_id = collisionObject->get_instance_id(); + r_motion_result->collider_shape = collision_result.other_compound_shape_index; + r_motion_result->collision_local_shape = collision_result.local_shape_most_recovered; + + // Use binary search to find the safe motion. + btScalar safe = 0.f; + btScalar unsafe = 1.f; + for (int step = 0; step < 8; step++) { + btScalar mid_point = (safe + unsafe) * 0.5f; + btTransform test_transform = body_transform; + test_transform.getOrigin() += motion * mid_point; + if (kinematic_collision_test(p_body, test_transform, p_infinite_inertia, discard_motion)) { + unsafe = mid_point; + } else { + safe = mid_point; + } + } + motion *= safe; } } + + B_TO_G(motion + recover_motion, r_motion_result->motion); + r_motion_result->remainder = p_motion - r_motion_result->motion; } - return has_penetration; + return is_colliding; } -int SpaceBullet::test_ray_separation(RigidBodyBullet *p_body, const Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, PhysicsServer3D::SeparationResult *r_results, int p_result_max, real_t p_margin) { +int SpaceBullet::test_ray_separation(RigidBodyBullet *p_body, const Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, PhysicsServer3D::SeparationResult *r_separation_results, int p_result_max, real_t p_margin) { btTransform body_transform; G_TO_B(p_transform, body_transform); UNSCALE_BT_BASIS(body_transform); - btVector3 recover_motion(0, 0, 0); - int rays_found = 0; - int rays_found_this_round = 0; - - for (int t(RECOVERING_MOVEMENT_CYCLES); 0 < t; --t) { - PhysicsServer3D::SeparationResult *next_results = &r_results[rays_found]; - rays_found_this_round = recover_from_penetration_ray(p_body, body_transform, RECOVERING_MOVEMENT_SCALE, p_infinite_inertia, p_result_max - rays_found, recover_motion, next_results); - - rays_found += rays_found_this_round; - if (rays_found_this_round == 0) { - body_transform.getOrigin() += recover_motion; - break; - } - } + int rays_found = kinematic_ray_separation(p_body, body_transform, p_infinite_inertia, p_result_max, recover_motion, r_separation_results); B_TO_G(recover_motion, r_recover_motion); + return rays_found; } -struct RecoverPenetrationBroadPhaseCallback : public btBroadphaseAabbCallback { +struct KinematicBroadPhaseCallback : public btBroadphaseAabbCallback { private: btDbvtVolume bounds; @@ -1097,11 +1038,11 @@ struct RecoverPenetrationBroadPhaseCallback : public btBroadphaseAabbCallback { struct CompoundLeafCallback : btDbvt::ICollide { private: - RecoverPenetrationBroadPhaseCallback *parent_callback = nullptr; + KinematicBroadPhaseCallback *parent_callback = nullptr; btCollisionObject *collision_object = nullptr; public: - CompoundLeafCallback(RecoverPenetrationBroadPhaseCallback *p_parent_callback, btCollisionObject *p_collision_object) : + CompoundLeafCallback(KinematicBroadPhaseCallback *p_parent_callback, btCollisionObject *p_collision_object) : parent_callback(p_parent_callback), collision_object(p_collision_object) { } @@ -1123,14 +1064,14 @@ struct RecoverPenetrationBroadPhaseCallback : public btBroadphaseAabbCallback { Vector results; public: - RecoverPenetrationBroadPhaseCallback(const btCollisionObject *p_self_collision_object, uint32_t p_collision_layer, uint32_t p_collision_mask, btVector3 p_aabb_min, btVector3 p_aabb_max) : + KinematicBroadPhaseCallback(const btCollisionObject *p_self_collision_object, uint32_t p_collision_layer, uint32_t p_collision_mask, btVector3 p_aabb_min, btVector3 p_aabb_max) : self_collision_object(p_self_collision_object), collision_layer(p_collision_layer), collision_mask(p_collision_mask) { bounds = btDbvtVolume::FromMM(p_aabb_min, p_aabb_max); } - virtual ~RecoverPenetrationBroadPhaseCallback() {} + virtual ~KinematicBroadPhaseCallback() {} virtual bool process(const btBroadphaseProxy *proxy) { btCollisionObject *co = static_cast(proxy->m_clientObject); @@ -1175,7 +1116,7 @@ struct RecoverPenetrationBroadPhaseCallback : public btBroadphaseAabbCallback { } }; -bool SpaceBullet::recover_from_penetration(RigidBodyBullet *p_body, const btTransform &p_body_position, btScalar p_recover_movement_scale, bool p_infinite_inertia, btVector3 &r_delta_recover_movement, RecoverResult *r_recover_result) { +bool SpaceBullet::kinematic_collision_test(RigidBodyBullet *p_body, const btTransform &p_body_position, bool p_infinite_inertia, btVector3 &r_recover_motion, KinematicCollisionResult *r_collision_result) { // Calculate the cumulative AABB of all shapes of the kinematic body btVector3 aabb_min, aabb_max; bool shapes_found = false; @@ -1192,8 +1133,6 @@ bool SpaceBullet::recover_from_penetration(RigidBodyBullet *p_body, const btTran } btTransform shape_transform = p_body_position * kin_shape.transform; - shape_transform.getOrigin() += r_delta_recover_movement; - btVector3 shape_aabb_min, shape_aabb_max; kin_shape.shape->getAabb(shape_transform, shape_aabb_min, shape_aabb_max); @@ -1212,16 +1151,18 @@ bool SpaceBullet::recover_from_penetration(RigidBodyBullet *p_body, const btTran } } - // If there are no shapes then there is no penetration either + // If there are no shapes then there are no collisions either. if (!shapes_found) { return false; } // Perform broadphase test - RecoverPenetrationBroadPhaseCallback recover_broad_result(p_body->get_bt_collision_object(), p_body->get_collision_layer(), p_body->get_collision_mask(), aabb_min, aabb_max); - dynamicsWorld->getBroadphase()->aabbTest(aabb_min, aabb_max, recover_broad_result); + KinematicBroadPhaseCallback kinematic_broadphase_callback(p_body->get_bt_collision_object(), p_body->get_collision_layer(), p_body->get_collision_mask(), aabb_min, aabb_max); + dynamicsWorld->getBroadphase()->aabbTest(aabb_min, aabb_max, kinematic_broadphase_callback); bool penetration = false; + btScalar max_x = 0.f, max_y = 0.f, max_z = 0.f; + btScalar min_x = 0.f, min_y = 0.f, min_z = 0.f; // Perform narrowphase per shape for (int kinIndex = p_body->get_kinematic_utilities()->shapes.size() - 1; 0 <= kinIndex; --kinIndex) { @@ -1236,69 +1177,87 @@ bool SpaceBullet::recover_from_penetration(RigidBodyBullet *p_body, const btTran } btTransform shape_transform = p_body_position * kin_shape.transform; - shape_transform.getOrigin() += r_delta_recover_movement; - for (int i = recover_broad_result.results.size() - 1; 0 <= i; --i) { - btCollisionObject *otherObject = recover_broad_result.results[i].collision_object; + for (int i = kinematic_broadphase_callback.results.size() - 1; 0 <= i; --i) { + btCollisionObject *otherObject = kinematic_broadphase_callback.results[i].collision_object; if (p_infinite_inertia && !otherObject->isStaticOrKinematicObject()) { - otherObject->activate(); // Force activation of hitten rigid, soft body + // Wake up rigid or soft body. + otherObject->activate(); continue; } else if (!p_body->get_bt_collision_object()->checkCollideWith(otherObject) || !otherObject->checkCollideWith(p_body->get_bt_collision_object())) { continue; } + btVector3 recover_motion(0, 0, 0); if (otherObject->getCollisionShape()->isCompound()) { const btCompoundShape *cs = static_cast(otherObject->getCollisionShape()); - int shape_idx = recover_broad_result.results[i].compound_child_index; - ERR_FAIL_COND_V(shape_idx < 0 || shape_idx >= cs->getNumChildShapes(), false); + int child_shape_idx = kinematic_broadphase_callback.results[i].compound_child_index; + ERR_FAIL_COND_V(child_shape_idx < 0 || child_shape_idx >= cs->getNumChildShapes(), false); - if (cs->getChildShape(shape_idx)->isConvex()) { - if (RFP_convex_convex_test(kin_shape.shape, static_cast(cs->getChildShape(shape_idx)), otherObject, kinIndex, shape_idx, shape_transform, otherObject->getWorldTransform() * cs->getChildTransform(shape_idx), p_recover_movement_scale, r_delta_recover_movement, r_recover_result)) { + if (cs->getChildShape(child_shape_idx)->isConvex()) { + if (convex_convex_test(kin_shape.shape, static_cast(cs->getChildShape(child_shape_idx)), otherObject, kinIndex, child_shape_idx, shape_transform, otherObject->getWorldTransform() * cs->getChildTransform(child_shape_idx), recover_motion, r_collision_result)) { penetration = true; } } else { - if (RFP_convex_world_test(kin_shape.shape, cs->getChildShape(shape_idx), p_body->get_bt_collision_object(), otherObject, kinIndex, shape_idx, shape_transform, otherObject->getWorldTransform() * cs->getChildTransform(shape_idx), p_recover_movement_scale, r_delta_recover_movement, r_recover_result)) { + if (convex_world_test(kin_shape.shape, cs->getChildShape(child_shape_idx), p_body->get_bt_collision_object(), otherObject, kinIndex, child_shape_idx, shape_transform, otherObject->getWorldTransform() * cs->getChildTransform(child_shape_idx), recover_motion, r_collision_result)) { penetration = true; } } } else if (otherObject->getCollisionShape()->isConvex()) { /// Execute GJK test against object shape - if (RFP_convex_convex_test(kin_shape.shape, static_cast(otherObject->getCollisionShape()), otherObject, kinIndex, 0, shape_transform, otherObject->getWorldTransform(), p_recover_movement_scale, r_delta_recover_movement, r_recover_result)) { + if (convex_convex_test(kin_shape.shape, static_cast(otherObject->getCollisionShape()), otherObject, kinIndex, 0, shape_transform, otherObject->getWorldTransform(), recover_motion, r_collision_result)) { penetration = true; } } else { - if (RFP_convex_world_test(kin_shape.shape, otherObject->getCollisionShape(), p_body->get_bt_collision_object(), otherObject, kinIndex, 0, shape_transform, otherObject->getWorldTransform(), p_recover_movement_scale, r_delta_recover_movement, r_recover_result)) { + if (convex_world_test(kin_shape.shape, otherObject->getCollisionShape(), p_body->get_bt_collision_object(), otherObject, kinIndex, 0, shape_transform, otherObject->getWorldTransform(), recover_motion, r_collision_result)) { penetration = true; } } + + if (recover_motion.getX() > max_x) + max_x = recover_motion.getX(); + if (recover_motion.getY() > max_y) + max_y = recover_motion.getY(); + if (recover_motion.getZ() > max_z) + max_z = recover_motion.getZ(); + if (recover_motion.getX() < min_x) + min_x = recover_motion.getX(); + if (recover_motion.getY() < min_y) + min_y = recover_motion.getY(); + if (recover_motion.getZ() < min_z) + min_z = recover_motion.getZ(); } } + r_recover_motion.setX(max_x + min_x); + r_recover_motion.setY(max_y + min_y); + r_recover_motion.setZ(max_z + min_z); + return penetration; } -bool SpaceBullet::RFP_convex_convex_test(const btConvexShape *p_shapeA, const btConvexShape *p_shapeB, 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) { +bool SpaceBullet::convex_convex_test(const btConvexShape *p_shapeA, const btConvexShape *p_shapeB, btCollisionObject *p_objectB, int p_shapeId_A, int p_shapeId_B, const btTransform &p_transformA, const btTransform &p_transformB, btVector3 &r_recover_motion, KinematicCollisionResult *r_collision_result) { // Initialize GJK input btGjkPairDetector::ClosestPointInput gjk_input; gjk_input.m_transformA = p_transformA; gjk_input.m_transformB = p_transformB; // Perform GJK test - btPointCollector result; + btPointCollector point_collector; btGjkPairDetector gjk_pair_detector(p_shapeA, p_shapeB, gjk_simplex_solver, gjk_epa_pen_solver); - gjk_pair_detector.getClosestPoints(gjk_input, result, nullptr); - if (0 > result.m_distance) { - // Has penetration - r_delta_recover_movement += result.m_normalOnBInWorld * (result.m_distance * -1 * p_recover_movement_scale); - - if (r_recover_result) { - if (result.m_distance < r_recover_result->penetration_distance) { - r_recover_result->hasPenetration = true; - r_recover_result->local_shape_most_recovered = p_shapeId_A; - 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->pointWorld = result.m_pointInWorld; - r_recover_result->normal = result.m_normalOnBInWorld; + gjk_pair_detector.getClosestPoints(gjk_input, point_collector, nullptr); + + // If distance between shapes is less than 0, the shapes are colliding. + if (point_collector.m_distance < 0) { + r_recover_motion = point_collector.m_normalOnBInWorld * -point_collector.m_distance; + if (r_collision_result) { + if (point_collector.m_distance < r_collision_result->penetration_distance) { + r_collision_result->isColliding = true; + r_collision_result->local_shape_most_recovered = p_shapeId_A; + r_collision_result->other_collision_object = p_objectB; + r_collision_result->other_compound_shape_index = p_shapeId_B; + r_collision_result->penetration_distance = point_collector.m_distance; + r_collision_result->pointWorld = point_collector.m_pointInWorld; + r_collision_result->normal = point_collector.m_normalOnBInWorld; } } return true; @@ -1306,34 +1265,30 @@ bool SpaceBullet::RFP_convex_convex_test(const btConvexShape *p_shapeA, const bt 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, btScalar p_recover_movement_scale, btVector3 &r_delta_recover_movement, RecoverResult *r_recover_result) { - /// Contact test - - btTransform tA(p_transformA); - - btCollisionObjectWrapper obA(nullptr, p_shapeA, p_objectA, tA, -1, p_shapeId_A); +bool SpaceBullet::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_motion, KinematicCollisionResult *r_collision_result) { + btCollisionObjectWrapper obA(nullptr, p_shapeA, p_objectA, p_transformA, -1, p_shapeId_A); btCollisionObjectWrapper obB(nullptr, p_shapeB, p_objectB, p_transformB, -1, p_shapeId_B); - btCollisionAlgorithm *algorithm = dispatcher->findAlgorithm(&obA, &obB, nullptr, BT_CONTACT_POINT_ALGORITHMS); + if (algorithm) { + // Discrete collision detection query GodotDeepPenetrationContactResultCallback contactPointResult(&obA, &obB); - //discrete collision detection query algorithm->processCollision(&obA, &obB, dynamicsWorld->getDispatchInfo(), &contactPointResult); algorithm->~btCollisionAlgorithm(); dispatcher->freeCollisionAlgorithm(algorithm); if (contactPointResult.hasHit()) { - r_delta_recover_movement += contactPointResult.m_pointNormalWorld * (contactPointResult.m_penetration_distance * -1 * p_recover_movement_scale); - if (r_recover_result) { - if (contactPointResult.m_penetration_distance < r_recover_result->penetration_distance) { - r_recover_result->hasPenetration = true; - r_recover_result->local_shape_most_recovered = p_shapeId_A; - 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->pointWorld = contactPointResult.m_pointWorld; - r_recover_result->normal = contactPointResult.m_pointNormalWorld; + r_recover_motion = contactPointResult.m_pointNormalWorld * -contactPointResult.m_penetration_distance; + if (r_collision_result) { + if (contactPointResult.m_penetration_distance < r_collision_result->penetration_distance) { + r_collision_result->isColliding = true; + r_collision_result->local_shape_most_recovered = p_shapeId_A; + r_collision_result->other_collision_object = p_objectB; + r_collision_result->other_compound_shape_index = p_shapeId_B; + r_collision_result->penetration_distance = contactPointResult.m_penetration_distance; + r_collision_result->pointWorld = contactPointResult.m_pointWorld; + r_collision_result->normal = contactPointResult.m_pointNormalWorld; } } return true; @@ -1342,28 +1297,7 @@ bool SpaceBullet::RFP_convex_world_test(const btConvexShape *p_shapeA, const btC return false; } -int SpaceBullet::add_separation_result(PhysicsServer3D::SeparationResult *r_result, const SpaceBullet::RecoverResult &p_recover_result, int p_shape_id, const btCollisionObject *p_other_object) const { - // optimize results (ignore non-colliding) - if (p_recover_result.penetration_distance < 0.0) { - 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; - - return 1; - } else { - return 0; - } -} - -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, PhysicsServer3D::SeparationResult *r_results) { +int SpaceBullet::kinematic_ray_separation(RigidBodyBullet *p_body, const btTransform &p_body_position, bool p_infinite_inertia, int p_result_max, btVector3 &r_recover_motion, PhysicsServer3D::SeparationResult *r_separation_results) { // Calculate the cumulative AABB of all shapes of the kinematic body btVector3 aabb_min, aabb_max; bool shapes_found = false; @@ -1379,7 +1313,6 @@ int SpaceBullet::recover_from_penetration_ray(RigidBodyBullet *p_body, const btT } btTransform shape_transform = p_body_position * kin_shape.transform; - shape_transform.getOrigin() += r_delta_recover_movement; btVector3 shape_aabb_min, shape_aabb_max; kin_shape.shape->getAabb(shape_transform, shape_aabb_min, shape_aabb_max); @@ -1399,16 +1332,18 @@ int SpaceBullet::recover_from_penetration_ray(RigidBodyBullet *p_body, const btT } } - // If there are no shapes then there is no penetration either + // If there are no shapes then there are no collisions either. if (!shapes_found) { return 0; } // Perform broadphase test - RecoverPenetrationBroadPhaseCallback recover_broad_result(p_body->get_bt_collision_object(), p_body->get_collision_layer(), p_body->get_collision_mask(), aabb_min, aabb_max); - dynamicsWorld->getBroadphase()->aabbTest(aabb_min, aabb_max, recover_broad_result); + KinematicBroadPhaseCallback kinematic_broadphase_callback(p_body->get_bt_collision_object(), p_body->get_collision_layer(), p_body->get_collision_mask(), aabb_min, aabb_max); + dynamicsWorld->getBroadphase()->aabbTest(aabb_min, aabb_max, kinematic_broadphase_callback); int ray_count = 0; + btScalar max_x = 0.f, max_y = 0.f, max_z = 0.f; + btScalar min_x = 0.f, min_y = 0.f, min_z = 0.f; // Perform narrowphase per shape for (int kinIndex = p_body->get_kinematic_utilities()->shapes.size() - 1; 0 <= kinIndex; --kinIndex) { @@ -1426,34 +1361,70 @@ int SpaceBullet::recover_from_penetration_ray(RigidBodyBullet *p_body, const btT } btTransform shape_transform = p_body_position * kin_shape.transform; - shape_transform.getOrigin() += r_delta_recover_movement; - for (int i = recover_broad_result.results.size() - 1; 0 <= i; --i) { - btCollisionObject *otherObject = recover_broad_result.results[i].collision_object; - if (p_infinite_inertia && !otherObject->isStaticOrKinematicObject()) { - otherObject->activate(); // Force activation of hitten rigid, soft body + for (int i = kinematic_broadphase_callback.results.size() - 1; 0 <= i; --i) { + btCollisionObject *other_object = kinematic_broadphase_callback.results[i].collision_object; + if (p_infinite_inertia && !other_object->isStaticOrKinematicObject()) { + // Wake up rigid or soft boddy. + other_object->activate(); continue; - } else if (!p_body->get_bt_collision_object()->checkCollideWith(otherObject) || !otherObject->checkCollideWith(p_body->get_bt_collision_object())) { + } else if (!p_body->get_bt_collision_object()->checkCollideWith(other_object) || !other_object->checkCollideWith(p_body->get_bt_collision_object())) { continue; } - if (otherObject->getCollisionShape()->isCompound()) { - const btCompoundShape *cs = static_cast(otherObject->getCollisionShape()); - int shape_idx = recover_broad_result.results[i].compound_child_index; - ERR_FAIL_COND_V(shape_idx < 0 || shape_idx >= cs->getNumChildShapes(), false); + btVector3 recover_motion(0, 0, 0); + KinematicCollisionResult collision_result; + if (other_object->getCollisionShape()->isCompound()) { + const btCompoundShape *cs = static_cast(other_object->getCollisionShape()); + int child_shape_idx = kinematic_broadphase_callback.results[i].compound_child_index; + ERR_FAIL_COND_V(child_shape_idx < 0 || child_shape_idx >= cs->getNumChildShapes(), false); - RecoverResult recover_result; - if (RFP_convex_world_test(kin_shape.shape, cs->getChildShape(shape_idx), p_body->get_bt_collision_object(), otherObject, kinIndex, shape_idx, shape_transform, otherObject->getWorldTransform() * cs->getChildTransform(shape_idx), p_recover_movement_scale, r_delta_recover_movement, &recover_result)) { - ray_count = add_separation_result(&r_results[ray_count], recover_result, kinIndex, otherObject); + if (convex_world_test(kin_shape.shape, cs->getChildShape(child_shape_idx), p_body->get_bt_collision_object(), other_object, kinIndex, child_shape_idx, shape_transform, other_object->getWorldTransform() * cs->getChildTransform(child_shape_idx), recover_motion, &collision_result)) { + ray_count += add_separation_result(collision_result, kinIndex, other_object, &r_separation_results[ray_count]); } } else { - RecoverResult recover_result; - if (RFP_convex_world_test(kin_shape.shape, otherObject->getCollisionShape(), p_body->get_bt_collision_object(), otherObject, kinIndex, 0, shape_transform, otherObject->getWorldTransform(), p_recover_movement_scale, r_delta_recover_movement, &recover_result)) { - ray_count = add_separation_result(&r_results[ray_count], recover_result, kinIndex, otherObject); + if (convex_world_test(kin_shape.shape, other_object->getCollisionShape(), p_body->get_bt_collision_object(), other_object, kinIndex, 0, shape_transform, other_object->getWorldTransform(), recover_motion, &collision_result)) { + ray_count += add_separation_result(collision_result, kinIndex, other_object, &r_separation_results[ray_count]); } } + + if (recover_motion.getX() > max_x) + max_x = recover_motion.getX(); + if (recover_motion.getY() > max_y) + max_y = recover_motion.getY(); + if (recover_motion.getZ() > max_z) + max_z = recover_motion.getZ(); + if (recover_motion.getX() < min_x) + min_x = recover_motion.getX(); + if (recover_motion.getY() < min_y) + min_y = recover_motion.getY(); + if (recover_motion.getZ() < min_z) + min_z = recover_motion.getZ(); } } + r_recover_motion = btVector3(max_x + min_x, max_y + min_y, max_z + min_z); + return ray_count; } + +int SpaceBullet::add_separation_result(const SpaceBullet::KinematicCollisionResult &p_collision_result, int p_shape_id, const btCollisionObject *p_other_object, PhysicsServer3D::SeparationResult *r_separation_results) const { + // Optimize results by ignoring non-colliding shapes. + if (p_collision_result.penetration_distance < 0.f) { + const btRigidBody *btRigid = static_cast(p_other_object); + CollisionObjectBullet *collisionObject = static_cast(p_other_object->getUserPointer()); + + r_separation_results->collision_depth = p_collision_result.penetration_distance; + B_TO_G(p_collision_result.pointWorld, r_separation_results->collision_point); + B_TO_G(p_collision_result.normal, r_separation_results->collision_normal); + B_TO_G(btRigid->getVelocityInLocalPoint(p_collision_result.pointWorld - btRigid->getWorldTransform().getOrigin()), r_separation_results->collider_velocity); + r_separation_results->collision_local_shape = p_shape_id; + r_separation_results->collider_id = collisionObject->get_instance_id(); + r_separation_results->collider = collisionObject->get_self(); + r_separation_results->collider_shape = p_collision_result.other_compound_shape_index; + + return 1; + } + + return 0; +} diff --git a/modules/bullet/space_bullet.h b/modules/bullet/space_bullet.h index 87aa2b9e93a9..4872ef893461 100644 --- a/modules/bullet/space_bullet.h +++ b/modules/bullet/space_bullet.h @@ -188,8 +188,8 @@ class SpaceBullet : public RIDBullet { real_t get_linear_damp() const { return linear_damp; } real_t get_angular_damp() const { return angular_damp; } - bool test_body_motion(RigidBodyBullet *p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, PhysicsServer3D::MotionResult *r_result, bool p_exclude_raycast_shapes); - int test_ray_separation(RigidBodyBullet *p_body, const Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, PhysicsServer3D::SeparationResult *r_results, int p_result_max, real_t p_margin); + bool test_body_motion(RigidBodyBullet *p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, PhysicsServer3D::MotionResult *r_motion_result, bool p_exclude_raycast_shapes); + int test_ray_separation(RigidBodyBullet *p_body, const Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, PhysicsServer3D::SeparationResult *r_separation_results, int p_result_max, real_t p_margin); private: void create_empty_world(bool p_create_soft_world); @@ -197,27 +197,25 @@ class SpaceBullet : public RIDBullet { void check_ghost_overlaps(); void check_body_collision(); - struct RecoverResult { - bool hasPenetration = false; + struct KinematicCollisionResult { + bool isColliding = false; btVector3 normal = btVector3(0, 0, 0); btVector3 pointWorld = btVector3(0, 0, 0); - btScalar penetration_distance = 1e20; // Negative mean penetration + btScalar penetration_distance = 1e20; int other_compound_shape_index = 0; const btCollisionObject *other_collision_object = nullptr; int local_shape_most_recovered = 0; - RecoverResult() {} + KinematicCollisionResult() {} }; - bool recover_from_penetration(RigidBodyBullet *p_body, const btTransform &p_body_position, btScalar p_recover_movement_scale, bool p_infinite_inertia, btVector3 &r_delta_recover_movement, RecoverResult *r_recover_result = nullptr); - /// This is an API that recover a kinematic object from penetration - /// This allow only Convex Convex test and it always use GJK algorithm, With this API we don't benefit of Bullet special accelerated functions - bool RFP_convex_convex_test(const btConvexShape *p_shapeA, const btConvexShape *p_shapeB, 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 = nullptr); - /// This is an API that recover a kinematic object from penetration - /// 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 = nullptr); + bool kinematic_collision_test(RigidBodyBullet *p_body, const btTransform &p_body_position, bool p_infinite_inertia, btVector3 &r_recover_motion, KinematicCollisionResult *r_collision_result = nullptr); + // Uses GJK algorithm to determine the collision between two convex shapes. + bool convex_convex_test(const btConvexShape *p_shapeA, const btConvexShape *p_shapeB, btCollisionObject *p_objectB, int p_shapeId_A, int p_shapeId_B, const btTransform &p_transformA, const btTransform &p_transformB, btVector3 &r_recover_motion, KinematicCollisionResult *r_collision_result = nullptr); + // Allows Bullet to select the best algorithm to determine the collision between two shapes. + bool 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_motion, KinematicCollisionResult *r_collision_result = nullptr); - int add_separation_result(PhysicsServer3D::SeparationResult *r_results, 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, PhysicsServer3D::SeparationResult *r_results); + int kinematic_ray_separation(RigidBodyBullet *p_body, const btTransform &p_body_position, bool p_infinite_inertia, int p_result_max, btVector3 &r_recover_movement, PhysicsServer3D::SeparationResult *r_separation_results); + int add_separation_result(const SpaceBullet::KinematicCollisionResult &p_collision_result, int p_shape_id, const btCollisionObject *p_other_object, PhysicsServer3D::SeparationResult *r_separation_results) const; }; #endif diff --git a/scene/3d/physics_body_3d.cpp b/scene/3d/physics_body_3d.cpp index 6d135c8283fa..ea6ec8756b42 100644 --- a/scene/3d/physics_body_3d.cpp +++ b/scene/3d/physics_body_3d.cpp @@ -1073,7 +1073,7 @@ bool KinematicBody3D::separate_raycast_shapes(bool p_infinite_inertia, Collision Vector3 recover; int hits = PhysicsServer3D::get_singleton()->body_test_ray_separation(get_rid(), gt, p_infinite_inertia, recover, sep_res, 8, margin); int deepest = -1; - real_t deepest_depth; + real_t deepest_depth = 0.f; for (int i = 0; i < hits; i++) { if (deepest == -1 || sep_res[i].collision_depth > deepest_depth) { deepest = i; diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp index 05136e2501c4..e4d487fccf9d 100644 --- a/servers/physics_2d/space_2d_sw.cpp +++ b/servers/physics_2d/space_2d_sw.cpp @@ -278,9 +278,9 @@ bool PhysicsDirectSpaceState2DSW::cast_motion(const RID &p_shape, const Transfor continue; } - //test initial overlap, ignore objects it's inside of. + //test initial overlap if (CollisionSolver2DSW::solve(shape, p_xform, Vector2(), col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, nullptr, p_margin)) { - continue; + return false; } //just do kinematic solving @@ -548,134 +548,123 @@ int Space2DSW::test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_t int rays_found = 0; - { - // raycast AND separate + // Raycast AND separate - const int max_results = 32; - int recover_attempts = 4; - Vector2 sr[max_results * 2]; - PhysicsServer2DSW::CollCbkData cbk; - cbk.max = max_results; - PhysicsServer2DSW::CollCbkData *cbkptr = &cbk; - CollisionSolver2DSW::CallbackResult cbkres = PhysicsServer2DSW::_shape_col_cbk; + const int max_results = 32; + Vector2 sr[max_results * 2]; + PhysicsServer2DSW::CollCbkData cbk; + cbk.max = max_results; + PhysicsServer2DSW::CollCbkData *cbkptr = &cbk; + CollisionSolver2DSW::CallbackResult cbkres = PhysicsServer2DSW::_shape_col_cbk; - do { - Vector2 recover_motion; + real_t max_x = 0.f, max_y = 0.f; + real_t min_x = 0.f, min_y = 0.f; - bool collided = false; + int amount = _cull_aabb_for_body(p_body, body_aabb); - int amount = _cull_aabb_for_body(p_body, body_aabb); + for (int j = 0; j < p_body->get_shape_count(); j++) { + if (p_body->is_shape_set_as_disabled(j)) { + continue; + } - for (int j = 0; j < p_body->get_shape_count(); j++) { - if (p_body->is_shape_set_as_disabled(j)) { - continue; - } + Shape2DSW *body_shape = p_body->get_shape(j); - Shape2DSW *body_shape = p_body->get_shape(j); + if (body_shape->get_type() != PhysicsServer2D::SHAPE_RAY) { + continue; + } - if (body_shape->get_type() != PhysicsServer2D::SHAPE_RAY) { - continue; - } + Transform2D body_shape_xform = body_transform * p_body->get_shape_transform(j); - Transform2D body_shape_xform = body_transform * p_body->get_shape_transform(j); + for (int i = 0; i < amount; i++) { + const CollisionObject2DSW *col_obj = intersection_query_results[i]; + int shape_idx = intersection_query_subindex_results[i]; - for (int i = 0; i < amount; i++) { - const CollisionObject2DSW *col_obj = intersection_query_results[i]; - int shape_idx = intersection_query_subindex_results[i]; + cbk.amount = 0; + cbk.passed = 0; + cbk.ptr = sr; + cbk.invalid_by_dir = 0; - cbk.amount = 0; - cbk.passed = 0; - cbk.ptr = sr; - cbk.invalid_by_dir = 0; + if (CollisionObject2DSW::TYPE_BODY == col_obj->get_type()) { + const Body2DSW *b = static_cast(col_obj); + if (p_infinite_inertia && PhysicsServer2D::BODY_MODE_STATIC != b->get_mode() && PhysicsServer2D::BODY_MODE_KINEMATIC != b->get_mode()) { + continue; + } + } - if (CollisionObject2DSW::TYPE_BODY == col_obj->get_type()) { - const Body2DSW *b = static_cast(col_obj); - if (p_infinite_inertia && PhysicsServer2D::BODY_MODE_STATIC != b->get_mode() && PhysicsServer2D::BODY_MODE_KINEMATIC != b->get_mode()) { - continue; - } - } + Transform2D col_obj_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); - Transform2D col_obj_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); + /* + * There is no point in supporting one way collisions with ray shapes, as they will always collide in the desired + * direction. Use a short ray shape if you want to achieve a similar effect. + * + if (col_obj->is_shape_set_as_one_way_collision(shape_idx)) { + cbk.valid_dir = col_obj_shape_xform.get_axis(1).normalized(); + cbk.valid_depth = p_margin; //only valid depth is the collision margin + cbk.invalid_by_dir = 0; + } else { + */ - /* - * There is no point in supporting one way collisions with ray shapes, as they will always collide in the desired - * direction. Use a short ray shape if you want to achieve a similar effect. - * - if (col_obj->is_shape_set_as_one_way_collision(shape_idx)) { - cbk.valid_dir = col_obj_shape_xform.get_axis(1).normalized(); - cbk.valid_depth = p_margin; //only valid depth is the collision margin - cbk.invalid_by_dir = 0; + cbk.valid_dir = Vector2(); + cbk.valid_depth = 0; + cbk.invalid_by_dir = 0; - } else { -*/ + /* + } + */ + + Shape2DSW *against_shape = col_obj->get_shape(shape_idx); + if (CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), cbkres, cbkptr, nullptr, p_margin)) { + int ray_index = -1; //reuse shape + for (int k = 0; k < rays_found; k++) { + if (r_results[ray_index].collision_local_shape == j) { + ray_index = k; + } + } - cbk.valid_dir = Vector2(); - cbk.valid_depth = 0; - cbk.invalid_by_dir = 0; + if (ray_index == -1 && rays_found < p_result_max) { + ray_index = rays_found; + rays_found++; + } - /* - } - */ + if (ray_index != -1) { + PhysicsServer2D::SeparationResult &result = r_results[ray_index]; - Shape2DSW *against_shape = col_obj->get_shape(shape_idx); - if (CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), cbkres, cbkptr, nullptr, p_margin)) { - if (cbk.amount > 0) { - collided = true; + for (int k = 0; k < cbk.amount; k++) { + Vector2 recover = sr[k * 2 + 1] - sr[k * 2 + 0]; + if (recover.x > max_x) { + max_x = recover.x; } - - int ray_index = -1; //reuse shape - for (int k = 0; k < rays_found; k++) { - if (r_results[ray_index].collision_local_shape == j) { - ray_index = k; - } + if (recover.y > max_y) { + max_y = recover.y; } - - if (ray_index == -1 && rays_found < p_result_max) { - ray_index = rays_found; - rays_found++; + if (recover.x < min_x) { + min_x = recover.x; + } + if (recover.y < min_y) { + min_y = recover.y; } - if (ray_index != -1) { - PhysicsServer2D::SeparationResult &result = r_results[ray_index]; - - for (int k = 0; k < cbk.amount; k++) { - Vector2 a = sr[k * 2 + 0]; - Vector2 b = sr[k * 2 + 1]; - - recover_motion += (b - a) / cbk.amount; - - real_t depth = a.distance_to(b); - if (depth > result.collision_depth) { - result.collision_depth = depth; - result.collision_point = b; - result.collision_normal = (b - a).normalized(); - result.collision_local_shape = j; - result.collider_shape = shape_idx; - result.collider = col_obj->get_self(); - result.collider_id = col_obj->get_instance_id(); - result.collider_metadata = col_obj->get_shape_metadata(shape_idx); - if (col_obj->get_type() == CollisionObject2DSW::TYPE_BODY) { - Body2DSW *body = (Body2DSW *)col_obj; - - Vector2 rel_vec = b - body->get_transform().get_origin(); - result.collider_velocity = Vector2(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity(); - } - } + real_t depth = recover.length(); + if (depth > result.collision_depth) { + result.collision_depth = depth; + result.collision_point = sr[k * 2 + 1]; + result.collision_normal = recover.normalized(); + result.collision_local_shape = j; + result.collider_shape = shape_idx; + result.collider = col_obj->get_self(); + result.collider_id = col_obj->get_instance_id(); + result.collider_metadata = col_obj->get_shape_metadata(shape_idx); + if (col_obj->get_type() == CollisionObject2DSW::TYPE_BODY) { + Body2DSW *body = (Body2DSW *)col_obj; + + Vector2 rel_vec = sr[k * 2 + 1] - body->get_transform().get_origin(); + result.collider_velocity = Vector2(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity(); } } } } } - - if (!collided || recover_motion == Vector2()) { - break; - } - - body_transform.elements[2] += recover_motion; - body_aabb.position += recover_motion; - - recover_attempts--; - } while (recover_attempts); + } } //optimize results (remove non colliding) @@ -686,7 +675,7 @@ int Space2DSW::test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_t } } - r_recover_motion = body_transform.elements[2] - p_transform.elements[2]; + r_recover_motion = Vector2(max_x + min_x, max_y + min_y); return rays_found; } @@ -747,122 +736,121 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co //STEP 1, FREE BODY IF STUCK const int max_results = 32; - int recover_attempts = 4; Vector2 sr[max_results * 2]; - do { - PhysicsServer2DSW::CollCbkData cbk; - cbk.max = max_results; - cbk.amount = 0; - cbk.passed = 0; - cbk.ptr = sr; - cbk.invalid_by_dir = 0; - excluded_shape_pair_count = 0; //last step is the one valid + PhysicsServer2DSW::CollCbkData cbk; + cbk.max = max_results; + cbk.amount = 0; + cbk.passed = 0; + cbk.ptr = sr; + cbk.invalid_by_dir = 0; + excluded_shape_pair_count = 0; //last step is the one valid - PhysicsServer2DSW::CollCbkData *cbkptr = &cbk; - CollisionSolver2DSW::CallbackResult cbkres = PhysicsServer2DSW::_shape_col_cbk; + PhysicsServer2DSW::CollCbkData *cbkptr = &cbk; + CollisionSolver2DSW::CallbackResult cbkres = PhysicsServer2DSW::_shape_col_cbk; - bool collided = false; + bool collided = false; - int amount = _cull_aabb_for_body(p_body, body_aabb); + int amount = _cull_aabb_for_body(p_body, body_aabb); - for (int j = 0; j < p_body->get_shape_count(); j++) { - if (p_body->is_shape_set_as_disabled(j)) { - continue; - } + for (int j = 0; j < p_body->get_shape_count(); j++) { + if (p_body->is_shape_set_as_disabled(j)) { + continue; + } - Shape2DSW *body_shape = p_body->get_shape(j); - if (p_exclude_raycast_shapes && body_shape->get_type() == PhysicsServer2D::SHAPE_RAY) { - continue; - } + Shape2DSW *body_shape = p_body->get_shape(j); + if (p_exclude_raycast_shapes && body_shape->get_type() == PhysicsServer2D::SHAPE_RAY) { + continue; + } - Transform2D body_shape_xform = body_transform * p_body->get_shape_transform(j); - for (int i = 0; i < amount; i++) { - const CollisionObject2DSW *col_obj = intersection_query_results[i]; - int shape_idx = intersection_query_subindex_results[i]; + Transform2D body_shape_xform = body_transform * p_body->get_shape_transform(j); + for (int i = 0; i < amount; i++) { + const CollisionObject2DSW *col_obj = intersection_query_results[i]; + int shape_idx = intersection_query_subindex_results[i]; - if (CollisionObject2DSW::TYPE_BODY == col_obj->get_type()) { - const Body2DSW *b = static_cast(col_obj); - if (p_infinite_inertia && PhysicsServer2D::BODY_MODE_STATIC != b->get_mode() && PhysicsServer2D::BODY_MODE_KINEMATIC != b->get_mode()) { - continue; - } + if (CollisionObject2DSW::TYPE_BODY == col_obj->get_type()) { + const Body2DSW *b = static_cast(col_obj); + if (p_infinite_inertia && PhysicsServer2D::BODY_MODE_STATIC != b->get_mode() && PhysicsServer2D::BODY_MODE_KINEMATIC != b->get_mode()) { + continue; } + } - Transform2D col_obj_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); - - if (col_obj->is_shape_set_as_one_way_collision(shape_idx)) { - cbk.valid_dir = col_obj_shape_xform.get_axis(1).normalized(); - - real_t owc_margin = col_obj->get_shape_one_way_collision_margin(shape_idx); - cbk.valid_depth = MAX(owc_margin, p_margin); //user specified, but never less than actual margin or it won't work - cbk.invalid_by_dir = 0; - - if (col_obj->get_type() == CollisionObject2DSW::TYPE_BODY) { - const Body2DSW *b = static_cast(col_obj); - if (b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_RIGID) { - //fix for moving platforms (kinematic and dynamic), margin is increased by how much it moved in the given direction - Vector2 lv = b->get_linear_velocity(); - //compute displacement from linear velocity - Vector2 motion = lv * PhysicsDirectBodyState2DSW::singleton->step; - real_t motion_len = motion.length(); - motion.normalize(); - cbk.valid_depth += motion_len * MAX(motion.dot(-cbk.valid_dir), 0.0); - } - } - } else { - cbk.valid_dir = Vector2(); - cbk.valid_depth = 0; - cbk.invalid_by_dir = 0; - } + Transform2D col_obj_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); - int current_passed = cbk.passed; //save how many points passed collision - bool did_collide = false; + if (col_obj->is_shape_set_as_one_way_collision(shape_idx)) { + cbk.valid_dir = col_obj_shape_xform.get_axis(1).normalized(); - Shape2DSW *against_shape = col_obj->get_shape(shape_idx); - if (CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), cbkres, cbkptr, nullptr, separation_margin)) { - did_collide = cbk.passed > current_passed; //more passed, so collision actually existed - } + real_t owc_margin = col_obj->get_shape_one_way_collision_margin(shape_idx); + cbk.valid_depth = MAX(owc_margin, p_margin); //user specified, but never less than actual margin or it won't work + cbk.invalid_by_dir = 0; - if (!did_collide && cbk.invalid_by_dir > 0) { - //this shape must be excluded - if (excluded_shape_pair_count < max_excluded_shape_pairs) { - ExcludedShapeSW esp; - esp.local_shape = body_shape; - esp.against_object = col_obj; - esp.against_shape_index = shape_idx; - excluded_shape_pairs[excluded_shape_pair_count++] = esp; + if (col_obj->get_type() == CollisionObject2DSW::TYPE_BODY) { + const Body2DSW *b = static_cast(col_obj); + if (b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_RIGID) { + //fix for moving platforms (kinematic and dynamic), margin is increased by how much it moved in the given direction + Vector2 lv = b->get_linear_velocity(); + //compute displacement from linear velocity + Vector2 motion = lv * PhysicsDirectBodyState2DSW::singleton->step; + real_t motion_len = motion.length(); + motion.normalize(); + cbk.valid_depth += motion_len * MAX(motion.dot(-cbk.valid_dir), 0.0); } } + } else { + cbk.valid_dir = Vector2(); + cbk.valid_depth = 0; + cbk.invalid_by_dir = 0; + } + + int current_passed = cbk.passed; //save how many points passed collision + bool did_collide = false; + + Shape2DSW *against_shape = col_obj->get_shape(shape_idx); + if (CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), cbkres, cbkptr, nullptr, separation_margin)) { + did_collide = cbk.passed > current_passed; //more passed, so collision actually existed + } - if (did_collide) { - collided = true; + if (!did_collide && cbk.invalid_by_dir > 0) { + //this shape must be excluded + if (excluded_shape_pair_count < max_excluded_shape_pairs) { + ExcludedShapeSW esp; + esp.local_shape = body_shape; + esp.against_object = col_obj; + esp.against_shape_index = shape_idx; + excluded_shape_pairs[excluded_shape_pair_count++] = esp; } } - } - if (!collided) { - break; + if (did_collide) { + collided = true; + } } + } - Vector2 recover_motion; + if (collided) { + real_t max_x = 0.f, max_y = 0.f; + real_t min_x = 0.f, min_y = 0.f; for (int i = 0; i < cbk.amount; i++) { - Vector2 a = sr[i * 2 + 0]; - Vector2 b = sr[i * 2 + 1]; - recover_motion += (b - a) / cbk.amount; - } - - if (recover_motion == Vector2()) { - collided = false; - break; + Vector2 recover = sr[i * 2 + 1] - sr[i * 2 + 0]; + if (recover.x > max_x) { + max_x = recover.x; + } + if (recover.y > max_y) { + max_y = recover.y; + } + if (recover.x < min_x) { + min_x = recover.x; + } + if (recover.y < min_y) { + min_y = recover.y; + } } + Vector2 recover_motion(max_x + min_x, max_y + min_y); body_transform.elements[2] += recover_motion; body_aabb.position += recover_motion; - - recover_attempts--; - - } while (recover_attempts); + } } real_t safe = 1.0; diff --git a/servers/physics_3d/space_3d_sw.cpp b/servers/physics_3d/space_3d_sw.cpp index 40537c70016d..583b66a951e1 100644 --- a/servers/physics_3d/space_3d_sw.cpp +++ b/servers/physics_3d/space_3d_sw.cpp @@ -274,11 +274,11 @@ bool PhysicsDirectSpaceState3DSW::cast_motion(const RID &p_shape, const Transfor continue; } - //test initial overlap, ignore objects it's inside of. + //test initial overlap sep_axis = p_motion.normalized(); if (!CollisionSolver3DSW::solve_distance(shape, p_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, aabb, &sep_axis)) { - continue; + return false; } //just do kinematic solving @@ -579,111 +579,107 @@ int Space3DSW::test_body_ray_separation(Body3DSW *p_body, const Transform &p_tra int rays_found = 0; - { - // raycast AND separate + // Raycast AND separate - const int max_results = 32; - int recover_attempts = 4; - Vector3 sr[max_results * 2]; - PhysicsServer3DSW::CollCbkData cbk; - cbk.max = max_results; - PhysicsServer3DSW::CollCbkData *cbkptr = &cbk; - CollisionSolver3DSW::CallbackResult cbkres = PhysicsServer3DSW::_shape_col_cbk; + const int max_results = 32; + Vector3 sr[max_results * 2]; + PhysicsServer3DSW::CollCbkData cbk; + cbk.max = max_results; + PhysicsServer3DSW::CollCbkData *cbkptr = &cbk; + CollisionSolver3DSW::CallbackResult cbkres = PhysicsServer3DSW::_shape_col_cbk; - do { - Vector3 recover_motion; + real_t max_x = 0.f, max_y = 0.f, max_z = 0.f; + real_t min_x = 0.f, min_y = 0.f, min_z = 0.f; - bool collided = false; + int amount = _cull_aabb_for_body(p_body, body_aabb); - int amount = _cull_aabb_for_body(p_body, body_aabb); + for (int j = 0; j < p_body->get_shape_count(); j++) { + if (p_body->is_shape_set_as_disabled(j)) { + continue; + } - for (int j = 0; j < p_body->get_shape_count(); j++) { - if (p_body->is_shape_set_as_disabled(j)) { - continue; - } + Shape3DSW *body_shape = p_body->get_shape(j); + + if (body_shape->get_type() != PhysicsServer3D::SHAPE_RAY) { + continue; + } + + Transform body_shape_xform = body_transform * p_body->get_shape_transform(j); + + for (int i = 0; i < amount; i++) { + const CollisionObject3DSW *col_obj = intersection_query_results[i]; + int shape_idx = intersection_query_subindex_results[i]; - Shape3DSW *body_shape = p_body->get_shape(j); + cbk.amount = 0; + cbk.ptr = sr; - if (body_shape->get_type() != PhysicsServer3D::SHAPE_RAY) { + if (CollisionObject3DSW::TYPE_BODY == col_obj->get_type()) { + const Body3DSW *b = static_cast(col_obj); + if (p_infinite_inertia && PhysicsServer3D::BODY_MODE_STATIC != b->get_mode() && PhysicsServer3D::BODY_MODE_KINEMATIC != b->get_mode()) { continue; } + } - Transform body_shape_xform = body_transform * p_body->get_shape_transform(j); + Shape3DSW *against_shape = col_obj->get_shape(shape_idx); + if (CollisionSolver3DSW::solve_static(body_shape, body_shape_xform, against_shape, col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, nullptr, p_margin)) { + int ray_index = -1; //reuse shape + for (int k = 0; k < rays_found; k++) { + if (r_results[k].collision_local_shape == j) { + ray_index = k; + } + } - for (int i = 0; i < amount; i++) { - const CollisionObject3DSW *col_obj = intersection_query_results[i]; - int shape_idx = intersection_query_subindex_results[i]; + if (ray_index == -1 && rays_found < p_result_max) { + ray_index = rays_found; + rays_found++; + } - cbk.amount = 0; - cbk.ptr = sr; + if (ray_index != -1) { + PhysicsServer3D::SeparationResult &result = r_results[ray_index]; - if (CollisionObject3DSW::TYPE_BODY == col_obj->get_type()) { - const Body3DSW *b = static_cast(col_obj); - if (p_infinite_inertia && PhysicsServer3D::BODY_MODE_STATIC != b->get_mode() && PhysicsServer3D::BODY_MODE_KINEMATIC != b->get_mode()) { - continue; + for (int k = 0; k < cbk.amount; k++) { + Vector3 recover = sr[k * 2 + 1] - sr[k * 2 + 0]; + if (recover.x > max_x) { + max_x = recover.x; } - } - - Shape3DSW *against_shape = col_obj->get_shape(shape_idx); - if (CollisionSolver3DSW::solve_static(body_shape, body_shape_xform, against_shape, col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, nullptr, p_margin)) { - if (cbk.amount > 0) { - collided = true; + if (recover.y > max_y) { + max_y = recover.y; } - - int ray_index = -1; //reuse shape - for (int k = 0; k < rays_found; k++) { - if (r_results[k].collision_local_shape == j) { - ray_index = k; - } + if (recover.z > max_z) { + max_z = recover.z; } - - if (ray_index == -1 && rays_found < p_result_max) { - ray_index = rays_found; - rays_found++; + if (recover.x < min_x) { + min_x = recover.x; + } + if (recover.y < min_y) { + min_y = recover.y; + } + if (recover.z < min_z) { + min_z = recover.z; } - if (ray_index != -1) { - PhysicsServer3D::SeparationResult &result = r_results[ray_index]; - - for (int k = 0; k < cbk.amount; k++) { - Vector3 a = sr[k * 2 + 0]; - Vector3 b = sr[k * 2 + 1]; - - recover_motion += (b - a) / cbk.amount; - - real_t depth = a.distance_to(b); - if (depth > result.collision_depth) { - result.collision_depth = depth; - result.collision_point = b; - result.collision_normal = (b - a).normalized(); - result.collision_local_shape = j; - result.collider = col_obj->get_self(); - result.collider_id = col_obj->get_instance_id(); - result.collider_shape = shape_idx; - //result.collider_metadata = col_obj->get_shape_metadata(shape_idx); - if (col_obj->get_type() == CollisionObject3DSW::TYPE_BODY) { - Body3DSW *body = (Body3DSW *)col_obj; - - Vector3 rel_vec = b - body->get_transform().get_origin(); - //result.collider_velocity = Vector3(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity(); - result.collider_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(body->get_transform().origin - rel_vec); // * mPos); - } - } + real_t depth = recover.length(); + if (depth > result.collision_depth) { + result.collision_depth = depth; + result.collision_point = sr[k * 2 + 1]; + result.collision_normal = recover.normalized(); + result.collision_local_shape = j; + result.collider = col_obj->get_self(); + result.collider_id = col_obj->get_instance_id(); + result.collider_shape = shape_idx; + //result.collider_metadata = col_obj->get_shape_metadata(shape_idx); + if (col_obj->get_type() == CollisionObject3DSW::TYPE_BODY) { + Body3DSW *body = (Body3DSW *)col_obj; + + Vector3 rel_vec = sr[k * 2 + 1] - body->get_transform().get_origin(); + //result.collider_velocity = Vector3(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity(); + result.collider_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(body->get_transform().origin - rel_vec); // * mPos); } } } } } - - if (!collided || recover_motion == Vector3()) { - break; - } - - body_transform.origin += recover_motion; - body_aabb.position += recover_motion; - - recover_attempts--; - } while (recover_attempts); + } } //optimize results (remove non colliding) @@ -694,7 +690,7 @@ int Space3DSW::test_body_ray_separation(Body3DSW *p_body, const Transform &p_tra } } - r_recover_motion = body_transform.origin - p_transform.origin; + r_recover_motion = Vector3(max_x + min_x, max_y + min_y, max_z + min_z); return rays_found; } @@ -745,66 +741,71 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons //STEP 1, FREE BODY IF STUCK const int max_results = 32; - int recover_attempts = 4; Vector3 sr[max_results * 2]; - do { - PhysicsServer3DSW::CollCbkData cbk; - cbk.max = max_results; - cbk.amount = 0; - cbk.ptr = sr; + PhysicsServer3DSW::CollCbkData cbk; + cbk.max = max_results; + cbk.amount = 0; + cbk.ptr = sr; - PhysicsServer3DSW::CollCbkData *cbkptr = &cbk; - CollisionSolver3DSW::CallbackResult cbkres = PhysicsServer3DSW::_shape_col_cbk; + PhysicsServer3DSW::CollCbkData *cbkptr = &cbk; + CollisionSolver3DSW::CallbackResult cbkres = PhysicsServer3DSW::_shape_col_cbk; - bool collided = false; + bool collided = false; - int amount = _cull_aabb_for_body(p_body, body_aabb); + int amount = _cull_aabb_for_body(p_body, body_aabb); - for (int j = 0; j < p_body->get_shape_count(); j++) { - if (p_body->is_shape_set_as_disabled(j)) { - continue; - } + for (int j = 0; j < p_body->get_shape_count(); j++) { + if (p_body->is_shape_set_as_disabled(j)) { + continue; + } - Transform body_shape_xform = body_transform * p_body->get_shape_transform(j); - Shape3DSW *body_shape = p_body->get_shape(j); - if (p_exclude_raycast_shapes && body_shape->get_type() == PhysicsServer3D::SHAPE_RAY) { - continue; - } + Transform body_shape_xform = body_transform * p_body->get_shape_transform(j); + Shape3DSW *body_shape = p_body->get_shape(j); + if (p_exclude_raycast_shapes && body_shape->get_type() == PhysicsServer3D::SHAPE_RAY) { + continue; + } - for (int i = 0; i < amount; i++) { - const CollisionObject3DSW *col_obj = intersection_query_results[i]; - int shape_idx = intersection_query_subindex_results[i]; + for (int i = 0; i < amount; i++) { + const CollisionObject3DSW *col_obj = intersection_query_results[i]; + int shape_idx = intersection_query_subindex_results[i]; - if (CollisionSolver3DSW::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, nullptr, p_margin)) { - collided = cbk.amount > 0; - } + if (CollisionSolver3DSW::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, nullptr, p_margin)) { + collided = cbk.amount > 0; } } + } - if (!collided) { - break; - } - - Vector3 recover_motion; + if (collided) { + real_t max_x = 0.f, max_y = 0.f, max_z = 0.f; + real_t min_x = 0.f, min_y = 0.f, min_z = 0.f; for (int i = 0; i < cbk.amount; i++) { - Vector3 a = sr[i * 2 + 0]; - Vector3 b = sr[i * 2 + 1]; - recover_motion += (b - a) / cbk.amount; - } - - if (recover_motion == Vector3()) { - collided = false; - break; + Vector3 recover = sr[i * 2 + 1] - sr[i * 2 + 0]; + if (recover.x > max_x) { + max_x = recover.x; + } + if (recover.y > max_y) { + max_y = recover.y; + } + if (recover.z > max_z) { + max_z = recover.z; + } + if (recover.x < min_x) { + min_x = recover.x; + } + if (recover.y < min_y) { + min_y = recover.y; + } + if (recover.z < min_z) { + min_z = recover.z; + } } + Vector3 recover_motion(max_x + min_x, max_y + min_y, max_z + min_z); body_transform.origin += recover_motion; body_aabb.position += recover_motion; - - recover_attempts--; - - } while (recover_attempts); + } } real_t safe = 1.0;