diff options
Diffstat (limited to 'servers/physics_3d')
45 files changed, 2379 insertions, 774 deletions
diff --git a/servers/physics_3d/area_3d_sw.cpp b/servers/physics_3d/area_3d_sw.cpp index 98237dd91c..b6c5b3003c 100644 --- a/servers/physics_3d/area_3d_sw.cpp +++ b/servers/physics_3d/area_3d_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -199,7 +199,7 @@ void Area3DSW::set_monitorable(bool p_monitorable) { } void Area3DSW::call_queries() { - if (monitor_callback_id.is_valid() && !monitored_bodies.empty()) { + if (monitor_callback_id.is_valid() && !monitored_bodies.is_empty()) { Variant res[5]; Variant *resptr[5]; for (int i = 0; i < 5; i++) { @@ -213,9 +213,10 @@ void Area3DSW::call_queries() { return; } - for (Map<BodyKey, BodyState>::Element *E = monitored_bodies.front(); E; E = E->next()) { - if (E->get().state == 0) { - continue; //nothing happened + for (Map<BodyKey, BodyState>::Element *E = monitored_bodies.front(); E;) { + if (E->get().state == 0) { // Nothing happened + E = E->next(); + continue; } res[0] = E->get().state > 0 ? PhysicsServer3D::AREA_BODY_ADDED : PhysicsServer3D::AREA_BODY_REMOVED; @@ -224,14 +225,16 @@ void Area3DSW::call_queries() { res[3] = E->key().body_shape; res[4] = E->key().area_shape; + Map<BodyKey, BodyState>::Element *next = E->next(); + monitored_bodies.erase(E); + E = next; + Callable::CallError ce; obj->call(monitor_callback_method, (const Variant **)resptr, 5, ce); } } - monitored_bodies.clear(); - - if (area_monitor_callback_id.is_valid() && !monitored_areas.empty()) { + if (area_monitor_callback_id.is_valid() && !monitored_areas.is_empty()) { Variant res[5]; Variant *resptr[5]; for (int i = 0; i < 5; i++) { @@ -245,9 +248,10 @@ void Area3DSW::call_queries() { return; } - for (Map<BodyKey, BodyState>::Element *E = monitored_areas.front(); E; E = E->next()) { - if (E->get().state == 0) { - continue; //nothing happened + for (Map<BodyKey, BodyState>::Element *E = monitored_areas.front(); E;) { + if (E->get().state == 0) { // Nothing happened + E = E->next(); + continue; } res[0] = E->get().state > 0 ? PhysicsServer3D::AREA_BODY_ADDED : PhysicsServer3D::AREA_BODY_REMOVED; @@ -256,13 +260,14 @@ void Area3DSW::call_queries() { res[3] = E->key().body_shape; res[4] = E->key().area_shape; + Map<BodyKey, BodyState>::Element *next = E->next(); + monitored_areas.erase(E); + E = next; + Callable::CallError ce; obj->call(area_monitor_callback_method, (const Variant **)resptr, 5, ce); } } - - monitored_areas.clear(); - //get_space()->area_remove_from_monitor_query_list(&monitor_query_list); } Area3DSW::Area3DSW() : diff --git a/servers/physics_3d/area_3d_sw.h b/servers/physics_3d/area_3d_sw.h index 6af3976167..8a0a1e963b 100644 --- a/servers/physics_3d/area_3d_sw.h +++ b/servers/physics_3d/area_3d_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -32,7 +32,7 @@ #define AREA_SW_H #include "collision_object_3d_sw.h" -#include "core/self_list.h" +#include "core/templates/self_list.h" #include "servers/physics_server_3d.h" //#include "servers/physics_3d/query_sw.h" diff --git a/servers/physics_3d/area_pair_3d_sw.cpp b/servers/physics_3d/area_pair_3d_sw.cpp index a5fb20fe2b..4de5f1ba47 100644 --- a/servers/physics_3d/area_pair_3d_sw.cpp +++ b/servers/physics_3d/area_pair_3d_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/servers/physics_3d/area_pair_3d_sw.h b/servers/physics_3d/area_pair_3d_sw.h index 992d4747b9..fbdaa25cbb 100644 --- a/servers/physics_3d/area_pair_3d_sw.h +++ b/servers/physics_3d/area_pair_3d_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/servers/physics_3d/body_3d_sw.cpp b/servers/physics_3d/body_3d_sw.cpp index a3bdc96c9f..82356e77ef 100644 --- a/servers/physics_3d/body_3d_sw.cpp +++ b/servers/physics_3d/body_3d_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -628,7 +628,6 @@ void Body3DSW::integrate_velocities(real_t p_step) { /* void BodySW::simulate_motion(const Transform& p_xform,real_t p_step) { - Transform inv_xform = p_xform.affine_inverse(); if (!get_space()) { _set_transform(p_xform); @@ -655,8 +654,6 @@ void BodySW::simulate_motion(const Transform& p_xform,real_t p_step) { get_space()->body_add_to_state_query_list(&direct_state_query_list); simulated_motion=true; _set_transform(p_xform); - - } */ @@ -750,7 +747,7 @@ Body3DSW::Body3DSW() : active = true; mass = 1; - kinematic_safe_margin = 0.01; + kinematic_safe_margin = 0.001; //_inv_inertia=Transform(); _inv_mass = 1; bounce = 0; diff --git a/servers/physics_3d/body_3d_sw.h b/servers/physics_3d/body_3d_sw.h index 483ea58620..8e21003a5f 100644 --- a/servers/physics_3d/body_3d_sw.h +++ b/servers/physics_3d/body_3d_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -33,7 +33,7 @@ #include "area_3d_sw.h" #include "collision_object_3d_sw.h" -#include "core/vset.h" +#include "core/templates/vset.h" class Constraint3DSW; @@ -178,7 +178,7 @@ public: } _FORCE_INLINE_ int get_max_contacts_reported() const { return contacts.size(); } - _FORCE_INLINE_ bool can_report_contacts() const { return !contacts.empty(); } + _FORCE_INLINE_ bool can_report_contacts() const { return !contacts.is_empty(); } _FORCE_INLINE_ void add_contact(const Vector3 &p_local_pos, const Vector3 &p_local_normal, real_t p_depth, int p_local_shape, const Vector3 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector3 &p_collider_velocity_at_pos); _FORCE_INLINE_ void add_exception(const RID &p_exception) { exceptions.insert(p_exception); } @@ -216,23 +216,23 @@ public: _FORCE_INLINE_ const Vector3 &get_biased_linear_velocity() const { return biased_linear_velocity; } _FORCE_INLINE_ const Vector3 &get_biased_angular_velocity() const { return biased_angular_velocity; } - _FORCE_INLINE_ void apply_central_impulse(const Vector3 &p_j) { - linear_velocity += p_j * _inv_mass; + _FORCE_INLINE_ void apply_central_impulse(const Vector3 &p_impulse) { + linear_velocity += p_impulse * _inv_mass; } - _FORCE_INLINE_ void apply_impulse(const Vector3 &p_pos, const Vector3 &p_j) { - linear_velocity += p_j * _inv_mass; - angular_velocity += _inv_inertia_tensor.xform((p_pos - center_of_mass).cross(p_j)); + _FORCE_INLINE_ void apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position = Vector3()) { + linear_velocity += p_impulse * _inv_mass; + angular_velocity += _inv_inertia_tensor.xform((p_position - center_of_mass).cross(p_impulse)); } - _FORCE_INLINE_ void apply_torque_impulse(const Vector3 &p_j) { - angular_velocity += _inv_inertia_tensor.xform(p_j); + _FORCE_INLINE_ void apply_torque_impulse(const Vector3 &p_impulse) { + angular_velocity += _inv_inertia_tensor.xform(p_impulse); } - _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; + _FORCE_INLINE_ void apply_bias_impulse(const Vector3 &p_impulse, const Vector3 &p_position = Vector3(), real_t p_max_delta_av = -1.0) { + biased_linear_velocity += p_impulse * _inv_mass; if (p_max_delta_av != 0.0) { - Vector3 delta_av = _inv_inertia_tensor.xform((p_pos - center_of_mass).cross(p_j)); + Vector3 delta_av = _inv_inertia_tensor.xform((p_position - center_of_mass).cross(p_impulse)); if (p_max_delta_av > 0 && delta_av.length() > p_max_delta_av) { delta_av = delta_av.normalized() * p_max_delta_av; } @@ -240,17 +240,17 @@ public: } } - _FORCE_INLINE_ void apply_bias_torque_impulse(const Vector3 &p_j) { - biased_angular_velocity += _inv_inertia_tensor.xform(p_j); + _FORCE_INLINE_ void apply_bias_torque_impulse(const Vector3 &p_impulse) { + biased_angular_velocity += _inv_inertia_tensor.xform(p_impulse); } _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) { + _FORCE_INLINE_ void add_force(const Vector3 &p_force, const Vector3 &p_position = Vector3()) { applied_force += p_force; - applied_torque += (p_pos - center_of_mass).cross(p_force); + applied_torque += (p_position - center_of_mass).cross(p_force); } _FORCE_INLINE_ void add_torque(const Vector3 &p_torque) { @@ -382,78 +382,82 @@ public: Body3DSW *body; real_t step; - virtual Vector3 get_total_gravity() const { return body->gravity; } // get gravity vector working on this body space/area - virtual real_t get_total_angular_damp() const { return body->area_angular_damp; } // get density of this body space/area - virtual real_t get_total_linear_damp() const { return body->area_linear_damp; } // get density of this body space/area + virtual Vector3 get_total_gravity() const override { return body->gravity; } // get gravity vector working on this body space/area + virtual real_t get_total_angular_damp() const override { return body->area_angular_damp; } // get density of this body space/area + virtual real_t get_total_linear_damp() const override { return body->area_linear_damp; } // get density of this body space/area - virtual Vector3 get_center_of_mass() const { return body->get_center_of_mass(); } - virtual Basis get_principal_inertia_axes() const { return body->get_principal_inertia_axes(); } + virtual Vector3 get_center_of_mass() const override { return body->get_center_of_mass(); } + virtual Basis get_principal_inertia_axes() const override { return body->get_principal_inertia_axes(); } - virtual real_t get_inverse_mass() const { return body->get_inv_mass(); } // get the mass - virtual Vector3 get_inverse_inertia() const { return body->get_inv_inertia(); } // get density of this body space - virtual Basis get_inverse_inertia_tensor() const { return body->get_inv_inertia_tensor(); } // get density of this body space + virtual real_t get_inverse_mass() const override { return body->get_inv_mass(); } // get the mass + virtual Vector3 get_inverse_inertia() const override { return body->get_inv_inertia(); } // get density of this body space + virtual Basis get_inverse_inertia_tensor() const override { return body->get_inv_inertia_tensor(); } // get density of this body space - virtual void set_linear_velocity(const Vector3 &p_velocity) { body->set_linear_velocity(p_velocity); } - virtual Vector3 get_linear_velocity() const { return body->get_linear_velocity(); } + virtual void set_linear_velocity(const Vector3 &p_velocity) override { body->set_linear_velocity(p_velocity); } + virtual Vector3 get_linear_velocity() const override { return body->get_linear_velocity(); } - virtual void set_angular_velocity(const Vector3 &p_velocity) { body->set_angular_velocity(p_velocity); } - virtual Vector3 get_angular_velocity() const { return body->get_angular_velocity(); } + virtual void set_angular_velocity(const Vector3 &p_velocity) override { body->set_angular_velocity(p_velocity); } + virtual Vector3 get_angular_velocity() const override { return body->get_angular_velocity(); } - virtual void set_transform(const Transform &p_transform) { body->set_state(PhysicsServer3D::BODY_STATE_TRANSFORM, p_transform); } - virtual Transform get_transform() const { return body->get_transform(); } + virtual void set_transform(const Transform &p_transform) override { body->set_state(PhysicsServer3D::BODY_STATE_TRANSFORM, p_transform); } + virtual Transform get_transform() const override { 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_central_impulse(const Vector3 &p_j) { body->apply_central_impulse(p_j); } - 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); } + virtual void add_central_force(const Vector3 &p_force) override { body->add_central_force(p_force); } + virtual void add_force(const Vector3 &p_force, const Vector3 &p_position = Vector3()) override { + body->add_force(p_force, p_position); + } + virtual void add_torque(const Vector3 &p_torque) override { body->add_torque(p_torque); } + virtual void apply_central_impulse(const Vector3 &p_impulse) override { body->apply_central_impulse(p_impulse); } + virtual void apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position = Vector3()) override { + body->apply_impulse(p_impulse, p_position); + } + virtual void apply_torque_impulse(const Vector3 &p_impulse) override { body->apply_torque_impulse(p_impulse); } - virtual void set_sleep_state(bool p_sleep) { body->set_active(!p_sleep); } - virtual bool is_sleeping() const { return !body->is_active(); } + virtual void set_sleep_state(bool p_sleep) override { body->set_active(!p_sleep); } + virtual bool is_sleeping() const override { return !body->is_active(); } - virtual int get_contact_count() const { return body->contact_count; } + virtual int get_contact_count() const override { return body->contact_count; } - virtual Vector3 get_contact_local_position(int p_contact_idx) const { + virtual Vector3 get_contact_local_position(int p_contact_idx) const override { ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3()); return body->contacts[p_contact_idx].local_pos; } - virtual Vector3 get_contact_local_normal(int p_contact_idx) const { + virtual Vector3 get_contact_local_normal(int p_contact_idx) const override { ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3()); return body->contacts[p_contact_idx].local_normal; } - virtual float get_contact_impulse(int p_contact_idx) const { + virtual real_t get_contact_impulse(int p_contact_idx) const override { return 0.0f; // Only implemented for bullet } - virtual int get_contact_local_shape(int p_contact_idx) const { + virtual int get_contact_local_shape(int p_contact_idx) const override { ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, -1); return body->contacts[p_contact_idx].local_shape; } - virtual RID get_contact_collider(int p_contact_idx) const { + virtual RID get_contact_collider(int p_contact_idx) const override { ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, RID()); return body->contacts[p_contact_idx].collider; } - virtual Vector3 get_contact_collider_position(int p_contact_idx) const { + virtual Vector3 get_contact_collider_position(int p_contact_idx) const override { ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3()); return body->contacts[p_contact_idx].collider_pos; } - virtual ObjectID get_contact_collider_id(int p_contact_idx) const { + virtual ObjectID get_contact_collider_id(int p_contact_idx) const override { ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, ObjectID()); return body->contacts[p_contact_idx].collider_instance_id; } - virtual int get_contact_collider_shape(int p_contact_idx) const { + virtual int get_contact_collider_shape(int p_contact_idx) const override { ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, 0); return body->contacts[p_contact_idx].collider_shape; } - virtual Vector3 get_contact_collider_velocity_at_position(int p_contact_idx) const { + virtual Vector3 get_contact_collider_velocity_at_position(int p_contact_idx) const override { ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3()); return body->contacts[p_contact_idx].collider_velocity_at_pos; } - virtual PhysicsDirectSpaceState3D *get_space_state(); + virtual PhysicsDirectSpaceState3D *get_space_state() override; - virtual real_t get_step() const { return step; } + virtual real_t get_step() const override { return step; } PhysicsDirectBodyState3DSW() { singleton = this; body = nullptr; diff --git a/servers/physics_3d/body_pair_3d_sw.cpp b/servers/physics_3d/body_pair_3d_sw.cpp index a4f86badbe..6012ff1522 100644 --- a/servers/physics_3d/body_pair_3d_sw.cpp +++ b/servers/physics_3d/body_pair_3d_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -321,8 +321,8 @@ bool BodyPair3DSW::setup(real_t p_step) { c.depth = depth; Vector3 j_vec = c.normal * c.acc_normal_impulse + c.acc_tangent_impulse; - A->apply_impulse(c.rA + A->get_center_of_mass(), -j_vec); - B->apply_impulse(c.rB + B->get_center_of_mass(), j_vec); + A->apply_impulse(-j_vec, c.rA + A->get_center_of_mass()); + B->apply_impulse(j_vec, c.rB + B->get_center_of_mass()); c.acc_bias_impulse = 0; c.acc_bias_impulse_center_of_mass = 0; @@ -367,8 +367,8 @@ void BodyPair3DSW::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, MAX_BIAS_ROTATION / p_step); - B->apply_bias_impulse(c.rB + B->get_center_of_mass(), jb, MAX_BIAS_ROTATION / p_step); + A->apply_bias_impulse(-jb, c.rA + A->get_center_of_mass(), MAX_BIAS_ROTATION / p_step); + B->apply_bias_impulse(jb, c.rB + B->get_center_of_mass(), MAX_BIAS_ROTATION / p_step); crbA = A->get_biased_angular_velocity().cross(c.rA); crbB = B->get_biased_angular_velocity().cross(c.rB); @@ -383,8 +383,8 @@ void BodyPair3DSW::solve(real_t p_step) { 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); + A->apply_bias_impulse(-jb_com, A->get_center_of_mass(), 0.0f); + B->apply_bias_impulse(jb_com, B->get_center_of_mass(), 0.0f); } c.active = true; @@ -404,8 +404,8 @@ void BodyPair3DSW::solve(real_t p_step) { Vector3 j = c.normal * (c.acc_normal_impulse - jnOld); - A->apply_impulse(c.rA + A->get_center_of_mass(), -j); - B->apply_impulse(c.rB + B->get_center_of_mass(), j); + A->apply_impulse(-j, c.rA + A->get_center_of_mass()); + B->apply_impulse(j, c.rB + B->get_center_of_mass()); c.active = true; } @@ -447,8 +447,8 @@ void BodyPair3DSW::solve(real_t p_step) { jt = c.acc_tangent_impulse - jtOld; - A->apply_impulse(c.rA + A->get_center_of_mass(), -jt); - B->apply_impulse(c.rB + B->get_center_of_mass(), jt); + A->apply_impulse(-jt, c.rA + A->get_center_of_mass()); + B->apply_impulse(jt, c.rB + B->get_center_of_mass()); c.active = true; } diff --git a/servers/physics_3d/body_pair_3d_sw.h b/servers/physics_3d/body_pair_3d_sw.h index 59e36e7ea5..4d049eafdc 100644 --- a/servers/physics_3d/body_pair_3d_sw.h +++ b/servers/physics_3d/body_pair_3d_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -36,7 +36,6 @@ class BodyPair3DSW : public Constraint3DSW { enum { - MAX_CONTACTS = 4 }; diff --git a/servers/physics_3d/broad_phase_3d_basic.cpp b/servers/physics_3d/broad_phase_3d_basic.cpp index 0f271b33af..b41c2530da 100644 --- a/servers/physics_3d/broad_phase_3d_basic.cpp +++ b/servers/physics_3d/broad_phase_3d_basic.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -29,8 +29,8 @@ /*************************************************************************/ #include "broad_phase_3d_basic.h" -#include "core/list.h" -#include "core/print_string.h" +#include "core/string/print_string.h" +#include "core/templates/list.h" BroadPhase3DSW::ID BroadPhase3DBasic::create(CollisionObject3DSW *p_object, int p_subindex) { ERR_FAIL_COND_V(p_object == nullptr, 0); @@ -190,8 +190,10 @@ void BroadPhase3DBasic::update() { void *data = nullptr; if (pair_callback) { data = pair_callback(elem_A->owner, elem_A->subindex, elem_B->owner, elem_B->subindex, unpair_userdata); + if (data) { + pair_map.insert(key, data); + } } - pair_map.insert(key, data); } } } diff --git a/servers/physics_3d/broad_phase_3d_basic.h b/servers/physics_3d/broad_phase_3d_basic.h index 4b644bf818..54d34e005f 100644 --- a/servers/physics_3d/broad_phase_3d_basic.h +++ b/servers/physics_3d/broad_phase_3d_basic.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -32,7 +32,7 @@ #define BROAD_PHASE_BASIC_H #include "broad_phase_3d_sw.h" -#include "core/map.h" +#include "core/templates/map.h" class BroadPhase3DBasic : public BroadPhase3DSW { struct Element { diff --git a/servers/physics_3d/broad_phase_3d_sw.cpp b/servers/physics_3d/broad_phase_3d_sw.cpp index 1a20fdd0cb..8aa64034ec 100644 --- a/servers/physics_3d/broad_phase_3d_sw.cpp +++ b/servers/physics_3d/broad_phase_3d_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/servers/physics_3d/broad_phase_3d_sw.h b/servers/physics_3d/broad_phase_3d_sw.h index 081e75810f..283c087b96 100644 --- a/servers/physics_3d/broad_phase_3d_sw.h +++ b/servers/physics_3d/broad_phase_3d_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/servers/physics_3d/broad_phase_octree.cpp b/servers/physics_3d/broad_phase_octree.cpp index 1ace1a4fcf..11324fa4e4 100644 --- a/servers/physics_3d/broad_phase_octree.cpp +++ b/servers/physics_3d/broad_phase_octree.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/servers/physics_3d/broad_phase_octree.h b/servers/physics_3d/broad_phase_octree.h index 761a90a051..ee681dda96 100644 --- a/servers/physics_3d/broad_phase_octree.h +++ b/servers/physics_3d/broad_phase_octree.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/servers/physics_3d/collision_object_3d_sw.cpp b/servers/physics_3d/collision_object_3d_sw.cpp index e12f0659e2..293a7e6606 100644 --- a/servers/physics_3d/collision_object_3d_sw.cpp +++ b/servers/physics_3d/collision_object_3d_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -43,7 +43,7 @@ void CollisionObject3DSW::add_shape(Shape3DSW *p_shape, const Transform &p_trans p_shape->add_owner(this); if (!pending_shape_update_list.in_list()) { - PhysicsServer3DSW::singleton->pending_shape_update_list.add(&pending_shape_update_list); + PhysicsServer3DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list); } //_update_shapes(); //_shapes_changed(); @@ -56,7 +56,7 @@ void CollisionObject3DSW::set_shape(int p_index, Shape3DSW *p_shape) { p_shape->add_owner(this); if (!pending_shape_update_list.in_list()) { - PhysicsServer3DSW::singleton->pending_shape_update_list.add(&pending_shape_update_list); + PhysicsServer3DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list); } //_update_shapes(); //_shapes_changed(); @@ -68,7 +68,7 @@ void CollisionObject3DSW::set_shape_transform(int p_index, const Transform &p_tr shapes.write[p_index].xform = p_transform; shapes.write[p_index].xform_inv = p_transform.affine_inverse(); if (!pending_shape_update_list.in_list()) { - PhysicsServer3DSW::singleton->pending_shape_update_list.add(&pending_shape_update_list); + PhysicsServer3DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list); } //_update_shapes(); //_shapes_changed(); @@ -77,7 +77,7 @@ void CollisionObject3DSW::set_shape_transform(int p_index, const Transform &p_tr void CollisionObject3DSW::set_shape_as_disabled(int p_idx, bool p_enable) { shapes.write[p_idx].disabled = p_enable; if (!pending_shape_update_list.in_list()) { - PhysicsServer3DSW::singleton->pending_shape_update_list.add(&pending_shape_update_list); + PhysicsServer3DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list); } } @@ -106,7 +106,7 @@ void CollisionObject3DSW::remove_shape(int p_index) { shapes.remove(p_index); if (!pending_shape_update_list.in_list()) { - PhysicsServer3DSW::singleton->pending_shape_update_list.add(&pending_shape_update_list); + PhysicsServer3DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list); } //_update_shapes(); //_shapes_changed(); diff --git a/servers/physics_3d/collision_object_3d_sw.h b/servers/physics_3d/collision_object_3d_sw.h index 9506f14402..3847b81381 100644 --- a/servers/physics_3d/collision_object_3d_sw.h +++ b/servers/physics_3d/collision_object_3d_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -32,7 +32,7 @@ #define COLLISION_OBJECT_SW_H #include "broad_phase_3d_sw.h" -#include "core/self_list.h" +#include "core/templates/self_list.h" #include "servers/physics_server_3d.h" #include "shape_3d_sw.h" @@ -142,10 +142,16 @@ public: return shapes[p_idx].disabled; } - _FORCE_INLINE_ void set_collision_layer(uint32_t p_layer) { collision_layer = p_layer; } + _FORCE_INLINE_ void set_collision_layer(uint32_t p_layer) { + collision_layer = p_layer; + _shape_changed(); + } _FORCE_INLINE_ uint32_t get_collision_layer() const { return collision_layer; } - _FORCE_INLINE_ void set_collision_mask(uint32_t p_mask) { collision_mask = p_mask; } + _FORCE_INLINE_ void set_collision_mask(uint32_t p_mask) { + collision_mask = p_mask; + _shape_changed(); + } _FORCE_INLINE_ uint32_t get_collision_mask() const { return collision_mask; } _FORCE_INLINE_ bool test_collision_mask(CollisionObject3DSW *p_other) const { diff --git a/servers/physics_3d/collision_solver_3d_sat.cpp b/servers/physics_3d/collision_solver_3d_sat.cpp index 736222c7d9..f507cacdc3 100644 --- a/servers/physics_3d/collision_solver_3d_sat.cpp +++ b/servers/physics_3d/collision_solver_3d_sat.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -29,9 +29,40 @@ /*************************************************************************/ #include "collision_solver_3d_sat.h" -#include "core/math/geometry.h" - -#define _EDGE_IS_VALID_SUPPORT_THRESHOLD 0.02 +#include "core/math/geometry_3d.h" + +#include "gjk_epa.h" + +#define fallback_collision_solver gjk_epa_calculate_penetration + +// Cylinder SAT analytic methods and face-circle contact points for cylinder-trimesh and cylinder-box collision are based on ODE colliders. + +/* + * Cylinder-trimesh and Cylinder-box colliders by Alen Ladavac + * Ported to ODE by Nguyen Binh + */ + +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ struct _CollectorCallback { CollisionSolver3DSW::CallbackResult callback; @@ -67,7 +98,7 @@ static void _generate_contacts_point_edge(const Vector3 *p_points_A, int p_point ERR_FAIL_COND(p_point_count_B != 2); #endif - Vector3 closest_B = Geometry::get_closest_point_to_segment_uncapped(*p_points_A, p_points_B); + Vector3 closest_B = Geometry3D::get_closest_point_to_segment_uncapped(*p_points_A, p_points_B); p_callback->call(*p_points_A, closest_B); } @@ -82,6 +113,17 @@ static void _generate_contacts_point_face(const Vector3 *p_points_A, int p_point p_callback->call(*p_points_A, closest_B); } +static void _generate_contacts_point_circle(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { +#ifdef DEBUG_ENABLED + ERR_FAIL_COND(p_point_count_A != 1); + ERR_FAIL_COND(p_point_count_B != 3); +#endif + + Vector3 closest_B = Plane(p_points_B[0], p_points_B[1], p_points_B[2]).project(*p_points_A); + + p_callback->call(*p_points_A, closest_B); +} + static void _generate_contacts_edge_edge(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { #ifdef DEBUG_ENABLED ERR_FAIL_COND(p_point_count_A != 2); @@ -124,10 +166,108 @@ static void _generate_contacts_edge_edge(const Vector3 *p_points_A, int p_point_ } Vector3 closest_A = p_points_A[0] + rel_A * d; - Vector3 closest_B = Geometry::get_closest_point_to_segment_uncapped(closest_A, p_points_B); + Vector3 closest_B = Geometry3D::get_closest_point_to_segment_uncapped(closest_A, p_points_B); p_callback->call(closest_A, closest_B); } +static void _generate_contacts_edge_circle(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { +#ifdef DEBUG_ENABLED + ERR_FAIL_COND(p_point_count_A != 2); + ERR_FAIL_COND(p_point_count_B != 3); +#endif + + const Vector3 &circle_B_pos = p_points_B[0]; + Vector3 circle_B_line_1 = p_points_B[1] - circle_B_pos; + Vector3 circle_B_line_2 = p_points_B[2] - circle_B_pos; + + real_t circle_B_radius = circle_B_line_1.length(); + Vector3 circle_B_normal = circle_B_line_1.cross(circle_B_line_2).normalized(); + + Plane circle_plane(circle_B_pos, circle_B_normal); + + static const int max_clip = 2; + Vector3 contact_points[max_clip]; + int num_points = 0; + + // Project edge point in circle plane. + const Vector3 &edge_A_1 = p_points_A[0]; + Vector3 proj_point_1 = circle_plane.project(edge_A_1); + + Vector3 dist_vec = proj_point_1 - circle_B_pos; + real_t dist_sq = dist_vec.length_squared(); + + // Point 1 is inside disk, add as contact point. + if (dist_sq <= circle_B_radius * circle_B_radius) { + contact_points[num_points] = edge_A_1; + ++num_points; + } + + const Vector3 &edge_A_2 = p_points_A[1]; + Vector3 proj_point_2 = circle_plane.project(edge_A_2); + + Vector3 dist_vec_2 = proj_point_2 - circle_B_pos; + real_t dist_sq_2 = dist_vec_2.length_squared(); + + // Point 2 is inside disk, add as contact point. + if (dist_sq_2 <= circle_B_radius * circle_B_radius) { + contact_points[num_points] = edge_A_2; + ++num_points; + } + + if (num_points < 2) { + Vector3 line_vec = proj_point_2 - proj_point_1; + real_t line_length_sq = line_vec.length_squared(); + + // Create a quadratic formula of the form ax^2 + bx + c = 0 + real_t a, b, c; + + a = line_length_sq; + b = 2.0 * dist_vec.dot(line_vec); + c = dist_sq - circle_B_radius * circle_B_radius; + + // Solve for t. + real_t sqrtterm = b * b - 4.0 * a * c; + + // If the term we intend to square root is less than 0 then the answer won't be real, + // so the line doesn't intersect. + if (sqrtterm >= 0) { + sqrtterm = Math::sqrt(sqrtterm); + + Vector3 edge_dir = edge_A_2 - edge_A_1; + + real_t fraction_1 = (-b - sqrtterm) / (2.0 * a); + if ((fraction_1 > 0.0) && (fraction_1 < 1.0)) { + Vector3 face_point_1 = edge_A_1 + fraction_1 * edge_dir; + ERR_FAIL_COND(num_points >= max_clip); + contact_points[num_points] = face_point_1; + ++num_points; + } + + real_t fraction_2 = (-b + sqrtterm) / (2.0 * a); + if ((fraction_2 > 0.0) && (fraction_2 < 1.0) && !Math::is_equal_approx(fraction_1, fraction_2)) { + Vector3 face_point_2 = edge_A_1 + fraction_2 * edge_dir; + ERR_FAIL_COND(num_points >= max_clip); + contact_points[num_points] = face_point_2; + ++num_points; + } + } + } + + // Generate contact points. + for (int i = 0; i < num_points; i++) { + const Vector3 &contact_point_A = contact_points[i]; + + real_t d = circle_plane.distance_to(contact_point_A); + Vector3 closest_B = contact_point_A - circle_plane.normal * d; + + if (p_callback->normal.dot(contact_point_A) >= p_callback->normal.dot(closest_B)) { + continue; + } + + p_callback->call(contact_point_A, closest_B); + } +} + static void _generate_contacts_face_face(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { #ifdef DEBUG_ENABLED ERR_FAIL_COND(p_point_count_A < 2); @@ -217,36 +357,229 @@ static void _generate_contacts_face_face(const Vector3 *p_points_A, int p_point_ } } -static void _generate_contacts_from_supports(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { +static void _generate_contacts_face_circle(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { +#ifdef DEBUG_ENABLED + ERR_FAIL_COND(p_point_count_A < 3); + ERR_FAIL_COND(p_point_count_B != 3); +#endif + + const Vector3 &circle_B_pos = p_points_B[0]; + Vector3 circle_B_line_1 = p_points_B[1] - circle_B_pos; + Vector3 circle_B_line_2 = p_points_B[2] - circle_B_pos; + + // Clip face with circle segments. + static const int circle_segments = 8; + Vector3 circle_points[circle_segments]; + + real_t angle_delta = 2.0 * Math_PI / circle_segments; + + for (int i = 0; i < circle_segments; ++i) { + Vector3 point_pos = circle_B_pos; + point_pos += circle_B_line_1 * Math::cos(i * angle_delta); + point_pos += circle_B_line_2 * Math::sin(i * angle_delta); + circle_points[i] = point_pos; + } + + _generate_contacts_face_face(p_points_A, p_point_count_A, circle_points, circle_segments, p_callback); + + // Clip face with circle plane. + Vector3 circle_B_normal = circle_B_line_1.cross(circle_B_line_2).normalized(); + + Plane circle_plane(circle_B_pos, circle_B_normal); + + static const int max_clip = 32; + Vector3 contact_points[max_clip]; + int num_points = 0; + + for (int i = 0; i < p_point_count_A; i++) { + int i_n = (i + 1) % p_point_count_A; + + const Vector3 &edge0_A = p_points_A[i]; + const Vector3 &edge1_A = p_points_A[i_n]; + + real_t dist0 = circle_plane.distance_to(edge0_A); + real_t dist1 = circle_plane.distance_to(edge1_A); + + // First point in front of plane, generate contact point. + if (dist0 * circle_plane.d >= 0) { + ERR_FAIL_COND(num_points >= max_clip); + contact_points[num_points] = edge0_A; + ++num_points; + } + + // Points on different sides, generate contact point. + if (dist0 * dist1 < 0) { + // calculate intersection + Vector3 rel = edge1_A - edge0_A; + real_t den = circle_plane.normal.dot(rel); + real_t dist = -(circle_plane.normal.dot(edge0_A) - circle_plane.d) / den; + Vector3 inters = edge0_A + rel * dist; + + ERR_FAIL_COND(num_points >= max_clip); + contact_points[num_points] = inters; + ++num_points; + } + } + + // Generate contact points. + for (int i = 0; i < num_points; i++) { + const Vector3 &contact_point_A = contact_points[i]; + + real_t d = circle_plane.distance_to(contact_point_A); + Vector3 closest_B = contact_point_A - circle_plane.normal * d; + + if (p_callback->normal.dot(contact_point_A) >= p_callback->normal.dot(closest_B)) { + continue; + } + + p_callback->call(contact_point_A, closest_B); + } +} + +static void _generate_contacts_circle_circle(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { +#ifdef DEBUG_ENABLED + ERR_FAIL_COND(p_point_count_A != 3); + ERR_FAIL_COND(p_point_count_B != 3); +#endif + + const Vector3 &circle_A_pos = p_points_A[0]; + Vector3 circle_A_line_1 = p_points_A[1] - circle_A_pos; + Vector3 circle_A_line_2 = p_points_A[2] - circle_A_pos; + + real_t circle_A_radius = circle_A_line_1.length(); + Vector3 circle_A_normal = circle_A_line_1.cross(circle_A_line_2).normalized(); + + const Vector3 &circle_B_pos = p_points_B[0]; + Vector3 circle_B_line_1 = p_points_B[1] - circle_B_pos; + Vector3 circle_B_line_2 = p_points_B[2] - circle_B_pos; + + real_t circle_B_radius = circle_B_line_1.length(); + Vector3 circle_B_normal = circle_B_line_1.cross(circle_B_line_2).normalized(); + + static const int max_clip = 4; + Vector3 contact_points[max_clip]; + int num_points = 0; + + Vector3 centers_diff = circle_B_pos - circle_A_pos; + Vector3 norm_proj = circle_A_normal.dot(centers_diff) * circle_A_normal; + Vector3 comp_proj = centers_diff - norm_proj; + real_t proj_dist = comp_proj.length(); + if (!Math::is_zero_approx(proj_dist)) { + comp_proj /= proj_dist; + if ((proj_dist > circle_A_radius - circle_B_radius) && (proj_dist > circle_B_radius - circle_A_radius)) { + // Circles are overlapping, use the 2 points of intersection as contacts. + real_t radius_a_sqr = circle_A_radius * circle_A_radius; + real_t radius_b_sqr = circle_B_radius * circle_B_radius; + real_t d_sqr = proj_dist * proj_dist; + real_t s = (1.0 + (radius_a_sqr - radius_b_sqr) / d_sqr) * 0.5; + real_t h = Math::sqrt(MAX(radius_a_sqr - d_sqr * s * s, 0.0)); + Vector3 midpoint = circle_A_pos + s * comp_proj * proj_dist; + Vector3 h_vec = h * circle_A_normal.cross(comp_proj); + + Vector3 point_A = midpoint + h_vec; + contact_points[num_points] = point_A; + ++num_points; + + point_A = midpoint - h_vec; + contact_points[num_points] = point_A; + ++num_points; + + // Add 2 points from circle A and B along the line between the centers. + point_A = circle_A_pos + comp_proj * circle_A_radius; + contact_points[num_points] = point_A; + ++num_points; + + point_A = circle_B_pos - comp_proj * circle_B_radius - norm_proj; + contact_points[num_points] = point_A; + ++num_points; + } // Otherwise one circle is inside the other one, use 3 arbitrary equidistant points. + } // Otherwise circles are concentric, use 3 arbitrary equidistant points. + + if (num_points == 0) { + // Generate equidistant points. + if (circle_A_radius < circle_B_radius) { + // Circle A inside circle B. + for (int i = 0; i < 3; ++i) { + Vector3 circle_A_point = circle_A_pos; + circle_A_point += circle_A_line_1 * Math::cos(2.0 * Math_PI * i / 3.0); + circle_A_point += circle_A_line_2 * Math::sin(2.0 * Math_PI * i / 3.0); + + contact_points[num_points] = circle_A_point; + ++num_points; + } + } else { + // Circle B inside circle A. + for (int i = 0; i < 3; ++i) { + Vector3 circle_B_point = circle_B_pos; + circle_B_point += circle_B_line_1 * Math::cos(2.0 * Math_PI * i / 3.0); + circle_B_point += circle_B_line_2 * Math::sin(2.0 * Math_PI * i / 3.0); + + Vector3 circle_A_point = circle_B_point - norm_proj; + + contact_points[num_points] = circle_A_point; + ++num_points; + } + } + } + + Plane circle_B_plane(circle_B_pos, circle_B_normal); + + // Generate contact points. + for (int i = 0; i < num_points; i++) { + const Vector3 &contact_point_A = contact_points[i]; + + real_t d = circle_B_plane.distance_to(contact_point_A); + Vector3 closest_B = contact_point_A - circle_B_plane.normal * d; + + if (p_callback->normal.dot(contact_point_A) >= p_callback->normal.dot(closest_B)) { + continue; + } + + p_callback->call(contact_point_A, closest_B); + } +} + +static void _generate_contacts_from_supports(const Vector3 *p_points_A, int p_point_count_A, Shape3DSW::FeatureType p_feature_type_A, const Vector3 *p_points_B, int p_point_count_B, Shape3DSW::FeatureType p_feature_type_B, _CollectorCallback *p_callback) { #ifdef DEBUG_ENABLED ERR_FAIL_COND(p_point_count_A < 1); ERR_FAIL_COND(p_point_count_B < 1); #endif - static const GenerateContactsFunc generate_contacts_func_table[3][3] = { + static const GenerateContactsFunc generate_contacts_func_table[4][4] = { { _generate_contacts_point_point, _generate_contacts_point_edge, _generate_contacts_point_face, + _generate_contacts_point_circle, }, { nullptr, _generate_contacts_edge_edge, _generate_contacts_face_face, + _generate_contacts_edge_circle, }, { nullptr, nullptr, _generate_contacts_face_face, - } + _generate_contacts_face_circle, + }, + { + nullptr, + nullptr, + nullptr, + _generate_contacts_circle_circle, + }, }; int pointcount_B; int pointcount_A; const Vector3 *points_A; const Vector3 *points_B; + int version_A; + int version_B; - if (p_point_count_A > p_point_count_B) { + if (p_feature_type_A > p_feature_type_B) { //swap p_callback->swap = !p_callback->swap; p_callback->normal = -p_callback->normal; @@ -255,16 +588,17 @@ static void _generate_contacts_from_supports(const Vector3 *p_points_A, int p_po pointcount_A = p_point_count_B; points_A = p_points_B; points_B = p_points_A; + version_A = p_feature_type_B; + version_B = p_feature_type_A; } else { pointcount_B = p_point_count_B; pointcount_A = p_point_count_A; points_A = p_points_A; points_B = p_points_B; + version_A = p_feature_type_A; + version_B = p_feature_type_B; } - int version_A = (pointcount_A > 3 ? 3 : pointcount_A) - 1; - int version_B = (pointcount_B > 3 ? 3 : pointcount_B) - 1; - GenerateContactsFunc contacts_func = generate_contacts_func_table[version_A][version_B]; ERR_FAIL_COND(!contacts_func); contacts_func(points_A, pointcount_A, points_B, pointcount_B, p_callback); @@ -346,6 +680,17 @@ public: return true; } + static _FORCE_INLINE_ void test_contact_points(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata) { + SeparatorAxisTest<ShapeA, ShapeB, withMargin> *separator = (SeparatorAxisTest<ShapeA, ShapeB, withMargin> *)p_userdata; + Vector3 axis = (p_point_B - p_point_A); + real_t depth = axis.length(); + + // Filter out bogus directions with a treshold and re-testing axis. + if (separator->best_depth - depth > 0.001) { + separator->test_axis(axis / depth); + } + } + _FORCE_INLINE_ void generate_contacts() { // nothing to do, don't generate if (best_axis == Vector3(0.0, 0.0, 0.0)) { @@ -365,7 +710,8 @@ public: Vector3 supports_A[max_supports]; int support_count_A; - shape_A->get_supports(transform_A->basis.xform_inv(-best_axis).normalized(), max_supports, supports_A, support_count_A); + Shape3DSW::FeatureType support_type_A; + shape_A->get_supports(transform_A->basis.xform_inv(-best_axis).normalized(), max_supports, supports_A, support_count_A, support_type_A); for (int i = 0; i < support_count_A; i++) { supports_A[i] = transform_A->xform(supports_A[i]); } @@ -378,7 +724,8 @@ public: Vector3 supports_B[max_supports]; int support_count_B; - shape_B->get_supports(transform_B->basis.xform_inv(best_axis).normalized(), max_supports, supports_B, support_count_B); + Shape3DSW::FeatureType support_type_B; + shape_B->get_supports(transform_B->basis.xform_inv(best_axis).normalized(), max_supports, supports_B, support_count_B, support_type_B); for (int i = 0; i < support_count_B; i++) { supports_B[i] = transform_B->xform(supports_B[i]); } @@ -393,7 +740,7 @@ public: if (callback->prev_axis) { *callback->prev_axis = best_axis; } - _generate_contacts_from_supports(supports_A, support_count_A, supports_B, support_count_B, callback); + _generate_contacts_from_supports(supports_A, support_count_A, support_type_A, supports_B, support_count_B, support_type_B, callback); callback->collided = true; } @@ -529,6 +876,61 @@ static void _collision_sphere_capsule(const Shape3DSW *p_a, const Transform &p_t template <bool withMargin> static void _collision_sphere_cylinder(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const SphereShape3DSW *sphere_A = static_cast<const SphereShape3DSW *>(p_a); + const CylinderShape3DSW *cylinder_B = static_cast<const CylinderShape3DSW *>(p_b); + + SeparatorAxisTest<SphereShape3DSW, CylinderShape3DSW, withMargin> separator(sphere_A, p_transform_a, cylinder_B, p_transform_b, p_collector, p_margin_a, p_margin_b); + + if (!separator.test_previous_axis()) { + return; + } + + // Cylinder B end caps. + Vector3 cylinder_B_axis = p_transform_b.basis.get_axis(1).normalized(); + if (!separator.test_axis(cylinder_B_axis)) { + return; + } + + Vector3 cylinder_diff = p_transform_b.origin - p_transform_a.origin; + + // Cylinder B lateral surface. + if (!separator.test_axis(cylinder_B_axis.cross(cylinder_diff).cross(cylinder_B_axis).normalized())) { + return; + } + + // Closest point to cylinder caps. + const Vector3 &sphere_center = p_transform_a.origin; + Vector3 cyl_axis = p_transform_b.basis.get_axis(1); + Vector3 cap_axis = p_transform_b.basis.get_axis(0); + real_t height_scale = cyl_axis.length(); + real_t cap_dist = cylinder_B->get_height() * 0.5 * height_scale; + cyl_axis /= height_scale; + real_t radius_scale = cap_axis.length(); + real_t cap_radius = cylinder_B->get_radius() * radius_scale; + + for (int i = 0; i < 2; i++) { + Vector3 cap_dir = ((i == 0) ? cyl_axis : -cyl_axis); + Vector3 cap_pos = p_transform_b.origin + cap_dir * cap_dist; + + Vector3 closest_point; + + Vector3 diff = sphere_center - cap_pos; + Vector3 proj = diff - cap_dir.dot(diff) * cap_dir; + + real_t proj_len = proj.length(); + if (Math::is_zero_approx(proj_len)) { + // Point is equidistant to all circle points. + continue; + } + + closest_point = cap_pos + (cap_radius / proj_len) * proj; + + if (!separator.test_axis((closest_point - sphere_center).normalized())) { + return; + } + } + + separator.generate_contacts(); } template <bool withMargin> @@ -542,11 +944,11 @@ static void _collision_sphere_convex_polygon(const Shape3DSW *p_a, const Transfo return; } - const Geometry::MeshData &mesh = convex_polygon_B->get_mesh(); + const Geometry3D::MeshData &mesh = convex_polygon_B->get_mesh(); - const Geometry::MeshData::Face *faces = mesh.faces.ptr(); + const Geometry3D::MeshData::Face *faces = mesh.faces.ptr(); int face_count = mesh.faces.size(); - const Geometry::MeshData::Edge *edges = mesh.edges.ptr(); + const Geometry3D::MeshData::Edge *edges = mesh.edges.ptr(); int edge_count = mesh.edges.size(); const Vector3 *vertices = mesh.vertices.ptr(); int vertex_count = mesh.vertices.size(); @@ -739,7 +1141,7 @@ static void _collision_box_capsule(const Shape3DSW *p_a, const Transform &p_tran // faces of A for (int i = 0; i < 3; i++) { - Vector3 axis = p_transform_a.basis.get_axis(i); + Vector3 axis = p_transform_a.basis.get_axis(i).normalized(); if (!separator.test_axis(axis)) { return; @@ -826,6 +1228,115 @@ static void _collision_box_capsule(const Shape3DSW *p_a, const Transform &p_tran template <bool withMargin> static void _collision_box_cylinder(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const BoxShape3DSW *box_A = static_cast<const BoxShape3DSW *>(p_a); + const CylinderShape3DSW *cylinder_B = static_cast<const CylinderShape3DSW *>(p_b); + + SeparatorAxisTest<BoxShape3DSW, CylinderShape3DSW, withMargin> separator(box_A, p_transform_a, cylinder_B, p_transform_b, p_collector, p_margin_a, p_margin_b); + + if (!separator.test_previous_axis()) { + return; + } + + // Faces of A. + for (int i = 0; i < 3; i++) { + Vector3 axis = p_transform_a.basis.get_axis(i).normalized(); + + if (!separator.test_axis(axis)) { + return; + } + } + + Vector3 cyl_axis = p_transform_b.basis.get_axis(1).normalized(); + + // Cylinder end caps. + { + if (!separator.test_axis(cyl_axis)) { + return; + } + } + + // Edges of A, cylinder lateral surface. + for (int i = 0; i < 3; i++) { + Vector3 box_axis = p_transform_a.basis.get_axis(i); + Vector3 axis = box_axis.cross(cyl_axis); + if (Math::is_zero_approx(axis.length_squared())) { + continue; + } + + if (!separator.test_axis(axis.normalized())) { + return; + } + } + + // Gather points of A. + Vector3 vertices_A[8]; + Vector3 box_extent = box_A->get_half_extents(); + for (int i = 0; i < 2; i++) { + for (int j = 0; j < 2; j++) { + for (int k = 0; k < 2; k++) { + Vector3 extent = box_extent; + extent.x *= (i * 2 - 1); + extent.y *= (j * 2 - 1); + extent.z *= (k * 2 - 1); + Vector3 &point = vertices_A[i * 2 * 2 + j * 2 + k]; + point = p_transform_a.origin; + for (int l = 0; l < 3; l++) { + point += p_transform_a.basis.get_axis(l) * extent[l]; + } + } + } + } + + // Points of A, cylinder lateral surface. + for (int i = 0; i < 8; i++) { + const Vector3 &point = vertices_A[i]; + Vector3 axis = Plane(cyl_axis, 0).project(point).normalized(); + + if (!separator.test_axis(axis)) { + return; + } + } + + // Edges of A, cylinder end caps rim. + int edges_start_A[12] = { 0, 2, 4, 6, 0, 1, 4, 5, 0, 1, 2, 3 }; + int edges_end_A[12] = { 1, 3, 5, 7, 2, 3, 6, 7, 4, 5, 6, 7 }; + + Vector3 cap_axis = cyl_axis * (cylinder_B->get_height() * 0.5); + + for (int i = 0; i < 2; i++) { + Vector3 cap_pos = p_transform_b.origin + ((i == 0) ? cap_axis : -cap_axis); + + for (int e = 0; e < 12; e++) { + const Vector3 &edge_start = vertices_A[edges_start_A[e]]; + const Vector3 &edge_end = vertices_A[edges_end_A[e]]; + + Vector3 edge_dir = (edge_end - edge_start); + edge_dir.normalize(); + + real_t edge_dot = edge_dir.dot(cyl_axis); + if (Math::is_zero_approx(edge_dot)) { + // Edge is perpendicular to cylinder axis. + continue; + } + + // Calculate intersection between edge and circle plane. + Vector3 edge_diff = cap_pos - edge_start; + real_t diff_dot = edge_diff.dot(cyl_axis); + Vector3 intersection = edge_start + edge_dir * diff_dot / edge_dot; + + // Calculate tangent that touches intersection. + Vector3 tangent = (cap_pos - intersection).cross(cyl_axis); + + // Axis is orthogonal both to tangent and edge direction. + Vector3 axis = tangent.cross(edge_dir); + + if (!separator.test_axis(axis.normalized())) { + return; + } + } + } + + separator.generate_contacts(); } template <bool withMargin> @@ -839,11 +1350,11 @@ static void _collision_box_convex_polygon(const Shape3DSW *p_a, const Transform return; } - const Geometry::MeshData &mesh = convex_polygon_B->get_mesh(); + const Geometry3D::MeshData &mesh = convex_polygon_B->get_mesh(); - const Geometry::MeshData::Face *faces = mesh.faces.ptr(); + const Geometry3D::MeshData::Face *faces = mesh.faces.ptr(); int face_count = mesh.faces.size(); - const Geometry::MeshData::Edge *edges = mesh.edges.ptr(); + const Geometry3D::MeshData::Edge *edges = mesh.edges.ptr(); int edge_count = mesh.edges.size(); const Vector3 *vertices = mesh.vertices.ptr(); int vertex_count = mesh.vertices.size(); @@ -1111,6 +1622,19 @@ static void _collision_capsule_capsule(const Shape3DSW *p_a, const Transform &p_ template <bool withMargin> static void _collision_capsule_cylinder(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const CapsuleShape3DSW *capsule_A = static_cast<const CapsuleShape3DSW *>(p_a); + const CylinderShape3DSW *cylinder_B = static_cast<const CylinderShape3DSW *>(p_b); + + SeparatorAxisTest<CapsuleShape3DSW, CylinderShape3DSW, withMargin> separator(capsule_A, p_transform_a, cylinder_B, p_transform_b, p_collector, p_margin_a, p_margin_b); + + CollisionSolver3DSW::CallbackResult callback = SeparatorAxisTest<CapsuleShape3DSW, CylinderShape3DSW, withMargin>::test_contact_points; + + // Fallback to generic algorithm to find the best separating axis. + if (!fallback_collision_solver(p_a, p_transform_a, p_b, p_transform_b, callback, &separator)) { + return; + } + + separator.generate_contacts(); } template <bool withMargin> @@ -1124,11 +1648,11 @@ static void _collision_capsule_convex_polygon(const Shape3DSW *p_a, const Transf return; } - const Geometry::MeshData &mesh = convex_polygon_B->get_mesh(); + const Geometry3D::MeshData &mesh = convex_polygon_B->get_mesh(); - const Geometry::MeshData::Face *faces = mesh.faces.ptr(); + const Geometry3D::MeshData::Face *faces = mesh.faces.ptr(); int face_count = mesh.faces.size(); - const Geometry::MeshData::Edge *edges = mesh.edges.ptr(); + const Geometry3D::MeshData::Edge *edges = mesh.edges.ptr(); int edge_count = mesh.edges.size(); const Vector3 *vertices = mesh.vertices.ptr(); @@ -1236,14 +1760,165 @@ static void _collision_capsule_face(const Shape3DSW *p_a, const Transform &p_tra template <bool withMargin> static void _collision_cylinder_cylinder(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const CylinderShape3DSW *cylinder_A = static_cast<const CylinderShape3DSW *>(p_a); + const CylinderShape3DSW *cylinder_B = static_cast<const CylinderShape3DSW *>(p_b); + + SeparatorAxisTest<CylinderShape3DSW, CylinderShape3DSW, withMargin> separator(cylinder_A, p_transform_a, cylinder_B, p_transform_b, p_collector, p_margin_a, p_margin_b); + + Vector3 cylinder_A_axis = p_transform_a.basis.get_axis(1); + Vector3 cylinder_B_axis = p_transform_b.basis.get_axis(1); + + if (!separator.test_previous_axis()) { + return; + } + + // Cylinder A end caps. + if (!separator.test_axis(cylinder_A_axis.normalized())) { + return; + } + + // Cylinder B end caps. + if (!separator.test_axis(cylinder_A_axis.normalized())) { + return; + } + + Vector3 cylinder_diff = p_transform_b.origin - p_transform_a.origin; + + // Cylinder A lateral surface. + if (!separator.test_axis(cylinder_A_axis.cross(cylinder_diff).cross(cylinder_A_axis).normalized())) { + return; + } + + // Cylinder B lateral surface. + if (!separator.test_axis(cylinder_B_axis.cross(cylinder_diff).cross(cylinder_B_axis).normalized())) { + return; + } + + real_t proj = cylinder_A_axis.cross(cylinder_B_axis).cross(cylinder_B_axis).dot(cylinder_A_axis); + if (Math::is_zero_approx(proj)) { + // Parallel cylinders, handle with specific axes only. + // Note: GJKEPA with no margin can lead to degenerate cases in this situation. + separator.generate_contacts(); + return; + } + + CollisionSolver3DSW::CallbackResult callback = SeparatorAxisTest<CylinderShape3DSW, CylinderShape3DSW, withMargin>::test_contact_points; + + // Fallback to generic algorithm to find the best separating axis. + if (!fallback_collision_solver(p_a, p_transform_a, p_b, p_transform_b, callback, &separator)) { + return; + } + + separator.generate_contacts(); } template <bool withMargin> static void _collision_cylinder_convex_polygon(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const CylinderShape3DSW *cylinder_A = static_cast<const CylinderShape3DSW *>(p_a); + const ConvexPolygonShape3DSW *convex_polygon_B = static_cast<const ConvexPolygonShape3DSW *>(p_b); + + SeparatorAxisTest<CylinderShape3DSW, ConvexPolygonShape3DSW, withMargin> separator(cylinder_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b); + + CollisionSolver3DSW::CallbackResult callback = SeparatorAxisTest<CylinderShape3DSW, ConvexPolygonShape3DSW, withMargin>::test_contact_points; + + // Fallback to generic algorithm to find the best separating axis. + if (!fallback_collision_solver(p_a, p_transform_a, p_b, p_transform_b, callback, &separator)) { + return; + } + + separator.generate_contacts(); } template <bool withMargin> static void _collision_cylinder_face(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const CylinderShape3DSW *cylinder_A = static_cast<const CylinderShape3DSW *>(p_a); + const FaceShape3DSW *face_B = static_cast<const FaceShape3DSW *>(p_b); + + SeparatorAxisTest<CylinderShape3DSW, FaceShape3DSW, withMargin> separator(cylinder_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b); + + if (!separator.test_previous_axis()) { + return; + } + + Vector3 vertex[3] = { + p_transform_b.xform(face_B->vertex[0]), + p_transform_b.xform(face_B->vertex[1]), + p_transform_b.xform(face_B->vertex[2]), + }; + + // Face B normal. + if (!separator.test_axis((vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized())) { + return; + } + + Vector3 cyl_axis = p_transform_a.basis.get_axis(1).normalized(); + + // Cylinder end caps. + { + if (!separator.test_axis(cyl_axis)) { + return; + } + } + + // Edges of B, cylinder lateral surface. + for (int i = 0; i < 3; i++) { + Vector3 edge_axis = vertex[i] - vertex[(i + 1) % 3]; + Vector3 axis = edge_axis.cross(cyl_axis); + if (Math::is_zero_approx(axis.length_squared())) { + continue; + } + + if (!separator.test_axis(axis.normalized())) { + return; + } + } + + // Points of B, cylinder lateral surface. + for (int i = 0; i < 3; i++) { + const Vector3 &point = vertex[i]; + Vector3 axis = Plane(cyl_axis, 0).project(point).normalized(); + + if (!separator.test_axis(axis)) { + return; + } + } + + // Edges of B, cylinder end caps rim. + Vector3 cap_axis = cyl_axis * (cylinder_A->get_height() * 0.5); + + for (int i = 0; i < 2; i++) { + Vector3 cap_pos = p_transform_a.origin + ((i == 0) ? cap_axis : -cap_axis); + + for (int j = 0; j < 3; j++) { + const Vector3 &edge_start = vertex[j]; + const Vector3 &edge_end = vertex[(j + 1) % 3]; + Vector3 edge_dir = edge_end - edge_start; + edge_dir.normalize(); + + real_t edge_dot = edge_dir.dot(cyl_axis); + if (Math::is_zero_approx(edge_dot)) { + // Edge is perpendicular to cylinder axis. + continue; + } + + // Calculate intersection between edge and circle plane. + Vector3 edge_diff = cap_pos - edge_start; + real_t diff_dot = edge_diff.dot(cyl_axis); + Vector3 intersection = edge_start + edge_dir * diff_dot / edge_dot; + + // Calculate tangent that touches intersection. + Vector3 tangent = (cap_pos - intersection).cross(cyl_axis); + + // Axis is orthogonal both to tangent and edge direction. + Vector3 axis = tangent.cross(edge_dir); + + if (!separator.test_axis(axis.normalized())) { + return; + } + } + } + + separator.generate_contacts(); } template <bool withMargin> @@ -1257,20 +1932,20 @@ static void _collision_convex_polygon_convex_polygon(const Shape3DSW *p_a, const return; } - const Geometry::MeshData &mesh_A = convex_polygon_A->get_mesh(); + const Geometry3D::MeshData &mesh_A = convex_polygon_A->get_mesh(); - const Geometry::MeshData::Face *faces_A = mesh_A.faces.ptr(); + const Geometry3D::MeshData::Face *faces_A = mesh_A.faces.ptr(); int face_count_A = mesh_A.faces.size(); - const Geometry::MeshData::Edge *edges_A = mesh_A.edges.ptr(); + const Geometry3D::MeshData::Edge *edges_A = mesh_A.edges.ptr(); int edge_count_A = mesh_A.edges.size(); const Vector3 *vertices_A = mesh_A.vertices.ptr(); int vertex_count_A = mesh_A.vertices.size(); - const Geometry::MeshData &mesh_B = convex_polygon_B->get_mesh(); + const Geometry3D::MeshData &mesh_B = convex_polygon_B->get_mesh(); - const Geometry::MeshData::Face *faces_B = mesh_B.faces.ptr(); + const Geometry3D::MeshData::Face *faces_B = mesh_B.faces.ptr(); int face_count_B = mesh_B.faces.size(); - const Geometry::MeshData::Edge *edges_B = mesh_B.edges.ptr(); + const Geometry3D::MeshData::Edge *edges_B = mesh_B.edges.ptr(); int edge_count_B = mesh_B.edges.size(); const Vector3 *vertices_B = mesh_B.vertices.ptr(); int vertex_count_B = mesh_B.vertices.size(); @@ -1362,11 +2037,11 @@ static void _collision_convex_polygon_face(const Shape3DSW *p_a, const Transform SeparatorAxisTest<ConvexPolygonShape3DSW, FaceShape3DSW, withMargin> separator(convex_polygon_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b); - const Geometry::MeshData &mesh = convex_polygon_A->get_mesh(); + const Geometry3D::MeshData &mesh = convex_polygon_A->get_mesh(); - const Geometry::MeshData::Face *faces = mesh.faces.ptr(); + const Geometry3D::MeshData::Face *faces = mesh.faces.ptr(); int face_count = mesh.faces.size(); - const Geometry::MeshData::Edge *edges = mesh.edges.ptr(); + const Geometry3D::MeshData::Edge *edges = mesh.edges.ptr(); int edge_count = mesh.edges.size(); const Vector3 *vertices = mesh.vertices.ptr(); int vertex_count = mesh.vertices.size(); diff --git a/servers/physics_3d/collision_solver_3d_sat.h b/servers/physics_3d/collision_solver_3d_sat.h index 5eccfda9ac..97454c0b4a 100644 --- a/servers/physics_3d/collision_solver_3d_sat.h +++ b/servers/physics_3d/collision_solver_3d_sat.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/servers/physics_3d/collision_solver_3d_sw.cpp b/servers/physics_3d/collision_solver_3d_sw.cpp index e2bfaf990d..fd9ea00d92 100644 --- a/servers/physics_3d/collision_solver_3d_sw.cpp +++ b/servers/physics_3d/collision_solver_3d_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -46,8 +46,24 @@ bool CollisionSolver3DSW::solve_static_plane(const Shape3DSW *p_shape_A, const T static const int max_supports = 16; Vector3 supports[max_supports]; int support_count; - - p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(), max_supports, supports, support_count); + Shape3DSW::FeatureType support_type; + p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(), max_supports, supports, support_count, support_type); + + if (support_type == Shape3DSW::FEATURE_CIRCLE) { + ERR_FAIL_COND_V(support_count != 3, false); + + Vector3 circle_pos = supports[0]; + Vector3 circle_axis_1 = supports[1] - circle_pos; + Vector3 circle_axis_2 = supports[2] - circle_pos; + + // Use 3 equidistant points on the circle. + for (int i = 0; i < 3; ++i) { + Vector3 vertex_pos = circle_pos; + vertex_pos += circle_axis_1 * Math::cos(2.0 * Math_PI * i / 3.0); + vertex_pos += circle_axis_2 * Math::sin(2.0 * Math_PI * i / 3.0); + supports[i] = vertex_pos; + } + } bool found = false; @@ -265,8 +281,25 @@ bool CollisionSolver3DSW::solve_distance_plane(const Shape3DSW *p_shape_A, const static const int max_supports = 16; Vector3 supports[max_supports]; int support_count; + Shape3DSW::FeatureType support_type; + + p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(), max_supports, supports, support_count, support_type); - p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(), max_supports, supports, support_count); + if (support_type == Shape3DSW::FEATURE_CIRCLE) { + ERR_FAIL_COND_V(support_count != 3, false); + + Vector3 circle_pos = supports[0]; + Vector3 circle_axis_1 = supports[1] - circle_pos; + Vector3 circle_axis_2 = supports[2] - circle_pos; + + // Use 3 equidistant points on the circle. + for (int i = 0; i < 3; ++i) { + Vector3 vertex_pos = circle_pos; + vertex_pos += circle_axis_1 * Math::cos(2.0 * Math_PI * i / 3.0); + vertex_pos += circle_axis_2 * Math::sin(2.0 * Math_PI * i / 3.0); + supports[i] = vertex_pos; + } + } bool collided = false; Vector3 closest; diff --git a/servers/physics_3d/collision_solver_3d_sw.h b/servers/physics_3d/collision_solver_3d_sw.h index 13f54ca8fb..81d87e9773 100644 --- a/servers/physics_3d/collision_solver_3d_sw.h +++ b/servers/physics_3d/collision_solver_3d_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/servers/physics_3d/constraint_3d_sw.h b/servers/physics_3d/constraint_3d_sw.h index 081ddb0382..2571335c43 100644 --- a/servers/physics_3d/constraint_3d_sw.h +++ b/servers/physics_3d/constraint_3d_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/servers/physics_3d/gjk_epa.cpp b/servers/physics_3d/gjk_epa.cpp index d99a2532f8..aa7c11eec5 100644 --- a/servers/physics_3d/gjk_epa.cpp +++ b/servers/physics_3d/gjk_epa.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -64,7 +64,7 @@ GJK-EPA collision solver by Nathanael Presson, 2008 /* GJK */ #define GJK_MAX_ITERATIONS 128 -#define GJK_ACCURARY ((real_t)0.0001) +#define GJK_ACCURACY ((real_t)0.0001) #define GJK_MIN_DISTANCE ((real_t)0.0001) #define GJK_DUPLICATED_EPS ((real_t)0.0001) #define GJK_SIMPLEX2_EPS ((real_t)0.0) @@ -72,10 +72,13 @@ GJK-EPA collision solver by Nathanael Presson, 2008 #define GJK_SIMPLEX4_EPS ((real_t)0.0) /* EPA */ -#define EPA_MAX_VERTICES 64 +#define EPA_MAX_VERTICES 128 #define EPA_MAX_FACES (EPA_MAX_VERTICES*2) #define EPA_MAX_ITERATIONS 255 -#define EPA_ACCURACY ((real_t)0.0001) +// -- GODOT start -- +//#define EPA_ACCURACY ((real_t)0.0001) +#define EPA_ACCURACY ((real_t)0.00001) +// -- GODOT end -- #define EPA_FALLBACK (10*EPA_ACCURACY) #define EPA_PLANE_EPS ((real_t)0.00001) #define EPA_INSIDE_EPS ((real_t)0.01) @@ -102,7 +105,6 @@ typedef unsigned char U1; // MinkowskiDiff struct MinkowskiDiff { - const Shape3DSW* m_shapes[2]; Transform transform_A; @@ -127,7 +129,6 @@ struct MinkowskiDiff { return ( Support1 ( d ) ); } else { return ( Support0 ( d ) ); - } } }; @@ -239,7 +240,7 @@ struct GJK /* Check for termination */ const real_t omega=vec3_dot(m_ray,w)/rl; alpha=MAX(omega,alpha); - if(((rl-alpha)-(GJK_ACCURARY*rl))<=0) + if(((rl-alpha)-(GJK_ACCURACY*rl))<=0) {/* Return old simplex */ removevertice(m_simplices[m_current]); break; @@ -281,7 +282,6 @@ struct GJK } } if(mask==15) { m_status=eStatus::Inside; - } } else @@ -312,12 +312,10 @@ struct GJK axis[i]=1; appendvertice(*m_simplex, axis); if(EncloseOrigin()) { return(true); - } removevertice(*m_simplex); appendvertice(*m_simplex,-axis); if(EncloseOrigin()) { return(true); - } removevertice(*m_simplex); } @@ -335,12 +333,10 @@ struct GJK { appendvertice(*m_simplex, p); if(EncloseOrigin()) { return(true); - } removevertice(*m_simplex); appendvertice(*m_simplex,-p); if(EncloseOrigin()) { return(true); - } removevertice(*m_simplex); } @@ -355,12 +351,10 @@ struct GJK { appendvertice(*m_simplex,n); if(EncloseOrigin()) { return(true); - } removevertice(*m_simplex); appendvertice(*m_simplex,-n); if(EncloseOrigin()) { return(true); - } removevertice(*m_simplex); } @@ -372,7 +366,6 @@ struct GJK m_simplex->c[1]->w-m_simplex->c[3]->w, m_simplex->c[2]->w-m_simplex->c[3]->w))>0) { return(true); - } } break; @@ -476,7 +469,7 @@ struct GJK if(ng&&(Math::abs(vl)>GJK_SIMPLEX4_EPS)) { real_t mindist=-1; - real_t subw[3]; + real_t subw[3] = {0.f, 0.f, 0.f}; U subm=0; for(U i=0;i<3;++i) { @@ -522,7 +515,6 @@ struct GJK { Vector3 n; real_t d; - real_t p; sSV* c[3]; sFace* f[3]; sFace* l[2]; @@ -580,7 +572,6 @@ struct GJK face->l[0] = nullptr; face->l[1] = list.root; if(list.root) { list.root->l[0]=face; - } list.root = face; ++list.count; @@ -588,13 +579,10 @@ struct GJK static inline void remove(sList& list,sFace* face) { if(face->l[1]) { face->l[1]->l[0]=face->l[0]; - } if(face->l[0]) { face->l[0]->l[1]=face->l[1]; - } if(face==list.root) { list.root=face->l[1]; - } --list.count; } @@ -616,7 +604,6 @@ struct GJK GJK::sSimplex& simplex=*gjk.m_simplex; if((simplex.rank>1)&&gjk.EncloseOrigin()) { - /* Clean up */ while(m_hull.root) { @@ -676,9 +663,7 @@ struct GJK remove(m_hull,best); append(m_stock,best); best=findbest(); - if(best->p>=outer.p) { outer=*best; - -} + outer=*best; } else { m_status=eStatus::InvalidHull;break; } } else { m_status=eStatus::AccuraryReached;break; } } else { m_status=eStatus::OutOfVertices;break; } @@ -704,25 +689,54 @@ struct GJK } } /* Fallback */ - m_status = eStatus::FallBack; - m_normal = -guess; - const real_t nl=m_normal.length(); - if(nl>0) { - m_normal = m_normal/nl; + m_status = eStatus::FallBack; + m_normal = -guess; + const real_t nl = m_normal.length(); + if (nl > 0) { + m_normal = m_normal/nl; } else { - m_normal = Vector3(1,0,0); - -} + m_normal = Vector3(1,0,0); + } m_depth = 0; m_result.rank=1; m_result.c[0]=simplex.c[0]; m_result.p[0]=1; return(m_status); } + + bool getedgedist(sFace* face, sSV* a, sSV* b, real_t& dist) + { + const Vector3 ba = b->w - a->w; + const Vector3 n_ab = vec3_cross(ba, face->n); // Outward facing edge normal direction, on triangle plane + const real_t a_dot_nab = vec3_dot(a->w, n_ab); // Only care about the sign to determine inside/outside, so not normalization required + + if (a_dot_nab < 0) { + // Outside of edge a->b + const real_t ba_l2 = ba.length_squared(); + const real_t a_dot_ba = vec3_dot(a->w, ba); + const real_t b_dot_ba = vec3_dot(b->w, ba); + + if (a_dot_ba > 0) { + // Pick distance vertex a + dist = a->w.length(); + } else if (b_dot_ba < 0) { + // Pick distance vertex b + dist = b->w.length(); + } else { + // Pick distance to edge a->b + const real_t a_dot_b = vec3_dot(a->w, b->w); + dist = Math::sqrt(MAX((a->w.length_squared() * b->w.length_squared() - a_dot_b * a_dot_b) / ba_l2, 0.0)); + } + + return true; + } + + return false; + } + sFace* newface(sSV* a,sSV* b,sSV* c,bool forced) { - if(m_stock.root) - { + if (m_stock.root) { sFace* face=m_stock.root; remove(m_stock,face); append(m_hull,face); @@ -733,25 +747,23 @@ struct GJK face->n = vec3_cross(b->w-a->w,c->w-a->w); const real_t l=face->n.length(); const bool v=l>EPA_ACCURACY; - face->p = MIN(MIN( - vec3_dot(a->w,vec3_cross(face->n,a->w-b->w)), - vec3_dot(b->w,vec3_cross(face->n,b->w-c->w))), - vec3_dot(c->w,vec3_cross(face->n,c->w-a->w))) / - (v?l:1); - face->p = face->p>=-EPA_INSIDE_EPS?0:face->p; - if(v) - { - face->d = vec3_dot(a->w,face->n)/l; + if (v) { + if (!(getedgedist(face, a, b, face->d) || + getedgedist(face, b, c, face->d) || + getedgedist(face, c, a, face->d))) { + // Origin projects to the interior of the triangle + // Use distance to triangle plane + face->d = vec3_dot(a->w, face->n) / l; + } face->n /= l; - if(forced||(face->d>=-EPA_PLANE_EPS)) - { + if (forced||(face->d>=-EPA_PLANE_EPS)) { return(face); - } else { m_status=eStatus::NonConvex; - -} - } else { m_status=eStatus::Degenerated; - -} + } else { + m_status=eStatus::NonConvex; + } + } else { + m_status=eStatus::Degenerated; + } remove(m_hull,face); append(m_stock,face); return(nullptr); @@ -766,15 +778,13 @@ struct GJK { sFace* minf=m_hull.root; real_t mind=minf->d*minf->d; - real_t maxp=minf->p; for(sFace* f=minf->l[1];f;f=f->l[1]) { const real_t sqd=f->d*f->d; - if((f->p>=maxp)&&(sqd<mind)) + if(sqd<mind) { minf=f; mind=sqd; - maxp=f->p; } } return(minf); @@ -793,7 +803,6 @@ struct GJK { bind(nf,0,f,e); if(horizon.cf) { bind(horizon.cf,1,nf,2); } else { horizon.ff=nf; - } horizon.cf=nf; ++horizon.nf; @@ -917,7 +926,6 @@ bool Penetration( const Shape3DSW* shape0, results.distance = -epa.m_depth; return(true); } else { results.status=sResults::EPA_Failed; - } } break; @@ -948,8 +956,6 @@ bool Penetration( const Shape3DSW* shape0, #undef EPA_FALLBACK #undef EPA_PLANE_EPS #undef EPA_INSIDE_EPS - - } // end of namespace /* clang-format on */ diff --git a/servers/physics_3d/gjk_epa.h b/servers/physics_3d/gjk_epa.h index dec0f269e1..be3ba4e664 100644 --- a/servers/physics_3d/gjk_epa.h +++ b/servers/physics_3d/gjk_epa.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp b/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp index 9d10ede608..9c4493f4a2 100644 --- a/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -92,9 +92,9 @@ ConeTwistJoint3DSW::ConeTwistJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Trans m_rbAFrame = rbAFrame; m_rbBFrame = rbBFrame; - m_swingSpan1 = Math_PI / 4.0; - m_swingSpan2 = Math_PI / 4.0; - m_twistSpan = Math_PI * 2; + m_swingSpan1 = Math_TAU / 8.0; + m_swingSpan2 = Math_TAU / 8.0; + m_twistSpan = Math_TAU; m_biasFactor = 0.3f; m_relaxationFactor = 1.0f; @@ -261,8 +261,8 @@ void ConeTwistJoint3DSW::solve(real_t p_timestep) { real_t impulse = depth * tau / p_timestep * jacDiagABInv - rel_vel * jacDiagABInv; m_appliedImpulse += impulse; Vector3 impulse_vector = normal * impulse; - A->apply_impulse(pivotAInW - A->get_transform().origin, impulse_vector); - B->apply_impulse(pivotBInW - B->get_transform().origin, -impulse_vector); + A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin); + B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin); } } diff --git a/servers/physics_3d/joints/cone_twist_joint_3d_sw.h b/servers/physics_3d/joints/cone_twist_joint_3d_sw.h index c713d8cf17..4e4d4e7f0c 100644 --- a/servers/physics_3d/joints/cone_twist_joint_3d_sw.h +++ b/servers/physics_3d/joints/cone_twist_joint_3d_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -102,7 +102,7 @@ public: bool m_solveSwingLimit; public: - virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_CONE_TWIST; } + virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_CONE_TWIST; } virtual bool setup(real_t p_timestep); virtual void solve(real_t p_timestep); diff --git a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp index 423bbc0dfd..13b389251f 100644 --- a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -132,7 +132,7 @@ real_t G6DOFRotationalLimitMotor3DSW::solveAngularLimits( real_t oldaccumImpulse = m_accumulatedImpulse; real_t sum = oldaccumImpulse + clippedMotorImpulse; - m_accumulatedImpulse = sum > hi ? real_t(0.) : sum < lo ? real_t(0.) : sum; + m_accumulatedImpulse = sum > hi ? real_t(0.) : (sum < lo ? real_t(0.) : sum); clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse; @@ -201,12 +201,12 @@ real_t G6DOFTranslationalLimitMotor3DSW::solveLinearAxis( real_t oldNormalImpulse = m_accumulatedImpulse[limit_index]; real_t sum = oldNormalImpulse + normalImpulse; - m_accumulatedImpulse[limit_index] = sum > hi ? real_t(0.) : sum < lo ? real_t(0.) : sum; + m_accumulatedImpulse[limit_index] = sum > hi ? real_t(0.) : (sum < lo ? real_t(0.) : sum); normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse; Vector3 impulse_vector = axis_normal_on_a * normalImpulse; - body1->apply_impulse(rel_pos1, impulse_vector); - body2->apply_impulse(rel_pos2, -impulse_vector); + body1->apply_impulse(impulse_vector, rel_pos1); + body2->apply_impulse(-impulse_vector, rel_pos2); return normalImpulse; } @@ -253,7 +253,6 @@ void Generic6DOFJoint3DSW::calculateAngleInfo() { /* if(m_debugDrawer) { - char buff[300]; sprintf(buff,"\n X: %.2f ; Y: %.2f ; Z: %.2f ", m_calculatedAxisAngleDiff[0], diff --git a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h index cc1423a1cb..d61a033231 100644 --- a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h +++ b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -103,19 +103,6 @@ public: m_enableLimit = false; } - G6DOFRotationalLimitMotor3DSW(const G6DOFRotationalLimitMotor3DSW &limot) { - m_targetVelocity = limot.m_targetVelocity; - m_maxMotorForce = limot.m_maxMotorForce; - m_limitSoftness = limot.m_limitSoftness; - m_loLimit = limot.m_loLimit; - m_hiLimit = limot.m_hiLimit; - m_ERP = limot.m_ERP; - m_bounce = limot.m_bounce; - m_currentLimit = limot.m_currentLimit; - m_currentLimitError = limot.m_currentLimitError; - m_enableMotor = limot.m_enableMotor; - } - //! Is limited bool isLimited() { return (m_loLimit < m_hiLimit); @@ -163,16 +150,6 @@ public: enable_limit[2] = true; } - G6DOFTranslationalLimitMotor3DSW(const G6DOFTranslationalLimitMotor3DSW &other) { - m_lowerLimit = other.m_lowerLimit; - m_upperLimit = other.m_upperLimit; - m_accumulatedImpulse = other.m_accumulatedImpulse; - - m_limitSoftness = other.m_limitSoftness; - m_damping = other.m_damping; - m_restitution = other.m_restitution; - } - //! Test limit /*! - free means upper < lower, @@ -242,11 +219,8 @@ protected: //!@} - Generic6DOFJoint3DSW &operator=(Generic6DOFJoint3DSW &other) { - ERR_PRINT("pito"); - (void)other; - return *this; - } + Generic6DOFJoint3DSW(Generic6DOFJoint3DSW const &) = delete; + void operator=(Generic6DOFJoint3DSW const &) = delete; void buildLinearJacobian( JacobianEntry3DSW &jacLinear, const Vector3 &normalWorld, @@ -260,7 +234,7 @@ protected: public: Generic6DOFJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform &frameInA, const Transform &frameInB, bool useLinearReferenceFrameA); - virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_6DOF; } + virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_6DOF; } virtual bool setup(real_t p_timestep); virtual void solve(real_t p_timestep); diff --git a/servers/physics_3d/joints/hinge_joint_3d_sw.cpp b/servers/physics_3d/joints/hinge_joint_3d_sw.cpp index a879b4ca7f..2b9f0038b4 100644 --- a/servers/physics_3d/joints/hinge_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/hinge_joint_3d_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -275,8 +275,8 @@ void HingeJoint3DSW::solve(real_t p_step) { real_t impulse = depth * tau / p_step * jacDiagABInv - rel_vel * jacDiagABInv; m_appliedImpulse += impulse; Vector3 impulse_vector = normal * impulse; - A->apply_impulse(pivotAInW - A->get_transform().origin, impulse_vector); - B->apply_impulse(pivotBInW - B->get_transform().origin, -impulse_vector); + A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin); + B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin); } } @@ -365,7 +365,6 @@ void HingeJoint3DSW::solve(real_t p_step) { void HingeJointSW::updateRHS(real_t timeStep) { (void)timeStep; - } */ diff --git a/servers/physics_3d/joints/hinge_joint_3d_sw.h b/servers/physics_3d/joints/hinge_joint_3d_sw.h index c5af888eca..b6117aa0bc 100644 --- a/servers/physics_3d/joints/hinge_joint_3d_sw.h +++ b/servers/physics_3d/joints/hinge_joint_3d_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -96,7 +96,7 @@ class HingeJoint3DSW : public Joint3DSW { real_t m_appliedImpulse; public: - virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_HINGE; } + virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_HINGE; } virtual bool setup(real_t p_step); virtual void solve(real_t p_step); diff --git a/servers/physics_3d/joints/jacobian_entry_3d_sw.h b/servers/physics_3d/joints/jacobian_entry_3d_sw.h index 1737c21b3d..2829a5caf7 100644 --- a/servers/physics_3d/joints/jacobian_entry_3d_sw.h +++ b/servers/physics_3d/joints/jacobian_entry_3d_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/servers/physics_3d/joints/pin_joint_3d_sw.cpp b/servers/physics_3d/joints/pin_joint_3d_sw.cpp index 230904408b..9f708ce151 100644 --- a/servers/physics_3d/joints/pin_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/pin_joint_3d_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -119,8 +119,8 @@ void PinJoint3DSW::solve(real_t p_step) { m_appliedImpulse += impulse; Vector3 impulse_vector = normal * impulse; - A->apply_impulse(pivotAInW - A->get_transform().origin, impulse_vector); - B->apply_impulse(pivotBInW - B->get_transform().origin, -impulse_vector); + A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin); + B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin); normal[i] = 0; } diff --git a/servers/physics_3d/joints/pin_joint_3d_sw.h b/servers/physics_3d/joints/pin_joint_3d_sw.h index 0181a4455b..1875983527 100644 --- a/servers/physics_3d/joints/pin_joint_3d_sw.h +++ b/servers/physics_3d/joints/pin_joint_3d_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -74,7 +74,7 @@ class PinJoint3DSW : public Joint3DSW { Vector3 m_pivotInB; public: - virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_PIN; } + virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_PIN; } virtual bool setup(real_t p_step); virtual void solve(real_t p_step); diff --git a/servers/physics_3d/joints/slider_joint_3d_sw.cpp b/servers/physics_3d/joints/slider_joint_3d_sw.cpp index 5b4609f24e..0adc471797 100644 --- a/servers/physics_3d/joints/slider_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/slider_joint_3d_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -105,7 +105,6 @@ void SliderJoint3DSW::initParams() { m_targetAngMotorVelocity = real_t(0.); m_maxAngMotorForce = real_t(0.); m_accumulatedAngMotorImpulse = real_t(0.0); - } // SliderJointSW::initParams() //----------------------------------------------------------------------------- @@ -197,8 +196,8 @@ void SliderJoint3DSW::solve(real_t p_step) { // calcutate and apply impulse real_t normalImpulse = softness * (restitution * depth / p_step - damping * rel_vel) * m_jacLinDiagABInv[i]; Vector3 impulse_vector = normal * normalImpulse; - A->apply_impulse(m_relPosA, impulse_vector); - B->apply_impulse(m_relPosB, -impulse_vector); + A->apply_impulse(impulse_vector, m_relPosA); + B->apply_impulse(-impulse_vector, m_relPosB); if (m_poweredLinMotor && (!i)) { // apply linear motor if (m_accumulatedLinMotorImpulse < m_maxLinMotorForce) { real_t desiredMotorVel = m_targetLinMotorVelocity; @@ -218,8 +217,8 @@ void SliderJoint3DSW::solve(real_t p_step) { m_accumulatedLinMotorImpulse = new_acc; // apply clamped impulse impulse_vector = normal * normalImpulse; - A->apply_impulse(m_relPosA, impulse_vector); - B->apply_impulse(m_relPosB, -impulse_vector); + A->apply_impulse(impulse_vector, m_relPosA); + B->apply_impulse(-impulse_vector, m_relPosB); } } } diff --git a/servers/physics_3d/joints/slider_joint_3d_sw.h b/servers/physics_3d/joints/slider_joint_3d_sw.h index 37394a1580..f52f6ace27 100644 --- a/servers/physics_3d/joints/slider_joint_3d_sw.h +++ b/servers/physics_3d/joints/slider_joint_3d_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -243,7 +243,7 @@ public: bool setup(real_t p_step); void solve(real_t p_step); - virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_SLIDER; } + virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_SLIDER; } }; #endif // SLIDER_JOINT_SW_H diff --git a/servers/physics_3d/joints_3d_sw.h b/servers/physics_3d/joints_3d_sw.h index 6a010ee771..1fe573c69e 100644 --- a/servers/physics_3d/joints_3d_sw.h +++ b/servers/physics_3d/joints_3d_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -36,7 +36,16 @@ class Joint3DSW : public Constraint3DSW { public: - virtual PhysicsServer3D::JointType get_type() const = 0; + virtual bool setup(real_t p_step) { return false; } + virtual void solve(real_t p_step) {} + + void copy_settings_from(Joint3DSW *p_joint) { + set_self(p_joint->get_self()); + set_priority(p_joint->get_priority()); + disable_collisions_between_bodies(p_joint->is_disabled_collisions_between_bodies()); + } + + virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_MAX; } _FORCE_INLINE_ Joint3DSW(Body3DSW **p_body_ptr = nullptr, int p_body_count = 0) : Constraint3DSW(p_body_ptr, p_body_count) { } diff --git a/servers/physics_3d/physics_server_3d_sw.cpp b/servers/physics_3d/physics_server_3d_sw.cpp index 1c2329f2dc..735e9094d2 100644 --- a/servers/physics_3d/physics_server_3d_sw.cpp +++ b/servers/physics_3d/physics_server_3d_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -43,47 +43,63 @@ #define FLUSH_QUERY_CHECK(m_object) \ ERR_FAIL_COND_MSG(m_object->get_space() && flushing_queries, "Can't change this state while flushing queries. Use call_deferred() or set_deferred() to change monitoring state instead."); -RID PhysicsServer3DSW::shape_create(ShapeType p_shape) { - Shape3DSW *shape = nullptr; - switch (p_shape) { - case SHAPE_PLANE: { - shape = memnew(PlaneShape3DSW); - } break; - case SHAPE_RAY: { - shape = memnew(RayShape3DSW); - } break; - case SHAPE_SPHERE: { - shape = memnew(SphereShape3DSW); - } break; - case SHAPE_BOX: { - shape = memnew(BoxShape3DSW); - } break; - case SHAPE_CAPSULE: { - shape = memnew(CapsuleShape3DSW); - } break; - case SHAPE_CYLINDER: { - ERR_FAIL_V_MSG(RID(), "CylinderShape3D is not supported in GodotPhysics3D. Please switch to Bullet in the Project Settings."); - } break; - case SHAPE_CONVEX_POLYGON: { - shape = memnew(ConvexPolygonShape3DSW); - } break; - case SHAPE_CONCAVE_POLYGON: { - shape = memnew(ConcavePolygonShape3DSW); - } break; - case SHAPE_HEIGHTMAP: { - shape = memnew(HeightMapShape3DSW); - } break; - case SHAPE_CUSTOM: { - ERR_FAIL_V(RID()); - - } break; - } - - RID id = shape_owner.make_rid(shape); - shape->set_self(id); - - return id; -}; +RID PhysicsServer3DSW::plane_shape_create() { + Shape3DSW *shape = memnew(PlaneShape3DSW); + RID rid = shape_owner.make_rid(shape); + shape->set_self(rid); + return rid; +} +RID PhysicsServer3DSW::ray_shape_create() { + Shape3DSW *shape = memnew(RayShape3DSW); + RID rid = shape_owner.make_rid(shape); + shape->set_self(rid); + return rid; +} +RID PhysicsServer3DSW::sphere_shape_create() { + Shape3DSW *shape = memnew(SphereShape3DSW); + RID rid = shape_owner.make_rid(shape); + shape->set_self(rid); + return rid; +} +RID PhysicsServer3DSW::box_shape_create() { + Shape3DSW *shape = memnew(BoxShape3DSW); + RID rid = shape_owner.make_rid(shape); + shape->set_self(rid); + return rid; +} +RID PhysicsServer3DSW::capsule_shape_create() { + Shape3DSW *shape = memnew(CapsuleShape3DSW); + RID rid = shape_owner.make_rid(shape); + shape->set_self(rid); + return rid; +} +RID PhysicsServer3DSW::cylinder_shape_create() { + Shape3DSW *shape = memnew(CylinderShape3DSW); + RID rid = shape_owner.make_rid(shape); + shape->set_self(rid); + return rid; +} +RID PhysicsServer3DSW::convex_polygon_shape_create() { + Shape3DSW *shape = memnew(ConvexPolygonShape3DSW); + RID rid = shape_owner.make_rid(shape); + shape->set_self(rid); + return rid; +} +RID PhysicsServer3DSW::concave_polygon_shape_create() { + Shape3DSW *shape = memnew(ConcavePolygonShape3DSW); + RID rid = shape_owner.make_rid(shape); + shape->set_self(rid); + return rid; +} +RID PhysicsServer3DSW::heightmap_shape_create() { + Shape3DSW *shape = memnew(HeightMapShape3DSW); + RID rid = shape_owner.make_rid(shape); + shape->set_self(rid); + return rid; +} +RID PhysicsServer3DSW::custom_shape_create() { + ERR_FAIL_V(RID()); +} void PhysicsServer3DSW::shape_set_data(RID p_shape, const Variant &p_data) { Shape3DSW *shape = shape_owner.getornull(p_shape); @@ -174,7 +190,7 @@ real_t PhysicsServer3DSW::space_get_param(RID p_space, SpaceParameter p_param) c PhysicsDirectSpaceState3D *PhysicsServer3DSW::space_get_direct_state(RID p_space) { Space3DSW *space = space_owner.getornull(p_space); ERR_FAIL_COND_V(!space, nullptr); - ERR_FAIL_COND_V_MSG(!doing_sync || space->is_locked(), nullptr, "Space state is inaccessible right now, wait for iteration or physics process notification."); + ERR_FAIL_COND_V_MSG((using_threads && !doing_sync) || space->is_locked(), nullptr, "Space state is inaccessible right now, wait for iteration or physics process notification."); return space->get_direct_state(); } @@ -413,13 +429,6 @@ void PhysicsServer3DSW::area_set_ray_pickable(RID p_area, bool p_enable) { area->set_ray_pickable(p_enable); } -bool PhysicsServer3DSW::area_is_ray_pickable(RID p_area) const { - Area3DSW *area = area_owner.getornull(p_area); - ERR_FAIL_COND_V(!area, false); - - return area->is_ray_pickable(); -} - void PhysicsServer3DSW::area_set_area_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) { Area3DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND(!area); @@ -429,14 +438,8 @@ void PhysicsServer3DSW::area_set_area_monitor_callback(RID p_area, Object *p_rec /* BODY API */ -RID PhysicsServer3DSW::body_create(BodyMode p_mode, bool p_init_sleeping) { +RID PhysicsServer3DSW::body_create() { Body3DSW *body = memnew(Body3DSW); - if (p_mode != BODY_MODE_RIGID) { - body->set_mode(p_mode); - } - if (p_init_sleeping) { - body->set_state(BODY_STATE_SLEEPING, p_init_sleeping); - } RID rid = body_owner.make_rid(body); body->set_self(rid); return rid; @@ -710,11 +713,11 @@ void PhysicsServer3DSW::body_add_central_force(RID p_body, const Vector3 &p_forc body->wakeup(); } -void PhysicsServer3DSW::body_add_force(RID p_body, const Vector3 &p_force, const Vector3 &p_pos) { +void PhysicsServer3DSW::body_add_force(RID p_body, const Vector3 &p_force, const Vector3 &p_position) { Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); - body->add_force(p_force, p_pos); + body->add_force(p_force, p_position); body->wakeup(); }; @@ -736,13 +739,13 @@ void PhysicsServer3DSW::body_apply_central_impulse(RID p_body, const Vector3 &p_ body->wakeup(); } -void PhysicsServer3DSW::body_apply_impulse(RID p_body, const Vector3 &p_pos, const Vector3 &p_impulse) { +void PhysicsServer3DSW::body_apply_impulse(RID p_body, const Vector3 &p_impulse, const Vector3 &p_position) { Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); _update_shapes(); - body->apply_impulse(p_pos, p_impulse); + body->apply_impulse(p_impulse, p_position); body->wakeup(); }; @@ -857,12 +860,6 @@ void PhysicsServer3DSW::body_set_ray_pickable(RID p_body, bool p_enable) { body->set_ray_pickable(p_enable); } -bool PhysicsServer3DSW::body_is_ray_pickable(RID p_body) const { - Body3DSW *body = body_owner.getornull(p_body); - ERR_FAIL_COND_V(!body, false); - return body->is_ray_pickable(); -} - bool PhysicsServer3DSW::body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, MotionResult *r_result, bool p_exclude_raycast_shapes) { Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, false); @@ -874,7 +871,7 @@ bool PhysicsServer3DSW::body_test_motion(RID p_body, const Transform &p_from, co return body->get_space()->test_body_motion(body, p_from, p_motion, p_infinite_inertia, body->get_kinematic_margin(), r_result, p_exclude_raycast_shapes); } -int PhysicsServer3DSW::body_test_ray_separation(RID p_body, const Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, SeparationResult *r_results, int p_result_max, float p_margin) { +int PhysicsServer3DSW::body_test_ray_separation(RID p_body, const Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, SeparationResult *r_results, int p_result_max, real_t p_margin) { Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, false); ERR_FAIL_COND_V(!body->get_space(), false); @@ -886,9 +883,11 @@ int PhysicsServer3DSW::body_test_ray_separation(RID p_body, const Transform &p_t } PhysicsDirectBodyState3D *PhysicsServer3DSW::body_get_direct_state(RID p_body) { + ERR_FAIL_COND_V_MSG((using_threads && !doing_sync), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification."); + Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, nullptr); - ERR_FAIL_COND_V_MSG(!doing_sync || body->get_space()->is_locked(), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification."); + ERR_FAIL_COND_V_MSG(body->get_space()->is_locked(), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification."); direct_state->body = body; return direct_state; @@ -896,30 +895,52 @@ PhysicsDirectBodyState3D *PhysicsServer3DSW::body_get_direct_state(RID p_body) { /* JOINT API */ -RID PhysicsServer3DSW::joint_create_pin(RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B) { +RID PhysicsServer3DSW::joint_create() { + Joint3DSW *joint = memnew(Joint3DSW); + RID rid = joint_owner.make_rid(joint); + joint->set_self(rid); + return rid; +} + +void PhysicsServer3DSW::joint_clear(RID p_joint) { + Joint3DSW *joint = joint_owner.getornull(p_joint); + if (joint->get_type() != JOINT_TYPE_MAX) { + Joint3DSW *empty_joint = memnew(Joint3DSW); + empty_joint->copy_settings_from(joint); + + joint_owner.replace(p_joint, empty_joint); + memdelete(joint); + } +} + +void PhysicsServer3DSW::joint_make_pin(RID p_joint, RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B) { Body3DSW *body_A = body_owner.getornull(p_body_A); - ERR_FAIL_COND_V(!body_A, RID()); + ERR_FAIL_COND(!body_A); if (!p_body_B.is_valid()) { - ERR_FAIL_COND_V(!body_A->get_space(), RID()); + ERR_FAIL_COND(!body_A->get_space()); p_body_B = body_A->get_space()->get_static_global_body(); } Body3DSW *body_B = body_owner.getornull(p_body_B); - ERR_FAIL_COND_V(!body_B, RID()); + ERR_FAIL_COND(!body_B); - ERR_FAIL_COND_V(body_A == body_B, RID()); + ERR_FAIL_COND(body_A == body_B); + + Joint3DSW *prev_joint = joint_owner.getornull(p_joint); + ERR_FAIL_COND(prev_joint == nullptr); Joint3DSW *joint = memnew(PinJoint3DSW(body_A, p_local_A, body_B, p_local_B)); - RID rid = joint_owner.make_rid(joint); - joint->set_self(rid); - return rid; + + joint->copy_settings_from(prev_joint); + joint_owner.replace(p_joint, joint); + memdelete(prev_joint); } void PhysicsServer3DSW::pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value) { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_PIN); + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_PIN); PinJoint3DSW *pin_joint = static_cast<PinJoint3DSW *>(joint); pin_joint->set_param(p_param, p_value); } @@ -927,7 +948,7 @@ void PhysicsServer3DSW::pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t PhysicsServer3DSW::pin_joint_get_param(RID p_joint, PinJointParam p_param) const { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND_V(!joint, 0); - ERR_FAIL_COND_V(joint->get_type() != JOINT_PIN, 0); + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_PIN, 0); PinJoint3DSW *pin_joint = static_cast<PinJoint3DSW *>(joint); return pin_joint->get_param(p_param); } @@ -935,7 +956,7 @@ real_t PhysicsServer3DSW::pin_joint_get_param(RID p_joint, PinJointParam p_param void PhysicsServer3DSW::pin_joint_set_local_a(RID p_joint, const Vector3 &p_A) { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_PIN); + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_PIN); PinJoint3DSW *pin_joint = static_cast<PinJoint3DSW *>(joint); pin_joint->set_pos_a(p_A); } @@ -943,7 +964,7 @@ void PhysicsServer3DSW::pin_joint_set_local_a(RID p_joint, const Vector3 &p_A) { Vector3 PhysicsServer3DSW::pin_joint_get_local_a(RID p_joint) const { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND_V(!joint, Vector3()); - ERR_FAIL_COND_V(joint->get_type() != JOINT_PIN, Vector3()); + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_PIN, Vector3()); PinJoint3DSW *pin_joint = static_cast<PinJoint3DSW *>(joint); return pin_joint->get_position_a(); } @@ -951,7 +972,7 @@ Vector3 PhysicsServer3DSW::pin_joint_get_local_a(RID p_joint) const { void PhysicsServer3DSW::pin_joint_set_local_b(RID p_joint, const Vector3 &p_B) { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_PIN); + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_PIN); PinJoint3DSW *pin_joint = static_cast<PinJoint3DSW *>(joint); pin_joint->set_pos_b(p_B); } @@ -959,55 +980,63 @@ void PhysicsServer3DSW::pin_joint_set_local_b(RID p_joint, const Vector3 &p_B) { Vector3 PhysicsServer3DSW::pin_joint_get_local_b(RID p_joint) const { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND_V(!joint, Vector3()); - ERR_FAIL_COND_V(joint->get_type() != JOINT_PIN, Vector3()); + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_PIN, Vector3()); PinJoint3DSW *pin_joint = static_cast<PinJoint3DSW *>(joint); return pin_joint->get_position_b(); } -RID PhysicsServer3DSW::joint_create_hinge(RID p_body_A, const Transform &p_frame_A, RID p_body_B, const Transform &p_frame_B) { +void PhysicsServer3DSW::joint_make_hinge(RID p_joint, RID p_body_A, const Transform &p_frame_A, RID p_body_B, const Transform &p_frame_B) { Body3DSW *body_A = body_owner.getornull(p_body_A); - ERR_FAIL_COND_V(!body_A, RID()); + ERR_FAIL_COND(!body_A); if (!p_body_B.is_valid()) { - ERR_FAIL_COND_V(!body_A->get_space(), RID()); + ERR_FAIL_COND(!body_A->get_space()); p_body_B = body_A->get_space()->get_static_global_body(); } Body3DSW *body_B = body_owner.getornull(p_body_B); - ERR_FAIL_COND_V(!body_B, RID()); + ERR_FAIL_COND(!body_B); + + ERR_FAIL_COND(body_A == body_B); - ERR_FAIL_COND_V(body_A == body_B, RID()); + Joint3DSW *prev_joint = joint_owner.getornull(p_joint); + ERR_FAIL_COND(prev_joint == nullptr); Joint3DSW *joint = memnew(HingeJoint3DSW(body_A, body_B, p_frame_A, p_frame_B)); - RID rid = joint_owner.make_rid(joint); - joint->set_self(rid); - return rid; + + joint->copy_settings_from(prev_joint); + joint_owner.replace(p_joint, joint); + memdelete(prev_joint); } -RID PhysicsServer3DSW::joint_create_hinge_simple(RID p_body_A, const Vector3 &p_pivot_A, const Vector3 &p_axis_A, RID p_body_B, const Vector3 &p_pivot_B, const Vector3 &p_axis_B) { +void PhysicsServer3DSW::joint_make_hinge_simple(RID p_joint, RID p_body_A, const Vector3 &p_pivot_A, const Vector3 &p_axis_A, RID p_body_B, const Vector3 &p_pivot_B, const Vector3 &p_axis_B) { Body3DSW *body_A = body_owner.getornull(p_body_A); - ERR_FAIL_COND_V(!body_A, RID()); + ERR_FAIL_COND(!body_A); if (!p_body_B.is_valid()) { - ERR_FAIL_COND_V(!body_A->get_space(), RID()); + ERR_FAIL_COND(!body_A->get_space()); p_body_B = body_A->get_space()->get_static_global_body(); } Body3DSW *body_B = body_owner.getornull(p_body_B); - ERR_FAIL_COND_V(!body_B, RID()); + ERR_FAIL_COND(!body_B); + + ERR_FAIL_COND(body_A == body_B); - ERR_FAIL_COND_V(body_A == body_B, RID()); + Joint3DSW *prev_joint = joint_owner.getornull(p_joint); + ERR_FAIL_COND(prev_joint == nullptr); Joint3DSW *joint = memnew(HingeJoint3DSW(body_A, body_B, p_pivot_A, p_pivot_B, p_axis_A, p_axis_B)); - RID rid = joint_owner.make_rid(joint); - joint->set_self(rid); - return rid; + + joint->copy_settings_from(prev_joint); + joint_owner.replace(p_joint, joint); + memdelete(prev_joint); } void PhysicsServer3DSW::hinge_joint_set_param(RID p_joint, HingeJointParam p_param, real_t p_value) { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_HINGE); + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_HINGE); HingeJoint3DSW *hinge_joint = static_cast<HingeJoint3DSW *>(joint); hinge_joint->set_param(p_param, p_value); } @@ -1015,7 +1044,7 @@ void PhysicsServer3DSW::hinge_joint_set_param(RID p_joint, HingeJointParam p_par real_t PhysicsServer3DSW::hinge_joint_get_param(RID p_joint, HingeJointParam p_param) const { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND_V(!joint, 0); - ERR_FAIL_COND_V(joint->get_type() != JOINT_HINGE, 0); + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_HINGE, 0); HingeJoint3DSW *hinge_joint = static_cast<HingeJoint3DSW *>(joint); return hinge_joint->get_param(p_param); } @@ -1023,7 +1052,7 @@ real_t PhysicsServer3DSW::hinge_joint_get_param(RID p_joint, HingeJointParam p_p void PhysicsServer3DSW::hinge_joint_set_flag(RID p_joint, HingeJointFlag p_flag, bool p_value) { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_HINGE); + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_HINGE); HingeJoint3DSW *hinge_joint = static_cast<HingeJoint3DSW *>(joint); hinge_joint->set_flag(p_flag, p_value); } @@ -1031,7 +1060,7 @@ void PhysicsServer3DSW::hinge_joint_set_flag(RID p_joint, HingeJointFlag p_flag, bool PhysicsServer3DSW::hinge_joint_get_flag(RID p_joint, HingeJointFlag p_flag) const { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND_V(!joint, false); - ERR_FAIL_COND_V(joint->get_type() != JOINT_HINGE, false); + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_HINGE, false); HingeJoint3DSW *hinge_joint = static_cast<HingeJoint3DSW *>(joint); return hinge_joint->get_flag(p_flag); } @@ -1077,34 +1106,38 @@ bool PhysicsServer3DSW::joint_is_disabled_collisions_between_bodies(RID p_joint) PhysicsServer3DSW::JointType PhysicsServer3DSW::joint_get_type(RID p_joint) const { Joint3DSW *joint = joint_owner.getornull(p_joint); - ERR_FAIL_COND_V(!joint, JOINT_PIN); + ERR_FAIL_COND_V(!joint, JOINT_TYPE_PIN); return joint->get_type(); } -RID PhysicsServer3DSW::joint_create_slider(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) { +void PhysicsServer3DSW::joint_make_slider(RID p_joint, RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) { Body3DSW *body_A = body_owner.getornull(p_body_A); - ERR_FAIL_COND_V(!body_A, RID()); + ERR_FAIL_COND(!body_A); if (!p_body_B.is_valid()) { - ERR_FAIL_COND_V(!body_A->get_space(), RID()); + ERR_FAIL_COND(!body_A->get_space()); p_body_B = body_A->get_space()->get_static_global_body(); } Body3DSW *body_B = body_owner.getornull(p_body_B); - ERR_FAIL_COND_V(!body_B, RID()); + ERR_FAIL_COND(!body_B); - ERR_FAIL_COND_V(body_A == body_B, RID()); + ERR_FAIL_COND(body_A == body_B); + + Joint3DSW *prev_joint = joint_owner.getornull(p_joint); + ERR_FAIL_COND(prev_joint == nullptr); Joint3DSW *joint = memnew(SliderJoint3DSW(body_A, body_B, p_local_frame_A, p_local_frame_B)); - RID rid = joint_owner.make_rid(joint); - joint->set_self(rid); - return rid; + + joint->copy_settings_from(prev_joint); + joint_owner.replace(p_joint, joint); + memdelete(prev_joint); } void PhysicsServer3DSW::slider_joint_set_param(RID p_joint, SliderJointParam p_param, real_t p_value) { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_SLIDER); + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_SLIDER); SliderJoint3DSW *slider_joint = static_cast<SliderJoint3DSW *>(joint); slider_joint->set_param(p_param, p_value); } @@ -1112,35 +1145,39 @@ void PhysicsServer3DSW::slider_joint_set_param(RID p_joint, SliderJointParam p_p real_t PhysicsServer3DSW::slider_joint_get_param(RID p_joint, SliderJointParam p_param) const { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND_V(!joint, 0); - ERR_FAIL_COND_V(joint->get_type() != JOINT_CONE_TWIST, 0); + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_CONE_TWIST, 0); SliderJoint3DSW *slider_joint = static_cast<SliderJoint3DSW *>(joint); return slider_joint->get_param(p_param); } -RID PhysicsServer3DSW::joint_create_cone_twist(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) { +void PhysicsServer3DSW::joint_make_cone_twist(RID p_joint, RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) { Body3DSW *body_A = body_owner.getornull(p_body_A); - ERR_FAIL_COND_V(!body_A, RID()); + ERR_FAIL_COND(!body_A); if (!p_body_B.is_valid()) { - ERR_FAIL_COND_V(!body_A->get_space(), RID()); + ERR_FAIL_COND(!body_A->get_space()); p_body_B = body_A->get_space()->get_static_global_body(); } Body3DSW *body_B = body_owner.getornull(p_body_B); - ERR_FAIL_COND_V(!body_B, RID()); + ERR_FAIL_COND(!body_B); - ERR_FAIL_COND_V(body_A == body_B, RID()); + ERR_FAIL_COND(body_A == body_B); + + Joint3DSW *prev_joint = joint_owner.getornull(p_joint); + ERR_FAIL_COND(prev_joint == nullptr); Joint3DSW *joint = memnew(ConeTwistJoint3DSW(body_A, body_B, p_local_frame_A, p_local_frame_B)); - RID rid = joint_owner.make_rid(joint); - joint->set_self(rid); - return rid; + + joint->copy_settings_from(prev_joint); + joint_owner.replace(p_joint, joint); + memdelete(prev_joint); } void PhysicsServer3DSW::cone_twist_joint_set_param(RID p_joint, ConeTwistJointParam p_param, real_t p_value) { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_CONE_TWIST); + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_CONE_TWIST); ConeTwistJoint3DSW *cone_twist_joint = static_cast<ConeTwistJoint3DSW *>(joint); cone_twist_joint->set_param(p_param, p_value); } @@ -1148,43 +1185,47 @@ void PhysicsServer3DSW::cone_twist_joint_set_param(RID p_joint, ConeTwistJointPa real_t PhysicsServer3DSW::cone_twist_joint_get_param(RID p_joint, ConeTwistJointParam p_param) const { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND_V(!joint, 0); - ERR_FAIL_COND_V(joint->get_type() != JOINT_CONE_TWIST, 0); + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_CONE_TWIST, 0); ConeTwistJoint3DSW *cone_twist_joint = static_cast<ConeTwistJoint3DSW *>(joint); return cone_twist_joint->get_param(p_param); } -RID PhysicsServer3DSW::joint_create_generic_6dof(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) { +void PhysicsServer3DSW::joint_make_generic_6dof(RID p_joint, RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) { Body3DSW *body_A = body_owner.getornull(p_body_A); - ERR_FAIL_COND_V(!body_A, RID()); + ERR_FAIL_COND(!body_A); if (!p_body_B.is_valid()) { - ERR_FAIL_COND_V(!body_A->get_space(), RID()); + ERR_FAIL_COND(!body_A->get_space()); p_body_B = body_A->get_space()->get_static_global_body(); } Body3DSW *body_B = body_owner.getornull(p_body_B); - ERR_FAIL_COND_V(!body_B, RID()); + ERR_FAIL_COND(!body_B); - ERR_FAIL_COND_V(body_A == body_B, RID()); + ERR_FAIL_COND(body_A == body_B); + + Joint3DSW *prev_joint = joint_owner.getornull(p_joint); + ERR_FAIL_COND(prev_joint == nullptr); Joint3DSW *joint = memnew(Generic6DOFJoint3DSW(body_A, body_B, p_local_frame_A, p_local_frame_B, true)); - RID rid = joint_owner.make_rid(joint); - joint->set_self(rid); - return rid; + + joint->copy_settings_from(prev_joint); + joint_owner.replace(p_joint, joint); + memdelete(prev_joint); } void PhysicsServer3DSW::generic_6dof_joint_set_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParam p_param, real_t p_value) { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_6DOF); + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_6DOF); Generic6DOFJoint3DSW *generic_6dof_joint = static_cast<Generic6DOFJoint3DSW *>(joint); generic_6dof_joint->set_param(p_axis, p_param, p_value); } -real_t PhysicsServer3DSW::generic_6dof_joint_get_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParam p_param) { +real_t PhysicsServer3DSW::generic_6dof_joint_get_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParam p_param) const { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND_V(!joint, 0); - ERR_FAIL_COND_V(joint->get_type() != JOINT_6DOF, 0); + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_6DOF, 0); Generic6DOFJoint3DSW *generic_6dof_joint = static_cast<Generic6DOFJoint3DSW *>(joint); return generic_6dof_joint->get_param(p_axis, p_param); } @@ -1192,15 +1233,15 @@ real_t PhysicsServer3DSW::generic_6dof_joint_get_param(RID p_joint, Vector3::Axi void PhysicsServer3DSW::generic_6dof_joint_set_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlag p_flag, bool p_enable) { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_6DOF); + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_6DOF); Generic6DOFJoint3DSW *generic_6dof_joint = static_cast<Generic6DOFJoint3DSW *>(joint); generic_6dof_joint->set_flag(p_axis, p_flag, p_enable); } -bool PhysicsServer3DSW::generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlag p_flag) { +bool PhysicsServer3DSW::generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlag p_flag) const { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND_V(!joint, false); - ERR_FAIL_COND_V(joint->get_type() != JOINT_6DOF, false); + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_6DOF, false); Generic6DOFJoint3DSW *generic_6dof_joint = static_cast<Generic6DOFJoint3DSW *>(joint); return generic_6dof_joint->get_flag(p_axis, p_flag); } @@ -1287,7 +1328,6 @@ void PhysicsServer3DSW::set_active(bool p_active) { }; void PhysicsServer3DSW::init() { - doing_sync = true; last_step = 0.001; iterations = 8; // 8? stepper = memnew(Step3DSW); @@ -1303,8 +1343,6 @@ void PhysicsServer3DSW::step(real_t p_step) { _update_shapes(); - doing_sync = false; - last_step = p_step; PhysicsDirectBodyState3DSW::singleton->step = p_step; @@ -1320,6 +1358,10 @@ void PhysicsServer3DSW::step(real_t p_step) { #endif } +void PhysicsServer3DSW::sync() { + doing_sync = true; +}; + void PhysicsServer3DSW::flush_queries() { #ifndef _3D_DISABLED @@ -1327,8 +1369,6 @@ void PhysicsServer3DSW::flush_queries() { return; } - doing_sync = true; - flushing_queries = true; uint64_t time_beg = OS::get_singleton()->get_ticks_usec(); @@ -1375,6 +1415,10 @@ void PhysicsServer3DSW::flush_queries() { #endif }; +void PhysicsServer3DSW::end_sync() { + doing_sync = false; +}; + void PhysicsServer3DSW::finish() { memdelete(stepper); memdelete(direct_state); @@ -1436,14 +1480,15 @@ void PhysicsServer3DSW::_shape_col_cbk(const Vector3 &p_point_A, const Vector3 & } } -PhysicsServer3DSW *PhysicsServer3DSW::singleton = nullptr; -PhysicsServer3DSW::PhysicsServer3DSW() { - singleton = this; +PhysicsServer3DSW *PhysicsServer3DSW::singletonsw = nullptr; +PhysicsServer3DSW::PhysicsServer3DSW(bool p_using_threads) { + singletonsw = this; BroadPhase3DSW::create_func = BroadPhaseOctree::_create; island_count = 0; active_objects = 0; collision_pairs = 0; - + using_threads = p_using_threads; active = true; flushing_queries = false; + doing_sync = false; }; diff --git a/servers/physics_3d/physics_server_3d_sw.h b/servers/physics_3d/physics_server_3d_sw.h index 26230ef674..afda161fa8 100644 --- a/servers/physics_3d/physics_server_3d_sw.h +++ b/servers/physics_3d/physics_server_3d_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -31,7 +31,7 @@ #ifndef PHYSICS_SERVER_SW #define PHYSICS_SERVER_SW -#include "core/rid_owner.h" +#include "core/templates/rid_owner.h" #include "joints_3d_sw.h" #include "servers/physics_server_3d.h" #include "shape_3d_sw.h" @@ -44,13 +44,14 @@ class PhysicsServer3DSW : public PhysicsServer3D { friend class PhysicsDirectSpaceState3DSW; bool active; int iterations; - bool doing_sync; real_t last_step; int island_count; int active_objects; int collision_pairs; + bool using_threads; + bool doing_sync; bool flushing_queries; Step3DSW *stepper; @@ -58,20 +59,20 @@ class PhysicsServer3DSW : public PhysicsServer3D { PhysicsDirectBodyState3DSW *direct_state; - mutable RID_PtrOwner<Shape3DSW> shape_owner; - mutable RID_PtrOwner<Space3DSW> space_owner; - mutable RID_PtrOwner<Area3DSW> area_owner; - mutable RID_PtrOwner<Body3DSW> body_owner; - mutable RID_PtrOwner<Joint3DSW> joint_owner; + mutable RID_PtrOwner<Shape3DSW, true> shape_owner; + mutable RID_PtrOwner<Space3DSW, true> space_owner; + mutable RID_PtrOwner<Area3DSW, true> area_owner; + mutable RID_PtrOwner<Body3DSW, true> body_owner; + mutable RID_PtrOwner<Joint3DSW, true> joint_owner; //void _clear_query(QuerySW *p_query); friend class CollisionObject3DSW; SelfList<CollisionObject3DSW>::List pending_shape_update_list; void _update_shapes(); -public: - static PhysicsServer3DSW *singleton; + static PhysicsServer3DSW *singletonsw; +public: struct CollCbkData { int max; int amount; @@ -80,300 +81,309 @@ public: static void _shape_col_cbk(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata); - virtual RID shape_create(ShapeType p_shape); - virtual void shape_set_data(RID p_shape, const Variant &p_data); - virtual void shape_set_custom_solver_bias(RID p_shape, real_t p_bias); + virtual RID plane_shape_create() override; + virtual RID ray_shape_create() override; + virtual RID sphere_shape_create() override; + virtual RID box_shape_create() override; + virtual RID capsule_shape_create() override; + virtual RID cylinder_shape_create() override; + virtual RID convex_polygon_shape_create() override; + virtual RID concave_polygon_shape_create() override; + virtual RID heightmap_shape_create() override; + virtual RID custom_shape_create() override; + + virtual void shape_set_data(RID p_shape, const Variant &p_data) override; + virtual void shape_set_custom_solver_bias(RID p_shape, real_t p_bias) override; - virtual ShapeType shape_get_type(RID p_shape) const; - virtual Variant shape_get_data(RID p_shape) const; + virtual ShapeType shape_get_type(RID p_shape) const override; + virtual Variant shape_get_data(RID p_shape) const override; - virtual void shape_set_margin(RID p_shape, real_t p_margin); - virtual real_t shape_get_margin(RID p_shape) const; + virtual void shape_set_margin(RID p_shape, real_t p_margin) override; + virtual real_t shape_get_margin(RID p_shape) const override; - virtual real_t shape_get_custom_solver_bias(RID p_shape) const; + virtual real_t shape_get_custom_solver_bias(RID p_shape) const override; /* SPACE API */ - virtual RID space_create(); - virtual void space_set_active(RID p_space, bool p_active); - virtual bool space_is_active(RID p_space) const; + virtual RID space_create() override; + virtual void space_set_active(RID p_space, bool p_active) override; + virtual bool space_is_active(RID p_space) const override; - virtual void space_set_param(RID p_space, SpaceParameter p_param, real_t p_value); - virtual real_t space_get_param(RID p_space, SpaceParameter p_param) const; + virtual void space_set_param(RID p_space, SpaceParameter p_param, real_t p_value) override; + virtual real_t space_get_param(RID p_space, SpaceParameter p_param) const override; // this function only works on physics process, errors and returns null otherwise - virtual PhysicsDirectSpaceState3D *space_get_direct_state(RID p_space); + virtual PhysicsDirectSpaceState3D *space_get_direct_state(RID p_space) override; - virtual void space_set_debug_contacts(RID p_space, int p_max_contacts); - virtual Vector<Vector3> space_get_contacts(RID p_space) const; - virtual int space_get_contact_count(RID p_space) const; + virtual void space_set_debug_contacts(RID p_space, int p_max_contacts) override; + virtual Vector<Vector3> space_get_contacts(RID p_space) const override; + virtual int space_get_contact_count(RID p_space) const override; /* AREA API */ - virtual RID area_create(); + virtual RID area_create() override; - virtual void area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode); - virtual AreaSpaceOverrideMode area_get_space_override_mode(RID p_area) const; + virtual void area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode) override; + virtual AreaSpaceOverrideMode area_get_space_override_mode(RID p_area) const override; - virtual void area_set_space(RID p_area, RID p_space); - virtual RID area_get_space(RID p_area) const; + virtual void area_set_space(RID p_area, RID p_space) override; + virtual RID area_get_space(RID p_area) const override; - virtual void area_add_shape(RID p_area, RID p_shape, const Transform &p_transform = Transform(), bool p_disabled = false); - virtual void area_set_shape(RID p_area, int p_shape_idx, RID p_shape); - virtual void area_set_shape_transform(RID p_area, int p_shape_idx, const Transform &p_transform); + virtual void area_add_shape(RID p_area, RID p_shape, const Transform &p_transform = Transform(), bool p_disabled = false) override; + virtual void area_set_shape(RID p_area, int p_shape_idx, RID p_shape) override; + virtual void area_set_shape_transform(RID p_area, int p_shape_idx, const Transform &p_transform) override; - virtual int area_get_shape_count(RID p_area) const; - virtual RID area_get_shape(RID p_area, int p_shape_idx) const; - virtual Transform area_get_shape_transform(RID p_area, int p_shape_idx) const; + virtual int area_get_shape_count(RID p_area) const override; + virtual RID area_get_shape(RID p_area, int p_shape_idx) const override; + virtual Transform area_get_shape_transform(RID p_area, int p_shape_idx) const override; - virtual void area_remove_shape(RID p_area, int p_shape_idx); - virtual void area_clear_shapes(RID p_area); + virtual void area_remove_shape(RID p_area, int p_shape_idx) override; + virtual void area_clear_shapes(RID p_area) override; - virtual void area_set_shape_disabled(RID p_area, int p_shape_idx, bool p_disabled); + virtual void area_set_shape_disabled(RID p_area, int p_shape_idx, bool p_disabled) override; - virtual void area_attach_object_instance_id(RID p_area, ObjectID p_id); - virtual ObjectID area_get_object_instance_id(RID p_area) const; + virtual void area_attach_object_instance_id(RID p_area, ObjectID p_id) override; + virtual ObjectID area_get_object_instance_id(RID p_area) const override; - virtual void area_set_param(RID p_area, AreaParameter p_param, const Variant &p_value); - virtual void area_set_transform(RID p_area, const Transform &p_transform); + virtual void area_set_param(RID p_area, AreaParameter p_param, const Variant &p_value) override; + virtual void area_set_transform(RID p_area, const Transform &p_transform) override; - virtual Variant area_get_param(RID p_area, AreaParameter p_param) const; - virtual Transform area_get_transform(RID p_area) const; + virtual Variant area_get_param(RID p_area, AreaParameter p_param) const override; + virtual Transform area_get_transform(RID p_area) const override; - virtual void area_set_ray_pickable(RID p_area, bool p_enable); - virtual bool area_is_ray_pickable(RID p_area) const; + virtual void area_set_ray_pickable(RID p_area, bool p_enable) override; - virtual void area_set_collision_mask(RID p_area, uint32_t p_mask); - virtual void area_set_collision_layer(RID p_area, uint32_t p_layer); + virtual void area_set_collision_mask(RID p_area, uint32_t p_mask) override; + virtual void area_set_collision_layer(RID p_area, uint32_t p_layer) override; - virtual void area_set_monitorable(RID p_area, bool p_monitorable); + virtual void area_set_monitorable(RID p_area, bool p_monitorable) override; - virtual void area_set_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method); - virtual void area_set_area_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method); + virtual void area_set_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) override; + virtual void area_set_area_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) override; /* BODY API */ // create a body of a given type - virtual RID body_create(BodyMode p_mode = BODY_MODE_RIGID, bool p_init_sleeping = false); + virtual RID body_create() override; - virtual void body_set_space(RID p_body, RID p_space); - virtual RID body_get_space(RID p_body) const; + virtual void body_set_space(RID p_body, RID p_space) override; + virtual RID body_get_space(RID p_body) const override; - virtual void body_set_mode(RID p_body, BodyMode p_mode); - virtual BodyMode body_get_mode(RID p_body) const; + virtual void body_set_mode(RID p_body, BodyMode p_mode) override; + virtual BodyMode body_get_mode(RID p_body) const override; - virtual void body_add_shape(RID p_body, RID p_shape, const Transform &p_transform = Transform(), bool p_disabled = false); - virtual void body_set_shape(RID p_body, int p_shape_idx, RID p_shape); - virtual void body_set_shape_transform(RID p_body, int p_shape_idx, const Transform &p_transform); + virtual void body_add_shape(RID p_body, RID p_shape, const Transform &p_transform = Transform(), bool p_disabled = false) override; + virtual void body_set_shape(RID p_body, int p_shape_idx, RID p_shape) override; + virtual void body_set_shape_transform(RID p_body, int p_shape_idx, const Transform &p_transform) override; - virtual int body_get_shape_count(RID p_body) const; - virtual RID body_get_shape(RID p_body, int p_shape_idx) const; - virtual Transform body_get_shape_transform(RID p_body, int p_shape_idx) const; + virtual int body_get_shape_count(RID p_body) const override; + virtual RID body_get_shape(RID p_body, int p_shape_idx) const override; + virtual Transform body_get_shape_transform(RID p_body, int p_shape_idx) const override; - virtual void body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled); + virtual void body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled) override; - virtual void body_remove_shape(RID p_body, int p_shape_idx); - virtual void body_clear_shapes(RID p_body); + virtual void body_remove_shape(RID p_body, int p_shape_idx) override; + virtual void body_clear_shapes(RID p_body) override; - virtual void body_attach_object_instance_id(RID p_body, ObjectID p_id); - virtual ObjectID body_get_object_instance_id(RID p_body) const; + virtual void body_attach_object_instance_id(RID p_body, ObjectID p_id) override; + virtual ObjectID body_get_object_instance_id(RID p_body) const override; - virtual void body_set_enable_continuous_collision_detection(RID p_body, bool p_enable); - virtual bool body_is_continuous_collision_detection_enabled(RID p_body) const; + virtual void body_set_enable_continuous_collision_detection(RID p_body, bool p_enable) override; + virtual bool body_is_continuous_collision_detection_enabled(RID p_body) const override; - virtual void body_set_collision_layer(RID p_body, uint32_t p_layer); - virtual uint32_t body_get_collision_layer(RID p_body) const; + virtual void body_set_collision_layer(RID p_body, uint32_t p_layer) override; + virtual uint32_t body_get_collision_layer(RID p_body) const override; - virtual void body_set_collision_mask(RID p_body, uint32_t p_mask); - virtual uint32_t body_get_collision_mask(RID p_body) const; + virtual void body_set_collision_mask(RID p_body, uint32_t p_mask) override; + virtual uint32_t body_get_collision_mask(RID p_body) const override; - virtual void body_set_user_flags(RID p_body, uint32_t p_flags); - virtual uint32_t body_get_user_flags(RID p_body) const; + virtual void body_set_user_flags(RID p_body, uint32_t p_flags) override; + virtual uint32_t body_get_user_flags(RID p_body) const override; - 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_param(RID p_body, BodyParameter p_param, real_t p_value) override; + virtual real_t body_get_param(RID p_body, BodyParameter p_param) const override; - virtual void body_set_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_kinematic_safe_margin(RID p_body, real_t p_margin) override; + virtual real_t body_get_kinematic_safe_margin(RID p_body) const override; - 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; + virtual void body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) override; + virtual Variant body_get_state(RID p_body, BodyState p_state) const override; - virtual void body_set_applied_force(RID p_body, const Vector3 &p_force); - virtual Vector3 body_get_applied_force(RID p_body) const; + virtual void body_set_applied_force(RID p_body, const Vector3 &p_force) override; + virtual Vector3 body_get_applied_force(RID p_body) const override; - virtual void body_set_applied_torque(RID p_body, const Vector3 &p_torque); - virtual Vector3 body_get_applied_torque(RID p_body) const; + virtual void body_set_applied_torque(RID p_body, const Vector3 &p_torque) override; + virtual Vector3 body_get_applied_torque(RID p_body) const override; - virtual void body_add_central_force(RID p_body, const Vector3 &p_force); - virtual void body_add_force(RID p_body, const Vector3 &p_force, const Vector3 &p_pos); - virtual void body_add_torque(RID p_body, const Vector3 &p_torque); + virtual void body_add_central_force(RID p_body, const Vector3 &p_force) override; + virtual void body_add_force(RID p_body, const Vector3 &p_force, const Vector3 &p_position = Vector3()) override; + virtual void body_add_torque(RID p_body, const Vector3 &p_torque) override; - virtual void body_apply_central_impulse(RID p_body, const Vector3 &p_impulse); - virtual void body_apply_impulse(RID p_body, const Vector3 &p_pos, const Vector3 &p_impulse); - 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_apply_central_impulse(RID p_body, const Vector3 &p_impulse) override; + virtual void body_apply_impulse(RID p_body, const Vector3 &p_impulse, const Vector3 &p_position = Vector3()) override; + virtual void body_apply_torque_impulse(RID p_body, const Vector3 &p_impulse) override; + virtual void body_set_axis_velocity(RID p_body, const Vector3 &p_axis_velocity) override; - 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_set_axis_lock(RID p_body, BodyAxis p_axis, bool p_lock) override; + virtual bool body_is_axis_locked(RID p_body, BodyAxis p_axis) const override; - 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); - virtual void body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions); + virtual void body_add_collision_exception(RID p_body, RID p_body_b) override; + virtual void body_remove_collision_exception(RID p_body, RID p_body_b) override; + virtual void body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) override; - virtual void body_set_contacts_reported_depth_threshold(RID p_body, real_t p_threshold); - virtual real_t body_get_contacts_reported_depth_threshold(RID p_body) const; + virtual void body_set_contacts_reported_depth_threshold(RID p_body, real_t p_threshold) override; + virtual real_t body_get_contacts_reported_depth_threshold(RID p_body) const override; - virtual void body_set_omit_force_integration(RID p_body, bool p_omit); - virtual bool body_is_omitting_force_integration(RID p_body) const; + virtual void body_set_omit_force_integration(RID p_body, bool p_omit) override; + virtual bool body_is_omitting_force_integration(RID p_body) const override; - virtual void body_set_max_contacts_reported(RID p_body, int p_contacts); - virtual int body_get_max_contacts_reported(RID p_body) const; + virtual void body_set_max_contacts_reported(RID p_body, int p_contacts) override; + virtual int body_get_max_contacts_reported(RID p_body) const override; - virtual void body_set_force_integration_callback(RID p_body, Object *p_receiver, const StringName &p_method, const Variant &p_udata = Variant()); + virtual void body_set_force_integration_callback(RID p_body, Object *p_receiver, const StringName &p_method, const Variant &p_udata = Variant()) override; - virtual void body_set_ray_pickable(RID p_body, bool p_enable); - virtual bool body_is_ray_pickable(RID p_body) const; + virtual void body_set_ray_pickable(RID p_body, bool p_enable) override; - virtual bool body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, MotionResult *r_result = nullptr, bool p_exclude_raycast_shapes = true); - virtual int body_test_ray_separation(RID p_body, const Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, SeparationResult *r_results, int p_result_max, float p_margin = 0.001); + virtual bool body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, MotionResult *r_result = nullptr, bool p_exclude_raycast_shapes = true) override; + virtual int body_test_ray_separation(RID p_body, const Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, SeparationResult *r_results, int p_result_max, real_t p_margin = 0.001) override; // this function only works on physics process, errors and returns null otherwise - virtual PhysicsDirectBodyState3D *body_get_direct_state(RID p_body); + virtual PhysicsDirectBodyState3D *body_get_direct_state(RID p_body) override; /* SOFT BODY */ - virtual RID soft_body_create(bool p_init_sleeping = false) { return RID(); } + virtual RID soft_body_create() override { return RID(); } - virtual void soft_body_update_rendering_server(RID p_body, class SoftBodyRenderingServerHandler *p_rendering_server_handler) {} + virtual void soft_body_update_rendering_server(RID p_body, class SoftBodyRenderingServerHandler *p_rendering_server_handler) override {} - virtual void soft_body_set_space(RID p_body, RID p_space) {} - virtual RID soft_body_get_space(RID p_body) const { return RID(); } + virtual void soft_body_set_space(RID p_body, RID p_space) override {} + virtual RID soft_body_get_space(RID p_body) const override { return RID(); } - virtual void soft_body_set_collision_layer(RID p_body, uint32_t p_layer) {} - virtual uint32_t soft_body_get_collision_layer(RID p_body) const { return 0; } + virtual void soft_body_set_collision_layer(RID p_body, uint32_t p_layer) override {} + virtual uint32_t soft_body_get_collision_layer(RID p_body) const override { return 0; } - virtual void soft_body_set_collision_mask(RID p_body, uint32_t p_mask) {} - virtual uint32_t soft_body_get_collision_mask(RID p_body) const { return 0; } + virtual void soft_body_set_collision_mask(RID p_body, uint32_t p_mask) override {} + virtual uint32_t soft_body_get_collision_mask(RID p_body) const override { return 0; } - virtual void soft_body_add_collision_exception(RID p_body, RID p_body_b) {} - virtual void soft_body_remove_collision_exception(RID p_body, RID p_body_b) {} - virtual void soft_body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) {} + virtual void soft_body_add_collision_exception(RID p_body, RID p_body_b) override {} + virtual void soft_body_remove_collision_exception(RID p_body, RID p_body_b) override {} + virtual void soft_body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) override {} - virtual void soft_body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) {} - virtual Variant soft_body_get_state(RID p_body, BodyState p_state) const { return Variant(); } + virtual void soft_body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) override {} + virtual Variant soft_body_get_state(RID p_body, BodyState p_state) const override { return Variant(); } - virtual void soft_body_set_transform(RID p_body, const Transform &p_transform) {} - virtual Vector3 soft_body_get_vertex_position(RID p_body, int vertex_index) const { return Vector3(); } + virtual void soft_body_set_transform(RID p_body, const Transform &p_transform) override {} + virtual Vector3 soft_body_get_vertex_position(RID p_body, int vertex_index) const override { return Vector3(); } - virtual void soft_body_set_ray_pickable(RID p_body, bool p_enable) {} - virtual bool soft_body_is_ray_pickable(RID p_body) const { return false; } + virtual void soft_body_set_ray_pickable(RID p_body, bool p_enable) override {} - virtual void soft_body_set_simulation_precision(RID p_body, int p_simulation_precision) {} - virtual int soft_body_get_simulation_precision(RID p_body) { return 0; } + virtual void soft_body_set_simulation_precision(RID p_body, int p_simulation_precision) override {} + virtual int soft_body_get_simulation_precision(RID p_body) const override { return 0; } - virtual void soft_body_set_total_mass(RID p_body, real_t p_total_mass) {} - virtual real_t soft_body_get_total_mass(RID p_body) { return 0.; } + virtual void soft_body_set_total_mass(RID p_body, real_t p_total_mass) override {} + virtual real_t soft_body_get_total_mass(RID p_body) const override { return 0.; } - virtual void soft_body_set_linear_stiffness(RID p_body, real_t p_stiffness) {} - virtual real_t soft_body_get_linear_stiffness(RID p_body) { return 0.; } + virtual void soft_body_set_linear_stiffness(RID p_body, real_t p_stiffness) override {} + virtual real_t soft_body_get_linear_stiffness(RID p_body) const override { return 0.; } - virtual void soft_body_set_areaAngular_stiffness(RID p_body, real_t p_stiffness) {} - virtual real_t soft_body_get_areaAngular_stiffness(RID p_body) { return 0.; } + virtual void soft_body_set_angular_stiffness(RID p_body, real_t p_stiffness) override {} + virtual real_t soft_body_get_angular_stiffness(RID p_body) const override { return 0.; } - virtual void soft_body_set_volume_stiffness(RID p_body, real_t p_stiffness) {} - virtual real_t soft_body_get_volume_stiffness(RID p_body) { return 0.; } + virtual void soft_body_set_volume_stiffness(RID p_body, real_t p_stiffness) override {} + virtual real_t soft_body_get_volume_stiffness(RID p_body) const override { return 0.; } - virtual void soft_body_set_pressure_coefficient(RID p_body, real_t p_pressure_coefficient) {} - virtual real_t soft_body_get_pressure_coefficient(RID p_body) { return 0.; } + virtual void soft_body_set_pressure_coefficient(RID p_body, real_t p_pressure_coefficient) override {} + virtual real_t soft_body_get_pressure_coefficient(RID p_body) const override { return 0.; } - virtual void soft_body_set_pose_matching_coefficient(RID p_body, real_t p_pose_matching_coefficient) {} - virtual real_t soft_body_get_pose_matching_coefficient(RID p_body) { return 0.; } + virtual void soft_body_set_pose_matching_coefficient(RID p_body, real_t p_pose_matching_coefficient) override {} + virtual real_t soft_body_get_pose_matching_coefficient(RID p_body) const override { return 0.; } - virtual void soft_body_set_damping_coefficient(RID p_body, real_t p_damping_coefficient) {} - virtual real_t soft_body_get_damping_coefficient(RID p_body) { return 0.; } + virtual void soft_body_set_damping_coefficient(RID p_body, real_t p_damping_coefficient) override {} + virtual real_t soft_body_get_damping_coefficient(RID p_body) const override { return 0.; } - virtual void soft_body_set_drag_coefficient(RID p_body, real_t p_drag_coefficient) {} - virtual real_t soft_body_get_drag_coefficient(RID p_body) { return 0.; } + virtual void soft_body_set_drag_coefficient(RID p_body, real_t p_drag_coefficient) override {} + virtual real_t soft_body_get_drag_coefficient(RID p_body) const override { return 0.; } - virtual void soft_body_set_mesh(RID p_body, const REF &p_mesh) {} + virtual void soft_body_set_mesh(RID p_body, const REF &p_mesh) override {} - virtual void soft_body_move_point(RID p_body, int p_point_index, const Vector3 &p_global_position) {} - virtual Vector3 soft_body_get_point_global_position(RID p_body, int p_point_index) { return Vector3(); } + virtual void soft_body_move_point(RID p_body, int p_point_index, const Vector3 &p_global_position) override {} + virtual Vector3 soft_body_get_point_global_position(RID p_body, int p_point_index) const override { return Vector3(); } - virtual Vector3 soft_body_get_point_offset(RID p_body, int p_point_index) const { return Vector3(); } + virtual Vector3 soft_body_get_point_offset(RID p_body, int p_point_index) const override { return Vector3(); } - virtual void soft_body_remove_all_pinned_points(RID p_body) {} - virtual void soft_body_pin_point(RID p_body, int p_point_index, bool p_pin) {} - virtual bool soft_body_is_point_pinned(RID p_body, int p_point_index) { return false; } + virtual void soft_body_remove_all_pinned_points(RID p_body) override {} + virtual void soft_body_pin_point(RID p_body, int p_point_index, bool p_pin) override {} + virtual bool soft_body_is_point_pinned(RID p_body, int p_point_index) const override { return false; } /* JOINT API */ - virtual RID joint_create_pin(RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B); + virtual RID joint_create() override; + + virtual void joint_clear(RID p_joint) override; //resets type - virtual void pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value); - virtual real_t pin_joint_get_param(RID p_joint, PinJointParam p_param) const; + virtual void joint_make_pin(RID p_joint, RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B) override; - virtual void pin_joint_set_local_a(RID p_joint, const Vector3 &p_A); - virtual Vector3 pin_joint_get_local_a(RID p_joint) const; + virtual void pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value) override; + virtual real_t pin_joint_get_param(RID p_joint, PinJointParam p_param) const override; - virtual void pin_joint_set_local_b(RID p_joint, const Vector3 &p_B); - virtual Vector3 pin_joint_get_local_b(RID p_joint) const; + virtual void pin_joint_set_local_a(RID p_joint, const Vector3 &p_A) override; + virtual Vector3 pin_joint_get_local_a(RID p_joint) const override; - virtual RID joint_create_hinge(RID p_body_A, const Transform &p_frame_A, RID p_body_B, const Transform &p_frame_B); - virtual RID joint_create_hinge_simple(RID p_body_A, const Vector3 &p_pivot_A, const Vector3 &p_axis_A, RID p_body_B, const Vector3 &p_pivot_B, const Vector3 &p_axis_B); + virtual void pin_joint_set_local_b(RID p_joint, const Vector3 &p_B) override; + virtual Vector3 pin_joint_get_local_b(RID p_joint) const override; - virtual void hinge_joint_set_param(RID p_joint, HingeJointParam p_param, real_t p_value); - virtual real_t hinge_joint_get_param(RID p_joint, HingeJointParam p_param) const; + virtual void joint_make_hinge(RID p_joint, RID p_body_A, const Transform &p_frame_A, RID p_body_B, const Transform &p_frame_B) override; + virtual void joint_make_hinge_simple(RID p_joint, RID p_body_A, const Vector3 &p_pivot_A, const Vector3 &p_axis_A, RID p_body_B, const Vector3 &p_pivot_B, const Vector3 &p_axis_B) override; - virtual void hinge_joint_set_flag(RID p_joint, HingeJointFlag p_flag, bool p_value); - virtual bool hinge_joint_get_flag(RID p_joint, HingeJointFlag p_flag) const; + virtual void hinge_joint_set_param(RID p_joint, HingeJointParam p_param, real_t p_value) override; + virtual real_t hinge_joint_get_param(RID p_joint, HingeJointParam p_param) const override; - virtual RID joint_create_slider(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B); //reference frame is A + virtual void hinge_joint_set_flag(RID p_joint, HingeJointFlag p_flag, bool p_value) override; + virtual bool hinge_joint_get_flag(RID p_joint, HingeJointFlag p_flag) const override; - virtual void slider_joint_set_param(RID p_joint, SliderJointParam p_param, real_t p_value); - virtual real_t slider_joint_get_param(RID p_joint, SliderJointParam p_param) const; + virtual void joint_make_slider(RID p_joint, RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) override; //reference frame is A - virtual RID joint_create_cone_twist(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B); //reference frame is A + virtual void slider_joint_set_param(RID p_joint, SliderJointParam p_param, real_t p_value) override; + virtual real_t slider_joint_get_param(RID p_joint, SliderJointParam p_param) const override; - virtual void cone_twist_joint_set_param(RID p_joint, ConeTwistJointParam p_param, real_t p_value); - virtual real_t cone_twist_joint_get_param(RID p_joint, ConeTwistJointParam p_param) const; + virtual void joint_make_cone_twist(RID p_joint, RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) override; //reference frame is A - virtual RID joint_create_generic_6dof(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B); //reference frame is A + virtual void cone_twist_joint_set_param(RID p_joint, ConeTwistJointParam p_param, real_t p_value) override; + virtual real_t cone_twist_joint_get_param(RID p_joint, ConeTwistJointParam p_param) const override; - virtual void generic_6dof_joint_set_param(RID p_joint, Vector3::Axis, G6DOFJointAxisParam p_param, real_t p_value); - virtual real_t generic_6dof_joint_get_param(RID p_joint, Vector3::Axis, G6DOFJointAxisParam p_param); + virtual void joint_make_generic_6dof(RID p_joint, RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) override; //reference frame is A - virtual void generic_6dof_joint_set_flag(RID p_joint, Vector3::Axis, G6DOFJointAxisFlag p_flag, bool p_enable); - virtual bool generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis, G6DOFJointAxisFlag p_flag); + virtual void generic_6dof_joint_set_param(RID p_joint, Vector3::Axis, G6DOFJointAxisParam p_param, real_t p_value) override; + virtual real_t generic_6dof_joint_get_param(RID p_joint, Vector3::Axis, G6DOFJointAxisParam p_param) const override; - virtual void generic_6dof_joint_set_precision(RID p_joint, int precision) {} - virtual int generic_6dof_joint_get_precision(RID p_joint) { return 0; } + virtual void generic_6dof_joint_set_flag(RID p_joint, Vector3::Axis, G6DOFJointAxisFlag p_flag, bool p_enable) override; + virtual bool generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis, G6DOFJointAxisFlag p_flag) const override; - virtual JointType joint_get_type(RID p_joint) const; + virtual JointType joint_get_type(RID p_joint) const override; - 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_set_solver_priority(RID p_joint, int p_priority) override; + virtual int joint_get_solver_priority(RID p_joint) const override; - 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; + virtual void joint_disable_collisions_between_bodies(RID p_joint, const bool p_disable) override; + virtual bool joint_is_disabled_collisions_between_bodies(RID p_joint) const override; /* MISC */ - virtual void free(RID p_rid); + virtual void free(RID p_rid) override; - virtual void set_active(bool p_active); - virtual void init(); - virtual void step(real_t p_step); - virtual void sync() {} - virtual void flush_queries(); - virtual void finish(); + virtual void set_active(bool p_active) override; + virtual void init() override; + virtual void step(real_t p_step) override; + virtual void sync() override; + virtual void flush_queries() override; + virtual void end_sync() override; + virtual void finish() override; - virtual bool is_flushing_queries() const { return flushing_queries; } + virtual bool is_flushing_queries() const override { return flushing_queries; } - int get_process_info(ProcessInfo p_info); + int get_process_info(ProcessInfo p_info) override; - PhysicsServer3DSW(); + PhysicsServer3DSW(bool p_using_threads = false); ~PhysicsServer3DSW() {} }; diff --git a/servers/physics_3d/physics_server_3d_wrap_mt.cpp b/servers/physics_3d/physics_server_3d_wrap_mt.cpp new file mode 100644 index 0000000000..f73f67a756 --- /dev/null +++ b/servers/physics_3d/physics_server_3d_wrap_mt.cpp @@ -0,0 +1,140 @@ +/*************************************************************************/ +/* physics_server_3d_wrap_mt.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "physics_server_3d_wrap_mt.h" + +#include "core/os/os.h" + +void PhysicsServer3DWrapMT::thread_exit() { + exit = true; +} + +void PhysicsServer3DWrapMT::thread_step(real_t p_delta) { + physics_3d_server->step(p_delta); + step_sem.post(); +} + +void PhysicsServer3DWrapMT::_thread_callback(void *_instance) { + PhysicsServer3DWrapMT *vsmt = reinterpret_cast<PhysicsServer3DWrapMT *>(_instance); + + vsmt->thread_loop(); +} + +void PhysicsServer3DWrapMT::thread_loop() { + server_thread = Thread::get_caller_id(); + + physics_3d_server->init(); + + exit = false; + step_thread_up = true; + while (!exit) { + // flush commands one by one, until exit is requested + command_queue.wait_and_flush_one(); + } + + command_queue.flush_all(); // flush all + + physics_3d_server->finish(); +} + +/* EVENT QUEUING */ + +void PhysicsServer3DWrapMT::step(real_t p_step) { + if (create_thread) { + command_queue.push(this, &PhysicsServer3DWrapMT::thread_step, p_step); + } else { + command_queue.flush_all(); //flush all pending from other threads + physics_3d_server->step(p_step); + } +} + +void PhysicsServer3DWrapMT::sync() { + if (create_thread) { + if (first_frame) { + first_frame = false; + } else { + step_sem.wait(); //must not wait if a step was not issued + } + } + physics_3d_server->sync(); +} + +void PhysicsServer3DWrapMT::flush_queries() { + physics_3d_server->flush_queries(); +} + +void PhysicsServer3DWrapMT::end_sync() { + physics_3d_server->end_sync(); +} + +void PhysicsServer3DWrapMT::init() { + if (create_thread) { + //OS::get_singleton()->release_rendering_thread(); + thread.start(_thread_callback, this); + while (!step_thread_up) { + OS::get_singleton()->delay_usec(1000); + } + } else { + physics_3d_server->init(); + } +} + +void PhysicsServer3DWrapMT::finish() { + if (thread.is_started()) { + command_queue.push(this, &PhysicsServer3DWrapMT::thread_exit); + thread.wait_to_finish(); + } else { + physics_3d_server->finish(); + } +} + +PhysicsServer3DWrapMT::PhysicsServer3DWrapMT(PhysicsServer3D *p_contained, bool p_create_thread) : + command_queue(p_create_thread) { + physics_3d_server = p_contained; + create_thread = p_create_thread; + step_pending = 0; + step_thread_up = false; + + pool_max_size = GLOBAL_GET("memory/limits/multithreaded_server/rid_pool_prealloc"); + + if (!p_create_thread) { + server_thread = Thread::get_caller_id(); + } else { + server_thread = 0; + } + + main_thread = Thread::get_caller_id(); + first_frame = true; +} + +PhysicsServer3DWrapMT::~PhysicsServer3DWrapMT() { + memdelete(physics_3d_server); + //finish(); +} diff --git a/servers/physics_3d/physics_server_3d_wrap_mt.h b/servers/physics_3d/physics_server_3d_wrap_mt.h new file mode 100644 index 0000000000..f60e1332d5 --- /dev/null +++ b/servers/physics_3d/physics_server_3d_wrap_mt.h @@ -0,0 +1,422 @@ +/*************************************************************************/ +/* physics_server_3d_wrap_mt.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#ifndef PHYSICS3DSERVERWRAPMT_H +#define PHYSICS3DSERVERWRAPMT_H + +#include "core/config/project_settings.h" +#include "core/os/thread.h" +#include "core/templates/command_queue_mt.h" +#include "servers/physics_server_3d.h" + +#ifdef DEBUG_SYNC +#define SYNC_DEBUG print_line("sync on: " + String(__FUNCTION__)); +#else +#define SYNC_DEBUG +#endif + +class PhysicsServer3DWrapMT : public PhysicsServer3D { + mutable PhysicsServer3D *physics_3d_server; + + mutable CommandQueueMT command_queue; + + static void _thread_callback(void *_instance); + void thread_loop(); + + Thread::ID server_thread; + Thread::ID main_thread; + volatile bool exit = false; + Thread thread; + volatile bool step_thread_up = false; + bool create_thread = false; + + Semaphore step_sem; + int step_pending; + void thread_step(real_t p_delta); + void thread_flush(); + + void thread_exit(); + + bool first_frame = true; + + Mutex alloc_mutex; + int pool_max_size = 0; + +public: +#define ServerName PhysicsServer3D +#define ServerNameWrapMT PhysicsServer3DWrapMT +#define server_name physics_3d_server +#define WRITE_ACTION + +#include "servers/server_wrap_mt_common.h" + + //FUNC1RID(shape,ShapeType); todo fix + FUNCRID(plane_shape) + FUNCRID(ray_shape) + FUNCRID(sphere_shape) + FUNCRID(box_shape) + FUNCRID(capsule_shape) + FUNCRID(cylinder_shape) + FUNCRID(convex_polygon_shape) + FUNCRID(concave_polygon_shape) + FUNCRID(heightmap_shape) + FUNCRID(custom_shape) + + FUNC2(shape_set_data, RID, const Variant &); + FUNC2(shape_set_custom_solver_bias, RID, real_t); + + FUNC2(shape_set_margin, RID, real_t) + FUNC1RC(real_t, shape_get_margin, RID) + + FUNC1RC(ShapeType, shape_get_type, RID); + FUNC1RC(Variant, shape_get_data, RID); + FUNC1RC(real_t, shape_get_custom_solver_bias, RID); +#if 0 + //these work well, but should be used from the main thread only + bool shape_collide(RID p_shape_A, const Transform &p_xform_A, const Vector3 &p_motion_A, RID p_shape_B, const Transform &p_xform_B, const Vector3 &p_motion_B, Vector3 *r_results, int p_result_max, int &r_result_count) { + ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), false); + return physics_3d_server->shape_collide(p_shape_A, p_xform_A, p_motion_A, p_shape_B, p_xform_B, p_motion_B, r_results, p_result_max, r_result_count); + } +#endif + /* SPACE API */ + + FUNCRID(space); + FUNC2(space_set_active, RID, bool); + FUNC1RC(bool, space_is_active, RID); + + FUNC3(space_set_param, RID, SpaceParameter, real_t); + FUNC2RC(real_t, space_get_param, RID, SpaceParameter); + + // this function only works on physics process, errors and returns null otherwise + PhysicsDirectSpaceState3D *space_get_direct_state(RID p_space) override { + ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), nullptr); + return physics_3d_server->space_get_direct_state(p_space); + } + + FUNC2(space_set_debug_contacts, RID, int); + virtual Vector<Vector3> space_get_contacts(RID p_space) const override { + ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), Vector<Vector3>()); + return physics_3d_server->space_get_contacts(p_space); + } + + virtual int space_get_contact_count(RID p_space) const override { + ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), 0); + return physics_3d_server->space_get_contact_count(p_space); + } + + /* AREA API */ + + //FUNC0RID(area); + FUNCRID(area); + + FUNC2(area_set_space, RID, RID); + FUNC1RC(RID, area_get_space, RID); + + FUNC2(area_set_space_override_mode, RID, AreaSpaceOverrideMode); + FUNC1RC(AreaSpaceOverrideMode, area_get_space_override_mode, RID); + + FUNC4(area_add_shape, RID, RID, const Transform &, bool); + FUNC3(area_set_shape, RID, int, RID); + FUNC3(area_set_shape_transform, RID, int, const Transform &); + FUNC3(area_set_shape_disabled, RID, int, bool); + + FUNC1RC(int, area_get_shape_count, RID); + FUNC2RC(RID, area_get_shape, RID, int); + FUNC2RC(Transform, area_get_shape_transform, RID, int); + FUNC2(area_remove_shape, RID, int); + FUNC1(area_clear_shapes, RID); + + FUNC2(area_attach_object_instance_id, RID, ObjectID); + FUNC1RC(ObjectID, area_get_object_instance_id, RID); + + FUNC3(area_set_param, RID, AreaParameter, const Variant &); + FUNC2(area_set_transform, RID, const Transform &); + + FUNC2RC(Variant, area_get_param, RID, AreaParameter); + FUNC1RC(Transform, area_get_transform, RID); + + FUNC2(area_set_collision_mask, RID, uint32_t); + FUNC2(area_set_collision_layer, RID, uint32_t); + + FUNC2(area_set_monitorable, RID, bool); + FUNC2(area_set_ray_pickable, RID, bool); + + FUNC3(area_set_monitor_callback, RID, Object *, const StringName &); + FUNC3(area_set_area_monitor_callback, RID, Object *, const StringName &); + + /* BODY API */ + + //FUNC2RID(body,BodyMode,bool); + FUNCRID(body) + + FUNC2(body_set_space, RID, RID); + FUNC1RC(RID, body_get_space, RID); + + FUNC2(body_set_mode, RID, BodyMode); + FUNC1RC(BodyMode, body_get_mode, RID); + + FUNC4(body_add_shape, RID, RID, const Transform &, bool); + FUNC3(body_set_shape, RID, int, RID); + FUNC3(body_set_shape_transform, RID, int, const Transform &); + + FUNC1RC(int, body_get_shape_count, RID); + FUNC2RC(Transform, body_get_shape_transform, RID, int); + FUNC2RC(RID, body_get_shape, RID, int); + + FUNC3(body_set_shape_disabled, RID, int, bool); + + FUNC2(body_remove_shape, RID, int); + FUNC1(body_clear_shapes, RID); + + FUNC2(body_attach_object_instance_id, RID, ObjectID); + FUNC1RC(ObjectID, body_get_object_instance_id, RID); + + FUNC2(body_set_enable_continuous_collision_detection, RID, bool); + FUNC1RC(bool, body_is_continuous_collision_detection_enabled, RID); + + FUNC2(body_set_collision_layer, RID, uint32_t); + FUNC1RC(uint32_t, body_get_collision_layer, RID); + + FUNC2(body_set_collision_mask, RID, uint32_t); + FUNC1RC(uint32_t, body_get_collision_mask, RID); + + FUNC2(body_set_user_flags, RID, uint32_t); + FUNC1RC(uint32_t, body_get_user_flags, RID); + + FUNC3(body_set_param, RID, BodyParameter, real_t); + FUNC2RC(real_t, body_get_param, RID, BodyParameter); + + FUNC2(body_set_kinematic_safe_margin, RID, real_t); + FUNC1RC(real_t, body_get_kinematic_safe_margin, RID); + + FUNC3(body_set_state, RID, BodyState, const Variant &); + FUNC2RC(Variant, body_get_state, RID, BodyState); + + FUNC2(body_set_applied_force, RID, const Vector3 &); + FUNC1RC(Vector3, body_get_applied_force, RID); + + FUNC2(body_set_applied_torque, RID, const Vector3 &); + FUNC1RC(Vector3, body_get_applied_torque, RID); + + FUNC2(body_add_central_force, RID, const Vector3 &); + FUNC3(body_add_force, RID, const Vector3 &, const Vector3 &); + FUNC2(body_add_torque, RID, const Vector3 &); + FUNC2(body_apply_torque_impulse, RID, const Vector3 &); + FUNC2(body_apply_central_impulse, RID, const Vector3 &); + FUNC3(body_apply_impulse, RID, const Vector3 &, const Vector3 &); + FUNC2(body_set_axis_velocity, RID, const Vector3 &); + + FUNC3(body_set_axis_lock, RID, BodyAxis, bool); + FUNC2RC(bool, body_is_axis_locked, RID, BodyAxis); + + FUNC2(body_add_collision_exception, RID, RID); + FUNC2(body_remove_collision_exception, RID, RID); + FUNC2S(body_get_collision_exceptions, RID, List<RID> *); + + FUNC2(body_set_max_contacts_reported, RID, int); + FUNC1RC(int, body_get_max_contacts_reported, RID); + + FUNC2(body_set_contacts_reported_depth_threshold, RID, real_t); + FUNC1RC(real_t, body_get_contacts_reported_depth_threshold, RID); + + FUNC2(body_set_omit_force_integration, RID, bool); + FUNC1RC(bool, body_is_omitting_force_integration, RID); + + FUNC4(body_set_force_integration_callback, RID, Object *, const StringName &, const Variant &); + + FUNC2(body_set_ray_pickable, RID, bool); + + bool body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, MotionResult *r_result = nullptr, bool p_exclude_raycast_shapes = true) override { + ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), false); + return physics_3d_server->body_test_motion(p_body, p_from, p_motion, p_infinite_inertia, r_result, p_exclude_raycast_shapes); + } + + int body_test_ray_separation(RID p_body, const Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, SeparationResult *r_results, int p_result_max, real_t p_margin = 0.001) override { + ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), false); + return physics_3d_server->body_test_ray_separation(p_body, p_transform, p_infinite_inertia, r_recover_motion, r_results, p_result_max, p_margin); + } + + // this function only works on physics process, errors and returns null otherwise + PhysicsDirectBodyState3D *body_get_direct_state(RID p_body) override { + ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), nullptr); + return physics_3d_server->body_get_direct_state(p_body); + } + + /* SOFT BODY API */ + + FUNCRID(soft_body) + + FUNC2(soft_body_update_rendering_server, RID, class SoftBodyRenderingServerHandler *) + + FUNC2(soft_body_set_space, RID, RID) + FUNC1RC(RID, soft_body_get_space, RID) + + FUNC2(soft_body_set_ray_pickable, RID, bool); + + FUNC2(soft_body_set_collision_layer, RID, uint32_t) + FUNC1RC(uint32_t, soft_body_get_collision_layer, RID) + + FUNC2(soft_body_set_collision_mask, RID, uint32_t) + FUNC1RC(uint32_t, soft_body_get_collision_mask, RID) + + FUNC2(soft_body_add_collision_exception, RID, RID) + FUNC2(soft_body_remove_collision_exception, RID, RID) + FUNC2S(soft_body_get_collision_exceptions, RID, List<RID> *) + + FUNC3(soft_body_set_state, RID, BodyState, const Variant &); + FUNC2RC(Variant, soft_body_get_state, RID, BodyState); + + FUNC2(soft_body_set_transform, RID, const Transform &); + FUNC2RC(Vector3, soft_body_get_vertex_position, RID, int); + + FUNC2(soft_body_set_simulation_precision, RID, int); + FUNC1RC(int, soft_body_get_simulation_precision, RID); + + FUNC2(soft_body_set_total_mass, RID, real_t); + FUNC1RC(real_t, soft_body_get_total_mass, RID); + + FUNC2(soft_body_set_linear_stiffness, RID, real_t); + FUNC1RC(real_t, soft_body_get_linear_stiffness, RID); + + FUNC2(soft_body_set_angular_stiffness, RID, real_t); + FUNC1RC(real_t, soft_body_get_angular_stiffness, RID); + + FUNC2(soft_body_set_volume_stiffness, RID, real_t); + FUNC1RC(real_t, soft_body_get_volume_stiffness, RID); + + FUNC2(soft_body_set_pressure_coefficient, RID, real_t); + FUNC1RC(real_t, soft_body_get_pressure_coefficient, RID); + + FUNC2(soft_body_set_pose_matching_coefficient, RID, real_t); + FUNC1RC(real_t, soft_body_get_pose_matching_coefficient, RID); + + FUNC2(soft_body_set_damping_coefficient, RID, real_t); + FUNC1RC(real_t, soft_body_get_damping_coefficient, RID); + + FUNC2(soft_body_set_drag_coefficient, RID, real_t); + FUNC1RC(real_t, soft_body_get_drag_coefficient, RID); + + FUNC2(soft_body_set_mesh, RID, const REF &); + + FUNC3(soft_body_move_point, RID, int, const Vector3 &); + FUNC2RC(Vector3, soft_body_get_point_global_position, RID, int); + FUNC2RC(Vector3, soft_body_get_point_offset, RID, int); + + FUNC1(soft_body_remove_all_pinned_points, RID); + FUNC3(soft_body_pin_point, RID, int, bool); + FUNC2RC(bool, soft_body_is_point_pinned, RID, int); + + /* JOINT API */ + + FUNCRID(joint) + + FUNC1(joint_clear, RID) + + FUNC5(joint_make_pin, RID, RID, const Vector3 &, RID, const Vector3 &) + + FUNC3(pin_joint_set_param, RID, PinJointParam, real_t) + FUNC2RC(real_t, pin_joint_get_param, RID, PinJointParam) + + FUNC2(pin_joint_set_local_a, RID, const Vector3 &) + FUNC1RC(Vector3, pin_joint_get_local_a, RID) + + FUNC2(pin_joint_set_local_b, RID, const Vector3 &) + FUNC1RC(Vector3, pin_joint_get_local_b, RID) + + FUNC5(joint_make_hinge, RID, RID, const Transform &, RID, const Transform &) + FUNC7(joint_make_hinge_simple, RID, RID, const Vector3 &, const Vector3 &, RID, const Vector3 &, const Vector3 &) + + FUNC3(hinge_joint_set_param, RID, HingeJointParam, real_t) + FUNC2RC(real_t, hinge_joint_get_param, RID, HingeJointParam) + + FUNC3(hinge_joint_set_flag, RID, HingeJointFlag, bool) + FUNC2RC(bool, hinge_joint_get_flag, RID, HingeJointFlag) + + FUNC5(joint_make_slider, RID, RID, const Transform &, RID, const Transform &) + + FUNC3(slider_joint_set_param, RID, SliderJointParam, real_t) + FUNC2RC(real_t, slider_joint_get_param, RID, SliderJointParam) + + FUNC5(joint_make_cone_twist, RID, RID, const Transform &, RID, const Transform &) + + FUNC3(cone_twist_joint_set_param, RID, ConeTwistJointParam, real_t) + FUNC2RC(real_t, cone_twist_joint_get_param, RID, ConeTwistJointParam) + + FUNC5(joint_make_generic_6dof, RID, RID, const Transform &, RID, const Transform &) + + FUNC4(generic_6dof_joint_set_param, RID, Vector3::Axis, G6DOFJointAxisParam, real_t) + FUNC3RC(real_t, generic_6dof_joint_get_param, RID, Vector3::Axis, G6DOFJointAxisParam) + + FUNC4(generic_6dof_joint_set_flag, RID, Vector3::Axis, G6DOFJointAxisFlag, bool) + FUNC3RC(bool, generic_6dof_joint_get_flag, RID, Vector3::Axis, G6DOFJointAxisFlag) + + FUNC1RC(JointType, joint_get_type, RID); + + FUNC2(joint_set_solver_priority, RID, int); + FUNC1RC(int, joint_get_solver_priority, RID); + + FUNC2(joint_disable_collisions_between_bodies, RID, const bool); + FUNC1RC(bool, joint_is_disabled_collisions_between_bodies, RID); + + /* MISC */ + + FUNC1(free, RID); + FUNC1(set_active, bool); + + virtual void init() override; + virtual void step(real_t p_step) override; + virtual void sync() override; + virtual void end_sync() override; + virtual void flush_queries() override; + virtual void finish() override; + + virtual bool is_flushing_queries() const override { + return physics_3d_server->is_flushing_queries(); + } + + int get_process_info(ProcessInfo p_info) override { + return physics_3d_server->get_process_info(p_info); + } + + PhysicsServer3DWrapMT(PhysicsServer3D *p_contained, bool p_create_thread); + ~PhysicsServer3DWrapMT(); + +#undef ServerNameWrapMT +#undef ServerName +#undef server_name +#undef WRITE_ACTION +}; + +#ifdef DEBUG_SYNC +#undef DEBUG_SYNC +#endif +#undef SYNC_DEBUG + +#endif // PHYSICS3DSERVERWRAPMT_H diff --git a/servers/physics_3d/shape_3d_sw.cpp b/servers/physics_3d/shape_3d_sw.cpp index eb0e87cec0..5bac4f19b9 100644 --- a/servers/physics_3d/shape_3d_sw.cpp +++ b/servers/physics_3d/shape_3d_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -30,14 +30,16 @@ #include "shape_3d_sw.h" -#include "core/math/geometry.h" +#include "core/math/geometry_3d.h" #include "core/math/quick_hull.h" -#include "core/sort_array.h" +#include "core/templates/sort_array.h" -#define _POINT_SNAP 0.001953125 #define _EDGE_IS_VALID_SUPPORT_THRESHOLD 0.0002 #define _FACE_IS_VALID_SUPPORT_THRESHOLD 0.9998 +#define _CYLINDER_EDGE_IS_VALID_SUPPORT_THRESHOLD 0.002 +#define _CYLINDER_FACE_IS_VALID_SUPPORT_THRESHOLD 0.999 + void Shape3DSW::configure(const AABB &p_aabb) { aabb = p_aabb; configured = true; @@ -50,7 +52,8 @@ void Shape3DSW::configure(const AABB &p_aabb) { Vector3 Shape3DSW::get_support(const Vector3 &p_normal) const { Vector3 res; int amnt; - get_supports(p_normal, 1, &res, amnt); + FeatureType type; + get_supports(p_normal, 1, &res, amnt, type); return res; } @@ -167,16 +170,19 @@ Vector3 RayShape3DSW::get_support(const Vector3 &p_normal) const { } } -void RayShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { +void RayShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { if (Math::abs(p_normal.z) < _EDGE_IS_VALID_SUPPORT_THRESHOLD) { r_amount = 2; + r_type = FEATURE_EDGE; r_supports[0] = Vector3(0, 0, 0); r_supports[1] = Vector3(0, 0, length); } else if (p_normal.z > 0) { r_amount = 1; + r_type = FEATURE_POINT; *r_supports = Vector3(0, 0, length); } else { r_amount = 1; + r_type = FEATURE_POINT; *r_supports = Vector3(0, 0, 0); } } @@ -195,7 +201,7 @@ Vector3 RayShape3DSW::get_closest_point_to(const Vector3 &p_point) const { Vector3(0, 0, length) }; - return Geometry::get_closest_point_to_segment(p_point, s); + return Geometry3D::get_closest_point_to_segment(p_point, s); } Vector3 RayShape3DSW::get_moment_of_inertia(real_t p_mass) const { @@ -246,13 +252,14 @@ Vector3 SphereShape3DSW::get_support(const Vector3 &p_normal) const { return p_normal * radius; } -void SphereShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { +void SphereShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { *r_supports = p_normal * radius; r_amount = 1; + r_type = FEATURE_POINT; } bool SphereShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { - return Geometry::segment_intersects_sphere(p_begin, p_end, Vector3(), radius, &r_result, &r_normal); + return Geometry3D::segment_intersects_sphere(p_begin, p_end, Vector3(), radius, &r_result, &r_normal); } bool SphereShape3DSW::intersect_point(const Vector3 &p_point) const { @@ -261,7 +268,7 @@ bool SphereShape3DSW::intersect_point(const Vector3 &p_point) const { Vector3 SphereShape3DSW::get_closest_point_to(const Vector3 &p_point) const { Vector3 p = p_point; - float l = p.length(); + real_t l = p.length(); if (l < radius) { return p_point; } @@ -312,7 +319,7 @@ Vector3 BoxShape3DSW::get_support(const Vector3 &p_normal) const { return point; } -void BoxShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { +void BoxShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { static const int next[3] = { 1, 2, 0 }; static const int next2[3] = { 2, 0, 1 }; @@ -325,6 +332,7 @@ void BoxShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_s bool neg = dot < 0; r_amount = 4; + r_type = FEATURE_FACE; Vector3 point; point[i] = half_extents[i]; @@ -333,7 +341,6 @@ void BoxShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_s int i_n2 = next2[i]; static const real_t sign[4][2] = { - { -1.0, 1.0 }, { 1.0, 1.0 }, { 1.0, -1.0 }, @@ -363,6 +370,7 @@ void BoxShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_s if (Math::abs(p_normal.dot(axis)) < _EDGE_IS_VALID_SUPPORT_THRESHOLD) { r_amount = 2; + r_type = FEATURE_EDGE; int i_n = next[i]; int i_n2 = next2[i]; @@ -390,6 +398,7 @@ void BoxShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_s (p_normal.z < 0) ? -half_extents.z : half_extents.z); r_amount = 1; + r_type = FEATURE_POINT; r_supports[0] = point; } @@ -430,7 +439,7 @@ Vector3 BoxShape3DSW::get_closest_point_to(const Vector3 &p_point) const { } //check segments - float min_distance = 1e20; + real_t min_distance = 1e20; Vector3 closest_vertex = half_extents * p_point.sign(); Vector3 s[2] = { closest_vertex, @@ -441,9 +450,9 @@ Vector3 BoxShape3DSW::get_closest_point_to(const Vector3 &p_point) const { s[1] = closest_vertex; s[1][i] = -s[1][i]; //edge - Vector3 closest_edge = Geometry::get_closest_point_to_segment(p_point, s); + Vector3 closest_edge = Geometry3D::get_closest_point_to_segment(p_point, s); - float d = p_point.distance_to(closest_edge); + real_t d = p_point.distance_to(closest_edge); if (d < min_distance) { min_point = closest_edge; min_distance = d; @@ -501,7 +510,7 @@ Vector3 CapsuleShape3DSW::get_support(const Vector3 &p_normal) const { return n; } -void CapsuleShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { +void CapsuleShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { Vector3 n = p_normal; real_t d = n.z; @@ -513,6 +522,7 @@ void CapsuleShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 n *= radius; r_amount = 2; + r_type = FEATURE_EDGE; r_supports[0] = n; r_supports[0].z += height * 0.5; r_supports[1] = n; @@ -524,6 +534,7 @@ void CapsuleShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 n *= radius; n.z += h * 0.5; r_amount = 1; + r_type = FEATURE_POINT; *r_supports = n; } } @@ -540,7 +551,7 @@ bool CapsuleShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 & // test against cylinder and spheres :-| - collided = Geometry::segment_intersects_cylinder(p_begin, p_end, height, radius, &auxres, &auxn); + collided = Geometry3D::segment_intersects_cylinder(p_begin, p_end, height, radius, &auxres, &auxn); if (collided) { real_t d = norm.dot(auxres); @@ -552,7 +563,7 @@ bool CapsuleShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 & } } - collided = Geometry::segment_intersects_sphere(p_begin, p_end, Vector3(0, 0, height * 0.5), radius, &auxres, &auxn); + collided = Geometry3D::segment_intersects_sphere(p_begin, p_end, Vector3(0, 0, height * 0.5), radius, &auxres, &auxn); if (collided) { real_t d = norm.dot(auxres); @@ -564,7 +575,7 @@ bool CapsuleShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 & } } - collided = Geometry::segment_intersects_sphere(p_begin, p_end, Vector3(0, 0, height * -0.5), radius, &auxres, &auxn); + collided = Geometry3D::segment_intersects_sphere(p_begin, p_end, Vector3(0, 0, height * -0.5), radius, &auxres, &auxn); if (collided) { real_t d = norm.dot(auxres); @@ -600,7 +611,7 @@ Vector3 CapsuleShape3DSW::get_closest_point_to(const Vector3 &p_point) const { Vector3(0, 0, height * 0.5), }; - Vector3 p = Geometry::get_closest_point_to_segment(p_point, s); + Vector3 p = Geometry3D::get_closest_point_to_segment(p_point, s); if (p.distance_to(p_point) < radius) { return p_point; @@ -643,6 +654,186 @@ CapsuleShape3DSW::CapsuleShape3DSW() { height = radius = 0; } +/********** CYLINDER *************/ + +void CylinderShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { + Vector3 cylinder_axis = p_transform.basis.get_axis(1).normalized(); + real_t axis_dot = cylinder_axis.dot(p_normal); + + Vector3 local_normal = p_transform.basis.xform_inv(p_normal); + real_t scale = local_normal.length(); + real_t scaled_radius = radius * scale; + real_t scaled_height = height * scale; + + real_t length; + if (Math::abs(axis_dot) > 1.0) { + length = scaled_height * 0.5; + } else { + length = Math::abs(axis_dot * scaled_height * 0.5) + scaled_radius * Math::sqrt(1.0 - axis_dot * axis_dot); + } + + real_t distance = p_normal.dot(p_transform.origin); + + r_min = distance - length; + r_max = distance + length; +} + +Vector3 CylinderShape3DSW::get_support(const Vector3 &p_normal) const { + Vector3 n = p_normal; + real_t h = (n.y > 0) ? height : -height; + real_t s = Math::sqrt(n.x * n.x + n.z * n.z); + if (Math::is_zero_approx(s)) { + n.x = radius; + n.y = h * 0.5; + n.z = 0.0; + } else { + real_t d = radius / s; + n.x = n.x * d; + n.y = h * 0.5; + n.z = n.z * d; + } + + return n; +} + +void CylinderShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { + real_t d = p_normal.y; + if (Math::abs(d) > _CYLINDER_FACE_IS_VALID_SUPPORT_THRESHOLD) { + real_t h = (d > 0) ? height : -height; + + Vector3 n = p_normal; + n.x = 0.0; + n.z = 0.0; + n.y = h * 0.5; + + r_amount = 3; + r_type = FEATURE_CIRCLE; + r_supports[0] = n; + r_supports[1] = n; + r_supports[1].x += radius; + r_supports[2] = n; + r_supports[2].z += radius; + } else if (Math::abs(d) < _CYLINDER_EDGE_IS_VALID_SUPPORT_THRESHOLD) { + // make it flat + Vector3 n = p_normal; + n.y = 0.0; + n.normalize(); + n *= radius; + + r_amount = 2; + r_type = FEATURE_EDGE; + r_supports[0] = n; + r_supports[0].y += height * 0.5; + r_supports[1] = n; + r_supports[1].y -= height * 0.5; + } else { + r_amount = 1; + r_type = FEATURE_POINT; + r_supports[0] = get_support(p_normal); + return; + + Vector3 n = p_normal; + real_t h = n.y * Math::sqrt(0.25 * height * height + radius * radius); + if (Math::abs(h) > 1.0) { + // Top or bottom surface. + n.y = (n.y > 0.0) ? height * 0.5 : -height * 0.5; + } else { + // Lateral surface. + n.y = height * 0.5 * h; + } + + real_t s = Math::sqrt(n.x * n.x + n.z * n.z); + if (Math::is_zero_approx(s)) { + n.x = 0.0; + n.z = 0.0; + } else { + real_t scaled_radius = radius / s; + n.x = n.x * scaled_radius; + n.z = n.z * scaled_radius; + } + + r_supports[0] = n; + } +} + +bool CylinderShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { + return Geometry3D::segment_intersects_cylinder(p_begin, p_end, height, radius, &r_result, &r_normal, 1); +} + +bool CylinderShape3DSW::intersect_point(const Vector3 &p_point) const { + if (Math::abs(p_point.y) < height * 0.5) { + return Vector3(p_point.x, 0, p_point.z).length() < radius; + } + return false; +} + +Vector3 CylinderShape3DSW::get_closest_point_to(const Vector3 &p_point) const { + if (Math::absf(p_point.y) > height * 0.5) { + // Project point to top disk. + real_t dir = p_point.y > 0.0 ? 1.0 : -1.0; + Vector3 circle_pos(0.0, dir * height * 0.5, 0.0); + Plane circle_plane(circle_pos, Vector3(0.0, dir, 0.0)); + Vector3 proj_point = circle_plane.project(p_point); + + // Clip position. + Vector3 delta_point_1 = proj_point - circle_pos; + real_t dist_point_1 = delta_point_1.length_squared(); + if (!Math::is_zero_approx(dist_point_1)) { + dist_point_1 = Math::sqrt(dist_point_1); + proj_point = circle_pos + delta_point_1 * MIN(dist_point_1, radius) / dist_point_1; + } + + return proj_point; + } else { + Vector3 s[2] = { + Vector3(0, -height * 0.5, 0), + Vector3(0, height * 0.5, 0), + }; + + Vector3 p = Geometry3D::get_closest_point_to_segment(p_point, s); + + if (p.distance_to(p_point) < radius) { + return p_point; + } + + return p + (p_point - p).normalized() * radius; + } +} + +Vector3 CylinderShape3DSW::get_moment_of_inertia(real_t p_mass) const { + // use bad AABB approximation + Vector3 extents = get_aabb().size * 0.5; + + return Vector3( + (p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z), + (p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z), + (p_mass / 3.0) * (extents.y * extents.y + extents.y * extents.y)); +} + +void CylinderShape3DSW::_setup(real_t p_height, real_t p_radius) { + height = p_height; + radius = p_radius; + configure(AABB(Vector3(-radius, -height * 0.5, -radius), Vector3(radius * 2.0, height, radius * 2.0))); +} + +void CylinderShape3DSW::set_data(const Variant &p_data) { + Dictionary d = p_data; + ERR_FAIL_COND(!d.has("radius")); + ERR_FAIL_COND(!d.has("height")); + _setup(d["height"], d["radius"]); +} + +Variant CylinderShape3DSW::get_data() const { + Dictionary d; + d["radius"] = radius; + d["height"] = height; + return d; +} + +CylinderShape3DSW::CylinderShape3DSW() { + height = radius = 0; +} + /********** CONVEX POLYGON *************/ void ConvexPolygonShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { @@ -690,11 +881,11 @@ Vector3 ConvexPolygonShape3DSW::get_support(const Vector3 &p_normal) const { return vrts[vert_support_idx]; } -void ConvexPolygonShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { - const Geometry::MeshData::Face *faces = mesh.faces.ptr(); +void ConvexPolygonShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { + const Geometry3D::MeshData::Face *faces = mesh.faces.ptr(); int fc = mesh.faces.size(); - const Geometry::MeshData::Edge *edges = mesh.edges.ptr(); + const Geometry3D::MeshData::Edge *edges = mesh.edges.ptr(); int ec = mesh.edges.size(); const Vector3 *vertices = mesh.vertices.ptr(); @@ -735,6 +926,7 @@ void ConvexPolygonShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Ve r_supports[j] = vertices[ind[j]]; } r_amount = m; + r_type = FEATURE_FACE; return; } } @@ -744,6 +936,7 @@ void ConvexPolygonShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Ve dot = ABS(dot); if (dot < _EDGE_IS_VALID_SUPPORT_THRESHOLD && (edges[i].a == vtx || edges[i].b == vtx)) { r_amount = 2; + r_type = FEATURE_EDGE; r_supports[0] = vertices[edges[i].a]; r_supports[1] = vertices[edges[i].b]; return; @@ -752,10 +945,11 @@ void ConvexPolygonShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Ve r_supports[0] = vertices[vtx]; r_amount = 1; + r_type = FEATURE_POINT; } bool ConvexPolygonShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { - const Geometry::MeshData::Face *faces = mesh.faces.ptr(); + const Geometry3D::MeshData::Face *faces = mesh.faces.ptr(); int fc = mesh.faces.size(); const Vector3 *vertices = mesh.vertices.ptr(); @@ -793,7 +987,7 @@ bool ConvexPolygonShape3DSW::intersect_segment(const Vector3 &p_begin, const Vec } bool ConvexPolygonShape3DSW::intersect_point(const Vector3 &p_point) const { - const Geometry::MeshData::Face *faces = mesh.faces.ptr(); + const Geometry3D::MeshData::Face *faces = mesh.faces.ptr(); int fc = mesh.faces.size(); for (int i = 0; i < fc; i++) { @@ -806,7 +1000,7 @@ bool ConvexPolygonShape3DSW::intersect_point(const Vector3 &p_point) const { } Vector3 ConvexPolygonShape3DSW::get_closest_point_to(const Vector3 &p_point) const { - const Geometry::MeshData::Face *faces = mesh.faces.ptr(); + const Geometry3D::MeshData::Face *faces = mesh.faces.ptr(); int fc = mesh.faces.size(); const Vector3 *vertices = mesh.vertices.ptr(); @@ -840,11 +1034,11 @@ Vector3 ConvexPolygonShape3DSW::get_closest_point_to(const Vector3 &p_point) con return p_point; } - float min_distance = 1e20; + real_t min_distance = 1e20; Vector3 min_point; //check edges - const Geometry::MeshData::Edge *edges = mesh.edges.ptr(); + const Geometry3D::MeshData::Edge *edges = mesh.edges.ptr(); int ec = mesh.edges.size(); for (int i = 0; i < ec; i++) { Vector3 s[2] = { @@ -852,8 +1046,8 @@ Vector3 ConvexPolygonShape3DSW::get_closest_point_to(const Vector3 &p_point) con vertices[edges[i].b] }; - Vector3 closest = Geometry::get_closest_point_to_segment(p_point, s); - float d = closest.distance_to(p_point); + Vector3 closest = Geometry3D::get_closest_point_to_segment(p_point, s); + real_t d = closest.distance_to(p_point); if (d < min_distance) { min_distance = d; min_point = closest; @@ -936,12 +1130,13 @@ Vector3 FaceShape3DSW::get_support(const Vector3 &p_normal) const { return vertex[vert_support_idx]; } -void FaceShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { +void FaceShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { Vector3 n = p_normal; /** TEST FACE AS SUPPORT **/ if (normal.dot(n) > _FACE_IS_VALID_SUPPORT_THRESHOLD) { r_amount = 3; + r_type = FEATURE_FACE; for (int i = 0; i < 3; i++) { r_supports[i] = vertex[i]; } @@ -975,6 +1170,7 @@ void FaceShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_ dot = ABS(dot); if (dot < _EDGE_IS_VALID_SUPPORT_THRESHOLD) { r_amount = 2; + r_type = FEATURE_EDGE; r_supports[0] = vertex[i]; r_supports[1] = vertex[nx]; return; @@ -982,11 +1178,12 @@ void FaceShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_ } r_amount = 1; + r_type = FEATURE_POINT; r_supports[0] = vertex[vert_support_idx]; } bool FaceShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { - bool c = Geometry::segment_intersects_triangle(p_begin, p_end, vertex[0], vertex[1], vertex[2], &r_result); + bool c = Geometry3D::segment_intersects_triangle(p_begin, p_end, vertex[0], vertex[1], vertex[2], &r_result); if (c) { r_normal = Plane(vertex[0], vertex[1], vertex[2]).normal; if (r_normal.dot(p_end - p_begin) > 0) { @@ -1095,7 +1292,7 @@ void ConcavePolygonShape3DSW::_cull_segment(int p_idx, _SegmentCullParams *p_par p_params->vertices[p_params->faces[bvh->face_index].indices[2]] }; - if (Geometry::segment_intersects_triangle( + if (Geometry3D::segment_intersects_triangle( p_params->from, p_params->to, vertices[0], diff --git a/servers/physics_3d/shape_3d_sw.h b/servers/physics_3d/shape_3d_sw.h index 848a1135ad..cafe978abb 100644 --- a/servers/physics_3d/shape_3d_sw.h +++ b/servers/physics_3d/shape_3d_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -31,7 +31,7 @@ #ifndef SHAPE_SW_H #define SHAPE_SW_H -#include "core/math/geometry.h" +#include "core/math/geometry_3d.h" #include "servers/physics_server_3d.h" /* @@ -67,8 +67,11 @@ protected: void configure(const AABB &p_aabb); public: - enum { - MAX_SUPPORTS = 8 + enum FeatureType { + FEATURE_POINT, + FEATURE_EDGE, + FEATURE_FACE, + FEATURE_CIRCLE, }; virtual real_t get_area() const { return aabb.get_area(); } @@ -85,7 +88,7 @@ public: virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const = 0; virtual Vector3 get_support(const Vector3 &p_normal) const; - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const = 0; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const = 0; virtual Vector3 get_closest_point_to(const Vector3 &p_point) const = 0; virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal) const = 0; virtual bool intersect_point(const Vector3 &p_point) const = 0; @@ -110,7 +113,7 @@ class ConcaveShape3DSW : public Shape3DSW { public: virtual bool is_concave() const { return true; } typedef void (*Callback)(void *p_userdata, Shape3DSW *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 get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { r_amount = 0; } virtual void cull(const AABB &p_local_aabb, Callback p_callback, void *p_userdata) const = 0; @@ -129,7 +132,7 @@ public: virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_PLANE; } virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; virtual Vector3 get_support(const Vector3 &p_normal) const; - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { r_amount = 0; } + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { r_amount = 0; } virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; virtual bool intersect_point(const Vector3 &p_point) const; @@ -156,7 +159,7 @@ public: virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_RAY; } virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; virtual Vector3 get_support(const Vector3 &p_normal) const; - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const; virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; virtual bool intersect_point(const Vector3 &p_point) const; @@ -184,7 +187,7 @@ public: virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; virtual Vector3 get_support(const Vector3 &p_normal) const; - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const; virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; virtual bool intersect_point(const Vector3 &p_point) const; virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; @@ -209,7 +212,7 @@ public: virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; virtual Vector3 get_support(const Vector3 &p_normal) const; - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const; virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; virtual bool intersect_point(const Vector3 &p_point) const; virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; @@ -238,7 +241,7 @@ public: virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; virtual Vector3 get_support(const Vector3 &p_normal) const; - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const; virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; virtual bool intersect_point(const Vector3 &p_point) const; virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; @@ -251,19 +254,48 @@ public: CapsuleShape3DSW(); }; +class CylinderShape3DSW : public Shape3DSW { + real_t height; + real_t radius; + + void _setup(real_t p_height, real_t p_radius); + +public: + _FORCE_INLINE_ real_t get_height() const { return height; } + _FORCE_INLINE_ real_t get_radius() const { return radius; } + + virtual real_t get_area() const { return 4.0 / 3.0 * Math_PI * radius * radius * radius + height * Math_PI * radius * radius; } + + virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_CYLINDER; } + + virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; + virtual Vector3 get_support(const Vector3 &p_normal) const; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; + virtual bool intersect_point(const Vector3 &p_point) const; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; + + virtual Vector3 get_moment_of_inertia(real_t p_mass) const; + + virtual void set_data(const Variant &p_data); + virtual Variant get_data() const; + + CylinderShape3DSW(); +}; + struct ConvexPolygonShape3DSW : public Shape3DSW { - Geometry::MeshData mesh; + Geometry3D::MeshData mesh; void _setup(const Vector<Vector3> &p_vertices); public: - const Geometry::MeshData &get_mesh() const { return mesh; } + const Geometry3D::MeshData &get_mesh() const { return mesh; } virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_CONVEX_POLYGON; } virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; virtual Vector3 get_support(const Vector3 &p_normal) const; - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const; virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; virtual bool intersect_point(const Vector3 &p_point) const; virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; @@ -399,7 +431,7 @@ struct FaceShape3DSW : public Shape3DSW { void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; Vector3 get_support(const Vector3 &p_normal) const; - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const; bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; virtual bool intersect_point(const Vector3 &p_point) const; virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; @@ -437,7 +469,7 @@ struct MotionShape3DSW : public Shape3DSW { } return support; } - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { r_amount = 0; } + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { r_amount = 0; } bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { return false; } virtual bool intersect_point(const Vector3 &p_point) const { return false; } virtual Vector3 get_closest_point_to(const Vector3 &p_point) const { return p_point; } diff --git a/servers/physics_3d/space_3d_sw.cpp b/servers/physics_3d/space_3d_sw.cpp index 4d272bdabd..c8741dc930 100644 --- a/servers/physics_3d/space_3d_sw.cpp +++ b/servers/physics_3d/space_3d_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -31,7 +31,7 @@ #include "space_3d_sw.h" #include "collision_solver_3d_sw.h" -#include "core/project_settings.h" +#include "core/config/project_settings.h" #include "physics_server_3d_sw.h" _FORCE_INLINE_ static bool _can_collide_with(CollisionObject3DSW *p_object, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { @@ -181,7 +181,7 @@ int PhysicsDirectSpaceState3DSW::intersect_shape(const RID &p_shape, const Trans return 0; } - Shape3DSW *shape = static_cast<PhysicsServer3DSW *>(PhysicsServer3D::get_singleton())->shape_owner.getornull(p_shape); + Shape3DSW *shape = PhysicsServer3DSW::singletonsw->shape_owner.getornull(p_shape); ERR_FAIL_COND_V(!shape, 0); AABB aabb = p_xform.xform(shape->get_aabb()); @@ -210,6 +210,10 @@ int PhysicsDirectSpaceState3DSW::intersect_shape(const RID &p_shape, const Trans const CollisionObject3DSW *col_obj = space->intersection_query_results[i]; int shape_idx = space->intersection_query_subindex_results[i]; + if (col_obj->is_shape_set_as_disabled(shape_idx)) { + continue; + } + if (!CollisionSolver3DSW::solve_static(shape, p_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), nullptr, nullptr, nullptr, p_margin, 0)) { continue; } @@ -232,7 +236,7 @@ int PhysicsDirectSpaceState3DSW::intersect_shape(const RID &p_shape, const Trans } bool PhysicsDirectSpaceState3DSW::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, bool p_collide_with_bodies, bool p_collide_with_areas, ShapeRestInfo *r_info) { - Shape3DSW *shape = static_cast<PhysicsServer3DSW *>(PhysicsServer3D::get_singleton())->shape_owner.getornull(p_shape); + Shape3DSW *shape = PhysicsServer3DSW::singletonsw->shape_owner.getornull(p_shape); ERR_FAIL_COND_V(!shape, false); AABB aabb = p_xform.xform(shape->get_aabb()); @@ -265,6 +269,10 @@ bool PhysicsDirectSpaceState3DSW::cast_motion(const RID &p_shape, const Transfor const CollisionObject3DSW *col_obj = space->intersection_query_results[i]; int shape_idx = space->intersection_query_subindex_results[i]; + if (col_obj->is_shape_set_as_disabled(shape_idx)) { + continue; + } + Vector3 point_A, point_B; Vector3 sep_axis = p_motion.normalized(); @@ -274,11 +282,11 @@ bool PhysicsDirectSpaceState3DSW::cast_motion(const RID &p_shape, const Transfor continue; } - //test initial overlap + //test initial overlap, ignore objects it's inside of. sep_axis = p_motion.normalized(); if (!CollisionSolver3DSW::solve_distance(shape, p_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, aabb, &sep_axis)) { - return false; + continue; } //just do kinematic solving @@ -340,7 +348,7 @@ bool PhysicsDirectSpaceState3DSW::collide_shape(RID p_shape, const Transform &p_ return false; } - Shape3DSW *shape = static_cast<PhysicsServer3DSW *>(PhysicsServer3D::get_singleton())->shape_owner.getornull(p_shape); + Shape3DSW *shape = PhysicsServer3DSW::singletonsw->shape_owner.getornull(p_shape); ERR_FAIL_COND_V(!shape, 0); AABB aabb = p_shape_xform.xform(shape->get_aabb()); @@ -365,12 +373,17 @@ bool PhysicsDirectSpaceState3DSW::collide_shape(RID p_shape, const Transform &p_ } const CollisionObject3DSW *col_obj = space->intersection_query_results[i]; - int shape_idx = space->intersection_query_subindex_results[i]; if (p_exclude.has(col_obj->get_self())) { continue; } + int shape_idx = space->intersection_query_subindex_results[i]; + + if (col_obj->is_shape_set_as_disabled(shape_idx)) { + continue; + } + if (CollisionSolver3DSW::solve_static(shape, p_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, nullptr, p_margin)) { collided = true; } @@ -384,6 +397,8 @@ bool PhysicsDirectSpaceState3DSW::collide_shape(RID p_shape, const Transform &p_ struct _RestCallbackData { const CollisionObject3DSW *object; const CollisionObject3DSW *best_object; + int local_shape; + int best_local_shape; int shape; int best_shape; Vector3 best_contact; @@ -409,10 +424,11 @@ static void _rest_cbk_result(const Vector3 &p_point_A, const Vector3 &p_point_B, rd->best_normal = contact_rel / len; rd->best_object = rd->object; rd->best_shape = rd->shape; + rd->best_local_shape = rd->local_shape; } bool PhysicsDirectSpaceState3DSW::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, bool p_collide_with_bodies, bool p_collide_with_areas) { - Shape3DSW *shape = static_cast<PhysicsServer3DSW *>(PhysicsServer3D::get_singleton())->shape_owner.getornull(p_shape); + Shape3DSW *shape = PhysicsServer3DSW::singletonsw->shape_owner.getornull(p_shape); ERR_FAIL_COND_V(!shape, 0); AABB aabb = p_shape_xform.xform(shape->get_aabb()); @@ -432,12 +448,17 @@ bool PhysicsDirectSpaceState3DSW::rest_info(RID p_shape, const Transform &p_shap } const CollisionObject3DSW *col_obj = space->intersection_query_results[i]; - int shape_idx = space->intersection_query_subindex_results[i]; if (p_exclude.has(col_obj->get_self())) { continue; } + int shape_idx = space->intersection_query_subindex_results[i]; + + if (col_obj->is_shape_set_as_disabled(shape_idx)) { + continue; + } + rcd.object = col_obj; rcd.shape = shape_idx; bool sc = CollisionSolver3DSW::solve_static(shape, p_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), _rest_cbk_result, &rcd, nullptr, p_margin); @@ -468,15 +489,15 @@ bool PhysicsDirectSpaceState3DSW::rest_info(RID p_shape, const Transform &p_shap } Vector3 PhysicsDirectSpaceState3DSW::get_closest_point_to_object_volume(RID p_object, const Vector3 p_point) const { - CollisionObject3DSW *obj = PhysicsServer3DSW::singleton->area_owner.getornull(p_object); + CollisionObject3DSW *obj = PhysicsServer3DSW::singletonsw->area_owner.getornull(p_object); if (!obj) { - obj = PhysicsServer3DSW::singleton->body_owner.getornull(p_object); + obj = PhysicsServer3DSW::singletonsw->body_owner.getornull(p_object); } ERR_FAIL_COND_V(!obj, Vector3()); ERR_FAIL_COND_V(obj->get_space() != space, Vector3()); - float min_distance = 1e20; + real_t min_distance = 1e20; Vector3 min_point; bool shapes_found = false; @@ -492,7 +513,7 @@ Vector3 PhysicsDirectSpaceState3DSW::get_closest_point_to_object_volume(RID p_ob Vector3 point = shape->get_closest_point_to(shape_xform.affine_inverse().xform(p_point)); point = shape_xform.xform(point); - float dist = point.distance_to(p_point); + real_t dist = point.distance_to(p_point); if (dist < min_distance) { min_distance = dist; min_point = point; @@ -649,9 +670,9 @@ int Space3DSW::test_body_ray_separation(Body3DSW *p_body, const Transform &p_tra Vector3 a = sr[k * 2 + 0]; Vector3 b = sr[k * 2 + 1]; - recover_motion += (b - a) * 0.4; + recover_motion += (b - a) / cbk.amount; - float depth = a.distance_to(b); + real_t depth = a.distance_to(b); if (depth > result.collision_depth) { result.collision_depth = depth; result.collision_point = b; @@ -739,8 +760,13 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons body_aabb = p_from.xform(p_body->get_inv_transform().xform(body_aabb)); body_aabb = body_aabb.grow(p_margin); + real_t motion_length = p_motion.length(); + Vector3 motion_normal = p_motion / motion_length; + Transform body_transform = p_from; + bool recovered = false; + { //STEP 1, FREE BODY IF STUCK @@ -791,7 +817,17 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons for (int i = 0; i < cbk.amount; i++) { Vector3 a = sr[i * 2 + 0]; Vector3 b = sr[i * 2 + 1]; - recover_motion += (b - a) * 0.4; + + // Compute plane on b towards a. + Vector3 n = (a - b).normalized(); + real_t d = n.dot(b); + + // Compute depth on recovered motion. + real_t depth = n.dot(a + recover_motion) - d; + if (depth > 0.0) { + // Only recover if there is penetration. + recover_motion -= n * depth * 0.4; + } } if (recover_motion == Vector3()) { @@ -799,6 +835,8 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons break; } + recovered = true; + body_transform.origin += recover_motion; body_aabb.position += recover_motion; @@ -848,14 +886,14 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons //test initial overlap, does it collide if going all the way? Vector3 point_A, point_B; - Vector3 sep_axis = p_motion.normalized(); + Vector3 sep_axis = motion_normal; Transform col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); //test initial overlap, does it collide if going all the way? if (CollisionSolver3DSW::solve_distance(&mshape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, motion_aabb, &sep_axis)) { continue; } - sep_axis = p_motion.normalized(); + sep_axis = motion_normal; if (!CollisionSolver3DSW::solve_distance(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, motion_aabb, &sep_axis)) { stuck = true; @@ -865,13 +903,12 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons //just do kinematic solving real_t low = 0; real_t hi = 1; - Vector3 mnormal = p_motion.normalized(); for (int k = 0; k < 8; k++) { //steps should be customizable.. real_t ofs = (low + hi) * 0.5; - Vector3 sep = mnormal; //important optimization for this to work fast enough + Vector3 sep = motion_normal; //important optimization for this to work fast enough mshape.motion = body_shape_xform_inv.basis.xform(p_motion * ofs); @@ -912,16 +949,11 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons } bool collided = false; - if (safe >= 1) { - //not collided - collided = false; - if (r_result) { - r_result->motion = p_motion; - r_result->remainder = Vector3(); - r_result->motion += (body_transform.get_origin() - p_from.get_origin()); + if (recovered || (safe < 1)) { + if (safe >= 1) { + best_shape = -1; //no best shape with cast, reset to -1 } - } else { //it collided, let's get the rest info in unsafe advance Transform ugt = body_transform; ugt.origin += p_motion * unsafe; @@ -930,25 +962,40 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons rcd.best_len = 0; rcd.best_object = nullptr; rcd.best_shape = 0; - rcd.min_allowed_depth = test_motion_min_contact_depth; - Transform body_shape_xform = ugt * p_body->get_shape_transform(best_shape); - Shape3DSW *body_shape = p_body->get_shape(best_shape); + // Allowed depth can't be lower than motion length, in order to handle contacts at low speed. + rcd.min_allowed_depth = MIN(motion_length, test_motion_min_contact_depth); - body_aabb.position += p_motion * unsafe; + int from_shape = best_shape != -1 ? best_shape : 0; + int to_shape = best_shape != -1 ? best_shape + 1 : p_body->get_shape_count(); - int amount = _cull_aabb_for_body(p_body, body_aabb); + for (int j = from_shape; j < to_shape; j++) { + if (p_body->is_shape_set_as_disabled(j)) { + continue; + } - for (int i = 0; i < amount; i++) { - const CollisionObject3DSW *col_obj = intersection_query_results[i]; - int shape_idx = intersection_query_subindex_results[i]; + Transform body_shape_xform = ugt * p_body->get_shape_transform(j); + Shape3DSW *body_shape = p_body->get_shape(j); - rcd.object = col_obj; - rcd.shape = shape_idx; - bool sc = CollisionSolver3DSW::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), _rest_cbk_result, &rcd, nullptr, p_margin); - if (!sc) { + if (p_exclude_raycast_shapes && body_shape->get_type() == PhysicsServer3D::SHAPE_RAY) { continue; } + + body_aabb.position += p_motion * unsafe; + + int amount = _cull_aabb_for_body(p_body, body_aabb); + + for (int i = 0; i < amount; i++) { + const CollisionObject3DSW *col_obj = intersection_query_results[i]; + int shape_idx = intersection_query_subindex_results[i]; + + rcd.object = col_obj; + rcd.shape = shape_idx; + bool sc = CollisionSolver3DSW::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), _rest_cbk_result, &rcd, nullptr, p_margin); + if (!sc) { + continue; + } + } } if (rcd.best_len != 0) { @@ -956,7 +1003,7 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons r_result->collider = rcd.best_object->get_self(); r_result->collider_id = rcd.best_object->get_instance_id(); r_result->collider_shape = rcd.best_shape; - r_result->collision_local_shape = best_shape; + r_result->collision_local_shape = rcd.best_local_shape; r_result->collision_normal = rcd.best_normal; r_result->collision_point = rcd.best_contact; //r_result->collider_metadata = rcd.best_object->get_shape_metadata(rcd.best_shape); @@ -972,21 +1019,23 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons } collided = true; - } else { - if (r_result) { - r_result->motion = p_motion; - r_result->remainder = Vector3(); - r_result->motion += (body_transform.get_origin() - p_from.get_origin()); - } - - collided = false; } } + if (!collided && r_result) { + r_result->motion = p_motion; + r_result->remainder = Vector3(); + r_result->motion += (body_transform.get_origin() - p_from.get_origin()); + } + return collided; } void *Space3DSW::_broadphase_pair(CollisionObject3DSW *A, int p_subindex_A, CollisionObject3DSW *B, int p_subindex_B, void *p_self) { + if (!A->test_collision_mask(B)) { + return nullptr; + } + CollisionObject3DSW::Type type_A = A->get_type(); CollisionObject3DSW::Type type_B = B->get_type(); if (type_A > type_B) { @@ -1019,6 +1068,10 @@ void *Space3DSW::_broadphase_pair(CollisionObject3DSW *A, int p_subindex_A, Coll } void Space3DSW::_broadphase_unpair(CollisionObject3DSW *A, int p_subindex_A, CollisionObject3DSW *B, int p_subindex_B, void *p_data, void *p_self) { + if (!p_data) { + return; + } + Space3DSW *self = (Space3DSW *)p_self; self->collision_pairs--; Constraint3DSW *c = (Constraint3DSW *)p_data; @@ -1203,7 +1256,7 @@ Space3DSW::Space3DSW() { constraint_bias = 0.01; body_linear_velocity_sleep_threshold = GLOBAL_DEF("physics/3d/sleep_threshold_linear", 0.1); - body_angular_velocity_sleep_threshold = GLOBAL_DEF("physics/3d/sleep_threshold_angular", (8.0 / 180.0 * Math_PI)); + body_angular_velocity_sleep_threshold = GLOBAL_DEF("physics/3d/sleep_threshold_angular", Math::deg2rad(8.0)); body_time_to_sleep = GLOBAL_DEF("physics/3d/time_before_sleep", 0.5); ProjectSettings::get_singleton()->set_custom_property_info("physics/3d/time_before_sleep", PropertyInfo(Variant::FLOAT, "physics/3d/time_before_sleep", PROPERTY_HINT_RANGE, "0,5,0.01,or_greater")); body_angular_velocity_damp_ratio = 10; diff --git a/servers/physics_3d/space_3d_sw.h b/servers/physics_3d/space_3d_sw.h index f9198e3d40..eed3d86a72 100644 --- a/servers/physics_3d/space_3d_sw.h +++ b/servers/physics_3d/space_3d_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -37,8 +37,8 @@ #include "body_pair_3d_sw.h" #include "broad_phase_3d_sw.h" #include "collision_object_3d_sw.h" -#include "core/hash_map.h" -#include "core/project_settings.h" +#include "core/config/project_settings.h" +#include "core/templates/hash_map.h" #include "core/typedefs.h" class PhysicsDirectSpaceState3DSW : public PhysicsDirectSpaceState3D { @@ -47,13 +47,13 @@ class PhysicsDirectSpaceState3DSW : public PhysicsDirectSpaceState3D { public: Space3DSW *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, bool p_collide_with_bodies = true, bool p_collide_with_areas = false); - 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_collide_with_bodies = true, bool p_collide_with_areas = false, 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, bool p_collide_with_bodies = true, bool p_collide_with_areas = false); - 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, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, ShapeRestInfo *r_info = nullptr); - 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, bool p_collide_with_bodies = true, bool p_collide_with_areas = false); - 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, bool p_collide_with_bodies = true, bool p_collide_with_areas = false); - virtual Vector3 get_closest_point_to_object_volume(RID p_object, const Vector3 p_point) const; + 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, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; + 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_collide_with_bodies = true, bool p_collide_with_areas = false, bool p_pick_ray = false) override; + 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, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; + 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, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, ShapeRestInfo *r_info = nullptr) override; + 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, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; + 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, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; + virtual Vector3 get_closest_point_to_object_volume(RID p_object, const Vector3 p_point) const override; PhysicsDirectSpaceState3DSW(); }; @@ -97,7 +97,6 @@ private: real_t test_motion_min_contact_depth; enum { - INTERSECTION_QUERY_MAX = 2048 }; @@ -183,7 +182,7 @@ public: PhysicsDirectSpaceState3DSW *get_direct_state(); void set_debug_contacts(int p_amount) { contact_debug.resize(p_amount); } - _FORCE_INLINE_ bool is_debugging_contacts() const { return !contact_debug.empty(); } + _FORCE_INLINE_ bool is_debugging_contacts() const { return !contact_debug.is_empty(); } _FORCE_INLINE_ void add_debug_contact(const Vector3 &p_contact) { if (contact_debug_count < contact_debug.size()) { contact_debug.write[contact_debug_count++] = p_contact; diff --git a/servers/physics_3d/step_3d_sw.cpp b/servers/physics_3d/step_3d_sw.cpp index 9a2a0073a1..d9370de6a3 100644 --- a/servers/physics_3d/step_3d_sw.cpp +++ b/servers/physics_3d/step_3d_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/servers/physics_3d/step_3d_sw.h b/servers/physics_3d/step_3d_sw.h index 9dbb61308f..55c48ec0eb 100644 --- a/servers/physics_3d/step_3d_sw.h +++ b/servers/physics_3d/step_3d_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ |