From 063a22851a4cc1568623363140e16bbaecfd7ec9 Mon Sep 17 00:00:00 2001 From: Juan Linietsky Date: Mon, 16 Jul 2018 20:04:07 -0300 Subject: [PATCH] -Added support for raycast in KinematicBody2D -Added support for snapping in KinematicBody2D --- scene/2d/physics_body_2d.cpp | 158 ++++++++++++----- scene/2d/physics_body_2d.h | 9 +- scene/3d/physics_body.h | 2 +- servers/physics_2d/collision_solver_2d_sw.cpp | 11 +- servers/physics_2d/collision_solver_2d_sw.h | 2 +- servers/physics_2d/physics_2d_server_sw.cpp | 14 +- servers/physics_2d/physics_2d_server_sw.h | 3 +- .../physics_2d/physics_2d_server_wrap_mt.h | 10 +- servers/physics_2d/space_2d_sw.cpp | 163 +++++++++++++++++- servers/physics_2d/space_2d_sw.h | 3 +- servers/physics_2d_server.h | 17 +- 11 files changed, 331 insertions(+), 61 deletions(-) diff --git a/scene/2d/physics_body_2d.cpp b/scene/2d/physics_body_2d.cpp index feb11089d0a..d8549af66ab 100644 --- a/scene/2d/physics_body_2d.cpp +++ b/scene/2d/physics_body_2d.cpp @@ -971,11 +971,11 @@ RigidBody2D::~RigidBody2D() { ////////////////////////// -Ref KinematicBody2D::_move(const Vector2 &p_motion, bool p_infinite_inertia) { +Ref KinematicBody2D::_move(const Vector2 &p_motion, bool p_infinite_inertia, bool p_exclude_raycast_shapes, bool p_only_move_if_collided, float p_custom_margin) { Collision col; - if (move_and_collide(p_motion, p_infinite_inertia, col)) { + if (move_and_collide(p_motion, p_infinite_inertia, col, p_exclude_raycast_shapes, p_only_move_if_collided, p_custom_margin)) { if (motion_cache.is_null()) { motion_cache.instance(); motion_cache->owner = this; @@ -989,11 +989,48 @@ Ref KinematicBody2D::_move(const Vector2 &p_motion, bool p return Ref(); } -bool KinematicBody2D::move_and_collide(const Vector2 &p_motion, bool p_infinite_inertia, Collision &r_collision) { +bool KinematicBody2D::separate_raycast_shapes(bool p_infinite_inertia, Collision &r_collision) { + + Physics2DServer::SeparationResult sep_res[8]; //max 8 rays + + Transform2D gt = get_global_transform(); + + Vector2 recover; + int hits = Physics2DServer::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.elements[2] += 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 = Vector2(); + + return true; + } else { + return false; + } +} + +bool KinematicBody2D::move_and_collide(const Vector2 &p_motion, bool p_infinite_inertia, Collision &r_collision, bool p_exclude_raycast_shapes, bool p_only_move_if_collided, float p_custom_margin) { Transform2D gt = get_global_transform(); Physics2DServer::MotionResult result; - bool colliding = Physics2DServer::get_singleton()->body_test_motion(get_rid(), gt, p_motion, p_infinite_inertia, margin, &result); + bool colliding = Physics2DServer::get_singleton()->body_test_motion(get_rid(), gt, p_motion, p_infinite_inertia, p_custom_margin >= 0 ? p_custom_margin : margin, &result, p_exclude_raycast_shapes); if (colliding) { r_collision.collider_metadata = result.collider_metadata; @@ -1007,13 +1044,15 @@ bool KinematicBody2D::move_and_collide(const Vector2 &p_motion, bool p_infinite_ r_collision.local_shape = result.collision_local_shape; } - gt.elements[2] += result.motion; - set_global_transform(gt); + if (!p_only_move_if_collided || colliding) { + gt.elements[2] += result.motion; + set_global_transform(gt); + } return colliding; } -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, float p_slope_stop_min_velocity, int p_max_slides, float p_floor_max_angle, const Vector2 &p_snap) { Vector2 motion = (floor_velocity + p_linear_velocity) * get_physics_process_delta_time(); Vector2 lv = p_linear_velocity; @@ -1027,53 +1066,85 @@ Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const while (p_max_slides) { Collision collision; + bool found_collision = false; - bool collided = move_and_collide(motion, p_infinite_inertia, collision); - - if (collided) { - - motion = collision.remainder; - - if (p_floor_direction == Vector2()) { - //all is a wall - on_wall = true; - } else { - if (collision.normal.dot(p_floor_direction) >= Math::cos(p_floor_max_angle)) { //floor - - on_floor = true; - 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(); - } - } else if (collision.normal.dot(-p_floor_direction) >= Math::cos(p_floor_max_angle)) { //ceiling - on_ceiling = true; - } else { - on_wall = true; + for (int i = 0; i < 2; i++) { + bool collided; + if (i == 0) { //collide + collided = move_and_collide(motion, p_infinite_inertia, collision); + if (!collided) { + motion = Vector2(); //clear because no collision happened and motion completed + } + } else { //separate raycasts (if any) + collided = separate_raycast_shapes(p_infinite_inertia, collision); + if (collided) { + collision.remainder = motion; //keep + collision.travel = Vector2(); } } - Vector2 n = collision.normal; - motion = motion.slide(n); - lv = lv.slide(n); + if (collided) { + found_collision = true; + } - colliders.push_back(collision); + if (collided) { - } else { - break; + motion = collision.remainder; + + if (p_floor_direction == Vector2()) { + //all is a wall + on_wall = true; + } else { + if (collision.normal.dot(p_floor_direction) >= Math::cos(p_floor_max_angle)) { //floor + + on_floor = true; + 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(); + } + } else if (collision.normal.dot(-p_floor_direction) >= Math::cos(p_floor_max_angle)) { //ceiling + on_ceiling = true; + } else { + on_wall = true; + } + } + + Vector2 n = collision.normal; + motion = motion.slide(n); + lv = lv.slide(n); + + colliders.push_back(collision); + } } + if (!found_collision) { + break; + } p_max_slides--; if (motion == Vector2()) break; } + if (p_snap != Vector2() && (on_floor || snapped)) { + + Collision collision; + if (move_and_collide(p_snap, p_infinite_inertia, collision, false, true, 0)) { + snapped = true; + } else { + snapped = false; + } + + } else { + snapped = false; + } + return lv; } @@ -1140,8 +1211,8 @@ Ref KinematicBody2D::_get_slide_collision(int p_bounce) { void KinematicBody2D::_bind_methods() { - ClassDB::bind_method(D_METHOD("move_and_collide", "rel_vec", "infinite_inertia"), &KinematicBody2D::_move, DEFVAL(true)); - 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_collide", "rel_vec", "infinite_inertia", "exclude_raycast_shapes", "only_move_if_collided", "custom_margin"), &KinematicBody2D::_move, DEFVAL(true), DEFVAL(true), DEFVAL(false), DEFVAL(-1.0)); + ClassDB::bind_method(D_METHOD("move_and_slide", "linear_velocity", "floor_normal", "infinite_inertia", "slope_stop_min_velocity", "max_bounces", "floor_max_angle", "snap"), &KinematicBody2D::move_and_slide, DEFVAL(Vector2(0, 0)), DEFVAL(true), DEFVAL(5), DEFVAL(4), DEFVAL(Math::deg2rad((float)45)), DEFVAL(Vector2())); ClassDB::bind_method(D_METHOD("test_move", "from", "rel_vec", "infinite_inertia"), &KinematicBody2D::test_move); @@ -1167,6 +1238,7 @@ KinematicBody2D::KinematicBody2D() : on_floor = false; on_ceiling = false; on_wall = false; + snapped = false; } KinematicBody2D::~KinematicBody2D() { if (motion_cache.is_valid()) { diff --git a/scene/2d/physics_body_2d.h b/scene/2d/physics_body_2d.h index 0fda3c5c05b..5ab07cc7542 100644 --- a/scene/2d/physics_body_2d.h +++ b/scene/2d/physics_body_2d.h @@ -290,26 +290,29 @@ private: bool on_floor; bool on_ceiling; bool on_wall; + bool snapped; Vector colliders; Vector > slide_colliders; Ref motion_cache; _FORCE_INLINE_ bool _ignores_mode(Physics2DServer::BodyMode) const; - Ref _move(const Vector2 &p_motion, bool p_infinite_inertia = true); + Ref _move(const Vector2 &p_motion, bool p_infinite_inertia = true, bool p_exclude_raycast_shapes = true, bool p_only_move_if_collided = false, float p_custom_margin = -1); Ref _get_slide_collision(int p_bounce); protected: static void _bind_methods(); public: - bool move_and_collide(const Vector2 &p_motion, bool p_infinite_inertia, Collision &r_collision); + bool move_and_collide(const Vector2 &p_motion, bool p_infinite_inertia, Collision &r_collision, bool p_exclude_raycast_shapes = true, bool p_only_move_if_collided = false, float p_custom_margin = -1); bool test_move(const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia); + bool separate_raycast_shapes(bool p_infinite_inertia, Collision &r_collision); + 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(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), const Vector2 &p_snap = Vector2()); bool is_on_floor() const; bool is_on_wall() const; bool is_on_ceiling() const; diff --git a/scene/3d/physics_body.h b/scene/3d/physics_body.h index 0190dcbfc3e..7236eba685d 100644 --- a/scene/3d/physics_body.h +++ b/scene/3d/physics_body.h @@ -294,7 +294,7 @@ protected: static void _bind_methods(); public: - bool move_and_collide(const Vector3 &p_motion, bool p_infinite_inertia, Collision &r_collision); + bool move_and_collide(const Vector3 &p_motion, bool p_infinite_inertia, Collision &r_collisionz); bool test_move(const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia); void set_axis_lock(PhysicsServer::BodyAxis p_axis, bool p_lock); diff --git a/servers/physics_2d/collision_solver_2d_sw.cpp b/servers/physics_2d/collision_solver_2d_sw.cpp index efee98a35aa..6ce019f36ef 100644 --- a/servers/physics_2d/collision_solver_2d_sw.cpp +++ b/servers/physics_2d/collision_solver_2d_sw.cpp @@ -72,7 +72,7 @@ bool CollisionSolver2DSW::solve_static_line(const Shape2DSW *p_shape_A, const Tr return found; } -bool CollisionSolver2DSW::solve_raycast(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *sep_axis) { +bool CollisionSolver2DSW::solve_raycast(const Shape2DSW *p_shape_A, const Vector2 &p_motion_A, const Transform2D &p_transform_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *sep_axis) { const RayShape2DSW *ray = static_cast(p_shape_A); if (p_shape_B->get_type() == Physics2DServer::SHAPE_RAY) @@ -80,6 +80,11 @@ bool CollisionSolver2DSW::solve_raycast(const Shape2DSW *p_shape_A, const Transf Vector2 from = p_transform_A.get_origin(); Vector2 to = from + p_transform_A[1] * ray->get_length(); + if (p_motion_A != Vector2()) { + //not the best but should be enough + Vector2 normal = (to - from).normalized(); + to += normal * MAX(0.0, normal.dot(p_motion_A)); + } Vector2 support_A = to; Transform2D invb = p_transform_B.affine_inverse(); @@ -270,9 +275,9 @@ bool CollisionSolver2DSW::solve(const Shape2DSW *p_shape_A, const Transform2D &p } if (swap) { - return solve_raycast(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true, sep_axis); + return solve_raycast(p_shape_B, p_motion_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true, sep_axis); } else { - return solve_raycast(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false, sep_axis); + return solve_raycast(p_shape_A, p_motion_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false, sep_axis); } } else if (concave_B) { diff --git a/servers/physics_2d/collision_solver_2d_sw.h b/servers/physics_2d/collision_solver_2d_sw.h index e39c41fb752..6faa1661156 100644 --- a/servers/physics_2d/collision_solver_2d_sw.h +++ b/servers/physics_2d/collision_solver_2d_sw.h @@ -41,7 +41,7 @@ private: static bool solve_static_line(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); static void concave_callback(void *p_userdata, Shape2DSW *p_convex); static bool solve_concave(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *sep_axis = NULL, real_t p_margin_A = 0, real_t p_margin_B = 0); - static bool solve_raycast(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *sep_axis = NULL); + static bool solve_raycast(const Shape2DSW *p_shape_A, const Vector2 &p_motion_A, const Transform2D &p_transform_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *sep_axis = NULL); public: static bool solve(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, Vector2 *sep_axis = NULL, real_t p_margin_A = 0, real_t p_margin_B = 0); diff --git a/servers/physics_2d/physics_2d_server_sw.cpp b/servers/physics_2d/physics_2d_server_sw.cpp index a14fed81849..faabed84e97 100644 --- a/servers/physics_2d/physics_2d_server_sw.cpp +++ b/servers/physics_2d/physics_2d_server_sw.cpp @@ -962,14 +962,24 @@ void Physics2DServerSW::body_set_pickable(RID p_body, bool p_pickable) { body->set_pickable(p_pickable); } -bool Physics2DServerSW::body_test_motion(RID p_body, const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia, real_t p_margin, MotionResult *r_result) { +bool Physics2DServerSW::body_test_motion(RID p_body, const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia, real_t p_margin, MotionResult *r_result, bool p_exclude_raycast_shapes) { Body2DSW *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); - return body->get_space()->test_body_motion(body, p_from, p_motion, p_infinite_inertia, p_margin, r_result); + return body->get_space()->test_body_motion(body, p_from, p_motion, p_infinite_inertia, p_margin, r_result, p_exclude_raycast_shapes); +} + +int Physics2DServerSW::body_test_ray_separation(RID p_body, const Transform2D &p_transform, bool p_infinite_inertia, Vector2 &r_recover_motion, SeparationResult *r_results, int p_result_max, float p_margin) { + + Body2DSW *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); + + return body->get_space()->test_body_ray_separation(body, p_transform, p_infinite_inertia, r_recover_motion, r_results, p_result_max, p_margin); } Physics2DDirectBodyState *Physics2DServerSW::body_get_direct_state(RID p_body) { diff --git a/servers/physics_2d/physics_2d_server_sw.h b/servers/physics_2d/physics_2d_server_sw.h index 036eb934e16..e5961b9011f 100644 --- a/servers/physics_2d/physics_2d_server_sw.h +++ b/servers/physics_2d/physics_2d_server_sw.h @@ -232,7 +232,8 @@ public: virtual void body_set_pickable(RID p_body, bool p_pickable); - virtual bool body_test_motion(RID p_body, const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia, real_t p_margin = 0.001, MotionResult *r_result = NULL); + virtual bool body_test_motion(RID p_body, const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia, real_t p_margin = 0.001, MotionResult *r_result = NULL, bool p_exclude_raycast_shapes = true); + virtual int body_test_ray_separation(RID p_body, const Transform2D &p_transform, bool p_infinite_inertia, Vector2 &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 Physics2DDirectBodyState *body_get_direct_state(RID p_body); diff --git a/servers/physics_2d/physics_2d_server_wrap_mt.h b/servers/physics_2d/physics_2d_server_wrap_mt.h index a15e8bde8bb..3119b930d77 100644 --- a/servers/physics_2d/physics_2d_server_wrap_mt.h +++ b/servers/physics_2d/physics_2d_server_wrap_mt.h @@ -245,10 +245,16 @@ public: FUNC2(body_set_pickable, RID, bool); - bool body_test_motion(RID p_body, const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia, real_t p_margin = 0.001, MotionResult *r_result = NULL) { + bool body_test_motion(RID p_body, const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia, real_t p_margin = 0.001, MotionResult *r_result = NULL, bool p_exclude_raycast_shapes = true) { ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), false); - return physics_2d_server->body_test_motion(p_body, p_from, p_motion, p_infinite_inertia, p_margin, r_result); + return physics_2d_server->body_test_motion(p_body, p_from, p_motion, p_infinite_inertia, p_margin, r_result, p_exclude_raycast_shapes); + } + + int body_test_ray_separation(RID p_body, const Transform2D &p_transform, bool p_infinite_inertia, Vector2 &r_recover_motion, SeparationResult *r_results, int p_result_max, float p_margin = 0.001) { + + ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), false); + return physics_2d_server->body_test_ray_separation(p_body, p_transform, p_infinite_inertia, r_recover_motion, r_results, p_result_max, p_margin); } // this function only works on physics process, errors and returns null otherwise diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp index 0e1f74d8d03..6e45951f42f 100644 --- a/servers/physics_2d/space_2d_sw.cpp +++ b/servers/physics_2d/space_2d_sw.cpp @@ -487,7 +487,156 @@ int Space2DSW::_cull_aabb_for_body(Body2DSW *p_body, const Rect2 &p_aabb) { return amount; } -bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia, real_t p_margin, Physics2DServer::MotionResult *r_result) { +int Space2DSW::test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_transform, bool p_infinite_inertia, Vector2 &r_recover_motion, Physics2DServer::SeparationResult *r_results, int p_result_max, real_t p_margin) { + + Rect2 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); + + Transform2D 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; + Vector2 sr[max_results * 2]; + Physics2DServerSW::CollCbkData cbk; + cbk.max = max_results; + Physics2DServerSW::CollCbkData *cbkptr = &cbk; + CollisionSolver2DSW::CallbackResult cbkres = Physics2DServerSW::_shape_col_cbk; + + do { + + Vector2 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; + + Shape2DSW *body_shape = p_body->get_shape(j); + + if (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++) { + + const CollisionObject2DSW *col_obj = intersection_query_results[i]; + int shape_idx = intersection_query_subindex_results[i]; + + cbk.amount = 0; + cbk.ptr = sr; + cbk.invalid_by_dir = 0; + + if (CollisionObject2DSW::TYPE_BODY == col_obj->get_type()) { + const Body2DSW *b = static_cast(col_obj); + if (p_infinite_inertia && Physics2DServer::BODY_MODE_STATIC != b->get_mode() && Physics2DServer::BODY_MODE_KINEMATIC != b->get_mode()) { + continue; + } + } + + if (col_obj->is_shape_set_as_one_way_collision(shape_idx)) { + + cbk.valid_dir = body_shape_xform.get_axis(1).normalized(); + cbk.valid_depth = p_margin; //only valid depth is the collision margin + 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->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), cbkres, cbkptr, NULL, p_margin)) { + if (cbk.amount > 0) { + collided = true; + } + + if (ray_index < p_result_max) { + 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]; + + 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() == 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(); + } + } + } + } + } + } + + ray_index++; + } + + rays_found = MAX(ray_index, rays_found); + + 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) + 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.elements[2] - p_transform.elements[2]; + return rays_found; +} + +bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia, real_t p_margin, Physics2DServer::MotionResult *r_result, bool p_exclude_raycast_shapes) { //give me back regular physics engine logic //this is madness @@ -547,8 +696,12 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co if (p_body->is_shape_set_as_disabled(j)) continue; - Transform2D body_shape_xform = body_transform * p_body->get_shape_transform(j); 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++) { const CollisionObject2DSW *col_obj = intersection_query_results[i]; @@ -635,8 +788,12 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co if (p_body->is_shape_set_as_disabled(body_shape_idx)) continue; - Transform2D body_shape_xform = body_transform * p_body->get_shape_transform(body_shape_idx); Shape2DSW *body_shape = p_body->get_shape(body_shape_idx); + if (p_exclude_raycast_shapes && body_shape->get_type() == Physics2DServer::SHAPE_RAY) { + continue; + } + + Transform2D body_shape_xform = body_transform * p_body->get_shape_transform(body_shape_idx); bool stuck = false; diff --git a/servers/physics_2d/space_2d_sw.h b/servers/physics_2d/space_2d_sw.h index 79349c46f30..959e15e12d5 100644 --- a/servers/physics_2d/space_2d_sw.h +++ b/servers/physics_2d/space_2d_sw.h @@ -182,7 +182,8 @@ public: int get_collision_pairs() const { return collision_pairs; } - bool test_body_motion(Body2DSW *p_body, const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia, real_t p_margin, Physics2DServer::MotionResult *r_result); + bool test_body_motion(Body2DSW *p_body, const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia, real_t p_margin, Physics2DServer::MotionResult *r_result, bool p_exclude_raycast_shapes = true); + int test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_transform, bool p_infinite_inertia, Vector2 &r_recover_motion, Physics2DServer::SeparationResult *r_results, int p_result_max, real_t p_margin); void set_debug_contacts(int p_amount) { contact_debug.resize(p_amount); } _FORCE_INLINE_ bool is_debugging_contacts() const { return !contact_debug.empty(); } diff --git a/servers/physics_2d_server.h b/servers/physics_2d_server.h index ba5232f7fe5..1d04fbc5c6e 100644 --- a/servers/physics_2d_server.h +++ b/servers/physics_2d_server.h @@ -479,7 +479,22 @@ public: Variant collider_metadata; }; - virtual bool body_test_motion(RID p_body, const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia, float p_margin = 0.001, MotionResult *r_result = NULL) = 0; + virtual bool body_test_motion(RID p_body, const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia, float p_margin = 0.001, MotionResult *r_result = NULL, bool p_exclude_raycast_shapes = true) = 0; + + struct SeparationResult { + + float collision_depth; + Vector2 collision_point; + Vector2 collision_normal; + Vector2 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 Transform2D &p_transform, bool p_infinite_inertia, Vector2 &r_recover_motion, SeparationResult *r_results, int p_result_max, float p_margin = 0.001) = 0; /* JOINT API */