From 7e0b110b0d0f680256ed9ae52ff6b1ab7509d441 Mon Sep 17 00:00:00 2001 From: Marcel Admiraal Date: Tue, 21 Jan 2020 09:53:15 +0000 Subject: [PATCH] Fix multiple issues with test_body_motion() and test_body_ray_separaton(). The test_body_motion() and test_body_ray_separation() functions perform an initial recovery step to attempt to move the body to a safe place before testing whether the body can safely move. Currently this is done in four partial steps, and during each step the detected penetrations are reversed by summing them together. This patch changes this approach to a single step and uses the maximum penetration instead of the sum. The same approach is applied to Godot Physics 2D, 3D and Bullet Physics, and to both CollisionShapes and RayShapes. Also, fixes Bullet physics not detecting collisions and relying on the recovery step. Furthermore, ensures that Bullet physics only moves a safe amount by using a binary search as done in Godot physics. --- modules/bullet/space_bullet.cpp | 448 +++++++++++++---------------- modules/bullet/space_bullet.h | 30 +- scene/3d/physics_body.cpp | 2 +- servers/physics/space_sw.cpp | 232 +++++++-------- servers/physics_2d/space_2d_sw.cpp | 342 ++++++++++------------ 5 files changed, 483 insertions(+), 571 deletions(-) diff --git a/modules/bullet/space_bullet.cpp b/modules/bullet/space_bullet.cpp index 9513c2103783..361333293320 100644 --- a/modules/bullet/space_bullet.cpp +++ b/modules/bullet/space_bullet.cpp @@ -902,109 +902,47 @@ 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 +bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, PhysicsServer::MotionResult *r_motion_result, bool p_exclude_raycast_shapes) { -#define RECOVERING_MOVEMENT_SCALE 0.4 -#define RECOVERING_MOVEMENT_CYCLES 4 - -#if debug_test_motion - -#include "scene/3d/immediate_geometry.h" - -static ImmediateGeometry *motionVec(NULL); -static ImmediateGeometry *normalLine(NULL); -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, PhysicsServer::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(ImmediateGeometry); - normalLine = memnew(ImmediateGeometry); - SceneTree::get_singleton()->get_current_scene()->add_child(motionVec); - SceneTree::get_singleton()->get_current_scene()->add_child(normalLine); - - motionVec->set_as_toplevel(true); - normalLine->set_as_toplevel(true); - - red_mat = Ref(memnew(SpatialMaterial)); - red_mat->set_flag(SpatialMaterial::FLAG_UNSHADED, true); - red_mat->set_line_width(20.0); - red_mat->set_feature(SpatialMaterial::FEATURE_TRANSPARENT, true); - red_mat->set_flag(SpatialMaterial::FLAG_ALBEDO_FROM_VERTEX_COLOR, true); - red_mat->set_flag(SpatialMaterial::FLAG_SRGB_VERTEX_COLOR, true); - red_mat->set_albedo(Color(1, 0, 0, 1)); - motionVec->set_material_override(red_mat); - - blue_mat = Ref(memnew(SpatialMaterial)); - blue_mat->set_flag(SpatialMaterial::FLAG_UNSHADED, true); - blue_mat->set_line_width(20.0); - blue_mat->set_feature(SpatialMaterial::FEATURE_TRANSPARENT, true); - blue_mat->set_flag(SpatialMaterial::FLAG_ALBEDO_FROM_VERTEX_COLOR, true); - blue_mat->set_flag(SpatialMaterial::FLAG_SRGB_VERTEX_COLOR, true); - blue_mat->set_albedo(Color(0, 0, 1, 1)); - normalLine->set_material_override(blue_mat); - } -#endif + 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, NULL); - motionVec->add_vertex(sup_line); - motionVec->add_vertex(sup_line + p_motion * 10); - motionVec->end(); -#endif + for (int shape_idx = 0; shape_idx < shape_count; ++shape_idx) { - for (int shIndex = 0; shIndex < shape_count; ++shIndex) { - if (p_body->is_shape_disabled(shIndex)) { + 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; @@ -1017,92 +955,101 @@ 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; + if (r_motion_result) { - { /// Phase three - contact test with margin + if (is_colliding) { - 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); + 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) { + KinematicCollisionResult collision_result; + if (kinematic_collision_test(p_body, collision_transform, p_infinite_inertia, discard_motion, &collision_result)) { - const btRigidBody *btRigid = static_cast(r_recover_result.other_collision_object); + // 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, NULL); - normalLine->add_vertex(r_result->collision_point); - normalLine->add_vertex(r_result->collision_point + r_result->collision_normal * 10); - normalLine->end(); -#endif - } else { - r_result->remainder = Vector3(); + 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, PhysicsServer::SeparationResult *r_results, int p_result_max, float p_margin) { +int SpaceBullet::test_ray_separation(RigidBodyBullet *p_body, const Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, PhysicsServer::SeparationResult *r_separation_results, int p_result_max, float 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) { - PhysicsServer::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; @@ -1112,11 +1059,11 @@ struct RecoverPenetrationBroadPhaseCallback : public btBroadphaseAabbCallback { struct CompoundLeafCallback : btDbvt::ICollide { private: - RecoverPenetrationBroadPhaseCallback *parent_callback; + KinematicBroadPhaseCallback *parent_callback; btCollisionObject *collision_object; 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) { } @@ -1138,7 +1085,7 @@ 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) { @@ -1146,7 +1093,7 @@ struct RecoverPenetrationBroadPhaseCallback : public btBroadphaseAabbCallback { bounds = btDbvtVolume::FromMM(p_aabb_min, p_aabb_max); } - virtual ~RecoverPenetrationBroadPhaseCallback() {} + virtual ~KinematicBroadPhaseCallback() {} virtual bool process(const btBroadphaseProxy *proxy) { @@ -1192,7 +1139,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; @@ -1211,8 +1158,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); @@ -1231,16 +1176,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) { @@ -1256,50 +1203,58 @@ 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); - - 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)) { + 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(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; @@ -1307,22 +1262,24 @@ bool SpaceBullet::RFP_convex_convex_test(const btConvexShape *p_shapeA, const bt 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, 0); - 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; @@ -1330,35 +1287,32 @@ 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); +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(NULL, p_shapeA, p_objectA, tA, -1, p_shapeId_A); - btCollisionObjectWrapper obB(NULL, p_shapeB, p_objectB, p_transformB, -1, p_shapeId_B); + 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); - btCollisionAlgorithm *algorithm = dispatcher->findAlgorithm(&obA, &obB, NULL, 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; @@ -1367,29 +1321,7 @@ bool SpaceBullet::RFP_convex_world_test(const btConvexShape *p_shapeA, const btC return false; } -int SpaceBullet::add_separation_result(PhysicsServer::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, PhysicsServer::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, PhysicsServer::SeparationResult *r_separation_results) { // Calculate the cumulative AABB of all shapes of the kinematic body btVector3 aabb_min, aabb_max; @@ -1407,7 +1339,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); @@ -1427,16 +1358,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) { @@ -1455,36 +1388,65 @@ 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); - - 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)) { + 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); - 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, PhysicsServer::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 4b01ed9220fd..fe78a510af09 100644 --- a/modules/bullet/space_bullet.h +++ b/modules/bullet/space_bullet.h @@ -183,8 +183,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, PhysicsServer::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, PhysicsServer::SeparationResult *r_results, int p_result_max, float p_margin); + bool test_body_motion(RigidBodyBullet *p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, PhysicsServer::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, PhysicsServer::SeparationResult *r_separation_results, int p_result_max, float p_margin); private: void create_empty_world(bool p_create_soft_world); @@ -192,17 +192,17 @@ class SpaceBullet : public RIDBullet { void check_ghost_overlaps(); void check_body_collision(); - struct RecoverResult { - bool hasPenetration; + struct KinematicCollisionResult { + bool isColliding; btVector3 normal; btVector3 pointWorld; - btScalar penetration_distance; // Negative mean penetration + btScalar penetration_distance; int other_compound_shape_index; const btCollisionObject *other_collision_object; int local_shape_most_recovered; - RecoverResult() : - hasPenetration(false), + KinematicCollisionResult() : + isColliding(false), normal(0, 0, 0), pointWorld(0, 0, 0), penetration_distance(1e20), @@ -211,15 +211,13 @@ class SpaceBullet : public RIDBullet { local_shape_most_recovered(0) {} }; - 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 = NULL); - /// 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 = NULL); - /// 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 = NULL); + 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(PhysicsServer::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, PhysicsServer::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, PhysicsServer::SeparationResult *r_separation_results); + int add_separation_result(const SpaceBullet::KinematicCollisionResult &p_collision_result, int p_shape_id, const btCollisionObject *p_other_object, PhysicsServer::SeparationResult *r_separation_results) const; }; #endif diff --git a/scene/3d/physics_body.cpp b/scene/3d/physics_body.cpp index c7a7709d0106..949bade5d0b6 100644 --- a/scene/3d/physics_body.cpp +++ b/scene/3d/physics_body.cpp @@ -1323,7 +1323,7 @@ bool KinematicBody::separate_raycast_shapes(bool p_infinite_inertia, Collision & Vector3 recover; int hits = PhysicsServer::get_singleton()->body_test_ray_separation(get_rid(), gt, p_infinite_inertia, recover, sep_res, 8, margin); int deepest = -1; - float deepest_depth; + float 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/space_sw.cpp b/servers/physics/space_sw.cpp index 0c00f8a5ae4a..cb8521a5a868 100644 --- a/servers/physics/space_sw.cpp +++ b/servers/physics/space_sw.cpp @@ -575,112 +575,97 @@ int SpaceSW::test_body_ray_separation(BodySW *p_body, const Transform &p_transfo int rays_found = 0; - { - // raycast AND separate + // Raycast AND separate - const int max_results = 32; - int recover_attempts = 4; - Vector3 sr[max_results * 2]; - PhysicsServerSW::CollCbkData cbk; - cbk.max = max_results; - PhysicsServerSW::CollCbkData *cbkptr = &cbk; - CollisionSolverSW::CallbackResult cbkres = PhysicsServerSW::_shape_col_cbk; + const int max_results = 32; + Vector3 sr[max_results * 2]; + PhysicsServerSW::CollCbkData cbk; + cbk.max = max_results; + PhysicsServerSW::CollCbkData *cbkptr = &cbk; + CollisionSolverSW::CallbackResult cbkres = PhysicsServerSW::_shape_col_cbk; - do { + 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; - Vector3 recover_motion; + int amount = _cull_aabb_for_body(p_body, body_aabb); - bool collided = false; + for (int j = 0; j < p_body->get_shape_count(); j++) { + if (p_body->is_shape_set_as_disabled(j)) + continue; - int amount = _cull_aabb_for_body(p_body, body_aabb); + ShapeSW *body_shape = p_body->get_shape(j); - for (int j = 0; j < p_body->get_shape_count(); j++) { - if (p_body->is_shape_set_as_disabled(j)) - continue; + if (body_shape->get_type() != PhysicsServer::SHAPE_RAY) + continue; - ShapeSW *body_shape = p_body->get_shape(j); + Transform body_shape_xform = body_transform * p_body->get_shape_transform(j); - if (body_shape->get_type() != PhysicsServer::SHAPE_RAY) - continue; + for (int i = 0; i < amount; i++) { - Transform body_shape_xform = body_transform * p_body->get_shape_transform(j); + const CollisionObjectSW *col_obj = intersection_query_results[i]; + int shape_idx = intersection_query_subindex_results[i]; - for (int i = 0; i < amount; i++) { + cbk.amount = 0; + cbk.ptr = sr; - const CollisionObjectSW *col_obj = intersection_query_results[i]; - int shape_idx = intersection_query_subindex_results[i]; + if (CollisionObjectSW::TYPE_BODY == col_obj->get_type()) { + const BodySW *b = static_cast(col_obj); + if (p_infinite_inertia && PhysicsServer::BODY_MODE_STATIC != b->get_mode() && PhysicsServer::BODY_MODE_KINEMATIC != b->get_mode()) { + continue; + } + } - cbk.amount = 0; - cbk.ptr = sr; + ShapeSW *against_shape = col_obj->get_shape(shape_idx); + if (CollisionSolverSW::solve_static(body_shape, body_shape_xform, against_shape, col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, NULL, p_margin)) { - if (CollisionObjectSW::TYPE_BODY == col_obj->get_type()) { - const BodySW *b = static_cast(col_obj); - if (p_infinite_inertia && PhysicsServer::BODY_MODE_STATIC != b->get_mode() && PhysicsServer::BODY_MODE_KINEMATIC != b->get_mode()) { - continue; - } + 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; } + } - ShapeSW *against_shape = col_obj->get_shape(shape_idx); - if (CollisionSolverSW::solve_static(body_shape, body_shape_xform, against_shape, col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, NULL, p_margin)) { - if (cbk.amount > 0) { - collided = true; - } - - 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 (ray_index == -1 && rays_found < p_result_max) { - ray_index = rays_found; - rays_found++; - } + if (ray_index == -1 && rays_found < p_result_max) { + ray_index = rays_found; + rays_found++; + } - if (ray_index != -1) { - PhysicsServer::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; - - float 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() == CollisionObjectSW::TYPE_BODY) { - BodySW *body = (BodySW *)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); - } - } + if (ray_index != -1) { + PhysicsServer::SeparationResult &result = r_results[ray_index]; + + 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; + 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; + + float 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() == CollisionObjectSW::TYPE_BODY) { + BodySW *body = (BodySW *)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) @@ -691,7 +676,7 @@ int SpaceSW::test_body_ray_separation(BodySW *p_body, const Transform &p_transfo } } - 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; } @@ -743,68 +728,61 @@ bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Ve //STEP 1, FREE BODY IF STUCK const int max_results = 32; - int recover_attempts = 4; Vector3 sr[max_results * 2]; - do { - - PhysicsServerSW::CollCbkData cbk; - cbk.max = max_results; - cbk.amount = 0; - cbk.ptr = sr; + PhysicsServerSW::CollCbkData cbk; + cbk.max = max_results; + cbk.amount = 0; + cbk.ptr = sr; - PhysicsServerSW::CollCbkData *cbkptr = &cbk; - CollisionSolverSW::CallbackResult cbkres = PhysicsServerSW::_shape_col_cbk; + PhysicsServerSW::CollCbkData *cbkptr = &cbk; + CollisionSolverSW::CallbackResult cbkres = PhysicsServerSW::_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); - ShapeSW *body_shape = p_body->get_shape(j); - if (p_exclude_raycast_shapes && body_shape->get_type() == PhysicsServer::SHAPE_RAY) { - continue; - } + Transform body_shape_xform = body_transform * p_body->get_shape_transform(j); + ShapeSW *body_shape = p_body->get_shape(j); + if (p_exclude_raycast_shapes && body_shape->get_type() == PhysicsServer::SHAPE_RAY) { + continue; + } - for (int i = 0; i < amount; i++) { + for (int i = 0; i < amount; i++) { - const CollisionObjectSW *col_obj = intersection_query_results[i]; - int shape_idx = intersection_query_subindex_results[i]; + const CollisionObjectSW *col_obj = intersection_query_results[i]; + int shape_idx = intersection_query_subindex_results[i]; - if (CollisionSolverSW::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, NULL, p_margin)) { - collided = cbk.amount > 0; - } + if (CollisionSolverSW::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, NULL, p_margin)) { + collided = cbk.amount > 0; } } + } - if (!collided) { - break; - } + if (collided) { - 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; 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; diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp index a23bae517f11..78a23283b96a 100644 --- a/servers/physics_2d/space_2d_sw.cpp +++ b/servers/physics_2d/space_2d_sw.cpp @@ -541,137 +541,120 @@ 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]; - Physics2DServerSW::CollCbkData cbk; - cbk.max = max_results; - Physics2DServerSW::CollCbkData *cbkptr = &cbk; - CollisionSolver2DSW::CallbackResult cbkres = Physics2DServerSW::_shape_col_cbk; + const int max_results = 32; + Vector2 sr[max_results * 2]; + Physics2DServerSW::CollCbkData cbk; + cbk.max = max_results; + Physics2DServerSW::CollCbkData *cbkptr = &cbk; + CollisionSolver2DSW::CallbackResult cbkres = Physics2DServerSW::_shape_col_cbk; - do { + real_t max_x = 0.f, max_y = 0.f; + real_t min_x = 0.f, min_y = 0.f; - Vector2 recover_motion; + int amount = _cull_aabb_for_body(p_body, body_aabb); - bool collided = false; + for (int j = 0; j < p_body->get_shape_count(); j++) { + if (p_body->is_shape_set_as_disabled(j)) + continue; - int amount = _cull_aabb_for_body(p_body, body_aabb); + Shape2DSW *body_shape = p_body->get_shape(j); - for (int j = 0; j < p_body->get_shape_count(); j++) { - if (p_body->is_shape_set_as_disabled(j)) - continue; + if (body_shape->get_type() != Physics2DServer::SHAPE_RAY) + continue; - Shape2DSW *body_shape = p_body->get_shape(j); + Transform2D body_shape_xform = body_transform * p_body->get_shape_transform(j); - if (body_shape->get_type() != Physics2DServer::SHAPE_RAY) - continue; + for (int i = 0; i < amount; i++) { - Transform2D body_shape_xform = body_transform * p_body->get_shape_transform(j); + const CollisionObject2DSW *col_obj = intersection_query_results[i]; + int shape_idx = intersection_query_subindex_results[i]; - for (int i = 0; i < amount; i++) { + cbk.amount = 0; + cbk.passed = 0; + cbk.ptr = sr; + cbk.invalid_by_dir = 0; - 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 && Physics2DServer::BODY_MODE_STATIC != b->get_mode() && Physics2DServer::BODY_MODE_KINEMATIC != b->get_mode()) { + continue; + } + } - cbk.amount = 0; - cbk.passed = 0; - cbk.ptr = sr; - cbk.invalid_by_dir = 0; + Transform2D col_obj_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); - if (CollisionObject2DSW::TYPE_BODY == col_obj->get_type()) { - const Body2DSW *b = static_cast(col_obj); - if (p_infinite_inertia && Physics2DServer::BODY_MODE_STATIC != b->get_mode() && Physics2DServer::BODY_MODE_KINEMATIC != b->get_mode()) { - continue; - } - } + /* + * 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)) { - Transform2D col_obj_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(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; - /* - * 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)) { + } else { + */ - 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 { -*/ + /* + } + */ - cbk.valid_dir = Vector2(); - cbk.valid_depth = 0; - cbk.invalid_by_dir = 0; + 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, NULL, 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; } - */ - - 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, NULL, p_margin)) { - if (cbk.amount > 0) { - collided = true; - } - - 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 (ray_index == -1 && rays_found < p_result_max) { - ray_index = rays_found; - rays_found++; - } + if (ray_index == -1 && rays_found < p_result_max) { + ray_index = rays_found; + rays_found++; + } - if (ray_index != -1) { + if (ray_index != -1) { - Physics2DServer::SeparationResult &result = r_results[ray_index]; + Physics2DServer::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]; + for (int k = 0; k < cbk.amount; k++) { - recover_motion += (b - a) / cbk.amount; + Vector2 recover = sr[k * 2 + 1] - sr[k * 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; - float depth = a.distance_to(b); - if (depth > result.collision_depth) { + float depth = recover.length(); + 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; + 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 = 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(); - } - } + 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) @@ -682,7 +665,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; } @@ -743,125 +726,116 @@ 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 { - - Physics2DServerSW::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 + Physics2DServerSW::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 - Physics2DServerSW::CollCbkData *cbkptr = &cbk; - CollisionSolver2DSW::CallbackResult cbkres = Physics2DServerSW::_shape_col_cbk; + Physics2DServerSW::CollCbkData *cbkptr = &cbk; + CollisionSolver2DSW::CallbackResult cbkres = Physics2DServerSW::_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() == Physics2DServer::SHAPE_RAY) { - continue; - } + Shape2DSW *body_shape = p_body->get_shape(j); + if (p_exclude_raycast_shapes && body_shape->get_type() == Physics2DServer::SHAPE_RAY) { + continue; + } - Transform2D body_shape_xform = body_transform * p_body->get_shape_transform(j); - for (int i = 0; i < amount; 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]; + 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 && Physics2DServer::BODY_MODE_STATIC != b->get_mode() && Physics2DServer::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 && Physics2DServer::BODY_MODE_STATIC != b->get_mode() && Physics2DServer::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); - if (col_obj->is_shape_set_as_one_way_collision(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(); + cbk.valid_dir = col_obj_shape_xform.get_axis(1).normalized(); - float 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; + float 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() == Physics2DServer::BODY_MODE_KINEMATIC || b->get_mode() == Physics2DServer::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 * Physics2DDirectBodyStateSW::singleton->step; - float motion_len = motion.length(); - motion.normalize(); - cbk.valid_depth += motion_len * MAX(motion.dot(-cbk.valid_dir), 0.0); - } + if (col_obj->get_type() == CollisionObject2DSW::TYPE_BODY) { + const Body2DSW *b = static_cast(col_obj); + if (b->get_mode() == Physics2DServer::BODY_MODE_KINEMATIC || b->get_mode() == Physics2DServer::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 * Physics2DDirectBodyStateSW::singleton->step; + float 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; } + } 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; + 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, NULL, separation_margin)) { - did_collide = cbk.passed > current_passed; //more passed, so collision actually existed - } + 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, NULL, separation_margin)) { + did_collide = cbk.passed > current_passed; //more passed, so collision actually existed + } - 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 (!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 (did_collide) { - collided = true; - } + if (did_collide) { + collided = true; } } + } - if (!collided) { - break; - } + if (collided) { - Vector2 recover_motion; + 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;