diff options
Diffstat (limited to 'servers/physics')
43 files changed, 423 insertions, 263 deletions
diff --git a/servers/physics/area_pair_sw.cpp b/servers/physics/area_pair_sw.cpp index 3e8d148f08..d2fef0ab77 100644 --- a/servers/physics/area_pair_sw.cpp +++ b/servers/physics/area_pair_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2018 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 */ @@ -27,6 +27,7 @@ /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ + #include "area_pair_sw.h" #include "collision_solver_sw.h" diff --git a/servers/physics/area_pair_sw.h b/servers/physics/area_pair_sw.h index 483f63284b..b66599dd11 100644 --- a/servers/physics/area_pair_sw.h +++ b/servers/physics/area_pair_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2018 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 */ @@ -27,6 +27,7 @@ /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ + #ifndef AREA_PAIR_SW_H #define AREA_PAIR_SW_H diff --git a/servers/physics/area_sw.cpp b/servers/physics/area_sw.cpp index 387230a478..8f9d4075b8 100644 --- a/servers/physics/area_sw.cpp +++ b/servers/physics/area_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2018 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 */ @@ -27,6 +27,7 @@ /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ + #include "area_sw.h" #include "body_sw.h" #include "space_sw.h" @@ -237,8 +238,10 @@ void AreaSW::call_queries() { //get_space()->area_remove_from_monitor_query_list(&monitor_query_list); } -AreaSW::AreaSW() - : CollisionObjectSW(TYPE_AREA), monitor_query_list(this), moved_list(this) { +AreaSW::AreaSW() : + CollisionObjectSW(TYPE_AREA), + monitor_query_list(this), + moved_list(this) { _set_static(true); //areas are never active space_override_mode = PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED; diff --git a/servers/physics/area_sw.h b/servers/physics/area_sw.h index 455c0b5c8b..ae19b0e04e 100644 --- a/servers/physics/area_sw.h +++ b/servers/physics/area_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2018 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 */ @@ -27,6 +27,7 @@ /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ + #ifndef AREA_SW_H #define AREA_SW_H diff --git a/servers/physics/body_pair_sw.cpp b/servers/physics/body_pair_sw.cpp index a289b4b0ca..882d201f61 100644 --- a/servers/physics/body_pair_sw.cpp +++ b/servers/physics/body_pair_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2018 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 */ @@ -27,6 +27,7 @@ /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ + #include "body_pair_sw.h" #include "collision_solver_sw.h" @@ -46,6 +47,7 @@ //#define ALLOWED_PENETRATION 0.01 #define RELAXATION_TIMESTEPS 3 #define MIN_VELOCITY 0.0001 +#define MAX_BIAS_ROTATION (Math_PI / 8) void BodyPairSW::_contact_added_callback(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata) { @@ -71,6 +73,7 @@ void BodyPairSW::contact_added_callback(const Vector3 &p_point_A, const Vector3 contact.acc_normal_impulse = 0; contact.acc_bias_impulse = 0; + contact.acc_bias_impulse_center_of_mass = 0; contact.acc_tangent_impulse = Vector3(); contact.local_A = local_A; contact.local_B = local_B; @@ -82,12 +85,12 @@ void BodyPairSW::contact_added_callback(const Vector3 &p_point_A, const Vector3 for (int i = 0; i < contact_count; i++) { Contact &c = contacts[i]; - if ( - c.local_A.distance_squared_to(local_A) < (contact_recycle_radius * contact_recycle_radius) && + if (c.local_A.distance_squared_to(local_A) < (contact_recycle_radius * contact_recycle_radius) && c.local_B.distance_squared_to(local_B) < (contact_recycle_radius * contact_recycle_radius)) { contact.acc_normal_impulse = c.acc_normal_impulse; contact.acc_bias_impulse = c.acc_bias_impulse; + contact.acc_bias_impulse_center_of_mass = c.acc_bias_impulse_center_of_mass; contact.acc_tangent_impulse = c.acc_tangent_impulse; new_index = i; break; @@ -325,9 +328,7 @@ bool BodyPairSW::setup(real_t p_step) { A->apply_impulse(c.rA + A->get_center_of_mass(), -j_vec); B->apply_impulse(c.rB + B->get_center_of_mass(), j_vec); c.acc_bias_impulse = 0; - Vector3 jb_vec = c.normal * c.acc_bias_impulse; - A->apply_bias_impulse(c.rA + A->get_center_of_mass(), -jb_vec); - B->apply_bias_impulse(c.rB + B->get_center_of_mass(), jb_vec); + c.acc_bias_impulse_center_of_mass = 0; c.bounce = MAX(A->get_bounce(), B->get_bounce()); if (c.bounce) { @@ -356,7 +357,7 @@ void BodyPairSW::solve(real_t p_step) { c.active = false; //try to deactivate, will activate itself if still needed - //bias impule + //bias impulse Vector3 crbA = A->get_biased_angular_velocity().cross(c.rA); Vector3 crbB = B->get_biased_angular_velocity().cross(c.rB); @@ -372,8 +373,26 @@ void BodyPairSW::solve(real_t p_step) { Vector3 jb = c.normal * (c.acc_bias_impulse - jbnOld); - A->apply_bias_impulse(c.rA + A->get_center_of_mass(), -jb); - B->apply_bias_impulse(c.rB + B->get_center_of_mass(), jb); + A->apply_bias_impulse(c.rA + A->get_center_of_mass(), -jb, MAX_BIAS_ROTATION / p_step); + B->apply_bias_impulse(c.rB + B->get_center_of_mass(), jb, MAX_BIAS_ROTATION / p_step); + + crbA = A->get_biased_angular_velocity().cross(c.rA); + crbB = B->get_biased_angular_velocity().cross(c.rB); + dbv = B->get_biased_linear_velocity() + crbB - A->get_biased_linear_velocity() - crbA; + + vbn = dbv.dot(c.normal); + + if (Math::abs(-vbn + c.bias) > MIN_VELOCITY) { + + real_t jbn_com = (-vbn + c.bias) / (A->get_inv_mass() + B->get_inv_mass()); + real_t jbnOld_com = c.acc_bias_impulse_center_of_mass; + c.acc_bias_impulse_center_of_mass = MAX(jbnOld_com + jbn_com, 0.0f); + + Vector3 jb_com = c.normal * (c.acc_bias_impulse_center_of_mass - jbnOld_com); + + A->apply_bias_impulse(A->get_center_of_mass(), -jb_com, 0.0f); + B->apply_bias_impulse(B->get_center_of_mass(), jb_com, 0.0f); + } c.active = true; } @@ -382,7 +401,7 @@ void BodyPairSW::solve(real_t p_step) { Vector3 crB = B->get_angular_velocity().cross(c.rB); Vector3 dv = B->get_linear_velocity() + crB - A->get_linear_velocity() - crA; - //normal impule + //normal impulse real_t vn = dv.dot(c.normal); if (Math::abs(vn) > MIN_VELOCITY) { @@ -399,7 +418,7 @@ void BodyPairSW::solve(real_t p_step) { c.active = true; } - //friction impule + //friction impulse real_t friction = A->get_friction() * B->get_friction(); @@ -446,8 +465,8 @@ void BodyPairSW::solve(real_t p_step) { } } -BodyPairSW::BodyPairSW(BodySW *p_A, int p_shape_A, BodySW *p_B, int p_shape_B) - : ConstraintSW(_arr, 2) { +BodyPairSW::BodyPairSW(BodySW *p_A, int p_shape_A, BodySW *p_B, int p_shape_B) : + ConstraintSW(_arr, 2) { A = p_A; B = p_B; diff --git a/servers/physics/body_pair_sw.h b/servers/physics/body_pair_sw.h index f09c977950..fd85d77718 100644 --- a/servers/physics/body_pair_sw.h +++ b/servers/physics/body_pair_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2018 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 */ @@ -27,6 +27,7 @@ /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ + #ifndef BODY_PAIR_SW_H #define BODY_PAIR_SW_H @@ -59,6 +60,7 @@ class BodyPairSW : public ConstraintSW { real_t acc_normal_impulse; // accumulated normal impulse (Pn) Vector3 acc_tangent_impulse; // accumulated tangent impulse (Pt) real_t acc_bias_impulse; // accumulated normal impulse for position bias (Pnb) + real_t acc_bias_impulse_center_of_mass; // accumulated normal impulse for position bias applied to com real_t mass_normal; real_t bias; real_t bounce; diff --git a/servers/physics/body_sw.cpp b/servers/physics/body_sw.cpp index 6ced004118..cc9681193c 100644 --- a/servers/physics/body_sw.cpp +++ b/servers/physics/body_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2018 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 */ @@ -27,6 +27,7 @@ /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ + #include "body_sw.h" #include "area_sw.h" #include "space_sw.h" @@ -422,6 +423,18 @@ void BodySW::_compute_area_gravity_and_dampenings(const AreaSW *p_area) { area_angular_damp += p_area->get_angular_damp(); } +void BodySW::set_axis_lock(PhysicsServer::BodyAxis p_axis, bool lock) { + if (lock) { + locked_axis |= p_axis; + } else { + locked_axis &= ~p_axis; + } +} + +bool BodySW::is_axis_locked(PhysicsServer::BodyAxis p_axis) const { + return locked_axis & p_axis; +} + void BodySW::integrate_forces(real_t p_step) { if (mode == PhysicsServer::BODY_MODE_STATIC) @@ -559,6 +572,22 @@ void BodySW::integrate_velocities(real_t p_step) { if (fi_callback) get_space()->body_add_to_state_query_list(&direct_state_query_list); + //apply axis lock linear + for (int i = 0; i < 3; i++) { + if (is_axis_locked((PhysicsServer::BodyAxis)(1 << i))) { + linear_velocity[i] = 0; + biased_linear_velocity[i] = 0; + new_transform.origin[i] = get_transform().origin[i]; + } + } + //apply axis lock angular + for (int i = 0; i < 3; i++) { + if (is_axis_locked((PhysicsServer::BodyAxis)(1 << (i + 3)))) { + angular_velocity[i] = 0; + biased_angular_velocity[i] = 0; + } + } + if (mode == PhysicsServer::BODY_MODE_KINEMATIC) { _set_transform(new_transform, false); @@ -569,22 +598,6 @@ void BodySW::integrate_velocities(real_t p_step) { return; } - //apply axis lock - if (axis_lock != PhysicsServer::BODY_AXIS_LOCK_DISABLED) { - - int axis = axis_lock - 1; - for (int i = 0; i < 3; i++) { - if (i == axis) { - linear_velocity[i] = 0; - biased_linear_velocity[i] = 0; - } else { - - angular_velocity[i] = 0; - biased_angular_velocity[i] = 0; - } - } - } - Vector3 total_angular_velocity = angular_velocity + biased_angular_velocity; real_t ang_vel = total_angular_velocity.length(); @@ -736,13 +749,22 @@ void BodySW::set_force_integration_callback(ObjectID p_id, const StringName &p_m } } -BodySW::BodySW() - : CollisionObjectSW(TYPE_BODY), active_list(this), inertia_update_list(this), direct_state_query_list(this) { +void BodySW::set_kinematic_margin(real_t p_margin) { + kinematic_safe_margin = p_margin; +} + +BodySW::BodySW() : + CollisionObjectSW(TYPE_BODY), + active_list(this), + inertia_update_list(this), + direct_state_query_list(this), + locked_axis(0) { mode = PhysicsServer::BODY_MODE_RIGID; active = true; mass = 1; + kinematic_safe_margin = 0.01; //_inv_inertia=Transform(); _inv_mass = 1; bounce = 0; @@ -767,7 +789,6 @@ BodySW::BodySW() continuous_cd = false; can_sleep = false; fi_callback = NULL; - axis_lock = PhysicsServer::BODY_AXIS_LOCK_DISABLED; } BodySW::~BodySW() { diff --git a/servers/physics/body_sw.h b/servers/physics/body_sw.h index 782bf14a4b..fd2ab16b84 100644 --- a/servers/physics/body_sw.h +++ b/servers/physics/body_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2018 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 */ @@ -27,6 +27,7 @@ /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ + #ifndef BODY_SW_H #define BODY_SW_H @@ -53,8 +54,9 @@ class BodySW : public CollisionObjectSW { real_t angular_damp; real_t gravity_scale; - PhysicsServer::BodyAxisLock axis_lock; + uint16_t locked_axis; + real_t kinematic_safe_margin; real_t _inv_mass; Vector3 _inv_inertia; // Relative to the principal axes of inertia @@ -149,6 +151,9 @@ class BodySW : public CollisionObjectSW { public: void set_force_integration_callback(ObjectID p_id, const StringName &p_method, const Variant &p_udata = Variant()); + void set_kinematic_margin(real_t p_margin); + _FORCE_INLINE_ real_t get_kinematic_margin() { return kinematic_safe_margin; } + _FORCE_INLINE_ void add_area(AreaSW *p_area) { int index = areas.find(AreaCMP(p_area)); if (index > -1) { @@ -223,10 +228,16 @@ public: angular_velocity += _inv_inertia_tensor.xform(p_j); } - _FORCE_INLINE_ void apply_bias_impulse(const Vector3 &p_pos, const Vector3 &p_j) { + _FORCE_INLINE_ void apply_bias_impulse(const Vector3 &p_pos, const Vector3 &p_j, real_t p_max_delta_av = -1.0) { biased_linear_velocity += p_j * _inv_mass; - biased_angular_velocity += _inv_inertia_tensor.xform((p_pos - center_of_mass).cross(p_j)); + if (p_max_delta_av != 0.0) { + Vector3 delta_av = _inv_inertia_tensor.xform((p_pos - center_of_mass).cross(p_j)); + if (p_max_delta_av > 0 && delta_av.length() > p_max_delta_av) { + delta_av = delta_av.normalized() * p_max_delta_av; + } + biased_angular_velocity += delta_av; + } } _FORCE_INLINE_ void apply_bias_torque_impulse(const Vector3 &p_j) { @@ -234,12 +245,21 @@ public: biased_angular_velocity += _inv_inertia_tensor.xform(p_j); } + _FORCE_INLINE_ void add_central_force(const Vector3 &p_force) { + + applied_force += p_force; + } + _FORCE_INLINE_ void add_force(const Vector3 &p_force, const Vector3 &p_pos) { applied_force += p_force; applied_torque += p_pos.cross(p_force); } + _FORCE_INLINE_ void add_torque(const Vector3 &p_torque) { + applied_torque += p_torque; + } + void set_active(bool p_active); _FORCE_INLINE_ bool is_active() const { return active; } @@ -278,8 +298,8 @@ public: _FORCE_INLINE_ Vector3 get_gravity() const { return gravity; } _FORCE_INLINE_ real_t get_bounce() const { return bounce; } - _FORCE_INLINE_ void set_axis_lock(PhysicsServer::BodyAxisLock p_lock) { axis_lock = p_lock; } - _FORCE_INLINE_ PhysicsServer::BodyAxisLock get_axis_lock() const { return axis_lock; } + void set_axis_lock(PhysicsServer::BodyAxis p_axis, bool lock); + bool is_axis_locked(PhysicsServer::BodyAxis p_axis) const; void integrate_forces(real_t p_step); void integrate_velocities(real_t p_step); @@ -390,7 +410,9 @@ public: virtual void set_transform(const Transform &p_transform) { body->set_state(PhysicsServer::BODY_STATE_TRANSFORM, p_transform); } virtual Transform get_transform() const { return body->get_transform(); } + virtual void add_central_force(const Vector3 &p_force) { body->add_central_force(p_force); } virtual void add_force(const Vector3 &p_force, const Vector3 &p_pos) { body->add_force(p_force, p_pos); } + virtual void add_torque(const Vector3 &p_torque) { body->add_torque(p_torque); } virtual void apply_impulse(const Vector3 &p_pos, const Vector3 &p_j) { body->apply_impulse(p_pos, p_j); } virtual void apply_torque_impulse(const Vector3 &p_j) { body->apply_torque_impulse(p_j); } diff --git a/servers/physics/broad_phase_basic.cpp b/servers/physics/broad_phase_basic.cpp index c6565ac2e9..52483a8b14 100644 --- a/servers/physics/broad_phase_basic.cpp +++ b/servers/physics/broad_phase_basic.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2018 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 */ @@ -27,6 +27,7 @@ /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ + #include "broad_phase_basic.h" #include "list.h" #include "print_string.h" @@ -46,7 +47,7 @@ BroadPhaseSW::ID BroadPhaseBasic::create(CollisionObjectSW *p_object, int p_subi return current; } -void BroadPhaseBasic::move(ID p_id, const Rect3 &p_aabb) { +void BroadPhaseBasic::move(ID p_id, const AABB &p_aabb) { Map<ID, Element>::Element *E = element_map.find(p_id); ERR_FAIL_COND(!E); @@ -109,7 +110,7 @@ int BroadPhaseBasic::cull_point(const Vector3 &p_point, CollisionObjectSW **p_re for (Map<ID, Element>::Element *E = element_map.front(); E; E = E->next()) { - const Rect3 aabb = E->get().aabb; + const AABB aabb = E->get().aabb; if (aabb.has_point(p_point)) { p_results[rc] = E->get().owner; @@ -129,7 +130,7 @@ int BroadPhaseBasic::cull_segment(const Vector3 &p_from, const Vector3 &p_to, Co for (Map<ID, Element>::Element *E = element_map.front(); E; E = E->next()) { - const Rect3 aabb = E->get().aabb; + const AABB aabb = E->get().aabb; if (aabb.intersects_segment(p_from, p_to)) { p_results[rc] = E->get().owner; @@ -142,13 +143,13 @@ int BroadPhaseBasic::cull_segment(const Vector3 &p_from, const Vector3 &p_to, Co return rc; } -int BroadPhaseBasic::cull_aabb(const Rect3 &p_aabb, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices) { +int BroadPhaseBasic::cull_aabb(const AABB &p_aabb, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices) { int rc = 0; for (Map<ID, Element>::Element *E = element_map.front(); E; E = E->next()) { - const Rect3 aabb = E->get().aabb; + const AABB aabb = E->get().aabb; if (aabb.intersects(p_aabb)) { p_results[rc] = E->get().owner; diff --git a/servers/physics/broad_phase_basic.h b/servers/physics/broad_phase_basic.h index 5c124c1792..47fcdb3060 100644 --- a/servers/physics/broad_phase_basic.h +++ b/servers/physics/broad_phase_basic.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2018 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 */ @@ -27,6 +27,7 @@ /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ + #ifndef BROAD_PHASE_BASIC_H #define BROAD_PHASE_BASIC_H @@ -39,7 +40,7 @@ class BroadPhaseBasic : public BroadPhaseSW { CollisionObjectSW *owner; bool _static; - Rect3 aabb; + AABB aabb; int subindex; }; @@ -83,7 +84,7 @@ class BroadPhaseBasic : public BroadPhaseSW { public: // 0 is an invalid ID virtual ID create(CollisionObjectSW *p_object, int p_subindex = 0); - virtual void move(ID p_id, const Rect3 &p_aabb); + virtual void move(ID p_id, const AABB &p_aabb); virtual void set_static(ID p_id, bool p_static); virtual void remove(ID p_id); @@ -93,7 +94,7 @@ public: virtual int cull_point(const Vector3 &p_point, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL); virtual int cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL); - virtual int cull_aabb(const Rect3 &p_aabb, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL); + virtual int cull_aabb(const AABB &p_aabb, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL); virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata); virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata); diff --git a/servers/physics/broad_phase_octree.cpp b/servers/physics/broad_phase_octree.cpp index e7111d9580..7f43fbdb6a 100644 --- a/servers/physics/broad_phase_octree.cpp +++ b/servers/physics/broad_phase_octree.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2018 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 */ @@ -27,16 +27,17 @@ /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ + #include "broad_phase_octree.h" #include "collision_object_sw.h" BroadPhaseSW::ID BroadPhaseOctree::create(CollisionObjectSW *p_object, int p_subindex) { - ID oid = octree.create(p_object, Rect3(), p_subindex, false, 1 << p_object->get_type(), 0); + ID oid = octree.create(p_object, AABB(), p_subindex, false, 1 << p_object->get_type(), 0); return oid; } -void BroadPhaseOctree::move(ID p_id, const Rect3 &p_aabb) { +void BroadPhaseOctree::move(ID p_id, const AABB &p_aabb) { octree.move(p_id, p_aabb); } @@ -76,7 +77,7 @@ int BroadPhaseOctree::cull_segment(const Vector3 &p_from, const Vector3 &p_to, C return octree.cull_segment(p_from, p_to, p_results, p_max_results, p_result_indices); } -int BroadPhaseOctree::cull_aabb(const Rect3 &p_aabb, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices) { +int BroadPhaseOctree::cull_aabb(const AABB &p_aabb, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices) { return octree.cull_aabb(p_aabb, p_results, p_max_results, p_result_indices); } diff --git a/servers/physics/broad_phase_octree.h b/servers/physics/broad_phase_octree.h index d28f2da13f..e7028eba98 100644 --- a/servers/physics/broad_phase_octree.h +++ b/servers/physics/broad_phase_octree.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2018 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 */ @@ -27,6 +27,7 @@ /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ + #ifndef BROAD_PHASE_OCTREE_H #define BROAD_PHASE_OCTREE_H @@ -48,7 +49,7 @@ class BroadPhaseOctree : public BroadPhaseSW { public: // 0 is an invalid ID virtual ID create(CollisionObjectSW *p_object, int p_subindex = 0); - virtual void move(ID p_id, const Rect3 &p_aabb); + virtual void move(ID p_id, const AABB &p_aabb); virtual void set_static(ID p_id, bool p_static); virtual void remove(ID p_id); @@ -58,7 +59,7 @@ public: virtual int cull_point(const Vector3 &p_point, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL); virtual int cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL); - virtual int cull_aabb(const Rect3 &p_aabb, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL); + virtual int cull_aabb(const AABB &p_aabb, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL); virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata); virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata); diff --git a/servers/physics/broad_phase_sw.cpp b/servers/physics/broad_phase_sw.cpp index 3e5b7e72c6..2a490c3e80 100644 --- a/servers/physics/broad_phase_sw.cpp +++ b/servers/physics/broad_phase_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2018 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 */ @@ -27,6 +27,7 @@ /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ + #include "broad_phase_sw.h" BroadPhaseSW::CreateFunction BroadPhaseSW::create_func = NULL; diff --git a/servers/physics/broad_phase_sw.h b/servers/physics/broad_phase_sw.h index 2b5ed629fe..7559942cd4 100644 --- a/servers/physics/broad_phase_sw.h +++ b/servers/physics/broad_phase_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2018 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 */ @@ -27,11 +27,12 @@ /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ + #ifndef BROAD_PHASE_SW_H #define BROAD_PHASE_SW_H +#include "aabb.h" #include "math_funcs.h" -#include "rect3.h" class CollisionObjectSW; @@ -49,7 +50,7 @@ public: // 0 is an invalid ID virtual ID create(CollisionObjectSW *p_object_, int p_subindex = 0) = 0; - virtual void move(ID p_id, const Rect3 &p_aabb) = 0; + virtual void move(ID p_id, const AABB &p_aabb) = 0; virtual void set_static(ID p_id, bool p_static) = 0; virtual void remove(ID p_id) = 0; @@ -59,7 +60,7 @@ public: virtual int cull_point(const Vector3 &p_point, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL) = 0; virtual int cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL) = 0; - virtual int cull_aabb(const Rect3 &p_aabb, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL) = 0; + virtual int cull_aabb(const AABB &p_aabb, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL) = 0; virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata) = 0; virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) = 0; diff --git a/servers/physics/collision_object_sw.cpp b/servers/physics/collision_object_sw.cpp index 3af8b542fa..f7a58a9cf2 100644 --- a/servers/physics/collision_object_sw.cpp +++ b/servers/physics/collision_object_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2018 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 */ @@ -27,6 +27,7 @@ /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ + #include "collision_object_sw.h" #include "servers/physics/physics_server_sw.h" #include "space_sw.h" @@ -149,7 +150,7 @@ void CollisionObjectSW::_update_shapes() { } //not quite correct, should compute the next matrix.. - Rect3 shape_aabb = s.shape->get_aabb(); + AABB shape_aabb = s.shape->get_aabb(); Transform xform = transform * s.xform; shape_aabb = xform.xform(shape_aabb); s.aabb_cache = shape_aabb; @@ -176,10 +177,10 @@ void CollisionObjectSW::_update_shapes_with_motion(const Vector3 &p_motion) { } //not quite correct, should compute the next matrix.. - Rect3 shape_aabb = s.shape->get_aabb(); + AABB shape_aabb = s.shape->get_aabb(); Transform xform = transform * s.xform; shape_aabb = xform.xform(shape_aabb); - shape_aabb = shape_aabb.merge(Rect3(shape_aabb.position + p_motion, shape_aabb.size)); //use motion + shape_aabb = shape_aabb.merge(AABB(shape_aabb.position + p_motion, shape_aabb.size)); //use motion s.aabb_cache = shape_aabb; space->get_broadphase()->move(s.bpid, shape_aabb); @@ -217,8 +218,8 @@ void CollisionObjectSW::_shape_changed() { _shapes_changed(); } -CollisionObjectSW::CollisionObjectSW(Type p_type) - : pending_shape_update_list(this) { +CollisionObjectSW::CollisionObjectSW(Type p_type) : + pending_shape_update_list(this) { _static = true; type = p_type; diff --git a/servers/physics/collision_object_sw.h b/servers/physics/collision_object_sw.h index 67a8a44944..f5d32e56a0 100644 --- a/servers/physics/collision_object_sw.h +++ b/servers/physics/collision_object_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2018 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 */ @@ -27,6 +27,7 @@ /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ + #ifndef COLLISION_OBJECT_SW_H #define COLLISION_OBJECT_SW_H @@ -61,7 +62,7 @@ private: Transform xform; Transform xform_inv; BroadPhaseSW::ID bpid; - Rect3 aabb_cache; //for rayqueries + AABB aabb_cache; //for rayqueries real_t area_cache; ShapeSW *shape; bool disabled; @@ -123,7 +124,7 @@ public: _FORCE_INLINE_ ShapeSW *get_shape(int p_index) const { return shapes[p_index].shape; } _FORCE_INLINE_ const Transform &get_shape_transform(int p_index) const { return shapes[p_index].xform; } _FORCE_INLINE_ const Transform &get_shape_inv_transform(int p_index) const { return shapes[p_index].xform_inv; } - _FORCE_INLINE_ const Rect3 &get_shape_aabb(int p_index) const { return shapes[p_index].aabb_cache; } + _FORCE_INLINE_ const AABB &get_shape_aabb(int p_index) const { return shapes[p_index].aabb_cache; } _FORCE_INLINE_ const real_t get_shape_area(int p_index) const { return shapes[p_index].area_cache; } _FORCE_INLINE_ Transform get_transform() const { return transform; } diff --git a/servers/physics/collision_solver_sat.cpp b/servers/physics/collision_solver_sat.cpp index a044df7f31..eefb0f0396 100644 --- a/servers/physics/collision_solver_sat.cpp +++ b/servers/physics/collision_solver_sat.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2018 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 */ @@ -27,6 +27,7 @@ /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ + #include "collision_solver_sat.h" #include "geometry.h" @@ -255,7 +256,8 @@ static void _generate_contacts_from_supports(const Vector3 *p_points_A, int p_po _generate_contacts_face_face, }, { - 0, 0, + 0, + 0, _generate_contacts_face_face, } }; diff --git a/servers/physics/collision_solver_sat.h b/servers/physics/collision_solver_sat.h index efe9bc87ac..1d83d9889d 100644 --- a/servers/physics/collision_solver_sat.h +++ b/servers/physics/collision_solver_sat.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2018 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 */ @@ -27,6 +27,7 @@ /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ + #ifndef COLLISION_SOLVER_SAT_H #define COLLISION_SOLVER_SAT_H diff --git a/servers/physics/collision_solver_sw.cpp b/servers/physics/collision_solver_sw.cpp index 7bef208237..0037b9a862 100644 --- a/servers/physics/collision_solver_sw.cpp +++ b/servers/physics/collision_solver_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2018 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 */ @@ -27,6 +27,7 @@ /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ + #include "collision_solver_sw.h" #include "collision_solver_sat.h" @@ -89,6 +90,10 @@ bool CollisionSolverSW::solve_ray(const ShapeSW *p_shape_A, const Transform &p_t return false; Vector3 support_B = p_transform_B.xform(p); + if (ray->get_slips_on_slope()) { + Vector3 global_n = ai.basis.xform_inv(n).normalized(); + support_B = support_A + (support_B - support_A).length() * global_n; + } if (p_result_callback) { if (p_swap_result) @@ -152,7 +157,7 @@ bool CollisionSolverSW::solve_concave(const ShapeSW *p_shape_A, const Transform //quickly compute a local AABB - Rect3 local_aabb; + AABB local_aabb; for (int i = 0; i < 3; i++) { Vector3 axis(p_transform_B.basis.get_axis(i)); @@ -291,7 +296,7 @@ bool CollisionSolverSW::solve_distance_plane(const ShapeSW *p_shape_A, const Tra return collided; } -bool CollisionSolverSW::solve_distance(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B, const Rect3 &p_concave_hint, Vector3 *r_sep_axis) { +bool CollisionSolverSW::solve_distance(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B, const AABB &p_concave_hint, Vector3 *r_sep_axis) { if (p_shape_A->is_concave()) return false; @@ -328,14 +333,14 @@ bool CollisionSolverSW::solve_distance(const ShapeSW *p_shape_A, const Transform //quickly compute a local AABB - bool use_cc_hint = p_concave_hint != Rect3(); - Rect3 cc_hint_aabb; + bool use_cc_hint = p_concave_hint != AABB(); + AABB cc_hint_aabb; if (use_cc_hint) { cc_hint_aabb = p_concave_hint; cc_hint_aabb.position -= p_transform_B.origin; } - Rect3 local_aabb; + AABB local_aabb; for (int i = 0; i < 3; i++) { Vector3 axis(p_transform_B.basis.get_axis(i)); diff --git a/servers/physics/collision_solver_sw.h b/servers/physics/collision_solver_sw.h index 1e38b1b54e..cbcb6b850a 100644 --- a/servers/physics/collision_solver_sw.h +++ b/servers/physics/collision_solver_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2018 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 */ @@ -27,6 +27,7 @@ /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ + #ifndef COLLISION_SOLVER_SW_H #define COLLISION_SOLVER_SW_H @@ -46,7 +47,7 @@ private: public: static bool solve_static(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, Vector3 *r_sep_axis = NULL, real_t p_margin_A = 0, real_t p_margin_B = 0); - static bool solve_distance(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B, const Rect3 &p_concave_hint, Vector3 *r_sep_axis = NULL); + static bool solve_distance(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B, const AABB &p_concave_hint, Vector3 *r_sep_axis = NULL); }; #endif // COLLISION_SOLVER__SW_H diff --git a/servers/physics/constraint_sw.h b/servers/physics/constraint_sw.h index 047513f635..41789600f6 100644 --- a/servers/physics/constraint_sw.h +++ b/servers/physics/constraint_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2018 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 */ @@ -27,6 +27,7 @@ /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ + #ifndef CONSTRAINT_SW_H #define CONSTRAINT_SW_H @@ -40,6 +41,7 @@ class ConstraintSW : public RID_Data { ConstraintSW *island_next; ConstraintSW *island_list_next; int priority; + bool disabled_collisions_between_bodies; RID self; @@ -49,6 +51,7 @@ protected: _body_count = p_body_count; island_step = 0; priority = 1; + disabled_collisions_between_bodies = true; } public: @@ -70,6 +73,9 @@ public: _FORCE_INLINE_ void set_priority(int p_priority) { priority = p_priority; } _FORCE_INLINE_ int get_priority() const { return priority; } + _FORCE_INLINE_ void disable_collisions_between_bodies(const bool p_disabled) { disabled_collisions_between_bodies = p_disabled; } + _FORCE_INLINE_ bool is_disabled_collisions_between_bodies() const { return disabled_collisions_between_bodies; } + virtual bool setup(real_t p_step) = 0; virtual void solve(real_t p_step) = 0; diff --git a/servers/physics/gjk_epa.cpp b/servers/physics/gjk_epa.cpp index 0f03bd917a..a42b7ed82b 100644 --- a/servers/physics/gjk_epa.cpp +++ b/servers/physics/gjk_epa.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2018 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 */ @@ -27,6 +27,7 @@ /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ + #include "gjk_epa.h" /* Disabling formatting for thirdparty code snippet */ diff --git a/servers/physics/gjk_epa.h b/servers/physics/gjk_epa.h index 6d8539d365..f6d052608e 100644 --- a/servers/physics/gjk_epa.h +++ b/servers/physics/gjk_epa.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2018 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 */ @@ -27,6 +27,7 @@ /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ + #ifndef GJK_EPA_H #define GJK_EPA_H diff --git a/servers/physics/joints/cone_twist_joint_sw.cpp b/servers/physics/joints/cone_twist_joint_sw.cpp index 3c900d4fbe..c06f27cc57 100644 --- a/servers/physics/joints/cone_twist_joint_sw.cpp +++ b/servers/physics/joints/cone_twist_joint_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2018 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 */ @@ -85,8 +85,8 @@ static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x) { return (y < 0.0f) ? -angle : angle; } -ConeTwistJointSW::ConeTwistJointSW(BodySW *rbA, BodySW *rbB, const Transform &rbAFrame, const Transform &rbBFrame) - : JointSW(_arr, 2) { +ConeTwistJointSW::ConeTwistJointSW(BodySW *rbA, BodySW *rbB, const Transform &rbAFrame, const Transform &rbBFrame) : + JointSW(_arr, 2) { A = rbA; B = rbB; diff --git a/servers/physics/joints/cone_twist_joint_sw.h b/servers/physics/joints/cone_twist_joint_sw.h index 0c722463be..3213562c3f 100644 --- a/servers/physics/joints/cone_twist_joint_sw.h +++ b/servers/physics/joints/cone_twist_joint_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2018 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 */ diff --git a/servers/physics/joints/generic_6dof_joint_sw.cpp b/servers/physics/joints/generic_6dof_joint_sw.cpp index 1e323be36c..c95e5cef32 100644 --- a/servers/physics/joints/generic_6dof_joint_sw.cpp +++ b/servers/physics/joints/generic_6dof_joint_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2018 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 */ @@ -209,9 +209,11 @@ real_t G6DOFTranslationalLimitMotorSW::solveLinearAxis( //////////////////////////// G6DOFTranslationalLimitMotorSW //////////////////////////////////// -Generic6DOFJointSW::Generic6DOFJointSW(BodySW *rbA, BodySW *rbB, const Transform &frameInA, const Transform &frameInB, bool useLinearReferenceFrameA) - : JointSW(_arr, 2), m_frameInA(frameInA), m_frameInB(frameInB), - m_useLinearReferenceFrameA(useLinearReferenceFrameA) { +Generic6DOFJointSW::Generic6DOFJointSW(BodySW *rbA, BodySW *rbB, const Transform &frameInA, const Transform &frameInB, bool useLinearReferenceFrameA) : + JointSW(_arr, 2), + m_frameInA(frameInA), + m_frameInB(frameInB), + m_useLinearReferenceFrameA(useLinearReferenceFrameA) { A = rbA; B = rbB; A->add_constraint(this, 0); diff --git a/servers/physics/joints/generic_6dof_joint_sw.h b/servers/physics/joints/generic_6dof_joint_sw.h index f6af0b1d6f..b350546c5d 100644 --- a/servers/physics/joints/generic_6dof_joint_sw.h +++ b/servers/physics/joints/generic_6dof_joint_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2018 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 */ diff --git a/servers/physics/joints/hinge_joint_sw.cpp b/servers/physics/joints/hinge_joint_sw.cpp index fe20a55b60..d660eba879 100644 --- a/servers/physics/joints/hinge_joint_sw.cpp +++ b/servers/physics/joints/hinge_joint_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2018 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 */ @@ -68,8 +68,8 @@ static void plane_space(const Vector3 &n, Vector3 &p, Vector3 &q) { } } -HingeJointSW::HingeJointSW(BodySW *rbA, BodySW *rbB, const Transform &frameA, const Transform &frameB) - : JointSW(_arr, 2) { +HingeJointSW::HingeJointSW(BodySW *rbA, BodySW *rbB, const Transform &frameA, const Transform &frameB) : + JointSW(_arr, 2) { A = rbA; B = rbB; @@ -101,8 +101,8 @@ HingeJointSW::HingeJointSW(BodySW *rbA, BodySW *rbB, const Transform &frameA, co } HingeJointSW::HingeJointSW(BodySW *rbA, BodySW *rbB, const Vector3 &pivotInA, const Vector3 &pivotInB, - const Vector3 &axisInA, const Vector3 &axisInB) - : JointSW(_arr, 2) { + const Vector3 &axisInA, const Vector3 &axisInB) : + JointSW(_arr, 2) { A = rbA; B = rbB; diff --git a/servers/physics/joints/hinge_joint_sw.h b/servers/physics/joints/hinge_joint_sw.h index 8e0f9a4da0..cc8af3b370 100644 --- a/servers/physics/joints/hinge_joint_sw.h +++ b/servers/physics/joints/hinge_joint_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2018 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 */ diff --git a/servers/physics/joints/jacobian_entry_sw.h b/servers/physics/joints/jacobian_entry_sw.h index 79ce0e6157..42c90c9ae9 100644 --- a/servers/physics/joints/jacobian_entry_sw.h +++ b/servers/physics/joints/jacobian_entry_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2018 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 */ @@ -64,8 +64,8 @@ public: const Vector3 &inertiaInvA, const real_t massInvA, const Vector3 &inertiaInvB, - const real_t massInvB) - : m_linearJointAxis(jointAxis) { + const real_t massInvB) : + m_linearJointAxis(jointAxis) { m_aJ = world2A.xform(rel_pos1.cross(m_linearJointAxis)); m_bJ = world2B.xform(rel_pos2.cross(-m_linearJointAxis)); m_0MinvJt = inertiaInvA * m_aJ; @@ -80,8 +80,8 @@ public: const Basis &world2A, const Basis &world2B, const Vector3 &inertiaInvA, - const Vector3 &inertiaInvB) - : m_linearJointAxis(Vector3(real_t(0.), real_t(0.), real_t(0.))) { + const Vector3 &inertiaInvB) : + m_linearJointAxis(Vector3(real_t(0.), real_t(0.), real_t(0.))) { m_aJ = world2A.xform(jointAxis); m_bJ = world2B.xform(-jointAxis); m_0MinvJt = inertiaInvA * m_aJ; @@ -95,8 +95,10 @@ public: JacobianEntrySW(const Vector3 &axisInA, const Vector3 &axisInB, const Vector3 &inertiaInvA, - const Vector3 &inertiaInvB) - : m_linearJointAxis(Vector3(real_t(0.), real_t(0.), real_t(0.))), m_aJ(axisInA), m_bJ(-axisInB) { + const Vector3 &inertiaInvB) : + m_linearJointAxis(Vector3(real_t(0.), real_t(0.), real_t(0.))), + m_aJ(axisInA), + m_bJ(-axisInB) { m_0MinvJt = inertiaInvA * m_aJ; m_1MinvJt = inertiaInvB * m_bJ; m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ); @@ -110,8 +112,8 @@ public: const Vector3 &rel_pos1, const Vector3 &rel_pos2, const Vector3 &jointAxis, const Vector3 &inertiaInvA, - const real_t massInvA) - : m_linearJointAxis(jointAxis) { + const real_t massInvA) : + m_linearJointAxis(jointAxis) { m_aJ = world2A.xform(rel_pos1.cross(jointAxis)); m_bJ = world2A.xform(rel_pos2.cross(-jointAxis)); m_0MinvJt = inertiaInvA * m_aJ; diff --git a/servers/physics/joints/pin_joint_sw.cpp b/servers/physics/joints/pin_joint_sw.cpp index 779e086858..a094e6c5ae 100644 --- a/servers/physics/joints/pin_joint_sw.cpp +++ b/servers/physics/joints/pin_joint_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2018 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 */ @@ -146,8 +146,8 @@ real_t PinJointSW::get_param(PhysicsServer::PinJointParam p_param) const { return 0; } -PinJointSW::PinJointSW(BodySW *p_body_a, const Vector3 &p_pos_a, BodySW *p_body_b, const Vector3 &p_pos_b) - : JointSW(_arr, 2) { +PinJointSW::PinJointSW(BodySW *p_body_a, const Vector3 &p_pos_a, BodySW *p_body_b, const Vector3 &p_pos_b) : + JointSW(_arr, 2) { A = p_body_a; B = p_body_b; diff --git a/servers/physics/joints/pin_joint_sw.h b/servers/physics/joints/pin_joint_sw.h index f6c11c49b0..7d843f053c 100644 --- a/servers/physics/joints/pin_joint_sw.h +++ b/servers/physics/joints/pin_joint_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2018 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 */ diff --git a/servers/physics/joints/slider_joint_sw.cpp b/servers/physics/joints/slider_joint_sw.cpp index 66585f2605..c0e9660b22 100644 --- a/servers/physics/joints/slider_joint_sw.cpp +++ b/servers/physics/joints/slider_joint_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2018 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 */ @@ -112,8 +112,10 @@ void SliderJointSW::initParams() { //----------------------------------------------------------------------------- -SliderJointSW::SliderJointSW(BodySW *rbA, BodySW *rbB, const Transform &frameInA, const Transform &frameInB) - : JointSW(_arr, 2), m_frameInA(frameInA), m_frameInB(frameInB) { +SliderJointSW::SliderJointSW(BodySW *rbA, BodySW *rbB, const Transform &frameInA, const Transform &frameInB) : + JointSW(_arr, 2), + m_frameInA(frameInA), + m_frameInB(frameInB) { A = rbA; B = rbB; diff --git a/servers/physics/joints/slider_joint_sw.h b/servers/physics/joints/slider_joint_sw.h index 5079c65707..4df632d75f 100644 --- a/servers/physics/joints/slider_joint_sw.h +++ b/servers/physics/joints/slider_joint_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2018 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 */ diff --git a/servers/physics/joints_sw.h b/servers/physics/joints_sw.h index b15ae08049..af27b1c25b 100644 --- a/servers/physics/joints_sw.h +++ b/servers/physics/joints_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2018 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 */ @@ -27,6 +27,7 @@ /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ + #ifndef JOINTS_SW_H #define JOINTS_SW_H @@ -37,8 +38,8 @@ class JointSW : public ConstraintSW { public: virtual PhysicsServer::JointType get_type() const = 0; - _FORCE_INLINE_ JointSW(BodySW **p_body_ptr = NULL, int p_body_count = 0) - : ConstraintSW(p_body_ptr, p_body_count) { + _FORCE_INLINE_ JointSW(BodySW **p_body_ptr = NULL, int p_body_count = 0) : + ConstraintSW(p_body_ptr, p_body_count) { } }; diff --git a/servers/physics/physics_server_sw.cpp b/servers/physics/physics_server_sw.cpp index 5ba935d47c..f2dbb635f8 100644 --- a/servers/physics/physics_server_sw.cpp +++ b/servers/physics/physics_server_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2018 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 */ @@ -27,6 +27,7 @@ /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ + #include "physics_server_sw.h" #include "broad_phase_basic.h" @@ -695,6 +696,19 @@ real_t PhysicsServerSW::body_get_param(RID p_body, BodyParameter p_param) const return body->get_param(p_param); }; +void PhysicsServerSW::body_set_kinematic_safe_margin(RID p_body, real_t p_margin) { + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + body->set_kinematic_margin(p_margin); +} + +real_t PhysicsServerSW::body_get_kinematic_safe_margin(RID p_body) const { + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND_V(!body, 0); + + return body->get_kinematic_margin(); +} + void PhysicsServerSW::body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) { BodySW *body = body_owner.get(p_body); @@ -781,20 +795,20 @@ void PhysicsServerSW::body_set_axis_velocity(RID p_body, const Vector3 &p_axis_v body->wakeup(); }; -void PhysicsServerSW::body_set_axis_lock(RID p_body, BodyAxisLock p_lock) { +void PhysicsServerSW::body_set_axis_lock(RID p_body, BodyAxis p_axis, bool p_lock) { BodySW *body = body_owner.get(p_body); ERR_FAIL_COND(!body); - body->set_axis_lock(p_lock); + body->set_axis_lock(p_axis, p_lock); body->wakeup(); } -PhysicsServerSW::BodyAxisLock PhysicsServerSW::body_get_axis_lock(RID p_body) const { +bool PhysicsServerSW::body_is_axis_locked(RID p_body, BodyAxis p_axis) const { const BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body, BODY_AXIS_LOCK_DISABLED); - return body->get_axis_lock(); + ERR_FAIL_COND_V(!body, 0); + return body->is_axis_locked(p_axis); } void PhysicsServerSW::body_add_collision_exception(RID p_body, RID p_body_b) { @@ -888,7 +902,7 @@ bool PhysicsServerSW::body_is_ray_pickable(RID p_body) const { return body->is_ray_pickable(); } -bool PhysicsServerSW::body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, float p_margin, MotionResult *r_result) { +bool PhysicsServerSW::body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, MotionResult *r_result) { BodySW *body = body_owner.get(p_body); ERR_FAIL_COND_V(!body, false); @@ -897,7 +911,7 @@ bool PhysicsServerSW::body_test_motion(RID p_body, const Transform &p_from, cons _update_shapes(); - return body->get_space()->test_body_motion(body, p_from, p_motion, p_margin, r_result); + return body->get_space()->test_body_motion(body, p_from, p_motion, p_infinite_inertia, body->get_kinematic_margin(), r_result); } PhysicsDirectBodyState *PhysicsServerSW::body_get_direct_state(RID p_body) { @@ -1079,6 +1093,33 @@ int PhysicsServerSW::joint_get_solver_priority(RID p_joint) const { return joint->get_priority(); } +void PhysicsServerSW::joint_disable_collisions_between_bodies(RID p_joint, const bool p_disable) { + JointSW *joint = joint_owner.get(p_joint); + ERR_FAIL_COND(!joint); + + joint->disable_collisions_between_bodies(p_disable); + + if (2 == joint->get_body_count()) { + BodySW *body_a = *joint->get_body_ptr(); + BodySW *body_b = *(joint->get_body_ptr() + 1); + + if (p_disable) { + body_add_collision_exception(body_a->get_self(), body_b->get_self()); + body_add_collision_exception(body_b->get_self(), body_a->get_self()); + } else { + body_remove_collision_exception(body_a->get_self(), body_b->get_self()); + body_remove_collision_exception(body_b->get_self(), body_a->get_self()); + } + } +} + +bool PhysicsServerSW::joint_is_disabled_collisions_between_bodies(RID p_joint) const { + JointSW *joint = joint_owner.get(p_joint); + ERR_FAIL_COND_V(!joint, true); + + return joint->is_disabled_collisions_between_bodies(); +} + PhysicsServerSW::JointType PhysicsServerSW::joint_get_type(RID p_joint) const { JointSW *joint = joint_owner.get(p_joint); diff --git a/servers/physics/physics_server_sw.h b/servers/physics/physics_server_sw.h index f9eb8fa454..3f56ba26d0 100644 --- a/servers/physics/physics_server_sw.h +++ b/servers/physics/physics_server_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2018 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 */ @@ -27,6 +27,7 @@ /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ + #ifndef PHYSICS_SERVER_SW #define PHYSICS_SERVER_SW @@ -187,6 +188,9 @@ public: virtual void body_set_param(RID p_body, BodyParameter p_param, real_t p_value); virtual real_t body_get_param(RID p_body, BodyParameter p_param) const; + virtual void body_set_kinematic_safe_margin(RID p_body, real_t p_margin); + virtual real_t body_get_kinematic_safe_margin(RID p_body) const; + virtual void body_set_state(RID p_body, BodyState p_state, const Variant &p_variant); virtual Variant body_get_state(RID p_body, BodyState p_state) const; @@ -200,8 +204,8 @@ public: virtual void body_apply_torque_impulse(RID p_body, const Vector3 &p_impulse); virtual void body_set_axis_velocity(RID p_body, const Vector3 &p_axis_velocity); - virtual void body_set_axis_lock(RID p_body, BodyAxisLock p_lock); - virtual BodyAxisLock body_get_axis_lock(RID p_body) const; + virtual void body_set_axis_lock(RID p_body, BodyAxis p_axis, bool p_lock); + virtual bool body_is_axis_locked(RID p_body, BodyAxis p_axis) const; virtual void body_add_collision_exception(RID p_body, RID p_body_b); virtual void body_remove_collision_exception(RID p_body, RID p_body_b); @@ -221,7 +225,7 @@ public: virtual void body_set_ray_pickable(RID p_body, bool p_enable); virtual bool body_is_ray_pickable(RID p_body) const; - virtual bool body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, float p_margin = 0.001, MotionResult *r_result = NULL); + virtual bool body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, MotionResult *r_result = NULL); // this function only works on physics process, errors and returns null otherwise virtual PhysicsDirectBodyState *body_get_direct_state(RID p_body); @@ -271,6 +275,9 @@ public: virtual void joint_set_solver_priority(RID p_joint, int p_priority); virtual int joint_get_solver_priority(RID p_joint) const; + virtual void joint_disable_collisions_between_bodies(RID p_joint, const bool p_disable); + virtual bool joint_is_disabled_collisions_between_bodies(RID p_joint) const; + /* MISC */ virtual void free(RID p_rid); diff --git a/servers/physics/shape_sw.cpp b/servers/physics/shape_sw.cpp index 6dafaac115..9d2e5e846d 100644 --- a/servers/physics/shape_sw.cpp +++ b/servers/physics/shape_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2018 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 */ @@ -27,6 +27,7 @@ /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ + #include "shape_sw.h" #include "geometry.h" @@ -37,7 +38,7 @@ #define _EDGE_IS_VALID_SUPPORT_THRESHOLD 0.0002 #define _FACE_IS_VALID_SUPPORT_THRESHOLD 0.9998 -void ShapeSW::configure(const Rect3 &p_aabb) { +void ShapeSW::configure(const AABB &p_aabb) { aabb = p_aabb; configured = true; for (Map<ShapeOwnerSW *, int>::Element *E = owners.front(); E; E = E->next()) { @@ -141,7 +142,7 @@ Vector3 PlaneShapeSW::get_moment_of_inertia(real_t p_mass) const { void PlaneShapeSW::_setup(const Plane &p_plane) { plane = p_plane; - configure(Rect3(Vector3(-1e4, -1e4, -1e4), Vector3(1e4 * 2, 1e4 * 2, 1e4 * 2))); + configure(AABB(Vector3(-1e4, -1e4, -1e4), Vector3(1e4 * 2, 1e4 * 2, 1e4 * 2))); } void PlaneShapeSW::set_data(const Variant &p_data) { @@ -164,6 +165,10 @@ real_t RayShapeSW::get_length() const { return length; } +bool RayShapeSW::get_slips_on_slope() const { + return slips_on_slope; +} + void RayShapeSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { // don't think this will be even used @@ -220,25 +225,31 @@ Vector3 RayShapeSW::get_moment_of_inertia(real_t p_mass) const { return Vector3(); } -void RayShapeSW::_setup(real_t p_length) { +void RayShapeSW::_setup(real_t p_length, bool p_slips_on_slope) { length = p_length; - configure(Rect3(Vector3(0, 0, 0), Vector3(0.1, 0.1, length))); + slips_on_slope = p_slips_on_slope; + configure(AABB(Vector3(0, 0, 0), Vector3(0.1, 0.1, length))); } void RayShapeSW::set_data(const Variant &p_data) { - _setup(p_data); + Dictionary d = p_data; + _setup(d["length"], d["slips_on_slope"]); } Variant RayShapeSW::get_data() const { - return length; + Dictionary d; + d["length"] = length; + d["slips_on_slope"] = slips_on_slope; + return d; } RayShapeSW::RayShapeSW() { length = 1; + slips_on_slope = false; } /********** SPHERE *************/ @@ -299,7 +310,7 @@ Vector3 SphereShapeSW::get_moment_of_inertia(real_t p_mass) const { void SphereShapeSW::_setup(real_t p_radius) { radius = p_radius; - configure(Rect3(Vector3(-radius, -radius, -radius), Vector3(radius * 2.0, radius * 2.0, radius * 2.0))); + configure(AABB(Vector3(-radius, -radius, -radius), Vector3(radius * 2.0, radius * 2.0, radius * 2.0))); } void SphereShapeSW::set_data(const Variant &p_data) { @@ -430,7 +441,7 @@ void BoxShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_sup bool BoxShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { - Rect3 aabb(-half_extents, half_extents * 2.0); + AABB aabb(-half_extents, half_extents * 2.0); return aabb.intersects_segment(p_begin, p_end, &r_result, &r_normal); } @@ -504,7 +515,7 @@ void BoxShapeSW::_setup(const Vector3 &p_half_extents) { half_extents = p_half_extents.abs(); - configure(Rect3(-half_extents, half_extents * 2)); + configure(AABB(-half_extents, half_extents * 2)); } void BoxShapeSW::set_data(const Variant &p_data) { @@ -671,7 +682,7 @@ Vector3 CapsuleShapeSW::get_closest_point_to(const Vector3 &p_point) const { Vector3 CapsuleShapeSW::get_moment_of_inertia(real_t p_mass) const { - // use crappy AABB approximation + // use bad AABB approximation Vector3 extents = get_aabb().size * 0.5; return Vector3( @@ -684,7 +695,7 @@ void CapsuleShapeSW::_setup(real_t p_height, real_t p_radius) { height = p_height; radius = p_radius; - configure(Rect3(Vector3(-radius, -radius, -height * 0.5 - radius), Vector3(radius * 2, radius * 2, height + radius * 2.0))); + configure(AABB(Vector3(-radius, -radius, -height * 0.5 - radius), Vector3(radius * 2, radius * 2, height + radius * 2.0))); } void CapsuleShapeSW::set_data(const Variant &p_data) { @@ -942,7 +953,7 @@ Vector3 ConvexPolygonShapeSW::get_closest_point_to(const Vector3 &p_point) const Vector3 ConvexPolygonShapeSW::get_moment_of_inertia(real_t p_mass) const { - // use crappy AABB approximation + // use bad AABB approximation Vector3 extents = get_aabb().size * 0.5; return Vector3( @@ -957,7 +968,7 @@ void ConvexPolygonShapeSW::_setup(const Vector<Vector3> &p_vertices) { if (err != OK) ERR_PRINT("Failed to build QuickHull"); - Rect3 _aabb; + AABB _aabb; for (int i = 0; i < mesh.vertices.size(); i++) { @@ -1102,7 +1113,7 @@ Vector3 FaceShapeSW::get_moment_of_inertia(real_t p_mass) const { FaceShapeSW::FaceShapeSW() { - configure(Rect3()); + configure(AABB()); } PoolVector<Vector3> ConcavePolygonShapeSW::get_faces() const { @@ -1300,13 +1311,13 @@ void ConcavePolygonShapeSW::_cull(int p_idx, _CullParams *p_params) const { } } -void ConcavePolygonShapeSW::cull(const Rect3 &p_local_aabb, Callback p_callback, void *p_userdata) const { +void ConcavePolygonShapeSW::cull(const AABB &p_local_aabb, Callback p_callback, void *p_userdata) const { // make matrix local to concave if (faces.size() == 0) return; - Rect3 local_aabb = p_local_aabb; + AABB local_aabb = p_local_aabb; // unlock data PoolVector<Face>::Read fr = faces.read(); @@ -1330,7 +1341,7 @@ void ConcavePolygonShapeSW::cull(const Rect3 &p_local_aabb, Callback p_callback, Vector3 ConcavePolygonShapeSW::get_moment_of_inertia(real_t p_mass) const { - // use crappy AABB approximation + // use bad AABB approximation Vector3 extents = get_aabb().size * 0.5; return Vector3( @@ -1341,7 +1352,7 @@ Vector3 ConcavePolygonShapeSW::get_moment_of_inertia(real_t p_mass) const { struct _VolumeSW_BVH_Element { - Rect3 aabb; + AABB aabb; Vector3 center; int face_index; }; @@ -1372,7 +1383,7 @@ struct _VolumeSW_BVH_CompareZ { struct _VolumeSW_BVH { - Rect3 aabb; + AABB aabb; _VolumeSW_BVH *left; _VolumeSW_BVH *right; @@ -1396,7 +1407,7 @@ _VolumeSW_BVH *_volume_sw_build_bvh(_VolumeSW_BVH_Element *p_elements, int p_siz bvh->face_index = -1; } - Rect3 aabb; + AABB aabb; for (int i = 0; i < p_size; i++) { if (i == 0) @@ -1467,7 +1478,7 @@ void ConcavePolygonShapeSW::_setup(PoolVector<Vector3> p_faces) { int src_face_count = p_faces.size(); if (src_face_count == 0) { - configure(Rect3()); + configure(AABB()); return; } ERR_FAIL_COND(src_face_count % 3); @@ -1491,7 +1502,7 @@ void ConcavePolygonShapeSW::_setup(PoolVector<Vector3> p_faces) { PoolVector<Vector3>::Write vw = vertices.write(); Vector3 *verticesw = vw.ptr(); - Rect3 _aabb; + AABB _aabb; for (int i = 0; i < src_face_count; i++) { @@ -1588,12 +1599,12 @@ Vector3 HeightMapShapeSW::get_closest_point_to(const Vector3 &p_point) const { return Vector3(); } -void HeightMapShapeSW::cull(const Rect3 &p_local_aabb, Callback p_callback, void *p_userdata) const { +void HeightMapShapeSW::cull(const AABB &p_local_aabb, Callback p_callback, void *p_userdata) const { } Vector3 HeightMapShapeSW::get_moment_of_inertia(real_t p_mass) const { - // use crappy AABB approximation + // use bad AABB approximation Vector3 extents = get_aabb().size * 0.5; return Vector3( @@ -1611,7 +1622,7 @@ void HeightMapShapeSW::_setup(PoolVector<real_t> p_heights, int p_width, int p_d PoolVector<real_t>::Read r = heights.read(); - Rect3 aabb; + AABB aabb; for (int i = 0; i < depth; i++) { diff --git a/servers/physics/shape_sw.h b/servers/physics/shape_sw.h index 151b84c054..7f7f9f4f98 100644 --- a/servers/physics/shape_sw.h +++ b/servers/physics/shape_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2018 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 */ @@ -27,6 +27,7 @@ /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ + #ifndef SHAPE_SW_H #define SHAPE_SW_H @@ -58,14 +59,14 @@ public: class ShapeSW : public RID_Data { RID self; - Rect3 aabb; + AABB aabb; bool configured; real_t custom_bias; Map<ShapeOwnerSW *, int> owners; protected: - void configure(const Rect3 &p_aabb); + void configure(const AABB &p_aabb); public: enum { @@ -79,7 +80,7 @@ public: virtual PhysicsServer::ShapeType get_type() const = 0; - _FORCE_INLINE_ Rect3 get_aabb() const { return aabb; } + _FORCE_INLINE_ AABB get_aabb() const { return aabb; } _FORCE_INLINE_ bool is_configured() const { return configured; } virtual bool is_concave() const { return false; } @@ -114,7 +115,7 @@ public: typedef void (*Callback)(void *p_userdata, ShapeSW *p_convex); virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { r_amount = 0; } - virtual void cull(const Rect3 &p_local_aabb, Callback p_callback, void *p_userdata) const = 0; + virtual void cull(const AABB &p_local_aabb, Callback p_callback, void *p_userdata) const = 0; ConcaveShapeSW() {} }; @@ -148,11 +149,13 @@ public: class RayShapeSW : public ShapeSW { real_t length; + bool slips_on_slope; - void _setup(real_t p_length); + void _setup(real_t p_length, bool p_slips_on_slope); public: real_t get_length() const; + bool get_slips_on_slope() const; virtual real_t get_area() const { return 0.0; } virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_RAY; } @@ -299,7 +302,7 @@ struct ConcavePolygonShapeSW : public ConcaveShapeSW { struct BVH { - Rect3 aabb; + AABB aabb; int left; int right; @@ -310,7 +313,7 @@ struct ConcavePolygonShapeSW : public ConcaveShapeSW { struct _CullParams { - Rect3 aabb; + AABB aabb; Callback callback; void *userdata; const Face *faces; @@ -353,7 +356,7 @@ public: virtual bool intersect_point(const Vector3 &p_point) const; virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; - virtual void cull(const Rect3 &p_local_aabb, Callback p_callback, void *p_userdata) const; + virtual void cull(const AABB &p_local_aabb, Callback p_callback, void *p_userdata) const; virtual Vector3 get_moment_of_inertia(real_t p_mass) const; @@ -389,7 +392,7 @@ public: virtual bool intersect_point(const Vector3 &p_point) const; virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; - virtual void cull(const Rect3 &p_local_aabb, Callback p_callback, void *p_userdata) const; + virtual void cull(const AABB &p_local_aabb, Callback p_callback, void *p_userdata) const; virtual Vector3 get_moment_of_inertia(real_t p_mass) const; @@ -462,7 +465,7 @@ struct MotionShapeSW : public ShapeSW { virtual void set_data(const Variant &p_data) {} virtual Variant get_data() const { return Variant(); } - MotionShapeSW() { configure(Rect3()); } + MotionShapeSW() { configure(AABB()); } }; struct _ShapeTestConvexBSPSW { diff --git a/servers/physics/space_sw.cpp b/servers/physics/space_sw.cpp index 7e68d54bfa..b604e5cdf6 100644 --- a/servers/physics/space_sw.cpp +++ b/servers/physics/space_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2018 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 */ @@ -27,26 +27,19 @@ /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ + #include "space_sw.h" #include "collision_solver_sw.h" #include "physics_server_sw.h" #include "project_settings.h" -_FORCE_INLINE_ static bool _match_object_type_query(CollisionObjectSW *p_object, uint32_t p_collision_mask, uint32_t p_type_mask) { - - if ((p_object->get_collision_layer() & p_collision_mask) == 0) - return false; - - if (p_object->get_type() == CollisionObjectSW::TYPE_AREA) - return p_type_mask & PhysicsDirectSpaceState::TYPE_MASK_AREA; - - BodySW *body = static_cast<BodySW *>(p_object); +_FORCE_INLINE_ static bool _can_collide_with(CollisionObjectSW *p_object, uint32_t p_collision_mask) { - return (1 << body->get_mode()) & p_type_mask; + return p_object->get_collision_layer() & p_collision_mask; } -int PhysicsDirectSpaceStateSW::intersect_point(const Vector3 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_mask, uint32_t p_object_type_mask) { +int PhysicsDirectSpaceStateSW::intersect_point(const Vector3 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_mask) { ERR_FAIL_COND_V(space->locked, false); int amount = space->broadphase->cull_point(p_point, space->intersection_query_results, SpaceSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); @@ -59,7 +52,7 @@ int PhysicsDirectSpaceStateSW::intersect_point(const Vector3 &p_point, ShapeResu if (cc >= p_result_max) break; - if (!_match_object_type_query(space->intersection_query_results[i], p_collision_mask, p_object_type_mask)) + if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask)) continue; //area can't be picked by ray (default) @@ -90,7 +83,7 @@ int PhysicsDirectSpaceStateSW::intersect_point(const Vector3 &p_point, ShapeResu return cc; } -bool PhysicsDirectSpaceStateSW::intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const Set<RID> &p_exclude, uint32_t p_collision_mask, uint32_t p_object_type_mask, bool p_pick_ray) { +bool PhysicsDirectSpaceStateSW::intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_pick_ray) { ERR_FAIL_COND_V(space->locked, false); @@ -112,7 +105,7 @@ bool PhysicsDirectSpaceStateSW::intersect_ray(const Vector3 &p_from, const Vecto for (int i = 0; i < amount; i++) { - if (!_match_object_type_query(space->intersection_query_results[i], p_collision_mask, p_object_type_mask)) + if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask)) continue; if (p_pick_ray && !(static_cast<CollisionObjectSW *>(space->intersection_query_results[i])->is_ray_pickable())) @@ -168,7 +161,7 @@ bool PhysicsDirectSpaceStateSW::intersect_ray(const Vector3 &p_from, const Vecto return true; } -int PhysicsDirectSpaceStateSW::intersect_shape(const RID &p_shape, const Transform &p_xform, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_mask, uint32_t p_object_type_mask) { +int PhysicsDirectSpaceStateSW::intersect_shape(const RID &p_shape, const Transform &p_xform, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_mask) { if (p_result_max <= 0) return 0; @@ -176,7 +169,7 @@ int PhysicsDirectSpaceStateSW::intersect_shape(const RID &p_shape, const Transfo ShapeSW *shape = static_cast<PhysicsServerSW *>(PhysicsServer::get_singleton())->shape_owner.get(p_shape); ERR_FAIL_COND_V(!shape, 0); - Rect3 aabb = p_xform.xform(shape->get_aabb()); + AABB aabb = p_xform.xform(shape->get_aabb()); int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, SpaceSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); @@ -189,7 +182,7 @@ int PhysicsDirectSpaceStateSW::intersect_shape(const RID &p_shape, const Transfo if (cc >= p_result_max) break; - if (!_match_object_type_query(space->intersection_query_results[i], p_collision_mask, p_object_type_mask)) + if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask)) continue; //area can't be picked by ray (default) @@ -219,13 +212,13 @@ int PhysicsDirectSpaceStateSW::intersect_shape(const RID &p_shape, const Transfo return cc; } -bool PhysicsDirectSpaceStateSW::cast_motion(const RID &p_shape, const Transform &p_xform, const Vector3 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set<RID> &p_exclude, uint32_t p_collision_mask, uint32_t p_object_type_mask, ShapeRestInfo *r_info) { +bool PhysicsDirectSpaceStateSW::cast_motion(const RID &p_shape, const Transform &p_xform, const Vector3 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set<RID> &p_exclude, uint32_t p_collision_mask, ShapeRestInfo *r_info) { ShapeSW *shape = static_cast<PhysicsServerSW *>(PhysicsServer::get_singleton())->shape_owner.get(p_shape); ERR_FAIL_COND_V(!shape, false); - Rect3 aabb = p_xform.xform(shape->get_aabb()); - aabb = aabb.merge(Rect3(aabb.position + p_motion, aabb.size)); //motion + AABB aabb = p_xform.xform(shape->get_aabb()); + aabb = aabb.merge(AABB(aabb.position + p_motion, aabb.size)); //motion aabb = aabb.grow(p_margin); /* @@ -249,7 +242,7 @@ bool PhysicsDirectSpaceStateSW::cast_motion(const RID &p_shape, const Transform for (int i = 0; i < amount; i++) { - if (!_match_object_type_query(space->intersection_query_results[i], p_collision_mask, p_object_type_mask)) + if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask)) continue; if (p_exclude.has(space->intersection_query_results[i]->get_self())) @@ -333,7 +326,7 @@ bool PhysicsDirectSpaceStateSW::cast_motion(const RID &p_shape, const Transform return true; } -bool PhysicsDirectSpaceStateSW::collide_shape(RID p_shape, const Transform &p_shape_xform, real_t p_margin, Vector3 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude, uint32_t p_collision_mask, uint32_t p_object_type_mask) { +bool PhysicsDirectSpaceStateSW::collide_shape(RID p_shape, const Transform &p_shape_xform, real_t p_margin, Vector3 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude, uint32_t p_collision_mask) { if (p_result_max <= 0) return 0; @@ -341,7 +334,7 @@ bool PhysicsDirectSpaceStateSW::collide_shape(RID p_shape, const Transform &p_sh ShapeSW *shape = static_cast<PhysicsServerSW *>(PhysicsServer::get_singleton())->shape_owner.get(p_shape); ERR_FAIL_COND_V(!shape, 0); - Rect3 aabb = p_shape_xform.xform(shape->get_aabb()); + AABB aabb = p_shape_xform.xform(shape->get_aabb()); aabb = aabb.grow(p_margin); int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, SpaceSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); @@ -363,7 +356,7 @@ bool PhysicsDirectSpaceStateSW::collide_shape(RID p_shape, const Transform &p_sh for (int i = 0; i < amount; i++) { - if (!_match_object_type_query(space->intersection_query_results[i], p_collision_mask, p_object_type_mask)) + if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask)) continue; const CollisionObjectSW *col_obj = space->intersection_query_results[i]; @@ -412,12 +405,12 @@ static void _rest_cbk_result(const Vector3 &p_point_A, const Vector3 &p_point_B, rd->best_object = rd->object; rd->best_shape = rd->shape; } -bool PhysicsDirectSpaceStateSW::rest_info(RID p_shape, const Transform &p_shape_xform, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude, uint32_t p_collision_mask, uint32_t p_object_type_mask) { +bool PhysicsDirectSpaceStateSW::rest_info(RID p_shape, const Transform &p_shape_xform, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude, uint32_t p_collision_mask) { ShapeSW *shape = static_cast<PhysicsServerSW *>(PhysicsServer::get_singleton())->shape_owner.get(p_shape); ERR_FAIL_COND_V(!shape, 0); - Rect3 aabb = p_shape_xform.xform(shape->get_aabb()); + AABB aabb = p_shape_xform.xform(shape->get_aabb()); aabb = aabb.grow(p_margin); int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, SpaceSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); @@ -429,7 +422,7 @@ bool PhysicsDirectSpaceStateSW::rest_info(RID p_shape, const Transform &p_shape_ for (int i = 0; i < amount; i++) { - if (!_match_object_type_query(space->intersection_query_results[i], p_collision_mask, p_object_type_mask)) + if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask)) continue; const CollisionObjectSW *col_obj = space->intersection_query_results[i]; @@ -514,7 +507,7 @@ PhysicsDirectSpaceStateSW::PhysicsDirectSpaceStateSW() { //////////////////////////////////////////////////////////////////////////////////////////////////////////// -int SpaceSW::_cull_aabb_for_body(BodySW *p_body, const Rect3 &p_aabb) { +int SpaceSW::_cull_aabb_for_body(BodySW *p_body, const AABB &p_aabb) { int amount = broadphase->cull_aabb(p_aabb, intersection_query_results, INTERSECTION_QUERY_MAX, intersection_query_subindex_results); @@ -548,7 +541,7 @@ int SpaceSW::_cull_aabb_for_body(BodySW *p_body, const Rect3 &p_aabb) { return amount; } -bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Vector3 &p_motion, real_t p_margin, PhysicsServer::MotionResult *r_result) { +bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, real_t p_margin, PhysicsServer::MotionResult *r_result) { //give me back regular physics engine logic //this is madness @@ -561,7 +554,7 @@ bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Ve r_result->collider_id = 0; r_result->collider_shape = 0; } - Rect3 body_aabb; + AABB body_aabb; for (int i = 0; i < p_body->get_shape_count(); i++) { @@ -648,7 +641,7 @@ bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Ve { // STEP 2 ATTEMPT MOTION - Rect3 motion_aabb = body_aabb; + AABB motion_aabb = body_aabb; motion_aabb.position += p_motion; motion_aabb = motion_aabb.merge(body_aabb); @@ -956,15 +949,15 @@ void SpaceSW::call_queries() { while (state_query_list.first()) { BodySW *b = state_query_list.first()->self(); - b->call_queries(); state_query_list.remove(state_query_list.first()); + b->call_queries(); } while (monitor_query_list.first()) { AreaSW *a = monitor_query_list.first()->self(); - a->call_queries(); monitor_query_list.remove(monitor_query_list.first()); + a->call_queries(); } } diff --git a/servers/physics/space_sw.h b/servers/physics/space_sw.h index 270e4ef1bd..2452d6a187 100644 --- a/servers/physics/space_sw.h +++ b/servers/physics/space_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2018 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 */ @@ -27,6 +27,7 @@ /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ + #ifndef SPACE_SW_H #define SPACE_SW_H @@ -47,12 +48,12 @@ class PhysicsDirectSpaceStateSW : public PhysicsDirectSpaceState { public: SpaceSW *space; - virtual int intersect_point(const Vector3 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION); - virtual bool intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION, bool p_pick_ray = false); - virtual int intersect_shape(const RID &p_shape, const Transform &p_xform, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION); - virtual bool cast_motion(const RID &p_shape, const Transform &p_xform, const Vector3 &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, uint32_t p_object_type_mask = TYPE_MASK_COLLISION, ShapeRestInfo *r_info = NULL); - virtual bool collide_shape(RID p_shape, const Transform &p_shape_xform, real_t p_margin, Vector3 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION); - virtual bool rest_info(RID p_shape, const Transform &p_shape_xform, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION); + virtual int intersect_point(const Vector3 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF); + virtual bool intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_pick_ray = false); + virtual int intersect_shape(const RID &p_shape, const Transform &p_xform, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF); + virtual bool cast_motion(const RID &p_shape, const Transform &p_xform, const Vector3 &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, ShapeRestInfo *r_info = NULL); + virtual bool collide_shape(RID p_shape, const Transform &p_shape_xform, real_t p_margin, Vector3 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF); + virtual bool rest_info(RID p_shape, const Transform &p_shape_xform, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF); virtual Vector3 get_closest_point_to_object_volume(RID p_object, const Vector3 p_point) const; PhysicsDirectSpaceStateSW(); @@ -122,7 +123,7 @@ private: friend class PhysicsDirectSpaceStateSW; - int _cull_aabb_for_body(BodySW *p_body, const Rect3 &p_aabb); + int _cull_aabb_for_body(BodySW *p_body, const AABB &p_aabb); public: _FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; } @@ -196,7 +197,7 @@ public: void set_elapsed_time(ElapsedTime p_time, uint64_t p_msec) { elapsed_time[p_time] = p_msec; } uint64_t get_elapsed_time(ElapsedTime p_time) const { return elapsed_time[p_time]; } - bool test_body_motion(BodySW *p_body, const Transform &p_from, const Vector3 &p_motion, real_t p_margin, PhysicsServer::MotionResult *r_result); + bool test_body_motion(BodySW *p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, real_t p_margin, PhysicsServer::MotionResult *r_result); SpaceSW(); ~SpaceSW(); diff --git a/servers/physics/step_sw.cpp b/servers/physics/step_sw.cpp index 76b097dda6..ad08cb6353 100644 --- a/servers/physics/step_sw.cpp +++ b/servers/physics/step_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2018 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 */ @@ -27,6 +27,7 @@ /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ + #include "step_sw.h" #include "joints_sw.h" diff --git a/servers/physics/step_sw.h b/servers/physics/step_sw.h index 56acd8ac37..25e9fedf79 100644 --- a/servers/physics/step_sw.h +++ b/servers/physics/step_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2018 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 */ @@ -27,6 +27,7 @@ /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ + #ifndef STEP_SW_H #define STEP_SW_H |