From d88d0d457d2828db2016f4854efff8c5f8b90e91 Mon Sep 17 00:00:00 2001 From: Juan Linietsky Date: Mon, 20 Aug 2018 17:31:55 -0300 Subject: [PATCH] Fixes to move and slide and ray separation, implement separation in Godot physics --- modules/bullet/bullet_physics_server.cpp | 4 +- modules/bullet/bullet_physics_server.h | 2 +- modules/bullet/space_bullet.cpp | 4 +- modules/bullet/space_bullet.h | 2 +- servers/physics/physics_server_sw.cpp | 16 ++- servers/physics/physics_server_sw.h | 4 +- servers/physics/space_sw.cpp | 147 ++++++++++++++++++++++- servers/physics/space_sw.h | 3 +- servers/physics_server.h | 2 +- 9 files changed, 171 insertions(+), 13 deletions(-) diff --git a/modules/bullet/bullet_physics_server.cpp b/modules/bullet/bullet_physics_server.cpp index 032654c3f7f..01398424700 100644 --- a/modules/bullet/bullet_physics_server.cpp +++ b/modules/bullet/bullet_physics_server.cpp @@ -861,12 +861,12 @@ PhysicsDirectBodyState *BulletPhysicsServer::body_get_direct_state(RID p_body) { return BulletPhysicsDirectBodyState::get_singleton(body); } -bool BulletPhysicsServer::body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, MotionResult *r_result) { +bool BulletPhysicsServer::body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, MotionResult *r_result, bool p_exclude_raycast_shapes) { RigidBodyBullet *body = rigid_body_owner.get(p_body); ERR_FAIL_COND_V(!body, false); ERR_FAIL_COND_V(!body->get_space(), false); - return body->get_space()->test_body_motion(body, p_from, p_motion, p_infinite_inertia, r_result); + return body->get_space()->test_body_motion(body, p_from, p_motion, p_infinite_inertia, r_result, p_exclude_raycast_shapes); } 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) { diff --git a/modules/bullet/bullet_physics_server.h b/modules/bullet/bullet_physics_server.h index 2ff83bfd7d7..76716f1d1b0 100644 --- a/modules/bullet/bullet_physics_server.h +++ b/modules/bullet/bullet_physics_server.h @@ -258,7 +258,7 @@ public: // this function only works on physics process, errors and returns null otherwise 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 bool body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, MotionResult *r_result = NULL, bool p_exclude_raycast_shapes = true); 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/space_bullet.cpp b/modules/bullet/space_bullet.cpp index c0fea910c28..e1a4e88db09 100644 --- a/modules/bullet/space_bullet.cpp +++ b/modules/bullet/space_bullet.cpp @@ -819,7 +819,7 @@ 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 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. @@ -895,7 +895,7 @@ bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_f continue; } - if (p_body->get_bt_shape(shIndex)->getShapeType() == CUSTOM_CONVEX_SHAPE_TYPE) { + 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 continue; } diff --git a/modules/bullet/space_bullet.h b/modules/bullet/space_bullet.h index 06f1c3f250f..95814d1fef7 100644 --- a/modules/bullet/space_bullet.h +++ b/modules/bullet/space_bullet.h @@ -174,7 +174,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); + 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); private: diff --git a/servers/physics/physics_server_sw.cpp b/servers/physics/physics_server_sw.cpp index 3a32c46a9bc..61b014713f0 100644 --- a/servers/physics/physics_server_sw.cpp +++ b/servers/physics/physics_server_sw.cpp @@ -941,7 +941,7 @@ bool PhysicsServerSW::body_is_ray_pickable(RID p_body) const { return body->is_ray_pickable(); } -bool PhysicsServerSW::body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, MotionResult *r_result) { +bool PhysicsServerSW::body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, MotionResult *r_result, bool p_exclude_raycast_shapes) { BodySW *body = body_owner.get(p_body); ERR_FAIL_COND_V(!body, false); @@ -950,7 +950,19 @@ bool PhysicsServerSW::body_test_motion(RID p_body, const Transform &p_from, cons _update_shapes(); - return body->get_space()->test_body_motion(body, p_from, p_motion, p_infinite_inertia, body->get_kinematic_margin(), r_result); + return body->get_space()->test_body_motion(body, p_from, p_motion, p_infinite_inertia, body->get_kinematic_margin(), r_result, p_exclude_raycast_shapes); +} + +int PhysicsServerSW::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) { + + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND_V(!body, false); + ERR_FAIL_COND_V(!body->get_space(), false); + ERR_FAIL_COND_V(body->get_space()->is_locked(), false); + + _update_shapes(); + + return body->get_space()->test_body_ray_separation(body, p_transform, p_infinite_inertia, r_recover_motion, r_results, p_result_max, p_margin); } PhysicsDirectBodyState *PhysicsServerSW::body_get_direct_state(RID p_body) { diff --git a/servers/physics/physics_server_sw.h b/servers/physics/physics_server_sw.h index 2b3d2ae91f4..ad7aa4aa287 100644 --- a/servers/physics/physics_server_sw.h +++ b/servers/physics/physics_server_sw.h @@ -230,8 +230,8 @@ public: virtual void body_set_ray_pickable(RID p_body, bool p_enable); 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; } + virtual bool body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, MotionResult *r_result = NULL, bool p_exclude_raycast_shapes = true); + 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); // 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/space_sw.cpp b/servers/physics/space_sw.cpp index b604e5cdf6e..9d6ec958b4d 100644 --- a/servers/physics/space_sw.cpp +++ b/servers/physics/space_sw.cpp @@ -541,7 +541,144 @@ int SpaceSW::_cull_aabb_for_body(BodySW *p_body, const AABB &p_aabb) { return amount; } -bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, real_t p_margin, PhysicsServer::MotionResult *r_result) { +int SpaceSW::test_body_ray_separation(BodySW *p_body, const Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, PhysicsServer::SeparationResult *r_results, int p_result_max, real_t p_margin) { + + AABB body_aabb; + + for (int i = 0; i < p_body->get_shape_count(); i++) { + + if (i == 0) + body_aabb = p_body->get_shape_aabb(i); + else + body_aabb = body_aabb.merge(p_body->get_shape_aabb(i)); + } + + // Undo the currently transform the physics server is aware of and apply the provided one + body_aabb = p_transform.xform(p_body->get_inv_transform().xform(body_aabb)); + body_aabb = body_aabb.grow(p_margin); + + Transform body_transform = p_transform; + + for (int i = 0; i < p_result_max; i++) { + //reset results + r_results[i].collision_depth = 0; + } + + int rays_found = 0; + + { + // 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; + + do { + + Vector3 recover_motion; + + bool collided = false; + + int amount = _cull_aabb_for_body(p_body, body_aabb); + int ray_index = 0; + + for (int j = 0; j < p_body->get_shape_count(); j++) { + if (p_body->is_shape_set_as_disabled(j)) + continue; + + ShapeSW *body_shape = p_body->get_shape(j); + + if (body_shape->get_type() != PhysicsServer::SHAPE_RAY) + continue; + + Transform body_shape_xform = body_transform * p_body->get_shape_transform(j); + + for (int i = 0; i < amount; i++) { + + const CollisionObjectSW *col_obj = intersection_query_results[i]; + int shape_idx = intersection_query_subindex_results[i]; + + cbk.amount = 0; + cbk.ptr = sr; + + 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; + } + } + + 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; + } + + if (ray_index < p_result_max) { + 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) * 0.4; + + 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 = 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() == 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); + } + } + } + } + } + } + + ray_index++; + } + + rays_found = MAX(ray_index, rays_found); + + 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) + 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]); + } + } + + r_recover_motion = body_transform.origin - p_transform.origin; + return rays_found; +} + +bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, real_t p_margin, PhysicsServer::MotionResult *r_result, bool p_exclude_raycast_shapes) { //give me back regular physics engine logic //this is madness @@ -597,6 +734,10 @@ bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Ve 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++) { const CollisionObjectSW *col_obj = intersection_query_results[i]; @@ -655,6 +796,10 @@ bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Ve 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_inv = body_shape_xform.affine_inverse(); MotionShapeSW mshape; mshape.shape = body_shape; diff --git a/servers/physics/space_sw.h b/servers/physics/space_sw.h index 4d864e9a510..ed4b274d133 100644 --- a/servers/physics/space_sw.h +++ b/servers/physics/space_sw.h @@ -197,7 +197,8 @@ public: void set_elapsed_time(ElapsedTime p_time, uint64_t p_msec) { elapsed_time[p_time] = p_msec; } uint64_t get_elapsed_time(ElapsedTime p_time) const { return elapsed_time[p_time]; } - bool test_body_motion(BodySW *p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, real_t p_margin, PhysicsServer::MotionResult *r_result); + int test_body_ray_separation(BodySW *p_body, const Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, PhysicsServer::SeparationResult *r_results, int p_result_max, real_t p_margin); + bool test_body_motion(BodySW *p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, real_t p_margin, PhysicsServer::MotionResult *r_result, bool p_exclude_raycast_shapes); SpaceSW(); ~SpaceSW(); diff --git a/servers/physics_server.h b/servers/physics_server.h index 982fcd68e32..4ced9151797 100644 --- a/servers/physics_server.h +++ b/servers/physics_server.h @@ -482,7 +482,7 @@ public: Variant collider_metadata; }; - 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; + virtual bool body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, MotionResult *r_result = NULL, bool p_exclude_raycast_shapes = true) = 0; struct SeparationResult {