From 9826456f2e78b3502f646ffa5445c76c6d31f7ea Mon Sep 17 00:00:00 2001 From: Andrea Catania Date: Sat, 11 Aug 2018 10:25:56 +0200 Subject: [PATCH 1/2] Improved move_and_slide function to stay on slope and fall on steep slope --- modules/bullet/space_bullet.cpp | 4 ++-- scene/3d/physics_body.cpp | 39 ++++++++++++++++++++------------- scene/3d/physics_body.h | 2 +- 3 files changed, 27 insertions(+), 18 deletions(-) diff --git a/modules/bullet/space_bullet.cpp b/modules/bullet/space_bullet.cpp index 97228a972ff..d1fb5b01d60 100644 --- a/modules/bullet/space_bullet.cpp +++ b/modules/bullet/space_bullet.cpp @@ -924,11 +924,11 @@ bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_f btVector3 __rec(0, 0, 0); RecoverResult r_recover_result; - has_penetration = recover_from_penetration(p_body, body_transform, 0, p_infinite_inertia, __rec, &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, r_result->motion); + B_TO_G(motion + initial_recover_motion + __rec, r_result->motion); if (has_penetration) { diff --git a/scene/3d/physics_body.cpp b/scene/3d/physics_body.cpp index e53ccb4cf45..52e23b0d95a 100644 --- a/scene/3d/physics_body.cpp +++ b/scene/3d/physics_body.cpp @@ -1128,7 +1128,7 @@ bool KinematicBody::move_and_collide(const Vector3 &p_motion, bool p_infinite_in //so, if you pass 45 as limit, avoid numerical precision erros when angle is 45. #define FLOOR_ANGLE_THRESHOLD 0.01 -Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Vector3 &p_floor_direction, float p_slope_stop_min_velocity, int p_max_slides, float p_floor_max_angle, bool p_infinite_inertia) { +Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Vector3 &p_floor_direction, bool p_stop_on_slope, int p_max_slides, float p_floor_max_angle, bool p_infinite_inertia) { Vector3 lv = p_linear_velocity; @@ -1146,14 +1146,18 @@ Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Ve colliders.clear(); floor_velocity = Vector3(); + Vector3 lv_n = p_linear_velocity.normalized(); + while (p_max_slides) { Collision collision; bool collided = move_and_collide(motion, p_infinite_inertia, collision); + bool is_on_slope = false; if (collided) { + colliders.push_back(collision); motion = collision.remainder; if (p_floor_direction == Vector3()) { @@ -1165,15 +1169,17 @@ Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Ve on_floor = true; floor_velocity = collision.collider_vel; - Vector3 rel_v = lv - floor_velocity; - Vector3 hv = rel_v - p_floor_direction * p_floor_direction.dot(rel_v); - - if (collision.travel.length() < 0.05 && hv.length() < p_slope_stop_min_velocity) { - Transform gt = get_global_transform(); - gt.origin -= collision.travel; - set_global_transform(gt); - return floor_velocity - p_floor_direction * p_floor_direction.dot(floor_velocity); + if (p_stop_on_slope) { + if (Vector3() == lv_n + p_floor_direction) { + Transform gt = get_global_transform(); + gt.origin -= collision.travel; + set_global_transform(gt); + return Vector3(); + } } + + is_on_slope = true; + } else if (collision.normal.dot(-p_floor_direction) >= Math::cos(p_floor_max_angle + FLOOR_ANGLE_THRESHOLD)) { //ceiling on_ceiling = true; } else { @@ -1181,9 +1187,14 @@ Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Ve } } - Vector3 n = collision.normal; - motion = motion.slide(n); - lv = lv.slide(n); + if (p_stop_on_slope && is_on_slope) { + motion = motion.slide(p_floor_direction); + lv = lv.slide(p_floor_direction); + } else { + Vector3 n = collision.normal; + motion = motion.slide(n); + lv = lv.slide(n); + } for (int i = 0; i < 3; i++) { if (locked_axis & (1 << i)) { @@ -1191,8 +1202,6 @@ Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Ve } } - colliders.push_back(collision); - } else { break; } @@ -1277,7 +1286,7 @@ Ref KinematicBody::_get_slide_collision(int p_bounce) { void KinematicBody::_bind_methods() { ClassDB::bind_method(D_METHOD("move_and_collide", "rel_vec", "infinite_inertia"), &KinematicBody::_move, DEFVAL(true)); - ClassDB::bind_method(D_METHOD("move_and_slide", "linear_velocity", "floor_normal", "slope_stop_min_velocity", "max_slides", "floor_max_angle", "infinite_inertia"), &KinematicBody::move_and_slide, DEFVAL(Vector3(0, 0, 0)), DEFVAL(0.05), DEFVAL(4), DEFVAL(Math::deg2rad((float)45)), DEFVAL(true)); + ClassDB::bind_method(D_METHOD("move_and_slide", "linear_velocity", "floor_normal", "max_slides", "floor_max_angle", "infinite_inertia"), &KinematicBody::move_and_slide, DEFVAL(Vector3(0, 0, 0)), DEFVAL(4), DEFVAL(Math::deg2rad((float)45)), DEFVAL(true)); ClassDB::bind_method(D_METHOD("test_move", "from", "rel_vec", "infinite_inertia"), &KinematicBody::test_move); diff --git a/scene/3d/physics_body.h b/scene/3d/physics_body.h index 80bf422c988..a39616893d8 100644 --- a/scene/3d/physics_body.h +++ b/scene/3d/physics_body.h @@ -323,7 +323,7 @@ public: void set_safe_margin(float p_margin); float get_safe_margin() const; - Vector3 move_and_slide(const Vector3 &p_linear_velocity, const Vector3 &p_floor_direction = Vector3(0, 0, 0), float p_slope_stop_min_velocity = 0.05, int p_max_slides = 4, float p_floor_max_angle = Math::deg2rad((float)45), bool p_infinite_inertia = true); + Vector3 move_and_slide(const Vector3 &p_linear_velocity, const Vector3 &p_floor_direction = Vector3(0, 0, 0), bool p_stop_on_slope = false, int p_max_slides = 4, float p_floor_max_angle = Math::deg2rad((float)45), bool p_infinite_inertia = true); bool is_on_floor() const; bool is_on_wall() const; bool is_on_ceiling() const; From 0010d9c82a89e89fc1bc83e806f59f34f3deb893 Mon Sep 17 00:00:00 2001 From: Andrea Catania Date: Tue, 14 Aug 2018 19:20:48 +0200 Subject: [PATCH 2/2] Added ray shape and move_and_slide with snapping on 3D. Added stop_on_slope on 2d part --- modules/bullet/bullet_physics_server.cpp | 8 + modules/bullet/bullet_physics_server.h | 1 + modules/bullet/godot_result_callbacks.cpp | 13 +- modules/bullet/space_bullet.cpp | 121 +++++++++++++- modules/bullet/space_bullet.h | 3 + scene/2d/physics_body_2d.cpp | 48 ++++-- scene/2d/physics_body_2d.h | 4 +- scene/3d/physics_body.cpp | 190 ++++++++++++++++------ scene/3d/physics_body.h | 9 +- servers/physics/physics_server_sw.h | 1 + servers/physics_server.h | 15 ++ 11 files changed, 338 insertions(+), 75 deletions(-) diff --git a/modules/bullet/bullet_physics_server.cpp b/modules/bullet/bullet_physics_server.cpp index 08576354922..032654c3f7f 100644 --- a/modules/bullet/bullet_physics_server.cpp +++ b/modules/bullet/bullet_physics_server.cpp @@ -869,6 +869,14 @@ bool BulletPhysicsServer::body_test_motion(RID p_body, const Transform &p_from, return body->get_space()->test_body_motion(body, p_from, p_motion, p_infinite_inertia, r_result); } +int BulletPhysicsServer::body_test_ray_separation(RID p_body, const Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, SeparationResult *r_results, int p_result_max, float p_margin) { + RigidBodyBullet *body = rigid_body_owner.get(p_body); + ERR_FAIL_COND_V(!body, 0); + ERR_FAIL_COND_V(!body->get_space(), 0); + + return body->get_space()->test_ray_separation(body, p_transform, p_infinite_inertia, r_recover_motion, r_results, p_result_max, p_margin); +} + RID BulletPhysicsServer::soft_body_create(bool p_init_sleeping) { SoftBodyBullet *body = bulletnew(SoftBodyBullet); body->set_collision_layer(1); diff --git a/modules/bullet/bullet_physics_server.h b/modules/bullet/bullet_physics_server.h index 0e858ff3115..2ff83bfd7d7 100644 --- a/modules/bullet/bullet_physics_server.h +++ b/modules/bullet/bullet_physics_server.h @@ -259,6 +259,7 @@ public: virtual PhysicsDirectBodyState *body_get_direct_state(RID p_body); virtual bool body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, MotionResult *r_result = NULL); + virtual int body_test_ray_separation(RID p_body, const Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, SeparationResult *r_results, int p_result_max, float p_margin = 0.001); /* SOFT BODY API */ diff --git a/modules/bullet/godot_result_callbacks.cpp b/modules/bullet/godot_result_callbacks.cpp index 197550d6867..d1b48cf5a9e 100644 --- a/modules/bullet/godot_result_callbacks.cpp +++ b/modules/bullet/godot_result_callbacks.cpp @@ -260,10 +260,19 @@ void GodotDeepPenetrationContactResultCallback::addContactPoint(const btVector3 if (m_penetration_distance > depth) { // Has penetration? - bool isSwapped = m_manifoldPtr->getBody0() != m_body0Wrap->getCollisionObject(); + const bool isSwapped = m_manifoldPtr->getBody0() != m_body0Wrap->getCollisionObject(); m_penetration_distance = depth; m_other_compound_shape_index = isSwapped ? m_index0 : m_index1; - m_pointNormalWorld = isSwapped ? normalOnBInWorld * -1 : normalOnBInWorld; m_pointWorld = isSwapped ? (pointInWorldOnB + (normalOnBInWorld * depth)) : pointInWorldOnB; + + const btCollisionObjectWrapper *bw0 = m_body0Wrap; + if (isSwapped) + bw0 = m_body1Wrap; + + if (bw0->getCollisionShape()->getShapeType() == CUSTOM_CONVEX_SHAPE_TYPE) { + m_pointNormalWorld = bw0->m_worldTransform.getBasis().transpose() * btVector3(0, 0, 1); + } else { + m_pointNormalWorld = isSwapped ? normalOnBInWorld * -1 : normalOnBInWorld; + } } } diff --git a/modules/bullet/space_bullet.cpp b/modules/bullet/space_bullet.cpp index d1fb5b01d60..c0fea910c28 100644 --- a/modules/bullet/space_bullet.cpp +++ b/modules/bullet/space_bullet.cpp @@ -894,6 +894,12 @@ bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_f // Skip no convex shape continue; } + + if (p_body->get_bt_shape(shIndex)->getShapeType() == CUSTOM_CONVEX_SHAPE_TYPE) { + // Skip rayshape in order to implement custom separation process + 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; @@ -964,6 +970,39 @@ bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_f return has_penetration; } +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) { + + btTransform body_transform; + G_TO_B(p_transform, body_transform); + UNSCALE_BT_BASIS(body_transform); + + btVector3 recover_motion(0, 0, 0); + + int rays_found; + + for (int t(RECOVERING_MOVEMENT_CYCLES); 0 < t; --t) { + int last_ray_index = recover_from_penetration_ray(p_body, body_transform, RECOVERING_MOVEMENT_SCALE, p_infinite_inertia, p_result_max, recover_motion, r_results); + + rays_found = MAX(last_ray_index, rays_found); + if (!rays_found) { + break; + } else { + body_transform.getOrigin() += recover_motion; + } + } + + //optimize results (remove non colliding) + for (int i = 0; i < rays_found; i++) { + if (r_results[i].collision_depth >= 0) { + rays_found--; + SWAP(r_results[i], r_results[rays_found]); + } + } + + B_TO_G(recover_motion, r_recover_motion); + return rays_found; +} + struct RecoverPenetrationBroadPhaseCallback : public btBroadphaseAabbCallback { private: const btCollisionObject *self_collision_object; @@ -1020,6 +1059,11 @@ bool SpaceBullet::recover_from_penetration(RigidBodyBullet *p_body, const btTran continue; } + if (kin_shape.shape->getShapeType() == CUSTOM_CONVEX_SHAPE_TYPE) { + // Skip rayshape in order to implement custom separation process + continue; + } + body_shape_position = p_body_position * kin_shape.transform; body_shape_position_recovered = body_shape_position; body_shape_position_recovered.getOrigin() += r_delta_recover_movement; @@ -1122,7 +1166,6 @@ bool SpaceBullet::RFP_convex_world_test(const btConvexShape *p_shapeA, const btC 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; @@ -1138,3 +1181,79 @@ bool SpaceBullet::RFP_convex_world_test(const btConvexShape *p_shapeA, const btC } return false; } + +int SpaceBullet::recover_from_penetration_ray(RigidBodyBullet *p_body, const btTransform &p_body_position, btScalar p_recover_movement_scale, bool p_infinite_inertia, int p_result_max, btVector3 &r_delta_recover_movement, PhysicsServer::SeparationResult *r_results) { + + RecoverPenetrationBroadPhaseCallback recover_broad_result(p_body->get_bt_collision_object(), p_body->get_collision_layer(), p_body->get_collision_mask()); + + btTransform body_shape_position; + btTransform body_shape_position_recovered; + + // Broad phase support + btVector3 minAabb, maxAabb; + + int ray_index = 0; + + // For each shape + for (int kinIndex = p_body->get_kinematic_utilities()->shapes.size() - 1; 0 <= kinIndex; --kinIndex) { + + recover_broad_result.reset(); + + if (ray_index >= p_result_max) { + break; + } + + const RigidBodyBullet::KinematicShape &kin_shape(p_body->get_kinematic_utilities()->shapes[kinIndex]); + if (!kin_shape.is_active()) { + continue; + } + + if (kin_shape.shape->getShapeType() != CUSTOM_CONVEX_SHAPE_TYPE) { + continue; + } + + body_shape_position = p_body_position * kin_shape.transform; + body_shape_position_recovered = body_shape_position; + body_shape_position_recovered.getOrigin() += r_delta_recover_movement; + + kin_shape.shape->getAabb(body_shape_position_recovered, minAabb, maxAabb); + dynamicsWorld->getBroadphase()->aabbTest(minAabb, maxAabb, recover_broad_result); + + for (int i = recover_broad_result.result_collision_objects.size() - 1; 0 <= i; --i) { + btCollisionObject *otherObject = recover_broad_result.result_collision_objects[i]; + if (p_infinite_inertia && !otherObject->isStaticOrKinematicObject()) { + otherObject->activate(); // Force activation of hitten rigid, soft body + continue; + } else if (!p_body->get_bt_collision_object()->checkCollideWith(otherObject) || !otherObject->checkCollideWith(p_body->get_bt_collision_object())) + continue; + + if (otherObject->getCollisionShape()->isCompound()) { + + // Each convex shape + btCompoundShape *cs = static_cast(otherObject->getCollisionShape()); + for (int x = cs->getNumChildShapes() - 1; 0 <= x; --x) { + + RecoverResult r_recover_result; + if (RFP_convex_world_test(kin_shape.shape, cs->getChildShape(x), p_body->get_bt_collision_object(), otherObject, kinIndex, x, body_shape_position, otherObject->getWorldTransform() * cs->getChildTransform(x), p_recover_movement_scale, r_delta_recover_movement, &r_recover_result)) { + + const btRigidBody *btRigid = static_cast(otherObject); + CollisionObjectBullet *collisionObject = static_cast(otherObject->getUserPointer()); + + r_results[ray_index].collision_depth = r_recover_result.penetration_distance; + B_TO_G(r_recover_result.pointWorld, r_results[ray_index].collision_point); + B_TO_G(r_recover_result.normal, r_results[ray_index].collision_normal); + B_TO_G(btRigid->getVelocityInLocalPoint(r_recover_result.pointWorld - btRigid->getWorldTransform().getOrigin()), r_results[ray_index].collider_velocity); + r_results[ray_index].collision_local_shape = kinIndex; + r_results[ray_index].collider_id = collisionObject->get_instance_id(); + r_results[ray_index].collider = collisionObject->get_self(); + r_results[ray_index].collider_shape = r_recover_result.other_compound_shape_index; + } + } + } + } + + ++ray_index; + } + + return ray_index; +} diff --git a/modules/bullet/space_bullet.h b/modules/bullet/space_bullet.h index 6b86fc2f036..06f1c3f250f 100644 --- a/modules/bullet/space_bullet.h +++ b/modules/bullet/space_bullet.h @@ -175,6 +175,7 @@ public: void update_gravity(); bool test_body_motion(RigidBodyBullet *p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, PhysicsServer::MotionResult *r_result); + 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); private: void create_empty_world(bool p_create_soft_world); @@ -208,5 +209,7 @@ private: /// 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); + + int recover_from_penetration_ray(RigidBodyBullet *p_body, const btTransform &p_body_position, btScalar p_recover_movement_scale, bool p_infinite_inertia, int p_result_max, btVector3 &r_delta_recover_movement, PhysicsServer::SeparationResult *r_results); }; #endif diff --git a/scene/2d/physics_body_2d.cpp b/scene/2d/physics_body_2d.cpp index 8758ffef9f1..02213e07d0f 100644 --- a/scene/2d/physics_body_2d.cpp +++ b/scene/2d/physics_body_2d.cpp @@ -1207,7 +1207,7 @@ bool KinematicBody2D::move_and_collide(const Vector2 &p_motion, bool p_infinite_ //so, if you pass 45 as limit, avoid numerical precision erros when angle is 45. #define FLOOR_ANGLE_THRESHOLD 0.01 -Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const Vector2 &p_floor_direction, bool p_infinite_inertia, float p_slope_stop_min_velocity, int p_max_slides, float p_floor_max_angle) { +Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const Vector2 &p_floor_direction, bool p_infinite_inertia, bool p_stop_on_slope, int p_max_slides, float p_floor_max_angle) { Vector2 floor_motion = floor_velocity; if (on_floor && on_floor_body.is_valid()) { @@ -1228,6 +1228,8 @@ Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const colliders.clear(); floor_velocity = Vector2(); + Vector2 lv_n = p_linear_velocity.normalized(); + while (p_max_slides) { Collision collision; @@ -1254,8 +1256,10 @@ Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const if (collided) { + colliders.push_back(collision); motion = collision.remainder; + bool is_on_slope = false; if (p_floor_direction == Vector2()) { //all is a wall on_wall = true; @@ -1266,15 +1270,17 @@ Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const on_floor_body = collision.collider_rid; floor_velocity = collision.collider_vel; - Vector2 rel_v = lv - floor_velocity; - Vector2 hv = rel_v - p_floor_direction * p_floor_direction.dot(rel_v); - - if (collision.travel.length() < 1 && hv.length() < p_slope_stop_min_velocity) { - Transform2D gt = get_global_transform(); - gt.elements[2] -= collision.travel; - set_global_transform(gt); - return Vector2(); + if (p_stop_on_slope) { + if (Vector2() == lv_n + p_floor_direction) { + Transform2D gt = get_global_transform(); + gt.elements[2] -= collision.travel; + set_global_transform(gt); + return Vector2(); + } } + + is_on_slope = true; + } else if (collision.normal.dot(-p_floor_direction) >= Math::cos(p_floor_max_angle + FLOOR_ANGLE_THRESHOLD)) { //ceiling on_ceiling = true; } else { @@ -1282,12 +1288,18 @@ Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const } } - Vector2 n = collision.normal; - motion = motion.slide(n); - lv = lv.slide(n); - - colliders.push_back(collision); + if (p_stop_on_slope && is_on_slope) { + motion = motion.slide(p_floor_direction); + lv = lv.slide(p_floor_direction); + } else { + Vector2 n = collision.normal; + motion = motion.slide(n); + lv = lv.slide(n); + } } + + if (p_stop_on_slope) + break; } if (!found_collision) { @@ -1301,11 +1313,11 @@ Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const return lv; } -Vector2 KinematicBody2D::move_and_slide_with_snap(const Vector2 &p_linear_velocity, const Vector2 &p_snap, const Vector2 &p_floor_direction, bool p_infinite_inertia, float p_slope_stop_min_velocity, int p_max_slides, float p_floor_max_angle) { +Vector2 KinematicBody2D::move_and_slide_with_snap(const Vector2 &p_linear_velocity, const Vector2 &p_snap, const Vector2 &p_floor_direction, bool p_infinite_inertia, bool p_stop_on_slope, int p_max_slides, float p_floor_max_angle) { bool was_on_floor = on_floor; - Vector2 ret = move_and_slide(p_linear_velocity, p_floor_direction, p_infinite_inertia, p_slope_stop_min_velocity, p_max_slides, p_floor_max_angle); + Vector2 ret = move_and_slide(p_linear_velocity, p_floor_direction, p_infinite_inertia, p_stop_on_slope, p_max_slides, p_floor_max_angle); if (!was_on_floor || p_snap == Vector2()) { return ret; } @@ -1439,8 +1451,8 @@ void KinematicBody2D::_notification(int p_what) { void KinematicBody2D::_bind_methods() { ClassDB::bind_method(D_METHOD("move_and_collide", "rel_vec", "infinite_inertia", "exclude_raycast_shapes", "test_only"), &KinematicBody2D::_move, DEFVAL(true), DEFVAL(true), DEFVAL(false)); - ClassDB::bind_method(D_METHOD("move_and_slide", "linear_velocity", "floor_normal", "infinite_inertia", "slope_stop_min_velocity", "max_bounces", "floor_max_angle"), &KinematicBody2D::move_and_slide, DEFVAL(Vector2(0, 0)), DEFVAL(true), DEFVAL(5), DEFVAL(4), DEFVAL(Math::deg2rad((float)45))); - ClassDB::bind_method(D_METHOD("move_and_slide_with_snap", "linear_velocity", "snap", "floor_normal", "infinite_inertia", "slope_stop_min_velocity", "max_bounces", "floor_max_angle"), &KinematicBody2D::move_and_slide_with_snap, DEFVAL(Vector2(0, 0)), DEFVAL(true), DEFVAL(5), DEFVAL(4), DEFVAL(Math::deg2rad((float)45))); + ClassDB::bind_method(D_METHOD("move_and_slide", "linear_velocity", "floor_normal", "infinite_inertia", "stop_on_slope", "max_bounces", "floor_max_angle"), &KinematicBody2D::move_and_slide, DEFVAL(Vector2(0, 0)), DEFVAL(true), DEFVAL(false), DEFVAL(4), DEFVAL(Math::deg2rad((float)45))); + ClassDB::bind_method(D_METHOD("move_and_slide_with_snap", "linear_velocity", "snap", "floor_normal", "infinite_inertia", "stop_on_slope", "max_bounces", "floor_max_angle"), &KinematicBody2D::move_and_slide_with_snap, DEFVAL(Vector2(0, 0)), DEFVAL(true), DEFVAL(false), DEFVAL(4), DEFVAL(Math::deg2rad((float)45))); ClassDB::bind_method(D_METHOD("test_move", "from", "rel_vec", "infinite_inertia"), &KinematicBody2D::test_move); diff --git a/scene/2d/physics_body_2d.h b/scene/2d/physics_body_2d.h index 0a2ce0918bf..852963a7211 100644 --- a/scene/2d/physics_body_2d.h +++ b/scene/2d/physics_body_2d.h @@ -338,8 +338,8 @@ public: void set_safe_margin(float p_margin); float get_safe_margin() const; - Vector2 move_and_slide(const Vector2 &p_linear_velocity, const Vector2 &p_floor_direction = Vector2(0, 0), bool p_infinite_inertia = true, float p_slope_stop_min_velocity = 5, int p_max_slides = 4, float p_floor_max_angle = Math::deg2rad((float)45)); - Vector2 move_and_slide_with_snap(const Vector2 &p_linear_velocity, const Vector2 &p_snap, const Vector2 &p_floor_direction = Vector2(0, 0), bool p_infinite_inertia = true, float p_slope_stop_min_velocity = 5, int p_max_slides = 4, float p_floor_max_angle = Math::deg2rad((float)45)); + Vector2 move_and_slide(const Vector2 &p_linear_velocity, const Vector2 &p_floor_direction = Vector2(0, 0), bool p_infinite_inertia = true, bool p_stop_on_slope = false, int p_max_slides = 4, float p_floor_max_angle = Math::deg2rad((float)45)); + Vector2 move_and_slide_with_snap(const Vector2 &p_linear_velocity, const Vector2 &p_snap, const Vector2 &p_floor_direction = Vector2(0, 0), bool p_infinite_inertia = true, bool p_stop_on_slope = false, int p_max_slides = 4, float p_floor_max_angle = Math::deg2rad((float)45)); bool is_on_floor() const; bool is_on_wall() const; bool is_on_ceiling() const; diff --git a/scene/3d/physics_body.cpp b/scene/3d/physics_body.cpp index 52e23b0d95a..d56ba5d4039 100644 --- a/scene/3d/physics_body.cpp +++ b/scene/3d/physics_body.cpp @@ -1078,10 +1078,10 @@ void RigidBody::_reload_physics_characteristics() { ////////////////////////////////////////////////////// ////////////////////////// -Ref KinematicBody::_move(const Vector3 &p_motion, bool p_infinite_inertia) { +Ref KinematicBody::_move(const Vector3 &p_motion, bool p_infinite_inertia, bool p_test_only) { Collision col; - if (move_and_collide(p_motion, p_infinite_inertia, col)) { + if (move_and_collide(p_motion, p_infinite_inertia, col, p_test_only)) { if (motion_cache.is_null()) { motion_cache.instance(); motion_cache->owner = this; @@ -1095,7 +1095,7 @@ Ref KinematicBody::_move(const Vector3 &p_motion, bool p_inf return Ref(); } -bool KinematicBody::move_and_collide(const Vector3 &p_motion, bool p_infinite_inertia, Collision &r_collision) { +bool KinematicBody::move_and_collide(const Vector3 &p_motion, bool p_infinite_inertia, Collision &r_collision, bool p_test_only) { Transform gt = get_global_transform(); PhysicsServer::MotionResult result; @@ -1108,6 +1108,7 @@ bool KinematicBody::move_and_collide(const Vector3 &p_motion, bool p_infinite_in r_collision.collision = result.collision_point; r_collision.normal = result.collision_normal; r_collision.collider = result.collider_id; + r_collision.collider_rid = result.collider; r_collision.travel = result.motion; r_collision.remainder = result.remainder; r_collision.local_shape = result.collision_local_shape; @@ -1119,8 +1120,10 @@ bool KinematicBody::move_and_collide(const Vector3 &p_motion, bool p_infinite_in } } - gt.origin += result.motion; - set_global_transform(gt); + if (!p_test_only) { + gt.origin += result.motion; + set_global_transform(gt); + } return colliding; } @@ -1152,72 +1155,121 @@ Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Ve Collision collision; - bool collided = move_and_collide(motion, p_infinite_inertia, collision); + bool found_collision = false; - bool is_on_slope = false; - if (collided) { + int test_type = 0; - colliders.push_back(collision); - motion = collision.remainder; - - if (p_floor_direction == Vector3()) { - //all is a wall - on_wall = true; + do { + bool collided; + if (test_type == 0) { //collide + collided = move_and_collide(motion, p_infinite_inertia, collision); + if (!collided) { + motion = Vector3(); //clear because no collision happened and motion completed + } } else { - if (collision.normal.dot(p_floor_direction) >= Math::cos(p_floor_max_angle + FLOOR_ANGLE_THRESHOLD)) { //floor + collided = separate_raycast_shapes(p_infinite_inertia, collision); + if (collided) { + collision.remainder = motion; //keep + collision.travel = Vector3(); + } + } - on_floor = true; - floor_velocity = collision.collider_vel; + if (collided) { + found_collision = true; + } - if (p_stop_on_slope) { - if (Vector3() == lv_n + p_floor_direction) { - Transform gt = get_global_transform(); - gt.origin -= collision.travel; - set_global_transform(gt); - return Vector3(); - } - } + if (collided) { - is_on_slope = true; + colliders.push_back(collision); + motion = collision.remainder; - } else if (collision.normal.dot(-p_floor_direction) >= Math::cos(p_floor_max_angle + FLOOR_ANGLE_THRESHOLD)) { //ceiling - on_ceiling = true; - } else { + bool is_on_slope = false; + if (p_floor_direction == Vector3()) { + //all is a wall on_wall = true; + } else { + if (collision.normal.dot(p_floor_direction) >= Math::cos(p_floor_max_angle + FLOOR_ANGLE_THRESHOLD)) { //floor + + on_floor = true; + on_floor_body = collision.collider_rid; + floor_velocity = collision.collider_vel; + + if (p_stop_on_slope) { + if (Vector3() == lv_n + p_floor_direction) { + Transform gt = get_global_transform(); + gt.origin -= collision.travel; + set_global_transform(gt); + return Vector3(); + } + } + + is_on_slope = true; + + } else if (collision.normal.dot(-p_floor_direction) >= Math::cos(p_floor_max_angle + FLOOR_ANGLE_THRESHOLD)) { //ceiling + on_ceiling = true; + } else { + on_wall = true; + } + } + + if (p_stop_on_slope && is_on_slope) { + motion = motion.slide(p_floor_direction); + lv = lv.slide(p_floor_direction); + } else { + Vector3 n = collision.normal; + motion = motion.slide(n); + lv = lv.slide(n); + } + + for (int i = 0; i < 3; i++) { + if (locked_axis & (1 << i)) { + lv[i] = 0; + } } } - if (p_stop_on_slope && is_on_slope) { - motion = motion.slide(p_floor_direction); - lv = lv.slide(p_floor_direction); - } else { - Vector3 n = collision.normal; - motion = motion.slide(n); - lv = lv.slide(n); - } + ++test_type; + } while (!p_stop_on_slope && test_type < 2); - for (int i = 0; i < 3; i++) { - if (locked_axis & (1 << i)) { - lv[i] = 0; - } - } - - } else { + if (!found_collision || motion == Vector3()) break; - } - p_max_slides--; - if (motion == Vector3()) - break; + --p_max_slides; } return lv; } +Vector3 KinematicBody::move_and_slide_with_snap(const Vector3 &p_linear_velocity, const Vector3 &p_snap, const Vector3 &p_floor_direction, bool p_infinite_inertia, bool p_stop_on_slope, int p_max_slides, float p_floor_max_angle) { + + bool was_on_floor = on_floor; + + Vector3 ret = move_and_slide(p_linear_velocity, p_floor_direction, p_stop_on_slope, p_max_slides, p_floor_max_angle, p_infinite_inertia); + if (!was_on_floor || p_snap == Vector3()) { + return ret; + } + + Collision col; + Transform gt = get_global_transform(); + + if (move_and_collide(p_snap, p_infinite_inertia, col, true)) { + gt.origin += col.travel; + if (p_floor_direction != Vector3() && Math::acos(p_floor_direction.normalized().dot(col.normal)) < p_floor_max_angle) { + on_floor = true; + on_floor_body = col.collider_rid; + floor_velocity = col.collider_vel; + } + set_global_transform(gt); + } + + return ret; +} + bool KinematicBody::is_on_floor() const { return on_floor; } + bool KinematicBody::is_on_wall() const { return on_wall; @@ -1239,6 +1291,43 @@ bool KinematicBody::test_move(const Transform &p_from, const Vector3 &p_motion, return PhysicsServer::get_singleton()->body_test_motion(get_rid(), p_from, p_motion, p_infinite_inertia); } +bool KinematicBody::separate_raycast_shapes(bool p_infinite_inertia, Collision &r_collision) { + + PhysicsServer::SeparationResult sep_res[8]; //max 8 rays + + Transform gt = get_global_transform(); + + 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; + for (int i = 0; i < hits; i++) { + if (deepest == -1 || sep_res[i].collision_depth > deepest_depth) { + deepest = i; + deepest_depth = sep_res[i].collision_depth; + } + } + + gt.origin += recover; + set_global_transform(gt); + + if (deepest != -1) { + r_collision.collider = sep_res[deepest].collider_id; + r_collision.collider_metadata = sep_res[deepest].collider_metadata; + r_collision.collider_shape = sep_res[deepest].collider_shape; + r_collision.collider_vel = sep_res[deepest].collider_velocity; + r_collision.collision = sep_res[deepest].collision_point; + r_collision.normal = sep_res[deepest].collision_normal; + r_collision.local_shape = sep_res[deepest].collision_local_shape; + r_collision.travel = recover; + r_collision.remainder = Vector3(); + + return true; + } else { + return false; + } +} + void KinematicBody::set_axis_lock(PhysicsServer::BodyAxis p_axis, bool p_lock) { PhysicsServer::get_singleton()->body_set_axis_lock(get_rid(), p_axis, p_lock); } @@ -1285,8 +1374,9 @@ Ref KinematicBody::_get_slide_collision(int p_bounce) { void KinematicBody::_bind_methods() { - ClassDB::bind_method(D_METHOD("move_and_collide", "rel_vec", "infinite_inertia"), &KinematicBody::_move, DEFVAL(true)); - ClassDB::bind_method(D_METHOD("move_and_slide", "linear_velocity", "floor_normal", "max_slides", "floor_max_angle", "infinite_inertia"), &KinematicBody::move_and_slide, DEFVAL(Vector3(0, 0, 0)), DEFVAL(4), DEFVAL(Math::deg2rad((float)45)), DEFVAL(true)); + ClassDB::bind_method(D_METHOD("move_and_collide", "rel_vec", "infinite_inertia", "test_only"), &KinematicBody::_move, DEFVAL(true), DEFVAL(false)); + ClassDB::bind_method(D_METHOD("move_and_slide", "linear_velocity", "floor_normal", "stop_on_slope", "max_slides", "floor_max_angle", "infinite_inertia"), &KinematicBody::move_and_slide, DEFVAL(Vector3(0, 0, 0)), DEFVAL(false), DEFVAL(4), DEFVAL(Math::deg2rad((float)45)), DEFVAL(true)); + ClassDB::bind_method(D_METHOD("move_and_slide_with_snap", "linear_velocity", "snap", "floor_normal", "infinite_inertia", "stop_on_slope", "max_bounces", "floor_max_angle"), &KinematicBody::move_and_slide_with_snap, DEFVAL(Vector3(0, 0, 0)), DEFVAL(true), DEFVAL(false), DEFVAL(4), DEFVAL(Math::deg2rad((float)45))); ClassDB::bind_method(D_METHOD("test_move", "from", "rel_vec", "infinite_inertia"), &KinematicBody::test_move); diff --git a/scene/3d/physics_body.h b/scene/3d/physics_body.h index a39616893d8..c4db41f5776 100644 --- a/scene/3d/physics_body.h +++ b/scene/3d/physics_body.h @@ -285,6 +285,7 @@ public: Vector3 normal; Vector3 collider_vel; ObjectID collider; + RID collider_rid; int collider_shape; Variant collider_metadata; Vector3 remainder; @@ -298,6 +299,7 @@ private: float margin; Vector3 floor_velocity; + RID on_floor_body; bool on_floor; bool on_ceiling; bool on_wall; @@ -307,16 +309,18 @@ private: _FORCE_INLINE_ bool _ignores_mode(PhysicsServer::BodyMode) const; - Ref _move(const Vector3 &p_motion, bool p_infinite_inertia = true); + Ref _move(const Vector3 &p_motion, bool p_infinite_inertia = true, bool p_test_only = false); Ref _get_slide_collision(int p_bounce); protected: static void _bind_methods(); public: - bool move_and_collide(const Vector3 &p_motion, bool p_infinite_inertia, Collision &r_collisionz); + bool move_and_collide(const Vector3 &p_motion, bool p_infinite_inertia, Collision &r_collisionz, bool p_test_only = false); bool test_move(const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia); + bool separate_raycast_shapes(bool p_infinite_inertia, Collision &r_collision); + void set_axis_lock(PhysicsServer::BodyAxis p_axis, bool p_lock); bool get_axis_lock(PhysicsServer::BodyAxis p_axis) const; @@ -324,6 +328,7 @@ public: float get_safe_margin() const; Vector3 move_and_slide(const Vector3 &p_linear_velocity, const Vector3 &p_floor_direction = Vector3(0, 0, 0), bool p_stop_on_slope = false, int p_max_slides = 4, float p_floor_max_angle = Math::deg2rad((float)45), bool p_infinite_inertia = true); + Vector3 move_and_slide_with_snap(const Vector3 &p_linear_velocity, const Vector3 &p_snap, const Vector3 &p_floor_direction = Vector3(0, 0, 0), bool p_infinite_inertia = true, bool p_stop_on_slope = false, int p_max_slides = 4, float p_floor_max_angle = Math::deg2rad((float)45)); bool is_on_floor() const; bool is_on_wall() const; bool is_on_ceiling() const; diff --git a/servers/physics/physics_server_sw.h b/servers/physics/physics_server_sw.h index 1c5754124d9..2b3d2ae91f4 100644 --- a/servers/physics/physics_server_sw.h +++ b/servers/physics/physics_server_sw.h @@ -231,6 +231,7 @@ public: virtual bool body_is_ray_pickable(RID p_body) const; virtual bool body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, MotionResult *r_result = NULL); + virtual int body_test_ray_separation(RID p_body, const Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, SeparationResult *r_results, int p_result_max, float p_margin = 0.001) { return 0; } // this function only works on physics process, errors and returns null otherwise virtual PhysicsDirectBodyState *body_get_direct_state(RID p_body); diff --git a/servers/physics_server.h b/servers/physics_server.h index 948aec1a2db..982fcd68e32 100644 --- a/servers/physics_server.h +++ b/servers/physics_server.h @@ -484,6 +484,21 @@ public: virtual bool body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, MotionResult *r_result = NULL) = 0; + struct SeparationResult { + + float collision_depth; + Vector3 collision_point; + Vector3 collision_normal; + Vector3 collider_velocity; + int collision_local_shape; + ObjectID collider_id; + RID collider; + int collider_shape; + Variant collider_metadata; + }; + + virtual int body_test_ray_separation(RID p_body, const Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, SeparationResult *r_results, int p_result_max, float p_margin = 0.001) = 0; + /* SOFT BODY */ virtual RID soft_body_create(bool p_init_sleeping = false) = 0;