diff options
Diffstat (limited to 'servers/physics_2d')
23 files changed, 905 insertions, 821 deletions
diff --git a/servers/physics_2d/area_2d_sw.cpp b/servers/physics_2d/area_2d_sw.cpp index 532cb259b3..663a47f273 100644 --- a/servers/physics_2d/area_2d_sw.cpp +++ b/servers/physics_2d/area_2d_sw.cpp @@ -274,6 +274,26 @@ void Area2DSW::call_queries() { } } +void Area2DSW::compute_gravity(const Vector2 &p_position, Vector2 &r_gravity) const { + if (is_gravity_point()) { + const real_t gravity_distance_scale = get_gravity_distance_scale(); + Vector2 v = get_transform().xform(get_gravity_vector()) - p_position; + if (gravity_distance_scale > 0) { + const real_t v_length = v.length(); + if (v_length > 0) { + const real_t v_scaled = v_length * gravity_distance_scale; + r_gravity = (v.normalized() * (get_gravity() / (v_scaled * v_scaled))); + } else { + r_gravity = Vector2(); + } + } else { + r_gravity = v.normalized() * get_gravity(); + } + } else { + r_gravity = get_gravity_vector() * get_gravity(); + } +} + Area2DSW::Area2DSW() : CollisionObject2DSW(TYPE_AREA), monitor_query_list(this), diff --git a/servers/physics_2d/area_2d_sw.h b/servers/physics_2d/area_2d_sw.h index 3bf603b30d..d9147d6f1d 100644 --- a/servers/physics_2d/area_2d_sw.h +++ b/servers/physics_2d/area_2d_sw.h @@ -34,7 +34,6 @@ #include "collision_object_2d_sw.h" #include "core/templates/self_list.h" #include "servers/physics_server_2d.h" -//#include "servers/physics_3d/query_sw.h" class Space2DSW; class Body2DSW; @@ -94,17 +93,12 @@ class Area2DSW : public CollisionObject2DSW { Map<BodyKey, BodyState> monitored_bodies; Map<BodyKey, BodyState> monitored_areas; - //virtual void shape_changed_notify(Shape2DSW *p_shape); - //virtual void shape_deleted_notify(Shape2DSW *p_shape); Set<Constraint2DSW *> constraints; virtual void _shapes_changed(); void _queue_monitor_update(); public: - //_FORCE_INLINE_ const Matrix32& get_inverse_transform() const { return inverse_transform; } - //_FORCE_INLINE_ SpaceSW* get_owner() { return owner; } - void set_monitor_callback(ObjectID p_id, const StringName &p_method); _FORCE_INLINE_ bool has_monitor_callback() const { return monitor_callback_id.is_valid(); } @@ -161,6 +155,8 @@ public: void call_queries(); + void compute_gravity(const Vector2 &p_position, Vector2 &r_gravity) const; + Area2DSW(); ~Area2DSW(); }; diff --git a/servers/physics_2d/area_pair_2d_sw.cpp b/servers/physics_2d/area_pair_2d_sw.cpp index 5ca16cb6fc..4f1148c26f 100644 --- a/servers/physics_2d/area_pair_2d_sw.cpp +++ b/servers/physics_2d/area_pair_2d_sw.cpp @@ -33,7 +33,7 @@ bool AreaPair2DSW::setup(real_t p_step) { bool result = false; - if (area->interacts_with(body) && CollisionSolver2DSW::solve(body->get_shape(body_shape), body->get_transform() * body->get_shape_transform(body_shape), Vector2(), area->get_shape(area_shape), area->get_transform() * area->get_shape_transform(area_shape), Vector2(), nullptr, this)) { + if (area->collides_with(body) && CollisionSolver2DSW::solve(body->get_shape(body_shape), body->get_transform() * body->get_shape_transform(body_shape), Vector2(), area->get_shape(area_shape), area->get_transform() * area->get_shape_transform(area_shape), Vector2(), nullptr, this)) { result = true; } @@ -109,46 +109,51 @@ AreaPair2DSW::~AreaPair2DSW() { ////////////////////////////////// bool Area2Pair2DSW::setup(real_t p_step) { - bool result = false; - if (area_a->interacts_with(area_b) && CollisionSolver2DSW::solve(area_a->get_shape(shape_a), area_a->get_transform() * area_a->get_shape_transform(shape_a), Vector2(), area_b->get_shape(shape_b), area_b->get_transform() * area_b->get_shape_transform(shape_b), Vector2(), nullptr, this)) { - result = true; + bool result_a = area_a->collides_with(area_b); + bool result_b = area_b->collides_with(area_a); + if ((result_a || result_b) && !CollisionSolver2DSW::solve(area_a->get_shape(shape_a), area_a->get_transform() * area_a->get_shape_transform(shape_a), Vector2(), area_b->get_shape(shape_b), area_b->get_transform() * area_b->get_shape_transform(shape_b), Vector2(), nullptr, this)) { + result_a = false; + result_b = false; } - process_collision = false; - if (result != colliding) { - if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) { - process_collision = true; - } else if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) { + bool process_collision = false; + + process_collision_a = false; + if (result_a != colliding_a) { + if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) { + process_collision_a = true; process_collision = true; } + colliding_a = result_a; + } - colliding = result; + process_collision_b = false; + if (result_b != colliding_b) { + if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) { + process_collision_b = true; + process_collision = true; + } + colliding_b = result_b; } return process_collision; } bool Area2Pair2DSW::pre_solve(real_t p_step) { - if (!process_collision) { - return false; + if (process_collision_a) { + if (colliding_a) { + area_a->add_area_to_query(area_b, shape_b, shape_a); + } else { + area_a->remove_area_from_query(area_b, shape_b, shape_a); + } } - if (colliding) { - if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) { + if (process_collision_b) { + if (colliding_b) { area_b->add_area_to_query(area_a, shape_a, shape_b); - } - - if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) { - area_a->add_area_to_query(area_b, shape_b, shape_a); - } - } else { - if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) { + } else { area_b->remove_area_from_query(area_a, shape_a, shape_b); } - - if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) { - area_a->remove_area_from_query(area_b, shape_b, shape_a); - } } return false; // Never do any post solving. @@ -168,16 +173,18 @@ Area2Pair2DSW::Area2Pair2DSW(Area2DSW *p_area_a, int p_shape_a, Area2DSW *p_area } Area2Pair2DSW::~Area2Pair2DSW() { - if (colliding) { - if (area_b->has_area_monitor_callback()) { - area_b->remove_area_from_query(area_a, shape_a, shape_b); - } - + if (colliding_a) { if (area_a->has_area_monitor_callback()) { area_a->remove_area_from_query(area_b, shape_b, shape_a); } } + if (colliding_b) { + if (area_b->has_area_monitor_callback()) { + area_b->remove_area_from_query(area_a, shape_a, shape_b); + } + } + area_a->remove_constraint(this); area_b->remove_constraint(this); } diff --git a/servers/physics_2d/area_pair_2d_sw.h b/servers/physics_2d/area_pair_2d_sw.h index 4632a307d9..66e9f1afee 100644 --- a/servers/physics_2d/area_pair_2d_sw.h +++ b/servers/physics_2d/area_pair_2d_sw.h @@ -57,8 +57,10 @@ class Area2Pair2DSW : public Constraint2DSW { Area2DSW *area_b = nullptr; int shape_a = 0; int shape_b = 0; - bool colliding = false; - bool process_collision = false; + bool colliding_a = false; + bool colliding_b = false; + bool process_collision_a = false; + bool process_collision_b = false; public: virtual bool setup(real_t p_step) override; diff --git a/servers/physics_2d/body_2d_sw.cpp b/servers/physics_2d/body_2d_sw.cpp index 7aa2f9b7de..edd769aa9a 100644 --- a/servers/physics_2d/body_2d_sw.cpp +++ b/servers/physics_2d/body_2d_sw.cpp @@ -29,51 +29,77 @@ /*************************************************************************/ #include "body_2d_sw.h" + #include "area_2d_sw.h" -#include "physics_server_2d_sw.h" +#include "body_direct_state_2d_sw.h" #include "space_2d_sw.h" -void Body2DSW::_update_inertia() { - if (!user_inertia && get_space() && !inertia_update_list.in_list()) { - get_space()->body_add_to_inertia_update_list(&inertia_update_list); +void Body2DSW::_mass_properties_changed() { + if (get_space() && !mass_properties_update_list.in_list() && (calculate_inertia || calculate_center_of_mass)) { + get_space()->body_add_to_mass_properties_update_list(&mass_properties_update_list); } } -void Body2DSW::update_inertias() { +void Body2DSW::update_mass_properties() { //update shapes and motions switch (mode) { case PhysicsServer2D::BODY_MODE_DYNAMIC: { - if (user_inertia) { - _inv_inertia = inertia > 0 ? (1.0 / inertia) : 0; - break; - } - //update tensor for allshapes, not the best way but should be somehow OK. (inspired from bullet) real_t total_area = 0; - for (int i = 0; i < get_shape_count(); i++) { + if (is_shape_disabled(i)) { + continue; + } total_area += get_shape_aabb(i).get_area(); } - inertia = 0; + if (calculate_center_of_mass) { + // We have to recompute the center of mass. + center_of_mass = Vector2(); - for (int i = 0; i < get_shape_count(); i++) { - if (is_shape_disabled(i)) { - continue; + if (total_area != 0.0) { + for (int i = 0; i < get_shape_count(); i++) { + if (is_shape_disabled(i)) { + continue; + } + + real_t area = get_shape_aabb(i).get_area(); + + real_t mass = area * this->mass / total_area; + + // NOTE: we assume that the shape origin is also its center of mass. + center_of_mass += mass * get_shape_transform(i).get_origin(); + } + + center_of_mass /= mass; } + } + + if (calculate_inertia) { + inertia = 0; + + for (int i = 0; i < get_shape_count(); i++) { + if (is_shape_disabled(i)) { + continue; + } - const Shape2DSW *shape = get_shape(i); + const Shape2DSW *shape = get_shape(i); - real_t area = get_shape_aabb(i).get_area(); + real_t area = get_shape_aabb(i).get_area(); + if (area == 0.0) { + continue; + } - real_t mass = area * this->mass / total_area; + real_t mass = area * this->mass / total_area; - Transform2D mtx = get_shape_transform(i); - Vector2 scale = mtx.get_scale(); - inertia += shape->get_moment_of_inertia(mass, scale) + mass * mtx.get_origin().length_squared(); + Transform2D mtx = get_shape_transform(i); + Vector2 scale = mtx.get_scale(); + Vector2 shape_origin = mtx.get_origin() - center_of_mass; + inertia += shape->get_moment_of_inertia(mass, scale) + mass * shape_origin.length_squared(); + } } - _inv_inertia = inertia > 0 ? (1.0 / inertia) : 0; + _inv_inertia = inertia > 0.0 ? (1.0 / inertia) : 0.0; if (mass) { _inv_mass = 1.0 / mass; @@ -93,9 +119,12 @@ void Body2DSW::update_inertias() { } break; } - //_update_inertia_tensor(); +} - //_update_shapes(); +void Body2DSW::reset_mass_properties() { + calculate_inertia = true; + calculate_center_of_mass = true; + _mass_properties_changed(); } void Body2DSW::set_active(bool p_active) { @@ -117,7 +146,7 @@ void Body2DSW::set_active(bool p_active) { } } -void Body2DSW::set_param(PhysicsServer2D::BodyParameter p_param, real_t p_value) { +void Body2DSW::set_param(PhysicsServer2D::BodyParameter p_param, const Variant &p_value) { switch (p_param) { case PhysicsServer2D::BODY_PARAM_BOUNCE: { bounce = p_value; @@ -126,21 +155,32 @@ void Body2DSW::set_param(PhysicsServer2D::BodyParameter p_param, real_t p_value) friction = p_value; } break; case PhysicsServer2D::BODY_PARAM_MASS: { - ERR_FAIL_COND(p_value <= 0); - mass = p_value; - _update_inertia(); - + real_t mass_value = p_value; + ERR_FAIL_COND(mass_value <= 0); + mass = mass_value; + if (mode >= PhysicsServer2D::BODY_MODE_DYNAMIC) { + _mass_properties_changed(); + } } break; case PhysicsServer2D::BODY_PARAM_INERTIA: { - if (p_value <= 0) { - user_inertia = false; - _update_inertia(); + real_t inertia_value = p_value; + if (inertia_value <= 0.0) { + calculate_inertia = true; + if (mode == PhysicsServer2D::BODY_MODE_DYNAMIC) { + _mass_properties_changed(); + } } else { - user_inertia = true; - inertia = p_value; - _inv_inertia = 1.0 / p_value; + calculate_inertia = false; + inertia = inertia_value; + if (mode == PhysicsServer2D::BODY_MODE_DYNAMIC) { + _inv_inertia = 1.0 / inertia; + } } } break; + case PhysicsServer2D::BODY_PARAM_CENTER_OF_MASS: { + calculate_center_of_mass = false; + center_of_mass = p_value; + } break; case PhysicsServer2D::BODY_PARAM_GRAVITY_SCALE: { gravity_scale = p_value; } break; @@ -155,7 +195,7 @@ void Body2DSW::set_param(PhysicsServer2D::BodyParameter p_param, real_t p_value) } } -real_t Body2DSW::get_param(PhysicsServer2D::BodyParameter p_param) const { +Variant Body2DSW::get_param(PhysicsServer2D::BodyParameter p_param) const { switch (p_param) { case PhysicsServer2D::BODY_PARAM_BOUNCE: { return bounce; @@ -169,6 +209,9 @@ real_t Body2DSW::get_param(PhysicsServer2D::BodyParameter p_param) const { case PhysicsServer2D::BODY_PARAM_INERTIA: { return inertia; } + case PhysicsServer2D::BODY_PARAM_CENTER_OF_MASS: { + return center_of_mass; + } case PhysicsServer2D::BODY_PARAM_GRAVITY_SCALE: { return gravity_scale; } @@ -206,7 +249,10 @@ void Body2DSW::set_mode(PhysicsServer2D::BodyMode p_mode) { } break; case PhysicsServer2D::BODY_MODE_DYNAMIC: { _inv_mass = mass > 0 ? (1.0 / mass) : 0; - _inv_inertia = inertia > 0 ? (1.0 / inertia) : 0; + if (!calculate_inertia) { + _inv_inertia = 1.0 / inertia; + } + _mass_properties_changed(); _set_static(false); set_active(true); @@ -214,18 +260,11 @@ void Body2DSW::set_mode(PhysicsServer2D::BodyMode p_mode) { case PhysicsServer2D::BODY_MODE_DYNAMIC_LOCKED: { _inv_mass = mass > 0 ? (1.0 / mass) : 0; _inv_inertia = 0; + angular_velocity = 0; _set_static(false); set_active(true); - angular_velocity = 0; - } break; - } - if (p_mode == PhysicsServer2D::BODY_MODE_DYNAMIC && _inv_inertia == 0) { - _update_inertia(); + } } - /* - if (get_space()) - _update_queries(); - */ } PhysicsServer2D::BodyMode Body2DSW::get_mode() const { @@ -233,7 +272,7 @@ PhysicsServer2D::BodyMode Body2DSW::get_mode() const { } void Body2DSW::_shapes_changed() { - _update_inertia(); + _mass_properties_changed(); wakeup_neighbours(); } @@ -268,11 +307,13 @@ void Body2DSW::set_state(PhysicsServer2D::BodyState p_state, const Variant &p_va } break; case PhysicsServer2D::BODY_STATE_LINEAR_VELOCITY: { linear_velocity = p_variant; + constant_linear_velocity = linear_velocity; wakeup(); } break; case PhysicsServer2D::BODY_STATE_ANGULAR_VELOCITY: { angular_velocity = p_variant; + constant_angular_velocity = angular_velocity; wakeup(); } break; @@ -295,7 +336,7 @@ void Body2DSW::set_state(PhysicsServer2D::BodyState p_state, const Variant &p_va } break; case PhysicsServer2D::BODY_STATE_CAN_SLEEP: { can_sleep = p_variant; - if (mode == PhysicsServer2D::BODY_MODE_DYNAMIC && !active && !can_sleep) { + if (mode >= PhysicsServer2D::BODY_MODE_DYNAMIC && !active && !can_sleep) { set_active(true); } @@ -329,8 +370,8 @@ void Body2DSW::set_space(Space2DSW *p_space) { if (get_space()) { wakeup_neighbours(); - if (inertia_update_list.in_list()) { - get_space()->body_remove_from_inertia_update_list(&inertia_update_list); + if (mass_properties_update_list.in_list()) { + get_space()->body_remove_from_mass_properties_update_list(&mass_properties_update_list); } if (active_list.in_list()) { get_space()->body_remove_from_active_list(&active_list); @@ -343,26 +384,17 @@ void Body2DSW::set_space(Space2DSW *p_space) { _set_space(p_space); if (get_space()) { - _update_inertia(); + _mass_properties_changed(); if (active) { get_space()->body_add_to_active_list(&active_list); } } - - first_integration = false; } -void Body2DSW::_compute_area_gravity_and_dampenings(const Area2DSW *p_area) { - if (p_area->is_gravity_point()) { - if (p_area->get_gravity_distance_scale() > 0) { - Vector2 v = p_area->get_transform().xform(p_area->get_gravity_vector()) - get_transform().get_origin(); - gravity += v.normalized() * (p_area->get_gravity() / Math::pow(v.length() * p_area->get_gravity_distance_scale() + 1, 2)); - } else { - gravity += (p_area->get_transform().xform(p_area->get_gravity_vector()) - get_transform().get_origin()).normalized() * p_area->get_gravity(); - } - } else { - gravity += p_area->get_gravity_vector() * p_area->get_gravity(); - } +void Body2DSW::_compute_area_gravity_and_damping(const Area2DSW *p_area) { + Vector2 area_gravity; + p_area->compute_gravity(get_transform().get_origin(), area_gravity); + gravity += area_gravity; area_linear_damp += p_area->get_linear_damp(); area_angular_damp += p_area->get_angular_damp(); @@ -391,7 +423,7 @@ void Body2DSW::integrate_forces(real_t p_step) { switch (mode) { case PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE: case PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: { - _compute_area_gravity_and_dampenings(aa[i].area); + _compute_area_gravity_and_damping(aa[i].area); stopped = mode == PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE; } break; case PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE: @@ -399,7 +431,7 @@ void Body2DSW::integrate_forces(real_t p_step) { gravity = Vector2(0, 0); area_angular_damp = 0; area_linear_damp = 0; - _compute_area_gravity_and_dampenings(aa[i].area); + _compute_area_gravity_and_damping(aa[i].area); stopped = mode == PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE; } break; default: { @@ -408,7 +440,7 @@ void Body2DSW::integrate_forces(real_t p_step) { } } if (!stopped) { - _compute_area_gravity_and_dampenings(def_area); + _compute_area_gravity_and_damping(def_area); } gravity *= gravity_scale; @@ -435,10 +467,10 @@ void Body2DSW::integrate_forces(real_t p_step) { if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC) { //compute motion, angular and etc. velocities from prev transform motion = new_transform.get_origin() - get_transform().get_origin(); - linear_velocity = motion / p_step; + linear_velocity = constant_linear_velocity + motion / p_step; real_t rot = new_transform.get_rotation() - get_transform().get_rotation(); - angular_velocity = remainder(rot, 2.0 * Math_PI) / p_step; + angular_velocity = constant_angular_velocity + remainder(rot, 2.0 * Math_PI) / p_step; do_motion = true; @@ -450,7 +482,7 @@ void Body2DSW::integrate_forces(real_t p_step) { */ } else { - if (!omit_force_integration && !first_integration) { + if (!omit_force_integration) { //overridden by direct state query Vector2 force = gravity * mass; @@ -484,7 +516,6 @@ void Body2DSW::integrate_forces(real_t p_step) { //motion=linear_velocity*p_step; - first_integration = false; biased_angular_velocity = 0; biased_linear_velocity = Vector2(); @@ -502,7 +533,7 @@ void Body2DSW::integrate_velocities(real_t p_step) { return; } - if (fi_callback) { + if (fi_callback_data || body_state_callback) { get_space()->body_add_to_state_query_list(&direct_state_query_list); } @@ -521,14 +552,22 @@ void Body2DSW::integrate_velocities(real_t p_step) { real_t angle = get_transform().get_rotation() + total_angular_velocity * p_step; Vector2 pos = get_transform().get_origin() + total_linear_velocity * p_step; + real_t center_of_mass_distance = center_of_mass.length(); + if (center_of_mass_distance > CMP_EPSILON) { + // Calculate displacement due to center of mass offset. + real_t prev_angle = get_transform().get_rotation(); + real_t angle_base = Math::atan2(center_of_mass.y, center_of_mass.x); + Vector2 point1(Math::cos(angle_base + prev_angle), Math::sin(angle_base + prev_angle)); + Vector2 point2(Math::cos(angle_base + angle), Math::sin(angle_base + angle)); + pos += center_of_mass_distance * (point1 - point2); + } + _set_transform(Transform2D(angle, pos), continuous_cd_mode == PhysicsServer2D::CCD_MODE_DISABLED); _set_inv_transform(get_transform().inverse()); if (continuous_cd_mode != PhysicsServer2D::CCD_MODE_DISABLED) { new_transform = get_transform(); } - - //_update_inertia_tensor(); } void Body2DSW::wakeup_neighbours() { @@ -542,7 +581,7 @@ void Body2DSW::wakeup_neighbours() { continue; } Body2DSW *b = n[i]; - if (b->mode != PhysicsServer2D::BODY_MODE_DYNAMIC) { + if (b->mode < PhysicsServer2D::BODY_MODE_DYNAMIC) { continue; } @@ -554,27 +593,27 @@ void Body2DSW::wakeup_neighbours() { } void Body2DSW::call_queries() { - if (fi_callback) { - PhysicsDirectBodyState2DSW *dbs = PhysicsDirectBodyState2DSW::singleton; - dbs->body = this; - - Variant v = dbs; - const Variant *vp[2] = { &v, &fi_callback->callback_udata }; - - Object *obj = fi_callback->callable.get_object(); - if (!obj) { + if (fi_callback_data) { + if (!fi_callback_data->callable.get_object()) { set_force_integration_callback(Callable()); } else { + Variant direct_state_variant = get_direct_state(); + const Variant *vp[2] = { &direct_state_variant, &fi_callback_data->udata }; + Callable::CallError ce; Variant rv; - if (fi_callback->callback_udata.get_type() != Variant::NIL) { - fi_callback->callable.call(vp, 2, rv, ce); + if (fi_callback_data->udata.get_type() != Variant::NIL) { + fi_callback_data->callable.call(vp, 2, rv, ce); } else { - fi_callback->callable.call(vp, 1, rv, ce); + fi_callback_data->callable.call(vp, 1, rv, ce); } } } + + if (body_state_callback) { + (body_state_callback)(body_state_callback_instance, get_direct_state()); + } } bool Body2DSW::sleep_test(real_t p_step) { @@ -594,78 +633,45 @@ bool Body2DSW::sleep_test(real_t p_step) { } } +void Body2DSW::set_state_sync_callback(void *p_instance, PhysicsServer2D::BodyStateCallback p_callback) { + body_state_callback_instance = p_instance; + body_state_callback = p_callback; +} + void Body2DSW::set_force_integration_callback(const Callable &p_callable, const Variant &p_udata) { - if (fi_callback) { - memdelete(fi_callback); - fi_callback = nullptr; + if (p_callable.get_object()) { + if (!fi_callback_data) { + fi_callback_data = memnew(ForceIntegrationCallbackData); + } + fi_callback_data->callable = p_callable; + fi_callback_data->udata = p_udata; + } else if (fi_callback_data) { + memdelete(fi_callback_data); + fi_callback_data = nullptr; } +} - if (p_callable.get_object()) { - fi_callback = memnew(ForceIntegrationCallback); - fi_callback->callable = p_callable; - fi_callback->callback_udata = p_udata; +PhysicsDirectBodyState2DSW *Body2DSW::get_direct_state() { + if (!direct_state) { + direct_state = memnew(PhysicsDirectBodyState2DSW); + direct_state->body = this; } + return direct_state; } Body2DSW::Body2DSW() : CollisionObject2DSW(TYPE_BODY), active_list(this), - inertia_update_list(this), + mass_properties_update_list(this), direct_state_query_list(this) { - mode = PhysicsServer2D::BODY_MODE_DYNAMIC; - active = true; - angular_velocity = 0; - biased_angular_velocity = 0; - mass = 1; - inertia = 0; - user_inertia = false; - _inv_inertia = 0; - _inv_mass = 1; - bounce = 0; - friction = 1; - omit_force_integration = false; - applied_torque = 0; - island_step = 0; _set_static(false); - first_time_kinematic = false; - linear_damp = -1; - angular_damp = -1; - area_angular_damp = 0; - area_linear_damp = 0; - contact_count = 0; - gravity_scale = 1.0; - first_integration = false; - - still_time = 0; - continuous_cd_mode = PhysicsServer2D::CCD_MODE_DISABLED; - can_sleep = true; - fi_callback = nullptr; } Body2DSW::~Body2DSW() { - if (fi_callback) { - memdelete(fi_callback); - } -} - -PhysicsDirectBodyState2DSW *PhysicsDirectBodyState2DSW::singleton = nullptr; - -PhysicsDirectSpaceState2D *PhysicsDirectBodyState2DSW::get_space_state() { - return body->get_space()->get_direct_state(); -} - -Variant PhysicsDirectBodyState2DSW::get_contact_collider_shape_metadata(int p_contact_idx) const { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Variant()); - - if (!PhysicsServer2DSW::singletonsw->body_owner.owns(body->contacts[p_contact_idx].collider)) { - return Variant(); + if (fi_callback_data) { + memdelete(fi_callback_data); } - Body2DSW *other = PhysicsServer2DSW::singletonsw->body_owner.getornull(body->contacts[p_contact_idx].collider); - - int sidx = body->contacts[p_contact_idx].collider_shape; - if (sidx < 0 || sidx >= other->get_shape_count()) { - return Variant(); + if (direct_state) { + memdelete(direct_state); } - - return other->get_shape_metadata(sidx); } diff --git a/servers/physics_2d/body_2d_sw.h b/servers/physics_2d/body_2d_sw.h index b4a95651cb..95e89786cd 100644 --- a/servers/physics_2d/body_2d_sw.h +++ b/servers/physics_2d/body_2d_sw.h @@ -38,50 +38,58 @@ #include "core/templates/vset.h" class Constraint2DSW; +class PhysicsDirectBodyState2DSW; class Body2DSW : public CollisionObject2DSW { - PhysicsServer2D::BodyMode mode; + PhysicsServer2D::BodyMode mode = PhysicsServer2D::BODY_MODE_DYNAMIC; Vector2 biased_linear_velocity; - real_t biased_angular_velocity; + real_t biased_angular_velocity = 0.0; Vector2 linear_velocity; - real_t angular_velocity; + real_t angular_velocity = 0.0; - real_t linear_damp; - real_t angular_damp; - real_t gravity_scale; + Vector2 constant_linear_velocity; + real_t constant_angular_velocity = 0.0; - real_t mass; - real_t inertia; - real_t bounce; - real_t friction; + real_t linear_damp = -1.0; + real_t angular_damp = -1.0; + real_t gravity_scale = 1.0; - real_t _inv_mass; - real_t _inv_inertia; - bool user_inertia; + real_t bounce = 0.0; + real_t friction = 1.0; + + real_t mass = 1.0; + real_t _inv_mass = 1.0; + + real_t inertia = 0.0; + real_t _inv_inertia = 0.0; + + Vector2 center_of_mass; + + bool calculate_inertia = true; + bool calculate_center_of_mass = true; Vector2 gravity; - real_t area_linear_damp; - real_t area_angular_damp; + real_t area_linear_damp = 0.0; + real_t area_angular_damp = 0.0; - real_t still_time; + real_t still_time = 0.0; Vector2 applied_force; - real_t applied_torque; + real_t applied_torque = 0.0; SelfList<Body2DSW> active_list; - SelfList<Body2DSW> inertia_update_list; + SelfList<Body2DSW> mass_properties_update_list; SelfList<Body2DSW> direct_state_query_list; VSet<RID> exceptions; - PhysicsServer2D::CCDMode continuous_cd_mode; - bool omit_force_integration; - bool active; - bool can_sleep; - bool first_time_kinematic; - bool first_integration; - void _update_inertia(); + PhysicsServer2D::CCDMode continuous_cd_mode = PhysicsServer2D::CCD_MODE_DISABLED; + bool omit_force_integration = false; + bool active = true; + bool can_sleep = true; + bool first_time_kinematic = false; + void _mass_properties_changed(); virtual void _shapes_changed(); Transform2D new_transform; @@ -114,24 +122,32 @@ class Body2DSW : public CollisionObject2DSW { }; Vector<Contact> contacts; //no contacts by default - int contact_count; + int contact_count = 0; + + void *body_state_callback_instance = nullptr; + PhysicsServer2D::BodyStateCallback body_state_callback = nullptr; - struct ForceIntegrationCallback { + struct ForceIntegrationCallbackData { Callable callable; - Variant callback_udata; + Variant udata; }; - ForceIntegrationCallback *fi_callback; + ForceIntegrationCallbackData *fi_callback_data = nullptr; - uint64_t island_step; + PhysicsDirectBodyState2DSW *direct_state = nullptr; - _FORCE_INLINE_ void _compute_area_gravity_and_dampenings(const Area2DSW *p_area); + uint64_t island_step = 0; + + _FORCE_INLINE_ void _compute_area_gravity_and_damping(const Area2DSW *p_area); friend class PhysicsDirectBodyState2DSW; // i give up, too many functions to expose public: + void set_state_sync_callback(void *p_instance, PhysicsServer2D::BodyStateCallback p_callback); void set_force_integration_callback(const Callable &p_callable, const Variant &p_udata = Variant()); + PhysicsDirectBodyState2DSW *get_direct_state(); + _FORCE_INLINE_ void add_area(Area2DSW *p_area) { int index = areas.find(AreaCMP(p_area)); if (index > -1) { @@ -198,7 +214,7 @@ public: _FORCE_INLINE_ void apply_impulse(const Vector2 &p_impulse, const Vector2 &p_position = Vector2()) { linear_velocity += p_impulse * _inv_mass; - angular_velocity += _inv_inertia * p_position.cross(p_impulse); + angular_velocity += _inv_inertia * (p_position - center_of_mass).cross(p_impulse); } _FORCE_INLINE_ void apply_torque_impulse(real_t p_torque) { @@ -207,7 +223,7 @@ public: _FORCE_INLINE_ void apply_bias_impulse(const Vector2 &p_impulse, const Vector2 &p_position = Vector2()) { biased_linear_velocity += p_impulse * _inv_mass; - biased_angular_velocity += _inv_inertia * p_position.cross(p_impulse); + biased_angular_velocity += _inv_inertia * (p_position - center_of_mass).cross(p_impulse); } void set_active(bool p_active); @@ -220,8 +236,8 @@ public: set_active(true); } - void set_param(PhysicsServer2D::BodyParameter p_param, real_t); - real_t get_param(PhysicsServer2D::BodyParameter p_param) const; + void set_param(PhysicsServer2D::BodyParameter p_param, const Variant &p_value); + Variant get_param(PhysicsServer2D::BodyParameter p_param) const; void set_mode(PhysicsServer2D::BodyMode p_mode); PhysicsServer2D::BodyMode get_mode() const; @@ -241,7 +257,7 @@ public: _FORCE_INLINE_ void add_force(const Vector2 &p_force, const Vector2 &p_position = Vector2()) { applied_force += p_force; - applied_torque += p_position.cross(p_force); + applied_torque += (p_position - center_of_mass).cross(p_force); } _FORCE_INLINE_ void add_torque(real_t p_torque) { @@ -253,8 +269,10 @@ public: void set_space(Space2DSW *p_space); - void update_inertias(); + void update_mass_properties(); + void reset_mass_properties(); + _FORCE_INLINE_ Vector2 get_center_of_mass() const { return center_of_mass; } _FORCE_INLINE_ real_t get_inv_mass() const { return _inv_mass; } _FORCE_INLINE_ real_t get_inv_inertia() const { return _inv_inertia; } _FORCE_INLINE_ real_t get_friction() const { return friction; } @@ -266,6 +284,10 @@ public: void integrate_forces(real_t p_step); void integrate_velocities(real_t p_step); + _FORCE_INLINE_ Vector2 get_velocity_in_local_point(const Vector2 &rel_pos) const { + return linear_velocity + Vector2(-angular_velocity * rel_pos.y, angular_velocity * rel_pos.x); + } + _FORCE_INLINE_ Vector2 get_motion() const { if (mode > PhysicsServer2D::BODY_MODE_KINEMATIC) { return new_transform.get_origin() - get_transform().get_origin(); @@ -328,85 +350,4 @@ void Body2DSW::add_contact(const Vector2 &p_local_pos, const Vector2 &p_local_no c[idx].collider_velocity_at_pos = p_collider_velocity_at_pos; } -class PhysicsDirectBodyState2DSW : public PhysicsDirectBodyState2D { - GDCLASS(PhysicsDirectBodyState2DSW, PhysicsDirectBodyState2D); - -public: - static PhysicsDirectBodyState2DSW *singleton; - Body2DSW *body; - real_t step; - - virtual Vector2 get_total_gravity() const override { return body->gravity; } // get gravity vector working on this body space/area - virtual real_t get_total_angular_damp() const override { return body->area_angular_damp; } // get density of this body space/area - virtual real_t get_total_linear_damp() const override { return body->area_linear_damp; } // get density of this body space/area - - virtual real_t get_inverse_mass() const override { return body->get_inv_mass(); } // get the mass - virtual real_t get_inverse_inertia() const override { return body->get_inv_inertia(); } // get density of this body space - - virtual void set_linear_velocity(const Vector2 &p_velocity) override { body->set_linear_velocity(p_velocity); } - virtual Vector2 get_linear_velocity() const override { return body->get_linear_velocity(); } - - virtual void set_angular_velocity(real_t p_velocity) override { body->set_angular_velocity(p_velocity); } - virtual real_t get_angular_velocity() const override { return body->get_angular_velocity(); } - - virtual void set_transform(const Transform2D &p_transform) override { body->set_state(PhysicsServer2D::BODY_STATE_TRANSFORM, p_transform); } - virtual Transform2D get_transform() const override { return body->get_transform(); } - - virtual void add_central_force(const Vector2 &p_force) override { body->add_central_force(p_force); } - virtual void add_force(const Vector2 &p_force, const Vector2 &p_position = Vector2()) override { body->add_force(p_force, p_position); } - virtual void add_torque(real_t p_torque) override { body->add_torque(p_torque); } - virtual void apply_central_impulse(const Vector2 &p_impulse) override { body->apply_central_impulse(p_impulse); } - virtual void apply_impulse(const Vector2 &p_impulse, const Vector2 &p_position = Vector2()) override { body->apply_impulse(p_impulse, p_position); } - virtual void apply_torque_impulse(real_t p_torque) override { body->apply_torque_impulse(p_torque); } - - virtual void set_sleep_state(bool p_enable) override { body->set_active(!p_enable); } - virtual bool is_sleeping() const override { return !body->is_active(); } - - virtual int get_contact_count() const override { return body->contact_count; } - - virtual Vector2 get_contact_local_position(int p_contact_idx) const override { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2()); - return body->contacts[p_contact_idx].local_pos; - } - virtual Vector2 get_contact_local_normal(int p_contact_idx) const override { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2()); - return body->contacts[p_contact_idx].local_normal; - } - virtual int get_contact_local_shape(int p_contact_idx) const override { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, -1); - return body->contacts[p_contact_idx].local_shape; - } - - virtual RID get_contact_collider(int p_contact_idx) const override { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, RID()); - return body->contacts[p_contact_idx].collider; - } - virtual Vector2 get_contact_collider_position(int p_contact_idx) const override { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2()); - return body->contacts[p_contact_idx].collider_pos; - } - virtual ObjectID get_contact_collider_id(int p_contact_idx) const override { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, ObjectID()); - return body->contacts[p_contact_idx].collider_instance_id; - } - virtual int get_contact_collider_shape(int p_contact_idx) const override { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, 0); - return body->contacts[p_contact_idx].collider_shape; - } - virtual Variant get_contact_collider_shape_metadata(int p_contact_idx) const override; - - virtual Vector2 get_contact_collider_velocity_at_position(int p_contact_idx) const override { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2()); - return body->contacts[p_contact_idx].collider_velocity_at_pos; - } - - virtual PhysicsDirectSpaceState2D *get_space_state() override; - - virtual real_t get_step() const override { return step; } - PhysicsDirectBodyState2DSW() { - singleton = this; - body = nullptr; - } -}; - #endif // BODY_2D_SW_H diff --git a/servers/physics_2d/body_direct_state_2d_sw.cpp b/servers/physics_2d/body_direct_state_2d_sw.cpp new file mode 100644 index 0000000000..58250c3077 --- /dev/null +++ b/servers/physics_2d/body_direct_state_2d_sw.cpp @@ -0,0 +1,186 @@ +/*************************************************************************/ +/* body_direct_state_2d_sw.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "body_direct_state_2d_sw.h" + +#include "body_2d_sw.h" +#include "physics_server_2d_sw.h" +#include "space_2d_sw.h" + +Vector2 PhysicsDirectBodyState2DSW::get_total_gravity() const { + return body->gravity; +} + +real_t PhysicsDirectBodyState2DSW::get_total_angular_damp() const { + return body->area_angular_damp; +} + +real_t PhysicsDirectBodyState2DSW::get_total_linear_damp() const { + return body->area_linear_damp; +} + +Vector2 PhysicsDirectBodyState2DSW::get_center_of_mass() const { + return body->get_center_of_mass(); +} + +real_t PhysicsDirectBodyState2DSW::get_inverse_mass() const { + return body->get_inv_mass(); +} + +real_t PhysicsDirectBodyState2DSW::get_inverse_inertia() const { + return body->get_inv_inertia(); +} + +void PhysicsDirectBodyState2DSW::set_linear_velocity(const Vector2 &p_velocity) { + body->set_linear_velocity(p_velocity); +} + +Vector2 PhysicsDirectBodyState2DSW::get_linear_velocity() const { + return body->get_linear_velocity(); +} + +void PhysicsDirectBodyState2DSW::set_angular_velocity(real_t p_velocity) { + body->set_angular_velocity(p_velocity); +} + +real_t PhysicsDirectBodyState2DSW::get_angular_velocity() const { + return body->get_angular_velocity(); +} + +void PhysicsDirectBodyState2DSW::set_transform(const Transform2D &p_transform) { + body->set_state(PhysicsServer2D::BODY_STATE_TRANSFORM, p_transform); +} + +Transform2D PhysicsDirectBodyState2DSW::get_transform() const { + return body->get_transform(); +} + +Vector2 PhysicsDirectBodyState2DSW::get_velocity_at_local_position(const Vector2 &p_position) const { + return body->get_velocity_in_local_point(p_position); +} + +void PhysicsDirectBodyState2DSW::add_central_force(const Vector2 &p_force) { + body->add_central_force(p_force); +} + +void PhysicsDirectBodyState2DSW::add_force(const Vector2 &p_force, const Vector2 &p_position) { + body->add_force(p_force, p_position); +} + +void PhysicsDirectBodyState2DSW::add_torque(real_t p_torque) { + body->add_torque(p_torque); +} + +void PhysicsDirectBodyState2DSW::apply_central_impulse(const Vector2 &p_impulse) { + body->apply_central_impulse(p_impulse); +} + +void PhysicsDirectBodyState2DSW::apply_impulse(const Vector2 &p_impulse, const Vector2 &p_position) { + body->apply_impulse(p_impulse, p_position); +} + +void PhysicsDirectBodyState2DSW::apply_torque_impulse(real_t p_torque) { + body->apply_torque_impulse(p_torque); +} + +void PhysicsDirectBodyState2DSW::set_sleep_state(bool p_enable) { + body->set_active(!p_enable); +} + +bool PhysicsDirectBodyState2DSW::is_sleeping() const { + return !body->is_active(); +} + +int PhysicsDirectBodyState2DSW::get_contact_count() const { + return body->contact_count; +} + +Vector2 PhysicsDirectBodyState2DSW::get_contact_local_position(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2()); + return body->contacts[p_contact_idx].local_pos; +} + +Vector2 PhysicsDirectBodyState2DSW::get_contact_local_normal(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2()); + return body->contacts[p_contact_idx].local_normal; +} + +int PhysicsDirectBodyState2DSW::get_contact_local_shape(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, -1); + return body->contacts[p_contact_idx].local_shape; +} + +RID PhysicsDirectBodyState2DSW::get_contact_collider(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, RID()); + return body->contacts[p_contact_idx].collider; +} +Vector2 PhysicsDirectBodyState2DSW::get_contact_collider_position(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2()); + return body->contacts[p_contact_idx].collider_pos; +} + +ObjectID PhysicsDirectBodyState2DSW::get_contact_collider_id(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, ObjectID()); + return body->contacts[p_contact_idx].collider_instance_id; +} + +int PhysicsDirectBodyState2DSW::get_contact_collider_shape(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, 0); + return body->contacts[p_contact_idx].collider_shape; +} + +Vector2 PhysicsDirectBodyState2DSW::get_contact_collider_velocity_at_position(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2()); + return body->contacts[p_contact_idx].collider_velocity_at_pos; +} + +Variant PhysicsDirectBodyState2DSW::get_contact_collider_shape_metadata(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Variant()); + + if (!PhysicsServer2DSW::singletonsw->body_owner.owns(body->contacts[p_contact_idx].collider)) { + return Variant(); + } + Body2DSW *other = PhysicsServer2DSW::singletonsw->body_owner.getornull(body->contacts[p_contact_idx].collider); + + int sidx = body->contacts[p_contact_idx].collider_shape; + if (sidx < 0 || sidx >= other->get_shape_count()) { + return Variant(); + } + + return other->get_shape_metadata(sidx); +} + +PhysicsDirectSpaceState2D *PhysicsDirectBodyState2DSW::get_space_state() { + return body->get_space()->get_direct_state(); +} + +real_t PhysicsDirectBodyState2DSW::get_step() const { + return body->get_space()->get_last_step(); +} diff --git a/servers/physics_2d/body_direct_state_2d_sw.h b/servers/physics_2d/body_direct_state_2d_sw.h new file mode 100644 index 0000000000..34faa174d8 --- /dev/null +++ b/servers/physics_2d/body_direct_state_2d_sw.h @@ -0,0 +1,92 @@ +/*************************************************************************/ +/* body_direct_state_2d_sw.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#ifndef BODY_DIRECT_STATE_2D_SW_H +#define BODY_DIRECT_STATE_2D_SW_H + +#include "servers/physics_server_2d.h" + +class Body2DSW; + +class PhysicsDirectBodyState2DSW : public PhysicsDirectBodyState2D { + GDCLASS(PhysicsDirectBodyState2DSW, PhysicsDirectBodyState2D); + +public: + Body2DSW *body = nullptr; + + virtual Vector2 get_total_gravity() const override; + virtual real_t get_total_angular_damp() const override; + virtual real_t get_total_linear_damp() const override; + + virtual Vector2 get_center_of_mass() const override; + virtual real_t get_inverse_mass() const override; + virtual real_t get_inverse_inertia() const override; + + virtual void set_linear_velocity(const Vector2 &p_velocity) override; + virtual Vector2 get_linear_velocity() const override; + + virtual void set_angular_velocity(real_t p_velocity) override; + virtual real_t get_angular_velocity() const override; + + virtual void set_transform(const Transform2D &p_transform) override; + virtual Transform2D get_transform() const override; + + virtual Vector2 get_velocity_at_local_position(const Vector2 &p_position) const override; + + virtual void add_central_force(const Vector2 &p_force) override; + virtual void add_force(const Vector2 &p_force, const Vector2 &p_position = Vector2()) override; + virtual void add_torque(real_t p_torque) override; + virtual void apply_central_impulse(const Vector2 &p_impulse) override; + virtual void apply_impulse(const Vector2 &p_impulse, const Vector2 &p_position = Vector2()) override; + virtual void apply_torque_impulse(real_t p_torque) override; + + virtual void set_sleep_state(bool p_enable) override; + virtual bool is_sleeping() const override; + + virtual int get_contact_count() const override; + + virtual Vector2 get_contact_local_position(int p_contact_idx) const override; + virtual Vector2 get_contact_local_normal(int p_contact_idx) const override; + virtual int get_contact_local_shape(int p_contact_idx) const override; + + virtual RID get_contact_collider(int p_contact_idx) const override; + virtual Vector2 get_contact_collider_position(int p_contact_idx) const override; + virtual ObjectID get_contact_collider_id(int p_contact_idx) const override; + virtual int get_contact_collider_shape(int p_contact_idx) const override; + virtual Variant get_contact_collider_shape_metadata(int p_contact_idx) const override; + + virtual Vector2 get_contact_collider_velocity_at_position(int p_contact_idx) const override; + + virtual PhysicsDirectSpaceState2D *get_space_state() override; + + virtual real_t get_step() const override; +}; + +#endif // BODY_2D_SW_H diff --git a/servers/physics_2d/body_pair_2d_sw.cpp b/servers/physics_2d/body_pair_2d_sw.cpp index 71d985a6c5..8bcc4609f4 100644 --- a/servers/physics_2d/body_pair_2d_sw.cpp +++ b/servers/physics_2d/body_pair_2d_sw.cpp @@ -226,16 +226,16 @@ real_t combine_friction(Body2DSW *A, Body2DSW *B) { } bool BodyPair2DSW::setup(real_t p_step) { - dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); - dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); - if (!A->interacts_with(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self())) { collided = false; return false; } + collide_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) && A->collides_with(B); + collide_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) && B->collides_with(A); + report_contacts_only = false; - if (!dynamic_A && !dynamic_B) { + if (!collide_A && !collide_B) { if ((A->get_max_contacts_reported() > 0) || (B->get_max_contacts_reported() > 0)) { report_contacts_only = true; } else { @@ -275,13 +275,13 @@ bool BodyPair2DSW::setup(real_t p_step) { if (!collided) { //test ccd (currently just a raycast) - if (A->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && dynamic_A) { + if (A->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && collide_A) { if (_test_ccd(p_step, A, shape_A, xform_A, B, shape_B, xform_B)) { collided = true; } } - if (B->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && dynamic_B) { + if (B->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && collide_B) { if (_test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A, true)) { collided = true; } @@ -298,7 +298,7 @@ bool BodyPair2DSW::setup(real_t p_step) { } if (!prev_collided) { - if (A->is_shape_set_as_one_way_collision(shape_A)) { + if (shape_B_ptr->allows_one_way_collision() && A->is_shape_set_as_one_way_collision(shape_A)) { Vector2 direction = xform_A.get_axis(1).normalized(); bool valid = false; for (int i = 0; i < contact_count; i++) { @@ -319,7 +319,7 @@ bool BodyPair2DSW::setup(real_t p_step) { } } - if (B->is_shape_set_as_one_way_collision(shape_B)) { + if (shape_A_ptr->allows_one_way_collision() && B->is_shape_set_as_one_way_collision(shape_B)) { Vector2 direction = xform_B.get_axis(1).normalized(); bool valid = false; for (int i = 0; i < contact_count; i++) { @@ -374,6 +374,12 @@ bool BodyPair2DSW::pre_solve(real_t p_step) { const Transform2D &transform_A = A->get_transform(); const Transform2D &transform_B = B->get_transform(); + real_t inv_inertia_A = collide_A ? A->get_inv_inertia() : 0.0; + real_t inv_inertia_B = collide_B ? B->get_inv_inertia() : 0.0; + + real_t inv_mass_A = collide_A ? A->get_inv_mass() : 0.0; + real_t inv_mass_B = collide_B ? B->get_inv_mass() : 0.0; + for (int i = 0; i < contact_count; i++) { Contact &c = contacts[i]; c.active = false; @@ -384,7 +390,7 @@ bool BodyPair2DSW::pre_solve(real_t p_step) { Vector2 axis = global_A - global_B; real_t depth = axis.dot(c.normal); - if (depth <= 0 || !c.reused) { + if (depth <= 0.0 || !c.reused) { continue; } @@ -416,15 +422,15 @@ bool BodyPair2DSW::pre_solve(real_t p_step) { // Precompute normal mass, tangent mass, and bias. real_t rnA = c.rA.dot(c.normal); real_t rnB = c.rB.dot(c.normal); - real_t kNormal = A->get_inv_mass() + B->get_inv_mass(); - kNormal += A->get_inv_inertia() * (c.rA.dot(c.rA) - rnA * rnA) + B->get_inv_inertia() * (c.rB.dot(c.rB) - rnB * rnB); + real_t kNormal = inv_mass_A + inv_mass_B; + kNormal += inv_inertia_A * (c.rA.dot(c.rA) - rnA * rnA) + inv_inertia_B * (c.rB.dot(c.rB) - rnB * rnB); c.mass_normal = 1.0f / kNormal; Vector2 tangent = c.normal.orthogonal(); real_t rtA = c.rA.dot(tangent); real_t rtB = c.rB.dot(tangent); - real_t kTangent = A->get_inv_mass() + B->get_inv_mass(); - kTangent += A->get_inv_inertia() * (c.rA.dot(c.rA) - rtA * rtA) + B->get_inv_inertia() * (c.rB.dot(c.rB) - rtB * rtB); + real_t kTangent = inv_mass_A + inv_mass_B; + kTangent += inv_inertia_A * (c.rA.dot(c.rA) - rtA * rtA) + inv_inertia_B * (c.rB.dot(c.rB) - rtB * rtB); c.mass_tangent = 1.0f / kTangent; c.bias = -bias * inv_dt * MIN(0.0f, -depth + max_penetration); @@ -436,10 +442,10 @@ bool BodyPair2DSW::pre_solve(real_t p_step) { // Apply normal + friction impulse Vector2 P = c.acc_normal_impulse * c.normal + c.acc_tangent_impulse * tangent; - if (dynamic_A) { + if (collide_A) { A->apply_impulse(-P, c.rA); } - if (dynamic_B) { + if (collide_B) { B->apply_impulse(P, c.rB); } } @@ -493,10 +499,10 @@ void BodyPair2DSW::solve(real_t p_step) { Vector2 jb = c.normal * (c.acc_bias_impulse - jbnOld); - if (dynamic_A) { + if (collide_A) { A->apply_bias_impulse(-jb, c.rA); } - if (dynamic_B) { + if (collide_B) { B->apply_bias_impulse(jb, c.rB); } @@ -513,10 +519,10 @@ void BodyPair2DSW::solve(real_t p_step) { Vector2 j = c.normal * (c.acc_normal_impulse - jnOld) + tangent * (c.acc_tangent_impulse - jtOld); - if (dynamic_A) { + if (collide_A) { A->apply_impulse(-j, c.rA); } - if (dynamic_B) { + if (collide_B) { B->apply_impulse(j, c.rB); } } diff --git a/servers/physics_2d/body_pair_2d_sw.h b/servers/physics_2d/body_pair_2d_sw.h index 4b42b44c92..849a7e2430 100644 --- a/servers/physics_2d/body_pair_2d_sw.h +++ b/servers/physics_2d/body_pair_2d_sw.h @@ -50,8 +50,8 @@ class BodyPair2DSW : public Constraint2DSW { int shape_A = 0; int shape_B = 0; - bool dynamic_A = false; - bool dynamic_B = false; + bool collide_A = false; + bool collide_B = false; Space2DSW *space = nullptr; diff --git a/servers/physics_2d/collision_object_2d_sw.h b/servers/physics_2d/collision_object_2d_sw.h index 14adb0bb18..55ffa9b1b8 100644 --- a/servers/physics_2d/collision_object_2d_sw.h +++ b/servers/physics_2d/collision_object_2d_sw.h @@ -186,8 +186,8 @@ public: void set_pickable(bool p_pickable) { pickable = p_pickable; } _FORCE_INLINE_ bool is_pickable() const { return pickable; } - _FORCE_INLINE_ bool layer_in_mask(CollisionObject2DSW *p_other) const { - return collision_layer & p_other->collision_mask; + _FORCE_INLINE_ bool collides_with(CollisionObject2DSW *p_other) const { + return p_other->collision_layer & collision_mask; } _FORCE_INLINE_ bool interacts_with(CollisionObject2DSW *p_other) const { diff --git a/servers/physics_2d/collision_solver_2d_sat.cpp b/servers/physics_2d/collision_solver_2d_sat.cpp index 29242a554b..b1aee01bde 100644 --- a/servers/physics_2d/collision_solver_2d_sat.cpp +++ b/servers/physics_2d/collision_solver_2d_sat.cpp @@ -560,16 +560,18 @@ static void _collision_segment_capsule(const Shape2DSW *p_a, const Transform2D & return; } - if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * 0.5))) { + real_t capsule_dir = capsule_B->get_height() * 0.5 - capsule_B->get_radius(); + + if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir))) { return; } - if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * -0.5))) { + if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), (p_transform_b.get_origin() - p_transform_b.elements[1] * capsule_dir))) { return; } - if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * 0.5))) { + if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir))) { return; } - if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * -0.5))) { + if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), (p_transform_b.get_origin() - p_transform_b.elements[1] * capsule_dir))) { return; } @@ -715,11 +717,13 @@ static void _collision_circle_capsule(const Shape2DSW *p_a, const Transform2D &p return; } + real_t capsule_dir = capsule_B->get_height() * 0.5 - capsule_B->get_radius(); + //capsule endpoints - if (TEST_POINT(p_transform_a.get_origin(), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * 0.5))) { + if (TEST_POINT(p_transform_a.get_origin(), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir))) { return; } - if (TEST_POINT(p_transform_a.get_origin(), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * -0.5))) { + if (TEST_POINT(p_transform_a.get_origin(), (p_transform_b.get_origin() - p_transform_b.elements[1] * capsule_dir))) { return; } @@ -864,9 +868,11 @@ static void _collision_rectangle_capsule(const Shape2DSW *p_a, const Transform2D Transform2D boxinv = p_transform_a.affine_inverse(); + real_t capsule_dir = capsule_B->get_height() * 0.5 - capsule_B->get_radius(); + for (int i = 0; i < 2; i++) { { - Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * (i == 0 ? 0.5 : -0.5); + Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir; if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) { return; @@ -874,7 +880,7 @@ static void _collision_rectangle_capsule(const Shape2DSW *p_a, const Transform2D } if (castA) { - Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * (i == 0 ? 0.5 : -0.5); + Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir; capsule_endpoint -= p_motion_a; if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) { @@ -883,7 +889,7 @@ static void _collision_rectangle_capsule(const Shape2DSW *p_a, const Transform2D } if (castB) { - Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * (i == 0 ? 0.5 : -0.5); + Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir; capsule_endpoint += p_motion_b; if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) { @@ -892,7 +898,7 @@ static void _collision_rectangle_capsule(const Shape2DSW *p_a, const Transform2D } if (castA && castB) { - Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * (i == 0 ? 0.5 : -0.5); + Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir; capsule_endpoint -= p_motion_a; capsule_endpoint += p_motion_b; @@ -900,6 +906,8 @@ static void _collision_rectangle_capsule(const Shape2DSW *p_a, const Transform2D return; } } + + capsule_dir *= -1.0; } separator.generate_contacts(); @@ -994,16 +1002,22 @@ static void _collision_capsule_capsule(const Shape2DSW *p_a, const Transform2D & //capsule endpoints + real_t capsule_dir_A = capsule_A->get_height() * 0.5 - capsule_A->get_radius(); for (int i = 0; i < 2; i++) { - Vector2 capsule_endpoint_A = p_transform_a.get_origin() + p_transform_a.elements[1] * capsule_A->get_height() * (i == 0 ? 0.5 : -0.5); + Vector2 capsule_endpoint_A = p_transform_a.get_origin() + p_transform_a.elements[1] * capsule_dir_A; + real_t capsule_dir_B = capsule_B->get_height() * 0.5 - capsule_B->get_radius(); for (int j = 0; j < 2; j++) { - Vector2 capsule_endpoint_B = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * (j == 0 ? 0.5 : -0.5); + Vector2 capsule_endpoint_B = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir_B; if (TEST_POINT(capsule_endpoint_A, capsule_endpoint_B)) { return; } + + capsule_dir_B *= -1.0; } + + capsule_dir_A *= -1.0; } separator.generate_contacts(); @@ -1034,12 +1048,15 @@ static void _collision_capsule_convex_polygon(const Shape2DSW *p_a, const Transf for (int i = 0; i < convex_B->get_point_count(); i++) { Vector2 cpoint = p_transform_b.xform(convex_B->get_point(i)); + real_t capsule_dir = capsule_A->get_height() * 0.5 - capsule_A->get_radius(); for (int j = 0; j < 2; j++) { - Vector2 capsule_endpoint_A = p_transform_a.get_origin() + p_transform_a.elements[1] * capsule_A->get_height() * (j == 0 ? 0.5 : -0.5); + Vector2 capsule_endpoint_A = p_transform_a.get_origin() + p_transform_a.elements[1] * capsule_dir; if (TEST_POINT(capsule_endpoint_A, cpoint)) { return; } + + capsule_dir *= -1.0; } if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) { @@ -1097,14 +1114,14 @@ static void _collision_convex_polygon_convex_polygon(const Shape2DSW *p_a, const bool sat_2d_calculate_penetration(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, CollisionSolver2DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap, Vector2 *sep_axis, real_t p_margin_A, real_t p_margin_B) { PhysicsServer2D::ShapeType type_A = p_shape_A->get_type(); - ERR_FAIL_COND_V(type_A == PhysicsServer2D::SHAPE_LINE, false); - //ERR_FAIL_COND_V(type_A==PhysicsServer2D::SHAPE_RAY,false); + ERR_FAIL_COND_V(type_A == PhysicsServer2D::SHAPE_WORLD_MARGIN, false); + ERR_FAIL_COND_V(type_A == PhysicsServer2D::SHAPE_SEPARATION_RAY, false); ERR_FAIL_COND_V(p_shape_A->is_concave(), false); PhysicsServer2D::ShapeType type_B = p_shape_B->get_type(); - ERR_FAIL_COND_V(type_B == PhysicsServer2D::SHAPE_LINE, false); - //ERR_FAIL_COND_V(type_B==PhysicsServer2D::SHAPE_RAY,false); + ERR_FAIL_COND_V(type_B == PhysicsServer2D::SHAPE_WORLD_MARGIN, false); + ERR_FAIL_COND_V(type_B == PhysicsServer2D::SHAPE_SEPARATION_RAY, false); ERR_FAIL_COND_V(p_shape_B->is_concave(), false); static const CollisionFunc collision_table[5][5] = { diff --git a/servers/physics_2d/collision_solver_2d_sw.cpp b/servers/physics_2d/collision_solver_2d_sw.cpp index 5bd4d498c6..ae50615953 100644 --- a/servers/physics_2d/collision_solver_2d_sw.cpp +++ b/servers/physics_2d/collision_solver_2d_sw.cpp @@ -34,14 +34,14 @@ #define collision_solver sat_2d_calculate_penetration //#define collision_solver gjk_epa_calculate_penetration -bool CollisionSolver2DSW::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) { - const LineShape2DSW *line = static_cast<const LineShape2DSW *>(p_shape_A); - if (p_shape_B->get_type() == PhysicsServer2D::SHAPE_LINE) { +bool CollisionSolver2DSW::solve_static_world_margin(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) { + const WorldMarginShape2DSW *world_margin = static_cast<const WorldMarginShape2DSW *>(p_shape_A); + if (p_shape_B->get_type() == PhysicsServer2D::SHAPE_WORLD_MARGIN) { return false; } - Vector2 n = p_transform_A.basis_xform(line->get_normal()).normalized(); - Vector2 p = p_transform_A.xform(line->get_normal() * line->get_d()); + Vector2 n = p_transform_A.basis_xform(world_margin->get_normal()).normalized(); + Vector2 p = p_transform_A.xform(world_margin->get_normal() * world_margin->get_d()); real_t d = n.dot(p); Vector2 supports[2]; @@ -73,14 +73,14 @@ bool CollisionSolver2DSW::solve_static_line(const Shape2DSW *p_shape_A, const Tr return found; } -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<const RayShape2DSW *>(p_shape_A); - if (p_shape_B->get_type() == PhysicsServer2D::SHAPE_RAY) { +bool CollisionSolver2DSW::solve_separation_ray(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 *r_sep_axis, real_t p_margin) { + const SeparationRayShape2DSW *ray = static_cast<const SeparationRayShape2DSW *>(p_shape_A); + if (p_shape_B->get_type() == PhysicsServer2D::SHAPE_SEPARATION_RAY) { return false; } Vector2 from = p_transform_A.get_origin(); - Vector2 to = from + p_transform_A[1] * ray->get_length(); + Vector2 to = from + p_transform_A[1] * (ray->get_length() + p_margin); if (p_motion_A != Vector2()) { //not the best but should be enough Vector2 normal = (to - from).normalized(); @@ -94,14 +94,30 @@ bool CollisionSolver2DSW::solve_raycast(const Shape2DSW *p_shape_A, const Vector Vector2 p, n; if (!p_shape_B->intersect_segment(from, to, p, n)) { - if (sep_axis) { - *sep_axis = p_transform_A[1].normalized(); + if (r_sep_axis) { + *r_sep_axis = p_transform_A[1].normalized(); + } + return false; + } + + // Discard contacts when the ray is fully contained inside the shape. + if (n == Vector2()) { + if (r_sep_axis) { + *r_sep_axis = p_transform_A[1].normalized(); + } + return false; + } + + // Discard contacts in the wrong direction. + if (n.dot(from - to) < CMP_EPSILON) { + if (r_sep_axis) { + *r_sep_axis = p_transform_A[1].normalized(); } return false; } Vector2 support_B = p_transform_B.xform(p); - if (ray->get_slips_on_slope()) { + if (ray->get_slide_on_slope()) { Vector2 global_n = invb.basis_xform_inv(n).normalized(); support_B = support_A + (support_B - support_A).length() * global_n; } @@ -133,23 +149,23 @@ struct _ConcaveCollisionInfo2D { Vector2 *sep_axis; }; -void CollisionSolver2DSW::concave_callback(void *p_userdata, Shape2DSW *p_convex) { +bool CollisionSolver2DSW::concave_callback(void *p_userdata, Shape2DSW *p_convex) { _ConcaveCollisionInfo2D &cinfo = *(_ConcaveCollisionInfo2D *)(p_userdata); cinfo.aabb_tests++; - if (!cinfo.result_callback && cinfo.collided) { - return; //already collided and no contacts requested, don't test anymore - } bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, cinfo.motion_A, p_convex, *cinfo.transform_B, cinfo.motion_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result, cinfo.sep_axis, cinfo.margin_A, cinfo.margin_B); if (!collided) { - return; + return false; } cinfo.collided = true; cinfo.collisions++; + + // Stop at first collision if contacts are not needed. + return !cinfo.result_callback; } -bool CollisionSolver2DSW::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, real_t p_margin_A, real_t p_margin_B) { +bool CollisionSolver2DSW::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 *r_sep_axis, real_t p_margin_A, real_t p_margin_B) { const ConcaveShape2DSW *concave_B = static_cast<const ConcaveShape2DSW *>(p_shape_B); _ConcaveCollisionInfo2D cinfo; @@ -162,7 +178,7 @@ bool CollisionSolver2DSW::solve_concave(const Shape2DSW *p_shape_A, const Transf cinfo.swap_result = p_swap_result; cinfo.collided = false; cinfo.collisions = 0; - cinfo.sep_axis = sep_axis; + cinfo.sep_axis = r_sep_axis; cinfo.margin_A = p_margin_A; cinfo.margin_B = p_margin_B; @@ -193,7 +209,7 @@ bool CollisionSolver2DSW::solve_concave(const Shape2DSW *p_shape_A, const Transf return cinfo.collided; } -bool CollisionSolver2DSW::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, real_t p_margin_A, real_t p_margin_B) { +bool CollisionSolver2DSW::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 *r_sep_axis, real_t p_margin_A, real_t p_margin_B) { PhysicsServer2D::ShapeType type_A = p_shape_A->get_type(); PhysicsServer2D::ShapeType type_B = p_shape_B->get_type(); bool concave_A = p_shape_A->is_concave(); @@ -209,26 +225,26 @@ bool CollisionSolver2DSW::solve(const Shape2DSW *p_shape_A, const Transform2D &p swap = true; } - if (type_A == PhysicsServer2D::SHAPE_LINE) { - if (type_B == PhysicsServer2D::SHAPE_LINE || type_B == PhysicsServer2D::SHAPE_RAY) { + if (type_A == PhysicsServer2D::SHAPE_WORLD_MARGIN) { + if (type_B == PhysicsServer2D::SHAPE_WORLD_MARGIN) { return false; } if (swap) { - return solve_static_line(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true); + return solve_static_world_margin(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true); } else { - return solve_static_line(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false); + return solve_static_world_margin(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false); } - } else if (type_A == PhysicsServer2D::SHAPE_RAY) { - if (type_B == PhysicsServer2D::SHAPE_RAY) { + } else if (type_A == PhysicsServer2D::SHAPE_SEPARATION_RAY) { + if (type_B == PhysicsServer2D::SHAPE_SEPARATION_RAY) { return false; //no ray-ray } if (swap) { - 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); + return solve_separation_ray(p_shape_B, p_motion_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true, r_sep_axis, p_margin_B); } else { - 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); + return solve_separation_ray(p_shape_A, p_motion_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false, r_sep_axis, p_margin_A); } } else if (concave_B) { @@ -237,12 +253,12 @@ bool CollisionSolver2DSW::solve(const Shape2DSW *p_shape_A, const Transform2D &p } if (!swap) { - return solve_concave(p_shape_A, p_transform_A, p_motion_A, p_shape_B, p_transform_B, p_motion_B, p_result_callback, p_userdata, false, sep_axis, margin_A, margin_B); + return solve_concave(p_shape_A, p_transform_A, p_motion_A, p_shape_B, p_transform_B, p_motion_B, p_result_callback, p_userdata, false, r_sep_axis, margin_A, margin_B); } else { - return solve_concave(p_shape_B, p_transform_B, p_motion_B, p_shape_A, p_transform_A, p_motion_A, p_result_callback, p_userdata, true, sep_axis, margin_A, margin_B); + return solve_concave(p_shape_B, p_transform_B, p_motion_B, p_shape_A, p_transform_A, p_motion_A, p_result_callback, p_userdata, true, r_sep_axis, margin_A, margin_B); } } else { - return collision_solver(p_shape_A, p_transform_A, p_motion_A, p_shape_B, p_transform_B, p_motion_B, p_result_callback, p_userdata, false, sep_axis, margin_A, margin_B); + return collision_solver(p_shape_A, p_transform_A, p_motion_A, p_shape_B, p_transform_B, p_motion_B, p_result_callback, p_userdata, false, r_sep_axis, margin_A, margin_B); } } diff --git a/servers/physics_2d/collision_solver_2d_sw.h b/servers/physics_2d/collision_solver_2d_sw.h index 4f12ca9e88..62fccc4ff3 100644 --- a/servers/physics_2d/collision_solver_2d_sw.h +++ b/servers/physics_2d/collision_solver_2d_sw.h @@ -38,13 +38,13 @@ public: typedef void (*CallbackResult)(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_userdata); 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 = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0); - 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 = nullptr); + static bool solve_static_world_margin(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 bool 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 *r_sep_axis = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0); + static bool solve_separation_ray(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 *r_sep_axis = nullptr, real_t p_margin = 0); 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 = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0); + 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 *r_sep_axis = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0); }; #endif // COLLISION_SOLVER_2D_SW_H diff --git a/servers/physics_2d/joints_2d_sw.cpp b/servers/physics_2d/joints_2d_sw.cpp index 5a0a628fbc..fa8499a81d 100644 --- a/servers/physics_2d/joints_2d_sw.cpp +++ b/servers/physics_2d/joints_2d_sw.cpp @@ -68,13 +68,13 @@ static inline real_t k_scalar(Body2DSW *a, Body2DSW *b, const Vector2 &rA, const { value += a->get_inv_mass(); - real_t rcn = rA.cross(n); + real_t rcn = (rA - a->get_center_of_mass()).cross(n); value += a->get_inv_inertia() * rcn * rcn; } if (b) { value += b->get_inv_mass(); - real_t rcn = rB.cross(n); + real_t rcn = (rB - b->get_center_of_mass()).cross(n); value += b->get_inv_inertia() * rcn * rcn; } @@ -83,9 +83,9 @@ static inline real_t k_scalar(Body2DSW *a, Body2DSW *b, const Vector2 &rA, const static inline Vector2 relative_velocity(Body2DSW *a, Body2DSW *b, Vector2 rA, Vector2 rB) { - Vector2 sum = a->get_linear_velocity() - rA.orthogonal() * a->get_angular_velocity(); + Vector2 sum = a->get_linear_velocity() - (rA - a->get_center_of_mass()).orthogonal() * a->get_angular_velocity(); if (b) { - return (b->get_linear_velocity() - rB.orthogonal() * b->get_angular_velocity()) - sum; + return (b->get_linear_velocity() - (rB - b->get_center_of_mass()).orthogonal() * b->get_angular_velocity()) - sum; } else { return -sum; } @@ -172,11 +172,11 @@ bool PinJoint2DSW::pre_solve(real_t p_step) { void PinJoint2DSW::solve(real_t p_step) { // compute relative velocity - Vector2 vA = A->get_linear_velocity() - custom_cross(rA, A->get_angular_velocity()); + Vector2 vA = A->get_linear_velocity() - custom_cross(rA - A->get_center_of_mass(), A->get_angular_velocity()); Vector2 rel_vel; if (B) { - rel_vel = B->get_linear_velocity() - custom_cross(rB, B->get_angular_velocity()) - vA; + rel_vel = B->get_linear_velocity() - custom_cross(rB - B->get_center_of_mass(), B->get_angular_velocity()) - vA; } else { rel_vel = -vA; } @@ -238,6 +238,9 @@ k_tensor(Body2DSW *a, Body2DSW *b, Vector2 r1, Vector2 r2, Vector2 *k1, Vector2 k21 = 0.0f; k22 = m_sum; + r1 -= a->get_center_of_mass(); + r2 -= b->get_center_of_mass(); + // add the influence from r1 real_t a_i_inv = a->get_inv_inertia(); real_t r1xsq = r1.x * r1.x * a_i_inv; diff --git a/servers/physics_2d/physics_server_2d_sw.cpp b/servers/physics_2d/physics_server_2d_sw.cpp index 467c664302..d0a42ca95b 100644 --- a/servers/physics_2d/physics_server_2d_sw.cpp +++ b/servers/physics_2d/physics_server_2d_sw.cpp @@ -30,6 +30,7 @@ #include "physics_server_2d_sw.h" +#include "body_direct_state_2d_sw.h" #include "broad_phase_2d_bvh.h" #include "collision_solver_2d_sw.h" #include "core/config/project_settings.h" @@ -42,11 +43,11 @@ RID PhysicsServer2DSW::_shape_create(ShapeType p_shape) { Shape2DSW *shape = nullptr; switch (p_shape) { - case SHAPE_LINE: { - shape = memnew(LineShape2DSW); + case SHAPE_WORLD_MARGIN: { + shape = memnew(WorldMarginShape2DSW); } break; - case SHAPE_RAY: { - shape = memnew(RayShape2DSW); + case SHAPE_SEPARATION_RAY: { + shape = memnew(SeparationRayShape2DSW); } break; case SHAPE_SEGMENT: { shape = memnew(SegmentShape2DSW); @@ -78,12 +79,12 @@ RID PhysicsServer2DSW::_shape_create(ShapeType p_shape) { return id; } -RID PhysicsServer2DSW::line_shape_create() { - return _shape_create(SHAPE_LINE); +RID PhysicsServer2DSW::world_margin_shape_create() { + return _shape_create(SHAPE_WORLD_MARGIN); } -RID PhysicsServer2DSW::ray_shape_create() { - return _shape_create(SHAPE_RAY); +RID PhysicsServer2DSW::separation_ray_shape_create() { + return _shape_create(SHAPE_SEPARATION_RAY); } RID PhysicsServer2DSW::segment_shape_create() { @@ -742,20 +743,27 @@ uint32_t PhysicsServer2DSW::body_get_collision_mask(RID p_body) const { return body->get_collision_mask(); }; -void PhysicsServer2DSW::body_set_param(RID p_body, BodyParameter p_param, real_t p_value) { +void PhysicsServer2DSW::body_set_param(RID p_body, BodyParameter p_param, const Variant &p_value) { Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); body->set_param(p_param, p_value); }; -real_t PhysicsServer2DSW::body_get_param(RID p_body, BodyParameter p_param) const { +Variant PhysicsServer2DSW::body_get_param(RID p_body, BodyParameter p_param) const { Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, 0); return body->get_param(p_param); }; +void PhysicsServer2DSW::body_reset_mass_properties(RID p_body) { + Body2DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND(!body); + + return body->reset_mass_properties(); +} + void PhysicsServer2DSW::body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) { Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -926,6 +934,12 @@ int PhysicsServer2DSW::body_get_max_contacts_reported(RID p_body) const { return body->get_max_contacts_reported(); } +void PhysicsServer2DSW::body_set_state_sync_callback(RID p_body, void *p_instance, BodyStateCallback p_callback) { + Body2DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND(!body); + body->set_state_sync_callback(p_instance, p_callback); +} + void PhysicsServer2DSW::body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata) { Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -946,7 +960,7 @@ void PhysicsServer2DSW::body_set_pickable(RID p_body, bool p_pickable) { body->set_pickable(p_pickable); } -bool PhysicsServer2DSW::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, const Set<RID> &p_exclude) { +bool PhysicsServer2DSW::body_test_motion(RID p_body, const Transform2D &p_from, const Vector2 &p_motion, real_t p_margin, MotionResult *r_result, bool p_collide_separation_ray, const Set<RID> &p_exclude) { Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, false); ERR_FAIL_COND_V(!body->get_space(), false); @@ -954,32 +968,19 @@ bool PhysicsServer2DSW::body_test_motion(RID p_body, const Transform2D &p_from, _update_shapes(); - return body->get_space()->test_body_motion(body, p_from, p_motion, p_infinite_inertia, p_margin, r_result, p_exclude_raycast_shapes, p_exclude); -} - -int PhysicsServer2DSW::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, real_t p_margin) { - Body2DSW *body = body_owner.getornull(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); + return body->get_space()->test_body_motion(body, p_from, p_motion, p_margin, r_result, p_collide_separation_ray, p_exclude); } PhysicsDirectBodyState2D *PhysicsServer2DSW::body_get_direct_state(RID p_body) { ERR_FAIL_COND_V_MSG((using_threads && !doing_sync), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification."); - if (!body_owner.owns(p_body)) { - return nullptr; - } - Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, nullptr); + ERR_FAIL_COND_V(!body->get_space(), nullptr); ERR_FAIL_COND_V_MSG(body->get_space()->is_locked(), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification."); - direct_state->body = body; - return direct_state; + return body->get_direct_state(); } /* JOINT API */ @@ -1243,10 +1244,8 @@ void PhysicsServer2DSW::set_collision_iterations(int p_iterations) { void PhysicsServer2DSW::init() { doing_sync = false; - last_step = 0.001; iterations = 8; // 8? stepper = memnew(Step2DSW); - direct_state = memnew(PhysicsDirectBodyState2DSW); }; void PhysicsServer2DSW::step(real_t p_step) { @@ -1256,8 +1255,6 @@ void PhysicsServer2DSW::step(real_t p_step) { _update_shapes(); - last_step = p_step; - PhysicsDirectBodyState2DSW::singleton->step = p_step; island_count = 0; active_objects = 0; collision_pairs = 0; @@ -1329,7 +1326,6 @@ void PhysicsServer2DSW::end_sync() { void PhysicsServer2DSW::finish() { memdelete(stepper); - memdelete(direct_state); }; void PhysicsServer2DSW::_update_shapes() { diff --git a/servers/physics_2d/physics_server_2d_sw.h b/servers/physics_2d/physics_server_2d_sw.h index f40b5a7c42..6a2d9e37e0 100644 --- a/servers/physics_2d/physics_server_2d_sw.h +++ b/servers/physics_2d/physics_server_2d_sw.h @@ -46,7 +46,6 @@ class PhysicsServer2DSW : public PhysicsServer2D { bool active; int iterations; bool doing_sync; - real_t last_step; int island_count; int active_objects; @@ -59,8 +58,6 @@ class PhysicsServer2DSW : public PhysicsServer2D { Step2DSW *stepper; Set<const Space2DSW *> active_spaces; - PhysicsDirectBodyState2DSW *direct_state; - mutable RID_PtrOwner<Shape2DSW, true> shape_owner; mutable RID_PtrOwner<Space2DSW, true> space_owner; mutable RID_PtrOwner<Area2DSW, true> area_owner; @@ -87,8 +84,8 @@ public: Vector2 *ptr; }; - virtual RID line_shape_create() override; - virtual RID ray_shape_create() override; + virtual RID world_margin_shape_create() override; + virtual RID separation_ray_shape_create() override; virtual RID segment_shape_create() override; virtual RID circle_shape_create() override; virtual RID rectangle_shape_create() override; @@ -208,8 +205,10 @@ public: virtual void body_set_collision_mask(RID p_body, uint32_t p_mask) override; virtual uint32_t body_get_collision_mask(RID p_body) const override; - virtual void body_set_param(RID p_body, BodyParameter p_param, real_t p_value) override; - virtual real_t body_get_param(RID p_body, BodyParameter p_param) const override; + virtual void body_set_param(RID p_body, BodyParameter p_param, const Variant &p_value) override; + virtual Variant body_get_param(RID p_body, BodyParameter p_param) const override; + + virtual void body_reset_mass_properties(RID p_body) override; virtual void body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) override; virtual Variant body_get_state(RID p_body, BodyState p_state) const override; @@ -242,13 +241,14 @@ public: virtual void body_set_max_contacts_reported(RID p_body, int p_contacts) override; virtual int body_get_max_contacts_reported(RID p_body) const override; + virtual void body_set_state_sync_callback(RID p_body, void *p_instance, BodyStateCallback p_callback) override; virtual void body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata = Variant()) override; + virtual bool body_collide_shape(RID p_body, int p_body_shape, RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, Vector2 *r_results, int p_result_max, int &r_result_count) override; virtual void body_set_pickable(RID p_body, bool p_pickable) override; - 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.08, MotionResult *r_result = nullptr, bool p_exclude_raycast_shapes = true, const Set<RID> &p_exclude = Set<RID>()) override; - 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, real_t p_margin = 0.08) override; + virtual bool body_test_motion(RID p_body, const Transform2D &p_from, const Vector2 &p_motion, real_t p_margin = 0.08, MotionResult *r_result = nullptr, bool p_collide_separation_ray = false, const Set<RID> &p_exclude = Set<RID>()) override; // this function only works on physics process, errors and returns null otherwise virtual PhysicsDirectBodyState2D *body_get_direct_state(RID p_body) override; diff --git a/servers/physics_2d/physics_server_2d_wrap_mt.h b/servers/physics_2d/physics_server_2d_wrap_mt.h index fb7235c031..e65c4f5f3a 100644 --- a/servers/physics_2d/physics_server_2d_wrap_mt.h +++ b/servers/physics_2d/physics_server_2d_wrap_mt.h @@ -79,8 +79,8 @@ public: #include "servers/server_wrap_mt_common.h" //FUNC1RID(shape,ShapeType); todo fix - FUNCRID(line_shape) - FUNCRID(ray_shape) + FUNCRID(world_margin_shape) + FUNCRID(separation_ray_shape) FUNCRID(segment_shape) FUNCRID(circle_shape) FUNCRID(rectangle_shape) @@ -212,8 +212,10 @@ public: FUNC2(body_set_collision_mask, RID, uint32_t); FUNC1RC(uint32_t, body_get_collision_mask, RID); - FUNC3(body_set_param, RID, BodyParameter, real_t); - FUNC2RC(real_t, body_get_param, RID, BodyParameter); + FUNC3(body_set_param, RID, BodyParameter, const Variant &); + FUNC2RC(Variant, body_get_param, RID, BodyParameter); + + FUNC1(body_reset_mass_properties, RID); FUNC3(body_set_state, RID, BodyState, const Variant &); FUNC2RC(Variant, body_get_state, RID, BodyState); @@ -245,6 +247,7 @@ public: FUNC2(body_set_omit_force_integration, RID, bool); FUNC1RC(bool, body_is_omitting_force_integration, RID); + FUNC3(body_set_state_sync_callback, RID, void *, BodyStateCallback); FUNC3(body_set_force_integration_callback, RID, const Callable &, const Variant &); bool body_collide_shape(RID p_body, int p_body_shape, RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, Vector2 *r_results, int p_result_max, int &r_result_count) override { @@ -253,14 +256,9 @@ 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.08, MotionResult *r_result = nullptr, bool p_exclude_raycast_shapes = true, const Set<RID> &p_exclude = Set<RID>()) override { - 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, p_exclude_raycast_shapes, p_exclude); - } - - 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, real_t p_margin = 0.08) override { + bool body_test_motion(RID p_body, const Transform2D &p_from, const Vector2 &p_motion, real_t p_margin = 0.08, MotionResult *r_result = nullptr, bool p_collide_separation_ray = false, const Set<RID> &p_exclude = Set<RID>()) override { 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); + return physics_2d_server->body_test_motion(p_body, p_from, p_motion, p_margin, r_result, p_collide_separation_ray, p_exclude); } // this function only works on physics process, errors and returns null otherwise diff --git a/servers/physics_2d/shape_2d_sw.cpp b/servers/physics_2d/shape_2d_sw.cpp index 6cc086b9b7..064c4afe52 100644 --- a/servers/physics_2d/shape_2d_sw.cpp +++ b/servers/physics_2d/shape_2d_sw.cpp @@ -88,15 +88,15 @@ Shape2DSW::~Shape2DSW() { /*********************************************************/ /*********************************************************/ -void LineShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { +void WorldMarginShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { r_amount = 0; } -bool LineShape2DSW::contains_point(const Vector2 &p_point) const { +bool WorldMarginShape2DSW::contains_point(const Vector2 &p_point) const { return normal.dot(p_point) < d; } -bool LineShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { +bool WorldMarginShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { Vector2 segment = p_begin - p_end; real_t den = normal.dot(segment); @@ -118,11 +118,11 @@ bool LineShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_e return true; } -real_t LineShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { +real_t WorldMarginShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { return 0; } -void LineShape2DSW::set_data(const Variant &p_data) { +void WorldMarginShape2DSW::set_data(const Variant &p_data) { ERR_FAIL_COND(p_data.get_type() != Variant::ARRAY); Array arr = p_data; ERR_FAIL_COND(arr.size() != 2); @@ -131,7 +131,7 @@ void LineShape2DSW::set_data(const Variant &p_data) { configure(Rect2(Vector2(-1e4, -1e4), Vector2(1e4 * 2, 1e4 * 2))); } -Variant LineShape2DSW::get_data() const { +Variant WorldMarginShape2DSW::get_data() const { Array arr; arr.resize(2); arr[0] = normal; @@ -143,7 +143,7 @@ Variant LineShape2DSW::get_data() const { /*********************************************************/ /*********************************************************/ -void RayShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { +void SeparationRayShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { r_amount = 1; if (p_normal.y > 0) { @@ -153,29 +153,29 @@ void RayShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, in } } -bool RayShape2DSW::contains_point(const Vector2 &p_point) const { +bool SeparationRayShape2DSW::contains_point(const Vector2 &p_point) const { return false; } -bool RayShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { +bool SeparationRayShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { return false; //rays can't be intersected } -real_t RayShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { +real_t SeparationRayShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { return 0; //rays are mass-less } -void RayShape2DSW::set_data(const Variant &p_data) { +void SeparationRayShape2DSW::set_data(const Variant &p_data) { Dictionary d = p_data; length = d["length"]; - slips_on_slope = d["slips_on_slope"]; + slide_on_slope = d["slide_on_slope"]; configure(Rect2(0, 0, 0.001, length)); } -Variant RayShape2DSW::get_data() const { +Variant SeparationRayShape2DSW::get_data() const { Dictionary d; d["length"] = length; - d["slips_on_slope"] = slips_on_slope; + d["slide_on_slope"] = slide_on_slope; return d; } @@ -383,15 +383,15 @@ void CapsuleShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports r_amount = 2; r_supports[0] = n; - r_supports[0].y += height * 0.5; + r_supports[0].y += height * 0.5 - radius; r_supports[1] = n; - r_supports[1].y -= height * 0.5; + r_supports[1].y -= height * 0.5 - radius; } else { - real_t h = (d > 0) ? height : -height; + real_t h = height * 0.5 - radius; n *= radius; - n.y += h * 0.5; + n.y += (d > 0) ? h : -h; r_amount = 1; *r_supports = n; } @@ -400,7 +400,7 @@ void CapsuleShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports bool CapsuleShape2DSW::contains_point(const Vector2 &p_point) const { Vector2 p = p_point; p.y = Math::abs(p.y); - p.y -= height * 0.5; + p.y -= height * 0.5 - radius; if (p.y < 0) { p.y = 0; } @@ -417,7 +417,7 @@ bool CapsuleShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 & for (int i = 0; i < 2; i++) { Vector2 begin = p_begin; Vector2 end = p_end; - real_t ofs = (i == 0) ? -height * 0.5 : height * 0.5; + real_t ofs = (i == 0) ? -height * 0.5 + radius : height * 0.5 - radius; begin.y += ofs; end.y += ofs; @@ -454,7 +454,7 @@ bool CapsuleShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 & } Vector2 rpos, rnorm; - if (Rect2(Point2(-radius, -height * 0.5), Size2(radius * 2.0, height)).intersects_segment(p_begin, p_end, &rpos, &rnorm)) { + if (Rect2(Point2(-radius, -height * 0.5 + radius), Size2(radius * 2.0, height - radius * 2)).intersects_segment(p_begin, p_end, &rpos, &rnorm)) { real_t pd = n.dot(rpos); if (pd < d) { r_point = rpos; @@ -469,7 +469,7 @@ bool CapsuleShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 & } real_t CapsuleShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { - Vector2 he2 = Vector2(radius * 2, height + radius * 2) * p_scale; + Vector2 he2 = Vector2(radius * 2, height) * p_scale; return p_mass * he2.dot(he2) / 12.0; } @@ -487,7 +487,7 @@ void CapsuleShape2DSW::set_data(const Variant &p_data) { height = p.y; } - Point2 he(radius, height * 0.5 + radius); + Point2 he(radius, height * 0.5); configure(Rect2(-he, he * 2)); } @@ -570,14 +570,7 @@ bool ConvexPolygonShape2DSW::intersect_segment(const Vector2 &p_begin, const Vec } } - if (inters) { - if (n.dot(r_normal) > 0) { - r_normal = -r_normal; - } - } - - //return get_aabb().intersects_segment(p_begin,p_end,&r_point,&r_normal); - return inters; //todo + return inters; } real_t ConvexPolygonShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { @@ -928,7 +921,7 @@ Variant ConcavePolygonShape2DSW::get_data() const { return rsegments; } -void ConcavePolygonShape2DSW::cull(const Rect2 &p_local_aabb, Callback p_callback, void *p_userdata) const { +void ConcavePolygonShape2DSW::cull(const Rect2 &p_local_aabb, QueryCallback p_callback, void *p_userdata) const { uint32_t *stack = (uint32_t *)alloca(sizeof(int) * bvh_depth); enum { @@ -976,7 +969,9 @@ void ConcavePolygonShape2DSW::cull(const Rect2 &p_local_aabb, Callback p_callbac SegmentShape2DSW ss(a, b, (b - a).orthogonal().normalized()); - p_callback(p_userdata, &ss); + if (p_callback(p_userdata, &ss)) { + return; + } stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; } else { diff --git a/servers/physics_2d/shape_2d_sw.h b/servers/physics_2d/shape_2d_sw.h index ee2730ebb5..1185d343ee 100644 --- a/servers/physics_2d/shape_2d_sw.h +++ b/servers/physics_2d/shape_2d_sw.h @@ -34,18 +34,6 @@ #include "servers/physics_server_2d.h" #define _SEGMENT_IS_VALID_SUPPORT_THRESHOLD 0.99998 -/* - -SHAPE_LINE, ///< plane:"plane" -SHAPE_SEGMENT, ///< real_t:"length" -SHAPE_CIRCLE, ///< real_t:"radius" -SHAPE_RECTANGLE, ///< vec3:"extents" -SHAPE_CONVEX_POLYGON, ///< array of planes:"planes" -SHAPE_CONCAVE_POLYGON, ///< Vector2 array:"triangles" , or Dictionary with "indices" (int array) and "triangles" (Vector2 array) -SHAPE_CUSTOM, ///< Server-Implementation based custom shape, calling shape_create() with this value will result in an error - -*/ - class Shape2DSW; class ShapeOwner2DSW { @@ -76,6 +64,8 @@ public: _FORCE_INLINE_ Rect2 get_aabb() const { return aabb; } _FORCE_INLINE_ bool is_configured() const { return configured; } + virtual bool allows_one_way_collision() const { return true; } + virtual bool is_concave() const { return false; } virtual bool contains_point(const Vector2 &p_point) const = 0; @@ -137,22 +127,22 @@ public: }; //let the optimizer do the magic -#define DEFAULT_PROJECT_RANGE_CAST \ - virtual void project_range_castv(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { \ - project_range_cast(p_cast, p_normal, p_transform, r_min, r_max); \ - } \ - _FORCE_INLINE_ void project_range_cast(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { \ - real_t mina, maxa; \ - real_t minb, maxb; \ - Transform2D ofsb = p_transform; \ - ofsb.elements[2] += p_cast; \ - project_range(p_normal, p_transform, mina, maxa); \ - project_range(p_normal, ofsb, minb, maxb); \ - r_min = MIN(mina, minb); \ - r_max = MAX(maxa, maxb); \ +#define DEFAULT_PROJECT_RANGE_CAST \ + virtual void project_range_castv(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { \ + project_range_cast(p_cast, p_normal, p_transform, r_min, r_max); \ + } \ + _FORCE_INLINE_ void project_range_cast(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { \ + real_t mina, maxa; \ + real_t minb, maxb; \ + Transform2D ofsb = p_transform; \ + ofsb.elements[2] += p_cast; \ + project_range(p_normal, p_transform, mina, maxa); \ + project_range(p_normal, ofsb, minb, maxb); \ + r_min = MIN(mina, minb); \ + r_max = MAX(maxa, maxb); \ } -class LineShape2DSW : public Shape2DSW { +class WorldMarginShape2DSW : public Shape2DSW { Vector2 normal; real_t d; @@ -160,17 +150,17 @@ public: _FORCE_INLINE_ Vector2 get_normal() const { return normal; } _FORCE_INLINE_ real_t get_d() const { return d; } - virtual PhysicsServer2D::ShapeType get_type() const { return PhysicsServer2D::SHAPE_LINE; } + virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_WORLD_MARGIN; } - virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal, p_transform, r_min, r_max); } - virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const; + virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); } + virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; - virtual bool contains_point(const Vector2 &p_point) const; - virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const; - virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const; + virtual bool contains_point(const Vector2 &p_point) const override; + virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; + virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; - virtual void set_data(const Variant &p_data); - virtual Variant get_data() const; + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { //real large @@ -178,7 +168,7 @@ public: r_max = 1e10; } - virtual void project_range_castv(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { + virtual void project_range_castv(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range_cast(p_cast, p_normal, p_transform, r_min, r_max); } @@ -189,25 +179,27 @@ public: } }; -class RayShape2DSW : public Shape2DSW { +class SeparationRayShape2DSW : public Shape2DSW { real_t length; - bool slips_on_slope; + bool slide_on_slope; public: _FORCE_INLINE_ real_t get_length() const { return length; } - _FORCE_INLINE_ bool get_slips_on_slope() const { return slips_on_slope; } + _FORCE_INLINE_ bool get_slide_on_slope() const { return slide_on_slope; } - virtual PhysicsServer2D::ShapeType get_type() const { return PhysicsServer2D::SHAPE_RAY; } + virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_SEPARATION_RAY; } - virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal, p_transform, r_min, r_max); } - virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const; + virtual bool allows_one_way_collision() const override { return false; } - virtual bool contains_point(const Vector2 &p_point) const; - virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const; - virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const; + virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); } + virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; - virtual void set_data(const Variant &p_data); - virtual Variant get_data() const; + virtual bool contains_point(const Vector2 &p_point) const override; + virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; + virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; + + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { //real large @@ -220,8 +212,8 @@ public: DEFAULT_PROJECT_RANGE_CAST - _FORCE_INLINE_ RayShape2DSW() {} - _FORCE_INLINE_ RayShape2DSW(real_t p_length) { length = p_length; } + _FORCE_INLINE_ SeparationRayShape2DSW() {} + _FORCE_INLINE_ SeparationRayShape2DSW(real_t p_length) { length = p_length; } }; class SegmentShape2DSW : public Shape2DSW { @@ -234,20 +226,20 @@ public: _FORCE_INLINE_ const Vector2 &get_b() const { return b; } _FORCE_INLINE_ const Vector2 &get_normal() const { return n; } - virtual PhysicsServer2D::ShapeType get_type() const { return PhysicsServer2D::SHAPE_SEGMENT; } + virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_SEGMENT; } _FORCE_INLINE_ Vector2 get_xformed_normal(const Transform2D &p_xform) const { return (p_xform.xform(b) - p_xform.xform(a)).normalized().orthogonal(); } - virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal, p_transform, r_min, r_max); } - virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const; + virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); } + virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; - virtual bool contains_point(const Vector2 &p_point) const; - virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const; - virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const; + virtual bool contains_point(const Vector2 &p_point) const override; + virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; + virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; - virtual void set_data(const Variant &p_data); - virtual Variant get_data() const; + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { //real large @@ -274,17 +266,17 @@ class CircleShape2DSW : public Shape2DSW { public: _FORCE_INLINE_ const real_t &get_radius() const { return radius; } - virtual PhysicsServer2D::ShapeType get_type() const { return PhysicsServer2D::SHAPE_CIRCLE; } + virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_CIRCLE; } - virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal, p_transform, r_min, r_max); } - virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const; + virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); } + virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; - virtual bool contains_point(const Vector2 &p_point) const; - virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const; - virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const; + virtual bool contains_point(const Vector2 &p_point) const override; + virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; + virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; - virtual void set_data(const Variant &p_data); - virtual Variant get_data() const; + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { //real large @@ -307,17 +299,17 @@ class RectangleShape2DSW : public Shape2DSW { public: _FORCE_INLINE_ const Vector2 &get_half_extents() const { return half_extents; } - virtual PhysicsServer2D::ShapeType get_type() const { return PhysicsServer2D::SHAPE_RECTANGLE; } + virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_RECTANGLE; } - virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal, p_transform, r_min, r_max); } - virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const; + virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); } + virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; - virtual bool contains_point(const Vector2 &p_point) const; - virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const; - virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const; + virtual bool contains_point(const Vector2 &p_point) const override; + virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; + virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; - virtual void set_data(const Variant &p_data); - virtual Variant get_data() const; + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { // no matter the angle, the box is mirrored anyway @@ -381,25 +373,25 @@ public: _FORCE_INLINE_ const real_t &get_radius() const { return radius; } _FORCE_INLINE_ const real_t &get_height() const { return height; } - virtual PhysicsServer2D::ShapeType get_type() const { return PhysicsServer2D::SHAPE_CAPSULE; } + virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_CAPSULE; } - virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal, p_transform, r_min, r_max); } - virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const; + virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); } + virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; - virtual bool contains_point(const Vector2 &p_point) const; - virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const; - virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const; + virtual bool contains_point(const Vector2 &p_point) const override; + virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; + virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; - virtual void set_data(const Variant &p_data); - virtual Variant get_data() const; + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { // no matter the angle, the box is mirrored anyway Vector2 n = p_transform.basis_xform_inv(p_normal).normalized(); - real_t h = (n.y > 0) ? height : -height; + real_t h = height * 0.5 - radius; n *= radius; - n.y += h * 0.5; + n.y += (n.y > 0) ? h : -h; r_max = p_normal.dot(p_transform.xform(n)); r_min = p_normal.dot(p_transform.xform(-n)); @@ -434,17 +426,17 @@ public: return (p_xform.xform(b) - p_xform.xform(a)).normalized().orthogonal(); } - virtual PhysicsServer2D::ShapeType get_type() const { return PhysicsServer2D::SHAPE_CONVEX_POLYGON; } + virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_CONVEX_POLYGON; } - virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal, p_transform, r_min, r_max); } - virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const; + virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); } + virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; - virtual bool contains_point(const Vector2 &p_point) const; - virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const; - virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const; + virtual bool contains_point(const Vector2 &p_point) const override; + virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; + virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; - virtual void set_data(const Variant &p_data); - virtual Variant get_data() const; + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { if (!points || point_count <= 0) { @@ -472,10 +464,12 @@ public: class ConcaveShape2DSW : public Shape2DSW { public: - virtual bool is_concave() const { return true; } - typedef void (*Callback)(void *p_userdata, Shape2DSW *p_convex); + virtual bool is_concave() const override { return true; } + + // Returns true to stop the query. + typedef bool (*QueryCallback)(void *p_userdata, Shape2DSW *p_convex); - virtual void cull(const Rect2 &p_local_aabb, Callback p_callback, void *p_userdata) const = 0; + virtual void cull(const Rect2 &p_local_aabb, QueryCallback p_callback, void *p_userdata) const = 0; }; class ConcavePolygonShape2DSW : public ConcaveShape2DSW { @@ -509,23 +503,31 @@ class ConcavePolygonShape2DSW : public ConcaveShape2DSW { int _generate_bvh(BVH *p_bvh, int p_len, int p_depth); public: - virtual PhysicsServer2D::ShapeType get_type() const { return PhysicsServer2D::SHAPE_CONCAVE_POLYGON; } + virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_CONCAVE_POLYGON; } - virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { /*project_range(p_normal,p_transform,r_min,r_max);*/ + virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { + r_min = 0; + r_max = 0; + ERR_FAIL_MSG("Unsupported call to project_rangev in ConcavePolygonShape2DSW"); } - virtual void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { /*project_range(p_normal,p_transform,r_min,r_max);*/ + + void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { + r_min = 0; + r_max = 0; + ERR_FAIL_MSG("Unsupported call to project_range in ConcavePolygonShape2DSW"); } - virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const; - virtual bool contains_point(const Vector2 &p_point) const; - virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const; + virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; + + virtual bool contains_point(const Vector2 &p_point) const override; + virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; - virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { return 0; } + virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override { return 0; } - virtual void set_data(const Variant &p_data); - virtual Variant get_data() const; + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; - virtual void cull(const Rect2 &p_local_aabb, Callback p_callback, void *p_userdata) const; + virtual void cull(const Rect2 &p_local_aabb, QueryCallback p_callback, void *p_userdata) const override; DEFAULT_PROJECT_RANGE_CAST }; diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp index 43329fed2f..7dbd1243cc 100644 --- a/servers/physics_2d/space_2d_sw.cpp +++ b/servers/physics_2d/space_2d_sw.cpp @@ -482,7 +482,7 @@ bool PhysicsDirectSpaceState2DSW::rest_info(RID p_shape, const Transform2D &p_sh r_info->metadata = rcd.best_object->get_shape_metadata(rcd.best_shape); if (rcd.best_object->get_type() == CollisionObject2DSW::TYPE_BODY) { const Body2DSW *body = static_cast<const Body2DSW *>(rcd.best_object); - Vector2 rel_vec = r_info->point - body->get_transform().get_origin(); + Vector2 rel_vec = r_info->point - (body->get_transform().get_origin() + body->get_center_of_mass()); r_info->linear_velocity = Vector2(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity(); } else { @@ -508,7 +508,7 @@ int Space2DSW::_cull_aabb_for_body(Body2DSW *p_body, const Rect2 &p_aabb) { keep = false; } else if (intersection_query_results[i]->get_type() == CollisionObject2DSW::TYPE_AREA) { keep = false; - } else if (!p_body->layer_in_mask(static_cast<Body2DSW *>(intersection_query_results[i]))) { + } else if (!p_body->collides_with(static_cast<Body2DSW *>(intersection_query_results[i]))) { keep = false; } else if (static_cast<Body2DSW *>(intersection_query_results[i])->has_exception(p_body->get_self()) || p_body->has_exception(intersection_query_results[i]->get_self())) { keep = false; @@ -528,188 +528,7 @@ int Space2DSW::_cull_aabb_for_body(Body2DSW *p_body, const Rect2 &p_aabb) { return amount; } -int Space2DSW::test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_transform, bool p_infinite_inertia, Vector2 &r_recover_motion, PhysicsServer2D::SeparationResult *r_results, int p_result_max, real_t p_margin) { - Rect2 body_aabb; - - bool shapes_found = false; - - for (int i = 0; i < p_body->get_shape_count(); i++) { - if (p_body->is_shape_disabled(i)) { - continue; - } - - if (p_body->get_shape(i)->get_type() != PhysicsServer2D::SHAPE_RAY) { - continue; - } - - if (!shapes_found) { - body_aabb = p_body->get_shape_aabb(i); - shapes_found = true; - } else { - body_aabb = body_aabb.merge(p_body->get_shape_aabb(i)); - } - } - - if (!shapes_found) { - return 0; - } - - // 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]; - PhysicsServer2DSW::CollCbkData cbk; - cbk.max = max_results; - PhysicsServer2DSW::CollCbkData *cbkptr = &cbk; - CollisionSolver2DSW::CallbackResult cbkres = PhysicsServer2DSW::_shape_col_cbk; - - do { - Vector2 recover_motion; - - bool collided = false; - - 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_disabled(j)) { - continue; - } - - Shape2DSW *body_shape = p_body->get_shape(j); - - if (body_shape->get_type() != PhysicsServer2D::SHAPE_RAY) { - continue; - } - - Transform2D body_shape_xform = body_transform * p_body->get_shape_transform(j); - - for (int i = 0; i < amount; i++) { - const CollisionObject2DSW *col_obj = intersection_query_results[i]; - int shape_idx = intersection_query_subindex_results[i]; - - cbk.amount = 0; - cbk.passed = 0; - cbk.ptr = sr; - cbk.invalid_by_dir = 0; - - if (CollisionObject2DSW::TYPE_BODY == col_obj->get_type()) { - const Body2DSW *b = static_cast<const Body2DSW *>(col_obj); - if (p_infinite_inertia && PhysicsServer2D::BODY_MODE_STATIC != b->get_mode() && PhysicsServer2D::BODY_MODE_KINEMATIC != b->get_mode()) { - continue; - } - } - - Transform2D col_obj_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); - - /* - * There is no point in supporting one way collisions with ray shapes, as they will always collide in the desired - * direction. Use a short ray shape if you want to achieve a similar effect. - * - if (col_obj->is_shape_set_as_one_way_collision(shape_idx)) { - cbk.valid_dir = col_obj_shape_xform.get_axis(1).normalized(); - cbk.valid_depth = p_margin; //only valid depth is the collision margin - cbk.invalid_by_dir = 0; - - } else { -*/ - - 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, nullptr, 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) { - PhysicsServer2D::SeparationResult &result = r_results[ray_index]; - - for (int k = 0; k < cbk.amount; k++) { - Vector2 a = sr[k * 2 + 0]; - Vector2 b = sr[k * 2 + 1]; - - recover_motion += (b - a) / cbk.amount; - - real_t depth = a.distance_to(b); - if (depth > result.collision_depth) { - result.collision_depth = depth; - result.collision_point = b; - result.collision_normal = (b - a).normalized(); - result.collision_local_shape = j; - result.collider_shape = shape_idx; - result.collider = col_obj->get_self(); - result.collider_id = col_obj->get_instance_id(); - result.collider_metadata = col_obj->get_shape_metadata(shape_idx); - if (col_obj->get_type() == CollisionObject2DSW::TYPE_BODY) { - Body2DSW *body = (Body2DSW *)col_obj; - - Vector2 rel_vec = b - body->get_transform().get_origin(); - result.collider_velocity = Vector2(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity(); - } - } - } - } - } - } - } - - 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, PhysicsServer2D::MotionResult *r_result, bool p_exclude_raycast_shapes, const Set<RID> &p_exclude) { +bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, const Vector2 &p_motion, real_t p_margin, PhysicsServer2D::MotionResult *r_result, bool p_collide_separation_ray, const Set<RID> &p_exclude) { //give me back regular physics engine logic //this is madness //and most people using this function will think @@ -730,10 +549,6 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co continue; } - if (p_exclude_raycast_shapes && p_body->get_shape(i)->get_type() == PhysicsServer2D::SHAPE_RAY) { - continue; - } - if (!shapes_found) { body_aabb = p_body->get_shape_aabb(i); shapes_found = true; @@ -745,7 +560,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co if (!shapes_found) { if (r_result) { *r_result = PhysicsServer2D::MotionResult(); - r_result->motion = p_motion; + r_result->travel = p_motion; } return false; } @@ -794,28 +609,19 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co } Shape2DSW *body_shape = p_body->get_shape(j); - if (p_exclude_raycast_shapes && body_shape->get_type() == PhysicsServer2D::SHAPE_RAY) { - continue; - } - Transform2D body_shape_xform = body_transform * p_body->get_shape_transform(j); + for (int i = 0; i < amount; i++) { const CollisionObject2DSW *col_obj = intersection_query_results[i]; if (p_exclude.has(col_obj->get_self())) { continue; } - int shape_idx = intersection_query_subindex_results[i]; - if (CollisionObject2DSW::TYPE_BODY == col_obj->get_type()) { - const Body2DSW *b = static_cast<const Body2DSW *>(col_obj); - if (p_infinite_inertia && PhysicsServer2D::BODY_MODE_STATIC != b->get_mode() && PhysicsServer2D::BODY_MODE_KINEMATIC != b->get_mode()) { - continue; - } - } + int shape_idx = intersection_query_subindex_results[i]; 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 (body_shape->allows_one_way_collision() && col_obj->is_shape_set_as_one_way_collision(shape_idx)) { cbk.valid_dir = col_obj_shape_xform.get_axis(1).normalized(); real_t owc_margin = col_obj->get_shape_one_way_collision_margin(shape_idx); @@ -828,7 +634,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co //fix for moving platforms (kinematic and dynamic), margin is increased by how much it moved in the given direction Vector2 lv = b->get_linear_velocity(); //compute displacement from linear velocity - Vector2 motion = lv * PhysicsDirectBodyState2DSW::singleton->step; + Vector2 motion = lv * last_step; real_t motion_len = motion.length(); motion.normalize(); cbk.valid_depth += motion_len * MAX(motion.dot(-cbk.valid_dir), 0.0); @@ -920,8 +726,14 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co } Shape2DSW *body_shape = p_body->get_shape(body_shape_idx); - if (p_exclude_raycast_shapes && body_shape->get_type() == PhysicsServer2D::SHAPE_RAY) { - continue; + + // Colliding separation rays allows to properly snap to the ground, + // otherwise it's not needed in regular motion. + if (!p_collide_separation_ray && (body_shape->get_type() == PhysicsServer2D::SHAPE_SEPARATION_RAY)) { + // When slide on slope is on, separation ray shape acts like a regular shape. + if (!static_cast<SeparationRayShape2DSW *>(body_shape)->get_slide_on_slope()) { + continue; + } } Transform2D body_shape_xform = body_transform * p_body->get_shape_transform(body_shape_idx); @@ -939,13 +751,6 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co int col_shape_idx = intersection_query_subindex_results[i]; Shape2DSW *against_shape = col_obj->get_shape(col_shape_idx); - if (CollisionObject2DSW::TYPE_BODY == col_obj->get_type()) { - const Body2DSW *b = static_cast<const Body2DSW *>(col_obj); - if (p_infinite_inertia && PhysicsServer2D::BODY_MODE_STATIC != b->get_mode() && PhysicsServer2D::BODY_MODE_KINEMATIC != b->get_mode()) { - continue; - } - } - bool excluded = false; for (int k = 0; k < excluded_shape_pair_count; k++) { @@ -967,7 +772,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co //test initial overlap if (CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), nullptr, nullptr, nullptr, 0)) { - if (col_obj->is_shape_set_as_one_way_collision(col_shape_idx)) { + if (body_shape->allows_one_way_collision() && col_obj->is_shape_set_as_one_way_collision(col_shape_idx)) { Vector2 direction = col_obj_shape_xform.get_axis(1).normalized(); if (motion_normal.dot(direction) < 0) { continue; @@ -1011,7 +816,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co } } - if (col_obj->is_shape_set_as_one_way_collision(col_shape_idx)) { + if (body_shape->allows_one_way_collision() && col_obj->is_shape_set_as_one_way_collision(col_shape_idx)) { Vector2 cd[2]; PhysicsServer2DSW::CollCbkData cbk; cbk.max = 1; @@ -1082,10 +887,6 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co Transform2D body_shape_xform = ugt * p_body->get_shape_transform(j); Shape2DSW *body_shape = p_body->get_shape(j); - if (p_exclude_raycast_shapes && body_shape->get_type() == PhysicsServer2D::SHAPE_RAY) { - continue; - } - body_aabb.position += p_motion * unsafe; int amount = _cull_aabb_for_body(p_body, body_aabb); @@ -1095,14 +896,8 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co if (p_exclude.has(col_obj->get_self())) { continue; } - int shape_idx = intersection_query_subindex_results[i]; - if (CollisionObject2DSW::TYPE_BODY == col_obj->get_type()) { - const Body2DSW *b = static_cast<const Body2DSW *>(col_obj); - if (p_infinite_inertia && PhysicsServer2D::BODY_MODE_STATIC != b->get_mode() && PhysicsServer2D::BODY_MODE_KINEMATIC != b->get_mode()) { - continue; - } - } + int shape_idx = intersection_query_subindex_results[i]; Shape2DSW *against_shape = col_obj->get_shape(shape_idx); @@ -1119,7 +914,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co 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 (body_shape->allows_one_way_collision() && col_obj->is_shape_set_as_one_way_collision(shape_idx)) { rcd.valid_dir = col_obj_shape_xform.get_axis(1).normalized(); real_t owc_margin = col_obj->get_shape_one_way_collision_margin(shape_idx); @@ -1131,7 +926,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co //fix for moving platforms (kinematic and dynamic), margin is increased by how much it moved in the given direction Vector2 lv = b->get_linear_velocity(); //compute displacement from linear velocity - Vector2 motion = lv * PhysicsDirectBodyState2DSW::singleton->step; + Vector2 motion = lv * last_step; real_t motion_len = motion.length(); motion.normalize(); rcd.valid_depth += motion_len * MAX(motion.dot(-rcd.valid_dir), 0.0); @@ -1166,12 +961,12 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co r_result->collider_metadata = rcd.best_object->get_shape_metadata(rcd.best_shape); const Body2DSW *body = static_cast<const Body2DSW *>(rcd.best_object); - Vector2 rel_vec = r_result->collision_point - body->get_transform().get_origin(); + Vector2 rel_vec = r_result->collision_point - (body->get_transform().get_origin() + body->get_center_of_mass()); r_result->collider_velocity = Vector2(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity(); - r_result->motion = safe * p_motion; + r_result->travel = safe * p_motion; r_result->remainder = p_motion - safe * p_motion; - r_result->motion += (body_transform.get_origin() - p_from.get_origin()); + r_result->travel += (body_transform.get_origin() - p_from.get_origin()); } collided = true; @@ -1179,9 +974,9 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co } if (!collided && r_result) { - r_result->motion = p_motion; + r_result->travel = p_motion; r_result->remainder = Vector2(); - r_result->motion += (body_transform.get_origin() - p_from.get_origin()); + r_result->travel += (body_transform.get_origin() - p_from.get_origin()); } return collided; @@ -1246,12 +1041,12 @@ void Space2DSW::body_remove_from_active_list(SelfList<Body2DSW> *p_body) { active_list.remove(p_body); } -void Space2DSW::body_add_to_inertia_update_list(SelfList<Body2DSW> *p_body) { - inertia_update_list.add(p_body); +void Space2DSW::body_add_to_mass_properties_update_list(SelfList<Body2DSW> *p_body) { + mass_properties_update_list.add(p_body); } -void Space2DSW::body_remove_from_inertia_update_list(SelfList<Body2DSW> *p_body) { - inertia_update_list.remove(p_body); +void Space2DSW::body_remove_from_mass_properties_update_list(SelfList<Body2DSW> *p_body) { + mass_properties_update_list.remove(p_body); } BroadPhase2DSW *Space2DSW::get_broadphase() { @@ -1317,9 +1112,9 @@ void Space2DSW::call_queries() { void Space2DSW::setup() { contact_debug_count = 0; - while (inertia_update_list.first()) { - inertia_update_list.first()->self()->update_inertias(); - inertia_update_list.remove(inertia_update_list.first()); + while (mass_properties_update_list.first()) { + mass_properties_update_list.first()->self()->update_mass_properties(); + mass_properties_update_list.remove(mass_properties_update_list.first()); } } diff --git a/servers/physics_2d/space_2d_sw.h b/servers/physics_2d/space_2d_sw.h index f7224e4661..ad82a14af5 100644 --- a/servers/physics_2d/space_2d_sw.h +++ b/servers/physics_2d/space_2d_sw.h @@ -49,13 +49,13 @@ class PhysicsDirectSpaceState2DSW : public PhysicsDirectSpaceState2D { public: Space2DSW *space; - virtual int intersect_point(const Vector2 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, bool p_pick_point = false) override; - virtual int intersect_point_on_canvas(const Vector2 &p_point, ObjectID p_canvas_instance_id, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, bool p_pick_point = false) override; - virtual bool intersect_ray(const Vector2 &p_from, const Vector2 &p_to, RayResult &r_result, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; - virtual int intersect_shape(const RID &p_shape, const Transform2D &p_xform, const Vector2 &p_motion, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; - virtual bool cast_motion(const RID &p_shape, const Transform2D &p_xform, const Vector2 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; - virtual bool collide_shape(RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, real_t p_margin, Vector2 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; - virtual bool rest_info(RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; + virtual int intersect_point(const Vector2 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, bool p_pick_point = false) override; + virtual int intersect_point_on_canvas(const Vector2 &p_point, ObjectID p_canvas_instance_id, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, bool p_pick_point = false) override; + virtual bool intersect_ray(const Vector2 &p_from, const Vector2 &p_to, RayResult &r_result, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; + virtual int intersect_shape(const RID &p_shape, const Transform2D &p_xform, const Vector2 &p_motion, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; + virtual bool cast_motion(const RID &p_shape, const Transform2D &p_xform, const Vector2 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; + virtual bool collide_shape(RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, real_t p_margin, Vector2 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; + virtual bool rest_info(RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; PhysicsDirectSpaceState2DSW(); }; @@ -86,7 +86,7 @@ private: BroadPhase2DSW *broadphase; SelfList<Body2DSW>::List active_list; - SelfList<Body2DSW>::List inertia_update_list; + SelfList<Body2DSW>::List mass_properties_update_list; SelfList<Body2DSW>::List state_query_list; SelfList<Area2DSW>::List monitor_query_list; SelfList<Area2DSW>::List area_moved_list; @@ -117,6 +117,8 @@ private: bool locked; + real_t last_step = 0.001; + int island_count; int active_objects; int collision_pairs; @@ -138,8 +140,8 @@ public: const SelfList<Body2DSW>::List &get_active_body_list() const; void body_add_to_active_list(SelfList<Body2DSW> *p_body); void body_remove_from_active_list(SelfList<Body2DSW> *p_body); - void body_add_to_inertia_update_list(SelfList<Body2DSW> *p_body); - void body_remove_from_inertia_update_list(SelfList<Body2DSW> *p_body); + void body_add_to_mass_properties_update_list(SelfList<Body2DSW> *p_body); + void body_remove_from_mass_properties_update_list(SelfList<Body2DSW> *p_body); void area_add_to_moved_list(SelfList<Area2DSW> *p_area); void area_remove_from_moved_list(SelfList<Area2DSW> *p_area); const SelfList<Area2DSW>::List &get_moved_area_list() const; @@ -172,6 +174,9 @@ public: void lock(); void unlock(); + real_t get_last_step() const { return last_step; } + void set_last_step(real_t p_step) { last_step = p_step; } + void set_param(PhysicsServer2D::SpaceParameter p_param, real_t p_value); real_t get_param(PhysicsServer2D::SpaceParameter p_param) const; @@ -183,8 +188,7 @@ 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, PhysicsServer2D::MotionResult *r_result, bool p_exclude_raycast_shapes = true, const Set<RID> &p_exclude = Set<RID>()); - int test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_transform, bool p_infinite_inertia, Vector2 &r_recover_motion, PhysicsServer2D::SeparationResult *r_results, int p_result_max, real_t p_margin); + bool test_body_motion(Body2DSW *p_body, const Transform2D &p_from, const Vector2 &p_motion, real_t p_margin, PhysicsServer2D::MotionResult *r_result, bool p_collide_separation_ray = false, const Set<RID> &p_exclude = Set<RID>()); void set_debug_contacts(int p_amount) { contact_debug.resize(p_amount); } _FORCE_INLINE_ bool is_debugging_contacts() const { return !contact_debug.is_empty(); } diff --git a/servers/physics_2d/step_2d_sw.cpp b/servers/physics_2d/step_2d_sw.cpp index 8b30160cc1..0306ec5050 100644 --- a/servers/physics_2d/step_2d_sw.cpp +++ b/servers/physics_2d/step_2d_sw.cpp @@ -129,6 +129,8 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) { p_space->setup(); //update inertias, etc + p_space->set_last_step(p_delta); + iterations = p_iterations; delta = p_delta; |