From cc39dca9f7d960d1bb137f1dcbbf1da5cec8a505 Mon Sep 17 00:00:00 2001 From: PouleyKetchoupp Date: Mon, 18 Oct 2021 12:24:30 -0700 Subject: Rename Godot Physics classes from *SW to Godot* Also moved MT physics server wrappers to the main servers folder, since they don't have to be implementation specific. --- servers/physics_2d/area_2d_sw.cpp | 305 ----- servers/physics_2d/area_2d_sw.h | 195 --- servers/physics_2d/area_pair_2d_sw.cpp | 190 --- servers/physics_2d/area_pair_2d_sw.h | 74 - servers/physics_2d/body_2d_sw.cpp | 677 ---------- servers/physics_2d/body_2d_sw.h | 353 ----- servers/physics_2d/body_direct_state_2d_sw.cpp | 178 --- servers/physics_2d/body_direct_state_2d_sw.h | 91 -- servers/physics_2d/body_pair_2d_sw.cpp | 545 -------- servers/physics_2d/body_pair_2d_sw.h | 98 -- servers/physics_2d/broad_phase_2d_bvh.cpp | 113 -- servers/physics_2d/broad_phase_2d_bvh.h | 73 - servers/physics_2d/broad_phase_2d_sw.cpp | 36 - servers/physics_2d/broad_phase_2d_sw.h | 71 - servers/physics_2d/collision_object_2d_sw.cpp | 243 ---- servers/physics_2d/collision_object_2d_sw.h | 189 --- servers/physics_2d/collision_solver_2d_sat.cpp | 1408 -------------------- servers/physics_2d/collision_solver_2d_sat.h | 38 - servers/physics_2d/collision_solver_2d_sw.cpp | 264 ---- servers/physics_2d/collision_solver_2d_sw.h | 50 - servers/physics_2d/constraint_2d_sw.h | 70 - servers/physics_2d/godot_area_2d.cpp | 305 +++++ servers/physics_2d/godot_area_2d.h | 196 +++ servers/physics_2d/godot_area_pair_2d.cpp | 190 +++ servers/physics_2d/godot_area_pair_2d.h | 74 + servers/physics_2d/godot_body_2d.cpp | 677 ++++++++++ servers/physics_2d/godot_body_2d.h | 354 +++++ servers/physics_2d/godot_body_direct_state_2d.cpp | 178 +++ servers/physics_2d/godot_body_direct_state_2d.h | 91 ++ servers/physics_2d/godot_body_pair_2d.cpp | 545 ++++++++ servers/physics_2d/godot_body_pair_2d.h | 98 ++ servers/physics_2d/godot_broad_phase_2d.cpp | 36 + servers/physics_2d/godot_broad_phase_2d.h | 71 + servers/physics_2d/godot_broad_phase_2d_bvh.cpp | 113 ++ servers/physics_2d/godot_broad_phase_2d_bvh.h | 74 + servers/physics_2d/godot_collision_object_2d.cpp | 243 ++++ servers/physics_2d/godot_collision_object_2d.h | 190 +++ servers/physics_2d/godot_collision_solver_2d.cpp | 264 ++++ servers/physics_2d/godot_collision_solver_2d.h | 50 + .../physics_2d/godot_collision_solver_2d_sat.cpp | 1408 ++++++++++++++++++++ servers/physics_2d/godot_collision_solver_2d_sat.h | 38 + servers/physics_2d/godot_constraint_2d.h | 70 + servers/physics_2d/godot_joints_2d.cpp | 486 +++++++ servers/physics_2d/godot_joints_2d.h | 178 +++ servers/physics_2d/godot_physics_server_2d.cpp | 1350 +++++++++++++++++++ servers/physics_2d/godot_physics_server_2d.h | 299 +++++ servers/physics_2d/godot_shape_2d.cpp | 995 ++++++++++++++ servers/physics_2d/godot_shape_2d.h | 536 ++++++++ servers/physics_2d/godot_space_2d.cpp | 1213 +++++++++++++++++ servers/physics_2d/godot_space_2d.h | 212 +++ servers/physics_2d/godot_step_2d.cpp | 308 +++++ servers/physics_2d/godot_step_2d.h | 63 + servers/physics_2d/joints_2d_sw.cpp | 486 ------- servers/physics_2d/joints_2d_sw.h | 178 --- servers/physics_2d/physics_server_2d_sw.cpp | 1349 ------------------- servers/physics_2d/physics_server_2d_sw.h | 299 ----- servers/physics_2d/physics_server_2d_wrap_mt.cpp | 137 -- servers/physics_2d/physics_server_2d_wrap_mt.h | 333 ----- servers/physics_2d/shape_2d_sw.cpp | 995 -------------- servers/physics_2d/shape_2d_sw.h | 536 -------- servers/physics_2d/space_2d_sw.cpp | 1212 ----------------- servers/physics_2d/space_2d_sw.h | 211 --- servers/physics_2d/step_2d_sw.cpp | 308 ----- servers/physics_2d/step_2d_sw.h | 63 - 64 files changed, 10905 insertions(+), 11368 deletions(-) delete mode 100644 servers/physics_2d/area_2d_sw.cpp delete mode 100644 servers/physics_2d/area_2d_sw.h delete mode 100644 servers/physics_2d/area_pair_2d_sw.cpp delete mode 100644 servers/physics_2d/area_pair_2d_sw.h delete mode 100644 servers/physics_2d/body_2d_sw.cpp delete mode 100644 servers/physics_2d/body_2d_sw.h delete mode 100644 servers/physics_2d/body_direct_state_2d_sw.cpp delete mode 100644 servers/physics_2d/body_direct_state_2d_sw.h delete mode 100644 servers/physics_2d/body_pair_2d_sw.cpp delete mode 100644 servers/physics_2d/body_pair_2d_sw.h delete mode 100644 servers/physics_2d/broad_phase_2d_bvh.cpp delete mode 100644 servers/physics_2d/broad_phase_2d_bvh.h delete mode 100644 servers/physics_2d/broad_phase_2d_sw.cpp delete mode 100644 servers/physics_2d/broad_phase_2d_sw.h delete mode 100644 servers/physics_2d/collision_object_2d_sw.cpp delete mode 100644 servers/physics_2d/collision_object_2d_sw.h delete mode 100644 servers/physics_2d/collision_solver_2d_sat.cpp delete mode 100644 servers/physics_2d/collision_solver_2d_sat.h delete mode 100644 servers/physics_2d/collision_solver_2d_sw.cpp delete mode 100644 servers/physics_2d/collision_solver_2d_sw.h delete mode 100644 servers/physics_2d/constraint_2d_sw.h create mode 100644 servers/physics_2d/godot_area_2d.cpp create mode 100644 servers/physics_2d/godot_area_2d.h create mode 100644 servers/physics_2d/godot_area_pair_2d.cpp create mode 100644 servers/physics_2d/godot_area_pair_2d.h create mode 100644 servers/physics_2d/godot_body_2d.cpp create mode 100644 servers/physics_2d/godot_body_2d.h create mode 100644 servers/physics_2d/godot_body_direct_state_2d.cpp create mode 100644 servers/physics_2d/godot_body_direct_state_2d.h create mode 100644 servers/physics_2d/godot_body_pair_2d.cpp create mode 100644 servers/physics_2d/godot_body_pair_2d.h create mode 100644 servers/physics_2d/godot_broad_phase_2d.cpp create mode 100644 servers/physics_2d/godot_broad_phase_2d.h create mode 100644 servers/physics_2d/godot_broad_phase_2d_bvh.cpp create mode 100644 servers/physics_2d/godot_broad_phase_2d_bvh.h create mode 100644 servers/physics_2d/godot_collision_object_2d.cpp create mode 100644 servers/physics_2d/godot_collision_object_2d.h create mode 100644 servers/physics_2d/godot_collision_solver_2d.cpp create mode 100644 servers/physics_2d/godot_collision_solver_2d.h create mode 100644 servers/physics_2d/godot_collision_solver_2d_sat.cpp create mode 100644 servers/physics_2d/godot_collision_solver_2d_sat.h create mode 100644 servers/physics_2d/godot_constraint_2d.h create mode 100644 servers/physics_2d/godot_joints_2d.cpp create mode 100644 servers/physics_2d/godot_joints_2d.h create mode 100644 servers/physics_2d/godot_physics_server_2d.cpp create mode 100644 servers/physics_2d/godot_physics_server_2d.h create mode 100644 servers/physics_2d/godot_shape_2d.cpp create mode 100644 servers/physics_2d/godot_shape_2d.h create mode 100644 servers/physics_2d/godot_space_2d.cpp create mode 100644 servers/physics_2d/godot_space_2d.h create mode 100644 servers/physics_2d/godot_step_2d.cpp create mode 100644 servers/physics_2d/godot_step_2d.h delete mode 100644 servers/physics_2d/joints_2d_sw.cpp delete mode 100644 servers/physics_2d/joints_2d_sw.h delete mode 100644 servers/physics_2d/physics_server_2d_sw.cpp delete mode 100644 servers/physics_2d/physics_server_2d_sw.h delete mode 100644 servers/physics_2d/physics_server_2d_wrap_mt.cpp delete mode 100644 servers/physics_2d/physics_server_2d_wrap_mt.h delete mode 100644 servers/physics_2d/shape_2d_sw.cpp delete mode 100644 servers/physics_2d/shape_2d_sw.h delete mode 100644 servers/physics_2d/space_2d_sw.cpp delete mode 100644 servers/physics_2d/space_2d_sw.h delete mode 100644 servers/physics_2d/step_2d_sw.cpp delete mode 100644 servers/physics_2d/step_2d_sw.h (limited to 'servers/physics_2d') diff --git a/servers/physics_2d/area_2d_sw.cpp b/servers/physics_2d/area_2d_sw.cpp deleted file mode 100644 index c85b1575e3..0000000000 --- a/servers/physics_2d/area_2d_sw.cpp +++ /dev/null @@ -1,305 +0,0 @@ -/*************************************************************************/ -/* area_2d_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/*************************************************************************/ - -#include "area_2d_sw.h" -#include "body_2d_sw.h" -#include "space_2d_sw.h" - -Area2DSW::BodyKey::BodyKey(Body2DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { - rid = p_body->get_self(); - instance_id = p_body->get_instance_id(); - body_shape = p_body_shape; - area_shape = p_area_shape; -} - -Area2DSW::BodyKey::BodyKey(Area2DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { - rid = p_body->get_self(); - instance_id = p_body->get_instance_id(); - body_shape = p_body_shape; - area_shape = p_area_shape; -} - -void Area2DSW::_shapes_changed() { - if (!moved_list.in_list() && get_space()) { - get_space()->area_add_to_moved_list(&moved_list); - } -} - -void Area2DSW::set_transform(const Transform2D &p_transform) { - if (!moved_list.in_list() && get_space()) { - get_space()->area_add_to_moved_list(&moved_list); - } - - _set_transform(p_transform); - _set_inv_transform(p_transform.affine_inverse()); -} - -void Area2DSW::set_space(Space2DSW *p_space) { - if (get_space()) { - if (monitor_query_list.in_list()) { - get_space()->area_remove_from_monitor_query_list(&monitor_query_list); - } - if (moved_list.in_list()) { - get_space()->area_remove_from_moved_list(&moved_list); - } - } - - monitored_bodies.clear(); - monitored_areas.clear(); - - _set_space(p_space); -} - -void Area2DSW::set_monitor_callback(ObjectID p_id, const StringName &p_method) { - if (p_id == monitor_callback_id) { - monitor_callback_method = p_method; - return; - } - - _unregister_shapes(); - - monitor_callback_id = p_id; - monitor_callback_method = p_method; - - monitored_bodies.clear(); - monitored_areas.clear(); - - _shape_changed(); - - if (!moved_list.in_list() && get_space()) { - get_space()->area_add_to_moved_list(&moved_list); - } -} - -void Area2DSW::set_area_monitor_callback(ObjectID p_id, const StringName &p_method) { - if (p_id == area_monitor_callback_id) { - area_monitor_callback_method = p_method; - return; - } - - _unregister_shapes(); - - area_monitor_callback_id = p_id; - area_monitor_callback_method = p_method; - - monitored_bodies.clear(); - monitored_areas.clear(); - - _shape_changed(); - - if (!moved_list.in_list() && get_space()) { - get_space()->area_add_to_moved_list(&moved_list); - } -} - -void Area2DSW::set_space_override_mode(PhysicsServer2D::AreaSpaceOverrideMode p_mode) { - bool do_override = p_mode != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED; - if (do_override == (space_override_mode != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED)) { - return; - } - _unregister_shapes(); - space_override_mode = p_mode; - _shape_changed(); -} - -void Area2DSW::set_param(PhysicsServer2D::AreaParameter p_param, const Variant &p_value) { - switch (p_param) { - case PhysicsServer2D::AREA_PARAM_GRAVITY: - gravity = p_value; - break; - case PhysicsServer2D::AREA_PARAM_GRAVITY_VECTOR: - gravity_vector = p_value; - break; - case PhysicsServer2D::AREA_PARAM_GRAVITY_IS_POINT: - gravity_is_point = p_value; - break; - case PhysicsServer2D::AREA_PARAM_GRAVITY_DISTANCE_SCALE: - gravity_distance_scale = p_value; - break; - case PhysicsServer2D::AREA_PARAM_GRAVITY_POINT_ATTENUATION: - point_attenuation = p_value; - break; - case PhysicsServer2D::AREA_PARAM_LINEAR_DAMP: - linear_damp = p_value; - break; - case PhysicsServer2D::AREA_PARAM_ANGULAR_DAMP: - angular_damp = p_value; - break; - case PhysicsServer2D::AREA_PARAM_PRIORITY: - priority = p_value; - break; - } -} - -Variant Area2DSW::get_param(PhysicsServer2D::AreaParameter p_param) const { - switch (p_param) { - case PhysicsServer2D::AREA_PARAM_GRAVITY: - return gravity; - case PhysicsServer2D::AREA_PARAM_GRAVITY_VECTOR: - return gravity_vector; - case PhysicsServer2D::AREA_PARAM_GRAVITY_IS_POINT: - return gravity_is_point; - case PhysicsServer2D::AREA_PARAM_GRAVITY_DISTANCE_SCALE: - return gravity_distance_scale; - case PhysicsServer2D::AREA_PARAM_GRAVITY_POINT_ATTENUATION: - return point_attenuation; - case PhysicsServer2D::AREA_PARAM_LINEAR_DAMP: - return linear_damp; - case PhysicsServer2D::AREA_PARAM_ANGULAR_DAMP: - return angular_damp; - case PhysicsServer2D::AREA_PARAM_PRIORITY: - return priority; - } - - return Variant(); -} - -void Area2DSW::_queue_monitor_update() { - ERR_FAIL_COND(!get_space()); - - if (!monitor_query_list.in_list()) { - get_space()->area_add_to_monitor_query_list(&monitor_query_list); - } -} - -void Area2DSW::set_monitorable(bool p_monitorable) { - if (monitorable == p_monitorable) { - return; - } - - monitorable = p_monitorable; - _set_static(!monitorable); -} - -void Area2DSW::call_queries() { - if (monitor_callback_id.is_valid() && !monitored_bodies.is_empty()) { - Variant res[5]; - Variant *resptr[5]; - for (int i = 0; i < 5; i++) { - resptr[i] = &res[i]; - } - - Object *obj = ObjectDB::get_instance(monitor_callback_id); - if (!obj) { - monitored_bodies.clear(); - monitor_callback_id = ObjectID(); - return; - } - - for (Map::Element *E = monitored_bodies.front(); E;) { - if (E->get().state == 0) { // Nothing happened - Map::Element *next = E->next(); - monitored_bodies.erase(E); - E = next; - continue; - } - - res[0] = E->get().state > 0 ? PhysicsServer2D::AREA_BODY_ADDED : PhysicsServer2D::AREA_BODY_REMOVED; - res[1] = E->key().rid; - res[2] = E->key().instance_id; - res[3] = E->key().body_shape; - res[4] = E->key().area_shape; - - Map::Element *next = E->next(); - monitored_bodies.erase(E); - E = next; - - Callable::CallError ce; - obj->call(monitor_callback_method, (const Variant **)resptr, 5, ce); - } - } - - if (area_monitor_callback_id.is_valid() && !monitored_areas.is_empty()) { - Variant res[5]; - Variant *resptr[5]; - for (int i = 0; i < 5; i++) { - resptr[i] = &res[i]; - } - - Object *obj = ObjectDB::get_instance(area_monitor_callback_id); - if (!obj) { - monitored_areas.clear(); - area_monitor_callback_id = ObjectID(); - return; - } - - for (Map::Element *E = monitored_areas.front(); E;) { - if (E->get().state == 0) { // Nothing happened - Map::Element *next = E->next(); - monitored_areas.erase(E); - E = next; - continue; - } - - res[0] = E->get().state > 0 ? PhysicsServer2D::AREA_BODY_ADDED : PhysicsServer2D::AREA_BODY_REMOVED; - res[1] = E->key().rid; - res[2] = E->key().instance_id; - res[3] = E->key().body_shape; - res[4] = E->key().area_shape; - - Map::Element *next = E->next(); - monitored_areas.erase(E); - E = next; - - Callable::CallError ce; - obj->call(area_monitor_callback_method, (const Variant **)resptr, 5, ce); - } - } -} - -void Area2DSW::compute_gravity(const Vector2 &p_position, Vector2 &r_gravity) const { - if (is_gravity_point()) { - const real_t gravity_distance_scale = get_gravity_distance_scale(); - Vector2 v = get_transform().xform(get_gravity_vector()) - p_position; - if (gravity_distance_scale > 0) { - const real_t v_length = v.length(); - if (v_length > 0) { - const real_t v_scaled = v_length * gravity_distance_scale; - r_gravity = (v.normalized() * (get_gravity() / (v_scaled * v_scaled))); - } else { - r_gravity = Vector2(); - } - } else { - r_gravity = v.normalized() * get_gravity(); - } - } else { - r_gravity = get_gravity_vector() * get_gravity(); - } -} - -Area2DSW::Area2DSW() : - CollisionObject2DSW(TYPE_AREA), - monitor_query_list(this), - moved_list(this) { - _set_static(true); //areas are not active by default -} - -Area2DSW::~Area2DSW() { -} diff --git a/servers/physics_2d/area_2d_sw.h b/servers/physics_2d/area_2d_sw.h deleted file mode 100644 index 0b7c791ed5..0000000000 --- a/servers/physics_2d/area_2d_sw.h +++ /dev/null @@ -1,195 +0,0 @@ -/*************************************************************************/ -/* area_2d_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/*************************************************************************/ - -#ifndef AREA_2D_SW_H -#define AREA_2D_SW_H - -#include "collision_object_2d_sw.h" -#include "core/templates/self_list.h" -#include "servers/physics_server_2d.h" - -class Space2DSW; -class Body2DSW; -class Constraint2DSW; - -class Area2DSW : public CollisionObject2DSW { - PhysicsServer2D::AreaSpaceOverrideMode space_override_mode = PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED; - real_t gravity = 9.80665; - Vector2 gravity_vector = Vector2(0, -1); - bool gravity_is_point = false; - real_t gravity_distance_scale = 0.0; - real_t point_attenuation = 1.0; - real_t linear_damp = 0.1; - real_t angular_damp = 1.0; - int priority = 0; - bool monitorable = false; - - ObjectID monitor_callback_id; - StringName monitor_callback_method; - - ObjectID area_monitor_callback_id; - StringName area_monitor_callback_method; - - SelfList monitor_query_list; - SelfList moved_list; - - struct BodyKey { - RID rid; - ObjectID instance_id; - uint32_t body_shape = 0; - uint32_t area_shape = 0; - - _FORCE_INLINE_ bool operator<(const BodyKey &p_key) const { - if (rid == p_key.rid) { - if (body_shape == p_key.body_shape) { - return area_shape < p_key.area_shape; - } else { - return body_shape < p_key.body_shape; - } - } else { - return rid < p_key.rid; - } - } - - _FORCE_INLINE_ BodyKey() {} - BodyKey(Body2DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape); - BodyKey(Area2DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape); - }; - - struct BodyState { - int state = 0; - _FORCE_INLINE_ void inc() { state++; } - _FORCE_INLINE_ void dec() { state--; } - }; - - Map monitored_bodies; - Map monitored_areas; - - Set constraints; - - virtual void _shapes_changed(); - void _queue_monitor_update(); - -public: - void set_monitor_callback(ObjectID p_id, const StringName &p_method); - _FORCE_INLINE_ bool has_monitor_callback() const { return monitor_callback_id.is_valid(); } - - void set_area_monitor_callback(ObjectID p_id, const StringName &p_method); - _FORCE_INLINE_ bool has_area_monitor_callback() const { return area_monitor_callback_id.is_valid(); } - - _FORCE_INLINE_ void add_body_to_query(Body2DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape); - _FORCE_INLINE_ void remove_body_from_query(Body2DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape); - - _FORCE_INLINE_ void add_area_to_query(Area2DSW *p_area, uint32_t p_area_shape, uint32_t p_self_shape); - _FORCE_INLINE_ void remove_area_from_query(Area2DSW *p_area, uint32_t p_area_shape, uint32_t p_self_shape); - - void set_param(PhysicsServer2D::AreaParameter p_param, const Variant &p_value); - Variant get_param(PhysicsServer2D::AreaParameter p_param) const; - - void set_space_override_mode(PhysicsServer2D::AreaSpaceOverrideMode p_mode); - PhysicsServer2D::AreaSpaceOverrideMode get_space_override_mode() const { return space_override_mode; } - - _FORCE_INLINE_ void set_gravity(real_t p_gravity) { gravity = p_gravity; } - _FORCE_INLINE_ real_t get_gravity() const { return gravity; } - - _FORCE_INLINE_ void set_gravity_vector(const Vector2 &p_gravity) { gravity_vector = p_gravity; } - _FORCE_INLINE_ Vector2 get_gravity_vector() const { return gravity_vector; } - - _FORCE_INLINE_ void set_gravity_as_point(bool p_enable) { gravity_is_point = p_enable; } - _FORCE_INLINE_ bool is_gravity_point() const { return gravity_is_point; } - - _FORCE_INLINE_ void set_gravity_distance_scale(real_t scale) { gravity_distance_scale = scale; } - _FORCE_INLINE_ real_t get_gravity_distance_scale() const { return gravity_distance_scale; } - - _FORCE_INLINE_ void set_point_attenuation(real_t p_point_attenuation) { point_attenuation = p_point_attenuation; } - _FORCE_INLINE_ real_t get_point_attenuation() const { return point_attenuation; } - - _FORCE_INLINE_ void set_linear_damp(real_t p_linear_damp) { linear_damp = p_linear_damp; } - _FORCE_INLINE_ real_t get_linear_damp() const { return linear_damp; } - - _FORCE_INLINE_ void set_angular_damp(real_t p_angular_damp) { angular_damp = p_angular_damp; } - _FORCE_INLINE_ real_t get_angular_damp() const { return angular_damp; } - - _FORCE_INLINE_ void set_priority(int p_priority) { priority = p_priority; } - _FORCE_INLINE_ int get_priority() const { return priority; } - - _FORCE_INLINE_ void add_constraint(Constraint2DSW *p_constraint) { constraints.insert(p_constraint); } - _FORCE_INLINE_ void remove_constraint(Constraint2DSW *p_constraint) { constraints.erase(p_constraint); } - _FORCE_INLINE_ const Set &get_constraints() const { return constraints; } - _FORCE_INLINE_ void clear_constraints() { constraints.clear(); } - - void set_monitorable(bool p_monitorable); - _FORCE_INLINE_ bool is_monitorable() const { return monitorable; } - - void set_transform(const Transform2D &p_transform); - - void set_space(Space2DSW *p_space); - - void call_queries(); - - void compute_gravity(const Vector2 &p_position, Vector2 &r_gravity) const; - - Area2DSW(); - ~Area2DSW(); -}; - -void Area2DSW::add_body_to_query(Body2DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { - BodyKey bk(p_body, p_body_shape, p_area_shape); - monitored_bodies[bk].inc(); - if (!monitor_query_list.in_list()) { - _queue_monitor_update(); - } -} - -void Area2DSW::remove_body_from_query(Body2DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { - BodyKey bk(p_body, p_body_shape, p_area_shape); - monitored_bodies[bk].dec(); - if (!monitor_query_list.in_list()) { - _queue_monitor_update(); - } -} - -void Area2DSW::add_area_to_query(Area2DSW *p_area, uint32_t p_area_shape, uint32_t p_self_shape) { - BodyKey bk(p_area, p_area_shape, p_self_shape); - monitored_areas[bk].inc(); - if (!monitor_query_list.in_list()) { - _queue_monitor_update(); - } -} - -void Area2DSW::remove_area_from_query(Area2DSW *p_area, uint32_t p_area_shape, uint32_t p_self_shape) { - BodyKey bk(p_area, p_area_shape, p_self_shape); - monitored_areas[bk].dec(); - if (!monitor_query_list.in_list()) { - _queue_monitor_update(); - } -} - -#endif // AREA_2D_SW_H diff --git a/servers/physics_2d/area_pair_2d_sw.cpp b/servers/physics_2d/area_pair_2d_sw.cpp deleted file mode 100644 index 4f1148c26f..0000000000 --- a/servers/physics_2d/area_pair_2d_sw.cpp +++ /dev/null @@ -1,190 +0,0 @@ -/*************************************************************************/ -/* area_pair_2d_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/*************************************************************************/ - -#include "area_pair_2d_sw.h" -#include "collision_solver_2d_sw.h" - -bool AreaPair2DSW::setup(real_t p_step) { - bool result = false; - if (area->collides_with(body) && CollisionSolver2DSW::solve(body->get_shape(body_shape), body->get_transform() * body->get_shape_transform(body_shape), Vector2(), area->get_shape(area_shape), area->get_transform() * area->get_shape_transform(area_shape), Vector2(), nullptr, this)) { - result = true; - } - - process_collision = false; - if (result != colliding) { - if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) { - process_collision = true; - } else if (area->has_monitor_callback()) { - process_collision = true; - } - - colliding = result; - } - - return process_collision; -} - -bool AreaPair2DSW::pre_solve(real_t p_step) { - if (!process_collision) { - return false; - } - - if (colliding) { - if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) { - body->add_area(area); - } - - if (area->has_monitor_callback()) { - area->add_body_to_query(body, body_shape, area_shape); - } - } else { - if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) { - body->remove_area(area); - } - - if (area->has_monitor_callback()) { - area->remove_body_from_query(body, body_shape, area_shape); - } - } - - return false; // Never do any post solving. -} - -void AreaPair2DSW::solve(real_t p_step) { - // Nothing to do. -} - -AreaPair2DSW::AreaPair2DSW(Body2DSW *p_body, int p_body_shape, Area2DSW *p_area, int p_area_shape) { - body = p_body; - area = p_area; - body_shape = p_body_shape; - area_shape = p_area_shape; - body->add_constraint(this, 0); - area->add_constraint(this); - if (p_body->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC) { //need to be active to process pair - p_body->set_active(true); - } -} - -AreaPair2DSW::~AreaPair2DSW() { - if (colliding) { - if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) { - body->remove_area(area); - } - if (area->has_monitor_callback()) { - area->remove_body_from_query(body, body_shape, area_shape); - } - } - body->remove_constraint(this, 0); - area->remove_constraint(this); -} - -////////////////////////////////// - -bool Area2Pair2DSW::setup(real_t p_step) { - bool result_a = area_a->collides_with(area_b); - bool result_b = area_b->collides_with(area_a); - if ((result_a || result_b) && !CollisionSolver2DSW::solve(area_a->get_shape(shape_a), area_a->get_transform() * area_a->get_shape_transform(shape_a), Vector2(), area_b->get_shape(shape_b), area_b->get_transform() * area_b->get_shape_transform(shape_b), Vector2(), nullptr, this)) { - result_a = false; - result_b = false; - } - - bool process_collision = false; - - process_collision_a = false; - if (result_a != colliding_a) { - if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) { - process_collision_a = true; - process_collision = true; - } - colliding_a = result_a; - } - - process_collision_b = false; - if (result_b != colliding_b) { - if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) { - process_collision_b = true; - process_collision = true; - } - colliding_b = result_b; - } - - return process_collision; -} - -bool Area2Pair2DSW::pre_solve(real_t p_step) { - if (process_collision_a) { - if (colliding_a) { - area_a->add_area_to_query(area_b, shape_b, shape_a); - } else { - area_a->remove_area_from_query(area_b, shape_b, shape_a); - } - } - - if (process_collision_b) { - if (colliding_b) { - area_b->add_area_to_query(area_a, shape_a, shape_b); - } else { - area_b->remove_area_from_query(area_a, shape_a, shape_b); - } - } - - return false; // Never do any post solving. -} - -void Area2Pair2DSW::solve(real_t p_step) { - // Nothing to do. -} - -Area2Pair2DSW::Area2Pair2DSW(Area2DSW *p_area_a, int p_shape_a, Area2DSW *p_area_b, int p_shape_b) { - area_a = p_area_a; - area_b = p_area_b; - shape_a = p_shape_a; - shape_b = p_shape_b; - area_a->add_constraint(this); - area_b->add_constraint(this); -} - -Area2Pair2DSW::~Area2Pair2DSW() { - if (colliding_a) { - if (area_a->has_area_monitor_callback()) { - area_a->remove_area_from_query(area_b, shape_b, shape_a); - } - } - - if (colliding_b) { - if (area_b->has_area_monitor_callback()) { - area_b->remove_area_from_query(area_a, shape_a, shape_b); - } - } - - area_a->remove_constraint(this); - area_b->remove_constraint(this); -} diff --git a/servers/physics_2d/area_pair_2d_sw.h b/servers/physics_2d/area_pair_2d_sw.h deleted file mode 100644 index 66e9f1afee..0000000000 --- a/servers/physics_2d/area_pair_2d_sw.h +++ /dev/null @@ -1,74 +0,0 @@ -/*************************************************************************/ -/* area_pair_2d_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/*************************************************************************/ - -#ifndef AREA_PAIR_2D_SW_H -#define AREA_PAIR_2D_SW_H - -#include "area_2d_sw.h" -#include "body_2d_sw.h" -#include "constraint_2d_sw.h" - -class AreaPair2DSW : public Constraint2DSW { - Body2DSW *body = nullptr; - Area2DSW *area = nullptr; - int body_shape = 0; - int area_shape = 0; - bool colliding = false; - bool process_collision = false; - -public: - virtual bool setup(real_t p_step) override; - virtual bool pre_solve(real_t p_step) override; - virtual void solve(real_t p_step) override; - - AreaPair2DSW(Body2DSW *p_body, int p_body_shape, Area2DSW *p_area, int p_area_shape); - ~AreaPair2DSW(); -}; - -class Area2Pair2DSW : public Constraint2DSW { - Area2DSW *area_a = nullptr; - Area2DSW *area_b = nullptr; - int shape_a = 0; - int shape_b = 0; - bool colliding_a = false; - bool colliding_b = false; - bool process_collision_a = false; - bool process_collision_b = false; - -public: - virtual bool setup(real_t p_step) override; - virtual bool pre_solve(real_t p_step) override; - virtual void solve(real_t p_step) override; - - Area2Pair2DSW(Area2DSW *p_area_a, int p_shape_a, Area2DSW *p_area_b, int p_shape_b); - ~Area2Pair2DSW(); -}; - -#endif // AREA_PAIR_2D_SW_H diff --git a/servers/physics_2d/body_2d_sw.cpp b/servers/physics_2d/body_2d_sw.cpp deleted file mode 100644 index 38b98b7bca..0000000000 --- a/servers/physics_2d/body_2d_sw.cpp +++ /dev/null @@ -1,677 +0,0 @@ -/*************************************************************************/ -/* body_2d_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/*************************************************************************/ - -#include "body_2d_sw.h" - -#include "area_2d_sw.h" -#include "body_direct_state_2d_sw.h" -#include "space_2d_sw.h" - -void Body2DSW::_mass_properties_changed() { - if (get_space() && !mass_properties_update_list.in_list() && (calculate_inertia || calculate_center_of_mass)) { - get_space()->body_add_to_mass_properties_update_list(&mass_properties_update_list); - } -} - -void Body2DSW::update_mass_properties() { - //update shapes and motions - - switch (mode) { - case PhysicsServer2D::BODY_MODE_DYNAMIC: { - real_t total_area = 0; - for (int i = 0; i < get_shape_count(); i++) { - if (is_shape_disabled(i)) { - continue; - } - total_area += get_shape_aabb(i).get_area(); - } - - if (calculate_center_of_mass) { - // We have to recompute the center of mass. - center_of_mass = Vector2(); - - if (total_area != 0.0) { - for (int i = 0; i < get_shape_count(); i++) { - if (is_shape_disabled(i)) { - continue; - } - - real_t area = get_shape_aabb(i).get_area(); - - real_t mass = area * this->mass / total_area; - - // NOTE: we assume that the shape origin is also its center of mass. - center_of_mass += mass * get_shape_transform(i).get_origin(); - } - - center_of_mass /= mass; - } - } - - if (calculate_inertia) { - inertia = 0; - - for (int i = 0; i < get_shape_count(); i++) { - if (is_shape_disabled(i)) { - continue; - } - - const Shape2DSW *shape = get_shape(i); - - real_t area = get_shape_aabb(i).get_area(); - if (area == 0.0) { - continue; - } - - real_t mass = area * this->mass / total_area; - - Transform2D mtx = get_shape_transform(i); - Vector2 scale = mtx.get_scale(); - Vector2 shape_origin = mtx.get_origin() - center_of_mass; - inertia += shape->get_moment_of_inertia(mass, scale) + mass * shape_origin.length_squared(); - } - } - - _inv_inertia = inertia > 0.0 ? (1.0 / inertia) : 0.0; - - if (mass) { - _inv_mass = 1.0 / mass; - } else { - _inv_mass = 0; - } - - } break; - case PhysicsServer2D::BODY_MODE_KINEMATIC: - case PhysicsServer2D::BODY_MODE_STATIC: { - _inv_inertia = 0; - _inv_mass = 0; - } break; - case PhysicsServer2D::BODY_MODE_DYNAMIC_LINEAR: { - _inv_inertia = 0; - _inv_mass = 1.0 / mass; - - } break; - } -} - -void Body2DSW::reset_mass_properties() { - calculate_inertia = true; - calculate_center_of_mass = true; - _mass_properties_changed(); -} - -void Body2DSW::set_active(bool p_active) { - if (active == p_active) { - return; - } - - active = p_active; - - if (active) { - if (mode == PhysicsServer2D::BODY_MODE_STATIC) { - // Static bodies can't be active. - active = false; - } else if (get_space()) { - get_space()->body_add_to_active_list(&active_list); - } - } else if (get_space()) { - get_space()->body_remove_from_active_list(&active_list); - } -} - -void Body2DSW::set_param(PhysicsServer2D::BodyParameter p_param, const Variant &p_value) { - switch (p_param) { - case PhysicsServer2D::BODY_PARAM_BOUNCE: { - bounce = p_value; - } break; - case PhysicsServer2D::BODY_PARAM_FRICTION: { - friction = p_value; - } break; - case PhysicsServer2D::BODY_PARAM_MASS: { - real_t mass_value = p_value; - ERR_FAIL_COND(mass_value <= 0); - mass = mass_value; - if (mode >= PhysicsServer2D::BODY_MODE_DYNAMIC) { - _mass_properties_changed(); - } - } break; - case PhysicsServer2D::BODY_PARAM_INERTIA: { - real_t inertia_value = p_value; - if (inertia_value <= 0.0) { - calculate_inertia = true; - if (mode == PhysicsServer2D::BODY_MODE_DYNAMIC) { - _mass_properties_changed(); - } - } else { - calculate_inertia = false; - inertia = inertia_value; - if (mode == PhysicsServer2D::BODY_MODE_DYNAMIC) { - _inv_inertia = 1.0 / inertia; - } - } - } break; - case PhysicsServer2D::BODY_PARAM_CENTER_OF_MASS: { - calculate_center_of_mass = false; - center_of_mass = p_value; - } break; - case PhysicsServer2D::BODY_PARAM_GRAVITY_SCALE: { - gravity_scale = p_value; - } break; - case PhysicsServer2D::BODY_PARAM_LINEAR_DAMP: { - linear_damp = p_value; - } break; - case PhysicsServer2D::BODY_PARAM_ANGULAR_DAMP: { - angular_damp = p_value; - } break; - default: { - } - } -} - -Variant Body2DSW::get_param(PhysicsServer2D::BodyParameter p_param) const { - switch (p_param) { - case PhysicsServer2D::BODY_PARAM_BOUNCE: { - return bounce; - } - case PhysicsServer2D::BODY_PARAM_FRICTION: { - return friction; - } - case PhysicsServer2D::BODY_PARAM_MASS: { - return mass; - } - case PhysicsServer2D::BODY_PARAM_INERTIA: { - return inertia; - } - case PhysicsServer2D::BODY_PARAM_CENTER_OF_MASS: { - return center_of_mass; - } - case PhysicsServer2D::BODY_PARAM_GRAVITY_SCALE: { - return gravity_scale; - } - case PhysicsServer2D::BODY_PARAM_LINEAR_DAMP: { - return linear_damp; - } - case PhysicsServer2D::BODY_PARAM_ANGULAR_DAMP: { - return angular_damp; - } - default: { - } - } - - return 0; -} - -void Body2DSW::set_mode(PhysicsServer2D::BodyMode p_mode) { - PhysicsServer2D::BodyMode prev = mode; - mode = p_mode; - - switch (p_mode) { - //CLEAR UP EVERYTHING IN CASE IT NOT WORKS! - case PhysicsServer2D::BODY_MODE_STATIC: - case PhysicsServer2D::BODY_MODE_KINEMATIC: { - _set_inv_transform(get_transform().affine_inverse()); - _inv_mass = 0; - _inv_inertia = 0; - _set_static(p_mode == PhysicsServer2D::BODY_MODE_STATIC); - set_active(p_mode == PhysicsServer2D::BODY_MODE_KINEMATIC && contacts.size()); - linear_velocity = Vector2(); - angular_velocity = 0; - if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC && prev != mode) { - first_time_kinematic = true; - } - } break; - case PhysicsServer2D::BODY_MODE_DYNAMIC: { - _inv_mass = mass > 0 ? (1.0 / mass) : 0; - if (!calculate_inertia) { - _inv_inertia = 1.0 / inertia; - } - _mass_properties_changed(); - _set_static(false); - set_active(true); - - } break; - case PhysicsServer2D::BODY_MODE_DYNAMIC_LINEAR: { - _inv_mass = mass > 0 ? (1.0 / mass) : 0; - _inv_inertia = 0; - angular_velocity = 0; - _set_static(false); - set_active(true); - } - } -} - -PhysicsServer2D::BodyMode Body2DSW::get_mode() const { - return mode; -} - -void Body2DSW::_shapes_changed() { - _mass_properties_changed(); - wakeup_neighbours(); -} - -void Body2DSW::set_state(PhysicsServer2D::BodyState p_state, const Variant &p_variant) { - switch (p_state) { - case PhysicsServer2D::BODY_STATE_TRANSFORM: { - if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC) { - new_transform = p_variant; - //wakeup_neighbours(); - set_active(true); - if (first_time_kinematic) { - _set_transform(p_variant); - _set_inv_transform(get_transform().affine_inverse()); - first_time_kinematic = false; - } - } else if (mode == PhysicsServer2D::BODY_MODE_STATIC) { - _set_transform(p_variant); - _set_inv_transform(get_transform().affine_inverse()); - wakeup_neighbours(); - } else { - Transform2D t = p_variant; - t.orthonormalize(); - new_transform = get_transform(); //used as old to compute motion - if (t == new_transform) { - break; - } - _set_transform(t); - _set_inv_transform(get_transform().inverse()); - } - wakeup(); - - } break; - case PhysicsServer2D::BODY_STATE_LINEAR_VELOCITY: { - linear_velocity = p_variant; - constant_linear_velocity = linear_velocity; - wakeup(); - - } break; - case PhysicsServer2D::BODY_STATE_ANGULAR_VELOCITY: { - angular_velocity = p_variant; - constant_angular_velocity = angular_velocity; - wakeup(); - - } break; - case PhysicsServer2D::BODY_STATE_SLEEPING: { - if (mode == PhysicsServer2D::BODY_MODE_STATIC || mode == PhysicsServer2D::BODY_MODE_KINEMATIC) { - break; - } - bool do_sleep = p_variant; - if (do_sleep) { - linear_velocity = Vector2(); - //biased_linear_velocity=Vector3(); - angular_velocity = 0; - //biased_angular_velocity=Vector3(); - set_active(false); - } else { - if (mode != PhysicsServer2D::BODY_MODE_STATIC) { - set_active(true); - } - } - } break; - case PhysicsServer2D::BODY_STATE_CAN_SLEEP: { - can_sleep = p_variant; - if (mode >= PhysicsServer2D::BODY_MODE_DYNAMIC && !active && !can_sleep) { - set_active(true); - } - - } break; - } -} - -Variant Body2DSW::get_state(PhysicsServer2D::BodyState p_state) const { - switch (p_state) { - case PhysicsServer2D::BODY_STATE_TRANSFORM: { - return get_transform(); - } - case PhysicsServer2D::BODY_STATE_LINEAR_VELOCITY: { - return linear_velocity; - } - case PhysicsServer2D::BODY_STATE_ANGULAR_VELOCITY: { - return angular_velocity; - } - case PhysicsServer2D::BODY_STATE_SLEEPING: { - return !is_active(); - } - case PhysicsServer2D::BODY_STATE_CAN_SLEEP: { - return can_sleep; - } - } - - return Variant(); -} - -void Body2DSW::set_space(Space2DSW *p_space) { - if (get_space()) { - wakeup_neighbours(); - - if (mass_properties_update_list.in_list()) { - get_space()->body_remove_from_mass_properties_update_list(&mass_properties_update_list); - } - if (active_list.in_list()) { - get_space()->body_remove_from_active_list(&active_list); - } - if (direct_state_query_list.in_list()) { - get_space()->body_remove_from_state_query_list(&direct_state_query_list); - } - } - - _set_space(p_space); - - if (get_space()) { - _mass_properties_changed(); - if (active) { - get_space()->body_add_to_active_list(&active_list); - } - } -} - -void Body2DSW::_compute_area_gravity_and_damping(const Area2DSW *p_area) { - Vector2 area_gravity; - p_area->compute_gravity(get_transform().get_origin(), area_gravity); - gravity += area_gravity; - - area_linear_damp += p_area->get_linear_damp(); - area_angular_damp += p_area->get_angular_damp(); -} - -void Body2DSW::integrate_forces(real_t p_step) { - if (mode == PhysicsServer2D::BODY_MODE_STATIC) { - return; - } - - Area2DSW *def_area = get_space()->get_default_area(); - // Area2DSW *damp_area = def_area; - ERR_FAIL_COND(!def_area); - - int ac = areas.size(); - bool stopped = false; - gravity = Vector2(0, 0); - area_angular_damp = 0; - area_linear_damp = 0; - if (ac) { - areas.sort(); - const AreaCMP *aa = &areas[0]; - // damp_area = aa[ac-1].area; - for (int i = ac - 1; i >= 0 && !stopped; i--) { - PhysicsServer2D::AreaSpaceOverrideMode mode = aa[i].area->get_space_override_mode(); - switch (mode) { - case PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE: - case PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: { - _compute_area_gravity_and_damping(aa[i].area); - stopped = mode == PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE; - } break; - case PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE: - case PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: { - gravity = Vector2(0, 0); - area_angular_damp = 0; - area_linear_damp = 0; - _compute_area_gravity_and_damping(aa[i].area); - stopped = mode == PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE; - } break; - default: { - } - } - } - } - if (!stopped) { - _compute_area_gravity_and_damping(def_area); - } - gravity *= gravity_scale; - - // If less than 0, override dampenings with that of the Body2D - if (angular_damp >= 0) { - area_angular_damp = angular_damp; - } - /* - else - area_angular_damp=damp_area->get_angular_damp(); - */ - - if (linear_damp >= 0) { - area_linear_damp = linear_damp; - } - /* - else - area_linear_damp=damp_area->get_linear_damp(); - */ - - Vector2 motion; - bool do_motion = false; - - if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC) { - //compute motion, angular and etc. velocities from prev transform - motion = new_transform.get_origin() - get_transform().get_origin(); - linear_velocity = constant_linear_velocity + motion / p_step; - - real_t rot = new_transform.get_rotation() - get_transform().get_rotation(); - angular_velocity = constant_angular_velocity + remainder(rot, 2.0 * Math_PI) / p_step; - - do_motion = true; - - /* - for(int i=0;ibody_add_to_state_query_list(&direct_state_query_list); - } - - if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC) { - _set_transform(new_transform, false); - _set_inv_transform(new_transform.affine_inverse()); - if (contacts.size() == 0 && linear_velocity == Vector2() && angular_velocity == 0) { - set_active(false); //stopped moving, deactivate - } - return; - } - - real_t total_angular_velocity = angular_velocity + biased_angular_velocity; - Vector2 total_linear_velocity = linear_velocity + biased_linear_velocity; - - real_t angle = get_transform().get_rotation() + total_angular_velocity * p_step; - Vector2 pos = get_transform().get_origin() + total_linear_velocity * p_step; - - real_t center_of_mass_distance = center_of_mass.length(); - if (center_of_mass_distance > CMP_EPSILON) { - // Calculate displacement due to center of mass offset. - real_t prev_angle = get_transform().get_rotation(); - real_t angle_base = Math::atan2(center_of_mass.y, center_of_mass.x); - Vector2 point1(Math::cos(angle_base + prev_angle), Math::sin(angle_base + prev_angle)); - Vector2 point2(Math::cos(angle_base + angle), Math::sin(angle_base + angle)); - pos += center_of_mass_distance * (point1 - point2); - } - - _set_transform(Transform2D(angle, pos), continuous_cd_mode == PhysicsServer2D::CCD_MODE_DISABLED); - _set_inv_transform(get_transform().inverse()); - - if (continuous_cd_mode != PhysicsServer2D::CCD_MODE_DISABLED) { - new_transform = get_transform(); - } -} - -void Body2DSW::wakeup_neighbours() { - for (const Pair &E : constraint_list) { - const Constraint2DSW *c = E.first; - Body2DSW **n = c->get_body_ptr(); - int bc = c->get_body_count(); - - for (int i = 0; i < bc; i++) { - if (i == E.second) { - continue; - } - Body2DSW *b = n[i]; - if (b->mode < PhysicsServer2D::BODY_MODE_DYNAMIC) { - continue; - } - - if (!b->is_active()) { - b->set_active(true); - } - } - } -} - -void Body2DSW::call_queries() { - if (fi_callback_data) { - if (!fi_callback_data->callable.get_object()) { - set_force_integration_callback(Callable()); - } else { - Variant direct_state_variant = get_direct_state(); - const Variant *vp[2] = { &direct_state_variant, &fi_callback_data->udata }; - - Callable::CallError ce; - Variant rv; - if (fi_callback_data->udata.get_type() != Variant::NIL) { - fi_callback_data->callable.call(vp, 2, rv, ce); - - } else { - fi_callback_data->callable.call(vp, 1, rv, ce); - } - } - } - - if (body_state_callback) { - (body_state_callback)(body_state_callback_instance, get_direct_state()); - } -} - -bool Body2DSW::sleep_test(real_t p_step) { - if (mode == PhysicsServer2D::BODY_MODE_STATIC || mode == PhysicsServer2D::BODY_MODE_KINEMATIC) { - return true; - } else if (!can_sleep) { - return false; - } - - if (Math::abs(angular_velocity) < get_space()->get_body_angular_velocity_sleep_threshold() && Math::abs(linear_velocity.length_squared()) < get_space()->get_body_linear_velocity_sleep_threshold() * get_space()->get_body_linear_velocity_sleep_threshold()) { - still_time += p_step; - - return still_time > get_space()->get_body_time_to_sleep(); - } else { - still_time = 0; //maybe this should be set to 0 on set_active? - return false; - } -} - -void Body2DSW::set_state_sync_callback(void *p_instance, PhysicsServer2D::BodyStateCallback p_callback) { - body_state_callback_instance = p_instance; - body_state_callback = p_callback; -} - -void Body2DSW::set_force_integration_callback(const Callable &p_callable, const Variant &p_udata) { - if (p_callable.get_object()) { - if (!fi_callback_data) { - fi_callback_data = memnew(ForceIntegrationCallbackData); - } - fi_callback_data->callable = p_callable; - fi_callback_data->udata = p_udata; - } else if (fi_callback_data) { - memdelete(fi_callback_data); - fi_callback_data = nullptr; - } -} - -PhysicsDirectBodyState2DSW *Body2DSW::get_direct_state() { - if (!direct_state) { - direct_state = memnew(PhysicsDirectBodyState2DSW); - direct_state->body = this; - } - return direct_state; -} - -Body2DSW::Body2DSW() : - CollisionObject2DSW(TYPE_BODY), - active_list(this), - mass_properties_update_list(this), - direct_state_query_list(this) { - _set_static(false); -} - -Body2DSW::~Body2DSW() { - if (fi_callback_data) { - memdelete(fi_callback_data); - } - if (direct_state) { - memdelete(direct_state); - } -} diff --git a/servers/physics_2d/body_2d_sw.h b/servers/physics_2d/body_2d_sw.h deleted file mode 100644 index 822ff76fae..0000000000 --- a/servers/physics_2d/body_2d_sw.h +++ /dev/null @@ -1,353 +0,0 @@ -/*************************************************************************/ -/* body_2d_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/*************************************************************************/ - -#ifndef BODY_2D_SW_H -#define BODY_2D_SW_H - -#include "area_2d_sw.h" -#include "collision_object_2d_sw.h" -#include "core/templates/list.h" -#include "core/templates/pair.h" -#include "core/templates/vset.h" - -class Constraint2DSW; -class PhysicsDirectBodyState2DSW; - -class Body2DSW : public CollisionObject2DSW { - PhysicsServer2D::BodyMode mode = PhysicsServer2D::BODY_MODE_DYNAMIC; - - Vector2 biased_linear_velocity; - real_t biased_angular_velocity = 0.0; - - Vector2 linear_velocity; - real_t angular_velocity = 0.0; - - Vector2 constant_linear_velocity; - real_t constant_angular_velocity = 0.0; - - real_t linear_damp = -1.0; - real_t angular_damp = -1.0; - real_t gravity_scale = 1.0; - - real_t bounce = 0.0; - real_t friction = 1.0; - - real_t mass = 1.0; - real_t _inv_mass = 1.0; - - real_t inertia = 0.0; - real_t _inv_inertia = 0.0; - - Vector2 center_of_mass; - - bool calculate_inertia = true; - bool calculate_center_of_mass = true; - - Vector2 gravity; - real_t area_linear_damp = 0.0; - real_t area_angular_damp = 0.0; - - real_t still_time = 0.0; - - Vector2 applied_force; - real_t applied_torque = 0.0; - - SelfList active_list; - SelfList mass_properties_update_list; - SelfList direct_state_query_list; - - VSet exceptions; - PhysicsServer2D::CCDMode continuous_cd_mode = PhysicsServer2D::CCD_MODE_DISABLED; - bool omit_force_integration = false; - bool active = true; - bool can_sleep = true; - bool first_time_kinematic = false; - void _mass_properties_changed(); - virtual void _shapes_changed(); - Transform2D new_transform; - - List> constraint_list; - - struct AreaCMP { - Area2DSW *area = nullptr; - int refCount = 0; - _FORCE_INLINE_ bool operator==(const AreaCMP &p_cmp) const { return area->get_self() == p_cmp.area->get_self(); } - _FORCE_INLINE_ bool operator<(const AreaCMP &p_cmp) const { return area->get_priority() < p_cmp.area->get_priority(); } - _FORCE_INLINE_ AreaCMP() {} - _FORCE_INLINE_ AreaCMP(Area2DSW *p_area) { - area = p_area; - refCount = 1; - } - }; - - Vector areas; - - struct Contact { - Vector2 local_pos; - Vector2 local_normal; - real_t depth = 0.0; - int local_shape = 0; - Vector2 collider_pos; - int collider_shape = 0; - ObjectID collider_instance_id; - RID collider; - Vector2 collider_velocity_at_pos; - }; - - Vector contacts; //no contacts by default - int contact_count = 0; - - void *body_state_callback_instance = nullptr; - PhysicsServer2D::BodyStateCallback body_state_callback = nullptr; - - struct ForceIntegrationCallbackData { - Callable callable; - Variant udata; - }; - - ForceIntegrationCallbackData *fi_callback_data = nullptr; - - PhysicsDirectBodyState2DSW *direct_state = nullptr; - - uint64_t island_step = 0; - - _FORCE_INLINE_ void _compute_area_gravity_and_damping(const Area2DSW *p_area); - - friend class PhysicsDirectBodyState2DSW; // i give up, too many functions to expose - -public: - void set_state_sync_callback(void *p_instance, PhysicsServer2D::BodyStateCallback p_callback); - void set_force_integration_callback(const Callable &p_callable, const Variant &p_udata = Variant()); - - PhysicsDirectBodyState2DSW *get_direct_state(); - - _FORCE_INLINE_ void add_area(Area2DSW *p_area) { - int index = areas.find(AreaCMP(p_area)); - if (index > -1) { - areas.write[index].refCount += 1; - } else { - areas.ordered_insert(AreaCMP(p_area)); - } - } - - _FORCE_INLINE_ void remove_area(Area2DSW *p_area) { - int index = areas.find(AreaCMP(p_area)); - if (index > -1) { - areas.write[index].refCount -= 1; - if (areas[index].refCount < 1) { - areas.remove(index); - } - } - } - - _FORCE_INLINE_ void set_max_contacts_reported(int p_size) { - contacts.resize(p_size); - contact_count = 0; - if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC && p_size) { - set_active(true); - } - } - - _FORCE_INLINE_ int get_max_contacts_reported() const { return contacts.size(); } - - _FORCE_INLINE_ bool can_report_contacts() const { return !contacts.is_empty(); } - _FORCE_INLINE_ void add_contact(const Vector2 &p_local_pos, const Vector2 &p_local_normal, real_t p_depth, int p_local_shape, const Vector2 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector2 &p_collider_velocity_at_pos); - - _FORCE_INLINE_ void add_exception(const RID &p_exception) { exceptions.insert(p_exception); } - _FORCE_INLINE_ void remove_exception(const RID &p_exception) { exceptions.erase(p_exception); } - _FORCE_INLINE_ bool has_exception(const RID &p_exception) const { return exceptions.has(p_exception); } - _FORCE_INLINE_ const VSet &get_exceptions() const { return exceptions; } - - _FORCE_INLINE_ uint64_t get_island_step() const { return island_step; } - _FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step = p_step; } - - _FORCE_INLINE_ void add_constraint(Constraint2DSW *p_constraint, int p_pos) { constraint_list.push_back({ p_constraint, p_pos }); } - _FORCE_INLINE_ void remove_constraint(Constraint2DSW *p_constraint, int p_pos) { constraint_list.erase({ p_constraint, p_pos }); } - const List> &get_constraint_list() const { return constraint_list; } - _FORCE_INLINE_ void clear_constraint_list() { constraint_list.clear(); } - - _FORCE_INLINE_ void set_omit_force_integration(bool p_omit_force_integration) { omit_force_integration = p_omit_force_integration; } - _FORCE_INLINE_ bool get_omit_force_integration() const { return omit_force_integration; } - - _FORCE_INLINE_ void set_linear_velocity(const Vector2 &p_velocity) { linear_velocity = p_velocity; } - _FORCE_INLINE_ Vector2 get_linear_velocity() const { return linear_velocity; } - - _FORCE_INLINE_ void set_angular_velocity(real_t p_velocity) { angular_velocity = p_velocity; } - _FORCE_INLINE_ real_t get_angular_velocity() const { return angular_velocity; } - - _FORCE_INLINE_ void set_biased_linear_velocity(const Vector2 &p_velocity) { biased_linear_velocity = p_velocity; } - _FORCE_INLINE_ Vector2 get_biased_linear_velocity() const { return biased_linear_velocity; } - - _FORCE_INLINE_ void set_biased_angular_velocity(real_t p_velocity) { biased_angular_velocity = p_velocity; } - _FORCE_INLINE_ real_t get_biased_angular_velocity() const { return biased_angular_velocity; } - - _FORCE_INLINE_ void apply_central_impulse(const Vector2 &p_impulse) { - linear_velocity += p_impulse * _inv_mass; - } - - _FORCE_INLINE_ void apply_impulse(const Vector2 &p_impulse, const Vector2 &p_position = Vector2()) { - linear_velocity += p_impulse * _inv_mass; - angular_velocity += _inv_inertia * (p_position - center_of_mass).cross(p_impulse); - } - - _FORCE_INLINE_ void apply_torque_impulse(real_t p_torque) { - angular_velocity += _inv_inertia * p_torque; - } - - _FORCE_INLINE_ void apply_bias_impulse(const Vector2 &p_impulse, const Vector2 &p_position = Vector2()) { - biased_linear_velocity += p_impulse * _inv_mass; - biased_angular_velocity += _inv_inertia * (p_position - center_of_mass).cross(p_impulse); - } - - void set_active(bool p_active); - _FORCE_INLINE_ bool is_active() const { return active; } - - _FORCE_INLINE_ void wakeup() { - if ((!get_space()) || mode == PhysicsServer2D::BODY_MODE_STATIC || mode == PhysicsServer2D::BODY_MODE_KINEMATIC) { - return; - } - set_active(true); - } - - void set_param(PhysicsServer2D::BodyParameter p_param, const Variant &p_value); - Variant get_param(PhysicsServer2D::BodyParameter p_param) const; - - void set_mode(PhysicsServer2D::BodyMode p_mode); - PhysicsServer2D::BodyMode get_mode() const; - - void set_state(PhysicsServer2D::BodyState p_state, const Variant &p_variant); - Variant get_state(PhysicsServer2D::BodyState p_state) const; - - void set_applied_force(const Vector2 &p_force) { applied_force = p_force; } - Vector2 get_applied_force() const { return applied_force; } - - void set_applied_torque(real_t p_torque) { applied_torque = p_torque; } - real_t get_applied_torque() const { return applied_torque; } - - _FORCE_INLINE_ void add_central_force(const Vector2 &p_force) { - applied_force += p_force; - } - - _FORCE_INLINE_ void add_force(const Vector2 &p_force, const Vector2 &p_position = Vector2()) { - applied_force += p_force; - applied_torque += (p_position - center_of_mass).cross(p_force); - } - - _FORCE_INLINE_ void add_torque(real_t p_torque) { - applied_torque += p_torque; - } - - _FORCE_INLINE_ void set_continuous_collision_detection_mode(PhysicsServer2D::CCDMode p_mode) { continuous_cd_mode = p_mode; } - _FORCE_INLINE_ PhysicsServer2D::CCDMode get_continuous_collision_detection_mode() const { return continuous_cd_mode; } - - void set_space(Space2DSW *p_space); - - void update_mass_properties(); - void reset_mass_properties(); - - _FORCE_INLINE_ Vector2 get_center_of_mass() const { return center_of_mass; } - _FORCE_INLINE_ real_t get_inv_mass() const { return _inv_mass; } - _FORCE_INLINE_ real_t get_inv_inertia() const { return _inv_inertia; } - _FORCE_INLINE_ real_t get_friction() const { return friction; } - _FORCE_INLINE_ Vector2 get_gravity() const { return gravity; } - _FORCE_INLINE_ real_t get_bounce() const { return bounce; } - _FORCE_INLINE_ real_t get_linear_damp() const { return linear_damp; } - _FORCE_INLINE_ real_t get_angular_damp() const { return angular_damp; } - - void integrate_forces(real_t p_step); - void integrate_velocities(real_t p_step); - - _FORCE_INLINE_ Vector2 get_velocity_in_local_point(const Vector2 &rel_pos) const { - return linear_velocity + Vector2(-angular_velocity * rel_pos.y, angular_velocity * rel_pos.x); - } - - _FORCE_INLINE_ Vector2 get_motion() const { - if (mode > PhysicsServer2D::BODY_MODE_KINEMATIC) { - return new_transform.get_origin() - get_transform().get_origin(); - } else if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC) { - return get_transform().get_origin() - new_transform.get_origin(); //kinematic simulates forward - } - return Vector2(); - } - - void call_queries(); - void wakeup_neighbours(); - - bool sleep_test(real_t p_step); - - Body2DSW(); - ~Body2DSW(); -}; - -//add contact inline - -void Body2DSW::add_contact(const Vector2 &p_local_pos, const Vector2 &p_local_normal, real_t p_depth, int p_local_shape, const Vector2 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector2 &p_collider_velocity_at_pos) { - int c_max = contacts.size(); - - if (c_max == 0) { - return; - } - - Contact *c = contacts.ptrw(); - - int idx = -1; - - if (contact_count < c_max) { - idx = contact_count++; - } else { - real_t least_depth = 1e20; - int least_deep = -1; - for (int i = 0; i < c_max; i++) { - if (i == 0 || c[i].depth < least_depth) { - least_deep = i; - least_depth = c[i].depth; - } - } - - if (least_deep >= 0 && least_depth < p_depth) { - idx = least_deep; - } - if (idx == -1) { - return; //none least deepe than this - } - } - - c[idx].local_pos = p_local_pos; - c[idx].local_normal = p_local_normal; - c[idx].depth = p_depth; - c[idx].local_shape = p_local_shape; - c[idx].collider_pos = p_collider_pos; - c[idx].collider_shape = p_collider_shape; - c[idx].collider_instance_id = p_collider_instance_id; - c[idx].collider = p_collider; - c[idx].collider_velocity_at_pos = p_collider_velocity_at_pos; -} - -#endif // BODY_2D_SW_H diff --git a/servers/physics_2d/body_direct_state_2d_sw.cpp b/servers/physics_2d/body_direct_state_2d_sw.cpp deleted file mode 100644 index b0673b9006..0000000000 --- a/servers/physics_2d/body_direct_state_2d_sw.cpp +++ /dev/null @@ -1,178 +0,0 @@ -/*************************************************************************/ -/* body_direct_state_2d_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/*************************************************************************/ - -#include "body_direct_state_2d_sw.h" - -#include "body_2d_sw.h" -#include "physics_server_2d_sw.h" -#include "space_2d_sw.h" - -Vector2 PhysicsDirectBodyState2DSW::get_total_gravity() const { - return body->gravity; -} - -real_t PhysicsDirectBodyState2DSW::get_total_angular_damp() const { - return body->area_angular_damp; -} - -real_t PhysicsDirectBodyState2DSW::get_total_linear_damp() const { - return body->area_linear_damp; -} - -Vector2 PhysicsDirectBodyState2DSW::get_center_of_mass() const { - return body->get_center_of_mass(); -} - -real_t PhysicsDirectBodyState2DSW::get_inverse_mass() const { - return body->get_inv_mass(); -} - -real_t PhysicsDirectBodyState2DSW::get_inverse_inertia() const { - return body->get_inv_inertia(); -} - -void PhysicsDirectBodyState2DSW::set_linear_velocity(const Vector2 &p_velocity) { - body->wakeup(); - body->set_linear_velocity(p_velocity); -} - -Vector2 PhysicsDirectBodyState2DSW::get_linear_velocity() const { - return body->get_linear_velocity(); -} - -void PhysicsDirectBodyState2DSW::set_angular_velocity(real_t p_velocity) { - body->wakeup(); - body->set_angular_velocity(p_velocity); -} - -real_t PhysicsDirectBodyState2DSW::get_angular_velocity() const { - return body->get_angular_velocity(); -} - -void PhysicsDirectBodyState2DSW::set_transform(const Transform2D &p_transform) { - body->set_state(PhysicsServer2D::BODY_STATE_TRANSFORM, p_transform); -} - -Transform2D PhysicsDirectBodyState2DSW::get_transform() const { - return body->get_transform(); -} - -Vector2 PhysicsDirectBodyState2DSW::get_velocity_at_local_position(const Vector2 &p_position) const { - return body->get_velocity_in_local_point(p_position); -} - -void PhysicsDirectBodyState2DSW::add_central_force(const Vector2 &p_force) { - body->wakeup(); - body->add_central_force(p_force); -} - -void PhysicsDirectBodyState2DSW::add_force(const Vector2 &p_force, const Vector2 &p_position) { - body->wakeup(); - body->add_force(p_force, p_position); -} - -void PhysicsDirectBodyState2DSW::add_torque(real_t p_torque) { - body->wakeup(); - body->add_torque(p_torque); -} - -void PhysicsDirectBodyState2DSW::apply_central_impulse(const Vector2 &p_impulse) { - body->wakeup(); - body->apply_central_impulse(p_impulse); -} - -void PhysicsDirectBodyState2DSW::apply_impulse(const Vector2 &p_impulse, const Vector2 &p_position) { - body->wakeup(); - body->apply_impulse(p_impulse, p_position); -} - -void PhysicsDirectBodyState2DSW::apply_torque_impulse(real_t p_torque) { - body->wakeup(); - body->apply_torque_impulse(p_torque); -} - -void PhysicsDirectBodyState2DSW::set_sleep_state(bool p_enable) { - body->set_active(!p_enable); -} - -bool PhysicsDirectBodyState2DSW::is_sleeping() const { - return !body->is_active(); -} - -int PhysicsDirectBodyState2DSW::get_contact_count() const { - return body->contact_count; -} - -Vector2 PhysicsDirectBodyState2DSW::get_contact_local_position(int p_contact_idx) const { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2()); - return body->contacts[p_contact_idx].local_pos; -} - -Vector2 PhysicsDirectBodyState2DSW::get_contact_local_normal(int p_contact_idx) const { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2()); - return body->contacts[p_contact_idx].local_normal; -} - -int PhysicsDirectBodyState2DSW::get_contact_local_shape(int p_contact_idx) const { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, -1); - return body->contacts[p_contact_idx].local_shape; -} - -RID PhysicsDirectBodyState2DSW::get_contact_collider(int p_contact_idx) const { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, RID()); - return body->contacts[p_contact_idx].collider; -} -Vector2 PhysicsDirectBodyState2DSW::get_contact_collider_position(int p_contact_idx) const { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2()); - return body->contacts[p_contact_idx].collider_pos; -} - -ObjectID PhysicsDirectBodyState2DSW::get_contact_collider_id(int p_contact_idx) const { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, ObjectID()); - return body->contacts[p_contact_idx].collider_instance_id; -} - -int PhysicsDirectBodyState2DSW::get_contact_collider_shape(int p_contact_idx) const { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, 0); - return body->contacts[p_contact_idx].collider_shape; -} - -Vector2 PhysicsDirectBodyState2DSW::get_contact_collider_velocity_at_position(int p_contact_idx) const { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2()); - return body->contacts[p_contact_idx].collider_velocity_at_pos; -} - -PhysicsDirectSpaceState2D *PhysicsDirectBodyState2DSW::get_space_state() { - return body->get_space()->get_direct_state(); -} - -real_t PhysicsDirectBodyState2DSW::get_step() const { - return body->get_space()->get_last_step(); -} diff --git a/servers/physics_2d/body_direct_state_2d_sw.h b/servers/physics_2d/body_direct_state_2d_sw.h deleted file mode 100644 index 4266b24842..0000000000 --- a/servers/physics_2d/body_direct_state_2d_sw.h +++ /dev/null @@ -1,91 +0,0 @@ -/*************************************************************************/ -/* body_direct_state_2d_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/*************************************************************************/ - -#ifndef BODY_DIRECT_STATE_2D_SW_H -#define BODY_DIRECT_STATE_2D_SW_H - -#include "servers/physics_server_2d.h" - -class Body2DSW; - -class PhysicsDirectBodyState2DSW : public PhysicsDirectBodyState2D { - GDCLASS(PhysicsDirectBodyState2DSW, PhysicsDirectBodyState2D); - -public: - Body2DSW *body = nullptr; - - virtual Vector2 get_total_gravity() const override; - virtual real_t get_total_angular_damp() const override; - virtual real_t get_total_linear_damp() const override; - - virtual Vector2 get_center_of_mass() const override; - virtual real_t get_inverse_mass() const override; - virtual real_t get_inverse_inertia() const override; - - virtual void set_linear_velocity(const Vector2 &p_velocity) override; - virtual Vector2 get_linear_velocity() const override; - - virtual void set_angular_velocity(real_t p_velocity) override; - virtual real_t get_angular_velocity() const override; - - virtual void set_transform(const Transform2D &p_transform) override; - virtual Transform2D get_transform() const override; - - virtual Vector2 get_velocity_at_local_position(const Vector2 &p_position) const override; - - virtual void add_central_force(const Vector2 &p_force) override; - virtual void add_force(const Vector2 &p_force, const Vector2 &p_position = Vector2()) override; - virtual void add_torque(real_t p_torque) override; - virtual void apply_central_impulse(const Vector2 &p_impulse) override; - virtual void apply_impulse(const Vector2 &p_impulse, const Vector2 &p_position = Vector2()) override; - virtual void apply_torque_impulse(real_t p_torque) override; - - virtual void set_sleep_state(bool p_enable) override; - virtual bool is_sleeping() const override; - - virtual int get_contact_count() const override; - - virtual Vector2 get_contact_local_position(int p_contact_idx) const override; - virtual Vector2 get_contact_local_normal(int p_contact_idx) const override; - virtual int get_contact_local_shape(int p_contact_idx) const override; - - virtual RID get_contact_collider(int p_contact_idx) const override; - virtual Vector2 get_contact_collider_position(int p_contact_idx) const override; - virtual ObjectID get_contact_collider_id(int p_contact_idx) const override; - virtual int get_contact_collider_shape(int p_contact_idx) const override; - - virtual Vector2 get_contact_collider_velocity_at_position(int p_contact_idx) const override; - - virtual PhysicsDirectSpaceState2D *get_space_state() override; - - virtual real_t get_step() const override; -}; - -#endif // BODY_2D_SW_H diff --git a/servers/physics_2d/body_pair_2d_sw.cpp b/servers/physics_2d/body_pair_2d_sw.cpp deleted file mode 100644 index 8bcc4609f4..0000000000 --- a/servers/physics_2d/body_pair_2d_sw.cpp +++ /dev/null @@ -1,545 +0,0 @@ -/*************************************************************************/ -/* body_pair_2d_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/*************************************************************************/ - -#include "body_pair_2d_sw.h" -#include "collision_solver_2d_sw.h" -#include "space_2d_sw.h" - -#define POSITION_CORRECTION -#define ACCUMULATE_IMPULSES - -void BodyPair2DSW::_add_contact(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_self) { - BodyPair2DSW *self = (BodyPair2DSW *)p_self; - - self->_contact_added_callback(p_point_A, p_point_B); -} - -void BodyPair2DSW::_contact_added_callback(const Vector2 &p_point_A, const Vector2 &p_point_B) { - // check if we already have the contact - - Vector2 local_A = A->get_inv_transform().basis_xform(p_point_A); - Vector2 local_B = B->get_inv_transform().basis_xform(p_point_B - offset_B); - - int new_index = contact_count; - - ERR_FAIL_COND(new_index >= (MAX_CONTACTS + 1)); - - Contact contact; - - contact.acc_normal_impulse = 0; - contact.acc_bias_impulse = 0; - contact.acc_tangent_impulse = 0; - contact.local_A = local_A; - contact.local_B = local_B; - contact.reused = true; - contact.normal = (p_point_A - p_point_B).normalized(); - contact.mass_normal = 0; // will be computed in setup() - - // attempt to determine if the contact will be reused - - real_t recycle_radius_2 = space->get_contact_recycle_radius() * space->get_contact_recycle_radius(); - - for (int i = 0; i < contact_count; i++) { - Contact &c = contacts[i]; - if ( - c.local_A.distance_squared_to(local_A) < (recycle_radius_2) && - c.local_B.distance_squared_to(local_B) < (recycle_radius_2)) { - contact.acc_normal_impulse = c.acc_normal_impulse; - contact.acc_tangent_impulse = c.acc_tangent_impulse; - contact.acc_bias_impulse = c.acc_bias_impulse; - new_index = i; - break; - } - } - - // figure out if the contact amount must be reduced to fit the new contact - - if (new_index == MAX_CONTACTS) { - // remove the contact with the minimum depth - - int least_deep = -1; - real_t min_depth = 1e10; - - const Transform2D &transform_A = A->get_transform(); - const Transform2D &transform_B = B->get_transform(); - - for (int i = 0; i <= contact_count; i++) { - Contact &c = (i == contact_count) ? contact : contacts[i]; - Vector2 global_A = transform_A.basis_xform(c.local_A); - Vector2 global_B = transform_B.basis_xform(c.local_B) + offset_B; - - Vector2 axis = global_A - global_B; - real_t depth = axis.dot(c.normal); - - if (depth < min_depth) { - min_depth = depth; - least_deep = i; - } - } - - ERR_FAIL_COND(least_deep == -1); - - if (least_deep < contact_count) { //replace the last deep contact by the new one - - contacts[least_deep] = contact; - } - - return; - } - - contacts[new_index] = contact; - - if (new_index == contact_count) { - contact_count++; - } -} - -void BodyPair2DSW::_validate_contacts() { - //make sure to erase contacts that are no longer valid - - real_t max_separation = space->get_contact_max_separation(); - real_t max_separation2 = max_separation * max_separation; - - const Transform2D &transform_A = A->get_transform(); - const Transform2D &transform_B = B->get_transform(); - - for (int i = 0; i < contact_count; i++) { - Contact &c = contacts[i]; - - bool erase = false; - if (!c.reused) { - //was left behind in previous frame - erase = true; - } else { - c.reused = false; - - Vector2 global_A = transform_A.basis_xform(c.local_A); - Vector2 global_B = transform_B.basis_xform(c.local_B) + offset_B; - Vector2 axis = global_A - global_B; - real_t depth = axis.dot(c.normal); - - if (depth < -max_separation || (global_B + c.normal * depth - global_A).length_squared() > max_separation2) { - erase = true; - } - } - - if (erase) { - // contact no longer needed, remove - - if ((i + 1) < contact_count) { - // swap with the last one - SWAP(contacts[i], contacts[contact_count - 1]); - } - - i--; - contact_count--; - } - } -} - -bool BodyPair2DSW::_test_ccd(real_t p_step, Body2DSW *p_A, int p_shape_A, const Transform2D &p_xform_A, Body2DSW *p_B, int p_shape_B, const Transform2D &p_xform_B, bool p_swap_result) { - Vector2 motion = p_A->get_linear_velocity() * p_step; - real_t mlen = motion.length(); - if (mlen < CMP_EPSILON) { - return false; - } - - Vector2 mnormal = motion / mlen; - - real_t min, max; - p_A->get_shape(p_shape_A)->project_rangev(mnormal, p_xform_A, min, max); - bool fast_object = mlen > (max - min) * 0.3; //going too fast in that direction - - if (!fast_object) { //did it move enough in this direction to even attempt raycast? let's say it should move more than 1/3 the size of the object in that axis - return false; - } - - //cast a segment from support in motion normal, in the same direction of motion by motion length - //support is the worst case collision point, so real collision happened before - int a; - Vector2 s[2]; - p_A->get_shape(p_shape_A)->get_supports(p_xform_A.basis_xform(mnormal).normalized(), s, a); - Vector2 from = p_xform_A.xform(s[0]); - Vector2 to = from + motion; - - Transform2D from_inv = p_xform_B.affine_inverse(); - - Vector2 local_from = from_inv.xform(from - mnormal * mlen * 0.1); //start from a little inside the bounding box - Vector2 local_to = from_inv.xform(to); - - Vector2 rpos, rnorm; - if (!p_B->get_shape(p_shape_B)->intersect_segment(local_from, local_to, rpos, rnorm)) { - return false; - } - - //ray hit something - - Vector2 hitpos = p_xform_B.xform(rpos); - - Vector2 contact_A = to; - Vector2 contact_B = hitpos; - - //create a contact - - if (p_swap_result) { - _contact_added_callback(contact_B, contact_A); - } else { - _contact_added_callback(contact_A, contact_B); - } - - return true; -} - -real_t combine_bounce(Body2DSW *A, Body2DSW *B) { - return CLAMP(A->get_bounce() + B->get_bounce(), 0, 1); -} - -real_t combine_friction(Body2DSW *A, Body2DSW *B) { - return ABS(MIN(A->get_friction(), B->get_friction())); -} - -bool BodyPair2DSW::setup(real_t p_step) { - if (!A->interacts_with(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self())) { - collided = false; - return false; - } - - collide_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) && A->collides_with(B); - collide_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) && B->collides_with(A); - - report_contacts_only = false; - if (!collide_A && !collide_B) { - if ((A->get_max_contacts_reported() > 0) || (B->get_max_contacts_reported() > 0)) { - report_contacts_only = true; - } else { - collided = false; - return false; - } - } - - //use local A coordinates to avoid numerical issues on collision detection - offset_B = B->get_transform().get_origin() - A->get_transform().get_origin(); - - _validate_contacts(); - - const Vector2 &offset_A = A->get_transform().get_origin(); - Transform2D xform_Au = A->get_transform().untranslated(); - Transform2D xform_A = xform_Au * A->get_shape_transform(shape_A); - - Transform2D xform_Bu = B->get_transform(); - xform_Bu.elements[2] -= offset_A; - Transform2D xform_B = xform_Bu * B->get_shape_transform(shape_B); - - Shape2DSW *shape_A_ptr = A->get_shape(shape_A); - Shape2DSW *shape_B_ptr = B->get_shape(shape_B); - - Vector2 motion_A, motion_B; - - if (A->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_SHAPE) { - motion_A = A->get_motion(); - } - if (B->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_SHAPE) { - motion_B = B->get_motion(); - } - - bool prev_collided = collided; - - collided = CollisionSolver2DSW::solve(shape_A_ptr, xform_A, motion_A, shape_B_ptr, xform_B, motion_B, _add_contact, this, &sep_axis); - if (!collided) { - //test ccd (currently just a raycast) - - if (A->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && collide_A) { - if (_test_ccd(p_step, A, shape_A, xform_A, B, shape_B, xform_B)) { - collided = true; - } - } - - if (B->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && collide_B) { - if (_test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A, true)) { - collided = true; - } - } - - if (!collided) { - oneway_disabled = false; - return false; - } - } - - if (oneway_disabled) { - return false; - } - - if (!prev_collided) { - if (shape_B_ptr->allows_one_way_collision() && A->is_shape_set_as_one_way_collision(shape_A)) { - Vector2 direction = xform_A.get_axis(1).normalized(); - bool valid = false; - for (int i = 0; i < contact_count; i++) { - Contact &c = contacts[i]; - if (!c.reused) { - continue; - } - if (c.normal.dot(direction) > -CMP_EPSILON) { //greater (normal inverted) - continue; - } - valid = true; - break; - } - if (!valid) { - collided = false; - oneway_disabled = true; - return false; - } - } - - if (shape_A_ptr->allows_one_way_collision() && B->is_shape_set_as_one_way_collision(shape_B)) { - Vector2 direction = xform_B.get_axis(1).normalized(); - bool valid = false; - for (int i = 0; i < contact_count; i++) { - Contact &c = contacts[i]; - if (!c.reused) { - continue; - } - if (c.normal.dot(direction) < CMP_EPSILON) { //less (normal ok) - continue; - } - valid = true; - break; - } - if (!valid) { - collided = false; - oneway_disabled = true; - return false; - } - } - } - - return true; -} - -bool BodyPair2DSW::pre_solve(real_t p_step) { - if (!collided || oneway_disabled) { - return false; - } - - real_t max_penetration = space->get_contact_max_allowed_penetration(); - - real_t bias = 0.3; - - Shape2DSW *shape_A_ptr = A->get_shape(shape_A); - Shape2DSW *shape_B_ptr = B->get_shape(shape_B); - - if (shape_A_ptr->get_custom_bias() || shape_B_ptr->get_custom_bias()) { - if (shape_A_ptr->get_custom_bias() == 0) { - bias = shape_B_ptr->get_custom_bias(); - } else if (shape_B_ptr->get_custom_bias() == 0) { - bias = shape_A_ptr->get_custom_bias(); - } else { - bias = (shape_B_ptr->get_custom_bias() + shape_A_ptr->get_custom_bias()) * 0.5; - } - } - - real_t inv_dt = 1.0 / p_step; - - bool do_process = false; - - const Vector2 &offset_A = A->get_transform().get_origin(); - const Transform2D &transform_A = A->get_transform(); - const Transform2D &transform_B = B->get_transform(); - - real_t inv_inertia_A = collide_A ? A->get_inv_inertia() : 0.0; - real_t inv_inertia_B = collide_B ? B->get_inv_inertia() : 0.0; - - real_t inv_mass_A = collide_A ? A->get_inv_mass() : 0.0; - real_t inv_mass_B = collide_B ? B->get_inv_mass() : 0.0; - - for (int i = 0; i < contact_count; i++) { - Contact &c = contacts[i]; - c.active = false; - - Vector2 global_A = transform_A.basis_xform(c.local_A); - Vector2 global_B = transform_B.basis_xform(c.local_B) + offset_B; - - Vector2 axis = global_A - global_B; - real_t depth = axis.dot(c.normal); - - if (depth <= 0.0 || !c.reused) { - continue; - } - -#ifdef DEBUG_ENABLED - if (space->is_debugging_contacts()) { - space->add_debug_contact(global_A + offset_A); - space->add_debug_contact(global_B + offset_A); - } -#endif - - c.rA = global_A; - c.rB = global_B - offset_B; - - if (A->can_report_contacts()) { - Vector2 crB(-B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x); - A->add_contact(global_A + offset_A, -c.normal, depth, shape_A, global_B + offset_A, shape_B, B->get_instance_id(), B->get_self(), crB + B->get_linear_velocity()); - } - - if (B->can_report_contacts()) { - Vector2 crA(-A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x); - B->add_contact(global_B + offset_A, c.normal, depth, shape_B, global_A + offset_A, shape_A, A->get_instance_id(), A->get_self(), crA + A->get_linear_velocity()); - } - - if (report_contacts_only) { - collided = false; - continue; - } - - // Precompute normal mass, tangent mass, and bias. - real_t rnA = c.rA.dot(c.normal); - real_t rnB = c.rB.dot(c.normal); - real_t kNormal = inv_mass_A + inv_mass_B; - kNormal += inv_inertia_A * (c.rA.dot(c.rA) - rnA * rnA) + inv_inertia_B * (c.rB.dot(c.rB) - rnB * rnB); - c.mass_normal = 1.0f / kNormal; - - Vector2 tangent = c.normal.orthogonal(); - real_t rtA = c.rA.dot(tangent); - real_t rtB = c.rB.dot(tangent); - real_t kTangent = inv_mass_A + inv_mass_B; - kTangent += inv_inertia_A * (c.rA.dot(c.rA) - rtA * rtA) + inv_inertia_B * (c.rB.dot(c.rB) - rtB * rtB); - c.mass_tangent = 1.0f / kTangent; - - c.bias = -bias * inv_dt * MIN(0.0f, -depth + max_penetration); - c.depth = depth; - //c.acc_bias_impulse=0; - -#ifdef ACCUMULATE_IMPULSES - { - // Apply normal + friction impulse - Vector2 P = c.acc_normal_impulse * c.normal + c.acc_tangent_impulse * tangent; - - if (collide_A) { - A->apply_impulse(-P, c.rA); - } - if (collide_B) { - B->apply_impulse(P, c.rB); - } - } -#endif - - c.bounce = combine_bounce(A, B); - if (c.bounce) { - Vector2 crA(-A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x); - Vector2 crB(-B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x); - Vector2 dv = B->get_linear_velocity() + crB - A->get_linear_velocity() - crA; - c.bounce = c.bounce * dv.dot(c.normal); - } - - c.active = true; - do_process = true; - } - - return do_process; -} - -void BodyPair2DSW::solve(real_t p_step) { - if (!collided || oneway_disabled) { - return; - } - - for (int i = 0; i < contact_count; ++i) { - Contact &c = contacts[i]; - - if (!c.active) { - continue; - } - - // Relative velocity at contact - - Vector2 crA(-A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x); - Vector2 crB(-B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x); - Vector2 dv = B->get_linear_velocity() + crB - A->get_linear_velocity() - crA; - - Vector2 crbA(-A->get_biased_angular_velocity() * c.rA.y, A->get_biased_angular_velocity() * c.rA.x); - Vector2 crbB(-B->get_biased_angular_velocity() * c.rB.y, B->get_biased_angular_velocity() * c.rB.x); - Vector2 dbv = B->get_biased_linear_velocity() + crbB - A->get_biased_linear_velocity() - crbA; - - real_t vn = dv.dot(c.normal); - real_t vbn = dbv.dot(c.normal); - Vector2 tangent = c.normal.orthogonal(); - real_t vt = dv.dot(tangent); - - real_t jbn = (c.bias - vbn) * c.mass_normal; - real_t jbnOld = c.acc_bias_impulse; - c.acc_bias_impulse = MAX(jbnOld + jbn, 0.0f); - - Vector2 jb = c.normal * (c.acc_bias_impulse - jbnOld); - - if (collide_A) { - A->apply_bias_impulse(-jb, c.rA); - } - if (collide_B) { - B->apply_bias_impulse(jb, c.rB); - } - - real_t jn = -(c.bounce + vn) * c.mass_normal; - real_t jnOld = c.acc_normal_impulse; - c.acc_normal_impulse = MAX(jnOld + jn, 0.0f); - - real_t friction = combine_friction(A, B); - - real_t jtMax = friction * c.acc_normal_impulse; - real_t jt = -vt * c.mass_tangent; - real_t jtOld = c.acc_tangent_impulse; - c.acc_tangent_impulse = CLAMP(jtOld + jt, -jtMax, jtMax); - - Vector2 j = c.normal * (c.acc_normal_impulse - jnOld) + tangent * (c.acc_tangent_impulse - jtOld); - - if (collide_A) { - A->apply_impulse(-j, c.rA); - } - if (collide_B) { - B->apply_impulse(j, c.rB); - } - } -} - -BodyPair2DSW::BodyPair2DSW(Body2DSW *p_A, int p_shape_A, Body2DSW *p_B, int p_shape_B) : - Constraint2DSW(_arr, 2) { - A = p_A; - B = p_B; - shape_A = p_shape_A; - shape_B = p_shape_B; - space = A->get_space(); - A->add_constraint(this, 0); - B->add_constraint(this, 1); -} - -BodyPair2DSW::~BodyPair2DSW() { - A->remove_constraint(this, 0); - B->remove_constraint(this, 1); -} diff --git a/servers/physics_2d/body_pair_2d_sw.h b/servers/physics_2d/body_pair_2d_sw.h deleted file mode 100644 index db4f3eba69..0000000000 --- a/servers/physics_2d/body_pair_2d_sw.h +++ /dev/null @@ -1,98 +0,0 @@ -/*************************************************************************/ -/* body_pair_2d_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/*************************************************************************/ - -#ifndef BODY_PAIR_2D_SW_H -#define BODY_PAIR_2D_SW_H - -#include "body_2d_sw.h" -#include "constraint_2d_sw.h" - -class BodyPair2DSW : public Constraint2DSW { - enum { - MAX_CONTACTS = 2 - }; - union { - struct { - Body2DSW *A; - Body2DSW *B; - }; - - Body2DSW *_arr[2] = { nullptr, nullptr }; - }; - - int shape_A = 0; - int shape_B = 0; - - bool collide_A = false; - bool collide_B = false; - - Space2DSW *space = nullptr; - - struct Contact { - Vector2 position; - Vector2 normal; - Vector2 local_A, local_B; - real_t acc_normal_impulse = 0.0; // accumulated normal impulse (Pn) - real_t acc_tangent_impulse = 0.0; // accumulated tangent impulse (Pt) - real_t acc_bias_impulse = 0.0; // accumulated normal impulse for position bias (Pnb) - real_t mass_normal, mass_tangent = 0.0; - real_t bias = 0.0; - - real_t depth = 0.0; - bool active = false; - Vector2 rA, rB; - bool reused = false; - real_t bounce = 0.0; - }; - - Vector2 offset_B; //use local A coordinates to avoid numerical issues on collision detection - - Vector2 sep_axis; - Contact contacts[MAX_CONTACTS]; - int contact_count = 0; - bool collided = false; - bool oneway_disabled = false; - bool report_contacts_only = false; - - bool _test_ccd(real_t p_step, Body2DSW *p_A, int p_shape_A, const Transform2D &p_xform_A, Body2DSW *p_B, int p_shape_B, const Transform2D &p_xform_B, bool p_swap_result = false); - void _validate_contacts(); - static void _add_contact(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_self); - _FORCE_INLINE_ void _contact_added_callback(const Vector2 &p_point_A, const Vector2 &p_point_B); - -public: - virtual bool setup(real_t p_step) override; - virtual bool pre_solve(real_t p_step) override; - virtual void solve(real_t p_step) override; - - BodyPair2DSW(Body2DSW *p_A, int p_shape_A, Body2DSW *p_B, int p_shape_B); - ~BodyPair2DSW(); -}; - -#endif // BODY_PAIR_2D_SW_H diff --git a/servers/physics_2d/broad_phase_2d_bvh.cpp b/servers/physics_2d/broad_phase_2d_bvh.cpp deleted file mode 100644 index 0df7086c5a..0000000000 --- a/servers/physics_2d/broad_phase_2d_bvh.cpp +++ /dev/null @@ -1,113 +0,0 @@ -/*************************************************************************/ -/* broad_phase_2d_bvh.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 "broad_phase_2d_bvh.h" -#include "collision_object_2d_sw.h" - -BroadPhase2DSW::ID BroadPhase2DBVH::create(CollisionObject2DSW *p_object, int p_subindex, const Rect2 &p_aabb, bool p_static) { - ID oid = bvh.create(p_object, true, p_aabb, p_subindex, !p_static, 1 << p_object->get_type(), p_static ? 0 : 0xFFFFF); // Pair everything, don't care? - return oid + 1; -} - -void BroadPhase2DBVH::move(ID p_id, const Rect2 &p_aabb) { - bvh.move(p_id - 1, p_aabb); -} - -void BroadPhase2DBVH::set_static(ID p_id, bool p_static) { - CollisionObject2DSW *it = bvh.get(p_id - 1); - bvh.set_pairable(p_id - 1, !p_static, 1 << it->get_type(), p_static ? 0 : 0xFFFFF, false); // Pair everything, don't care? -} - -void BroadPhase2DBVH::remove(ID p_id) { - bvh.erase(p_id - 1); -} - -CollisionObject2DSW *BroadPhase2DBVH::get_object(ID p_id) const { - CollisionObject2DSW *it = bvh.get(p_id - 1); - ERR_FAIL_COND_V(!it, nullptr); - return it; -} - -bool BroadPhase2DBVH::is_static(ID p_id) const { - return !bvh.is_pairable(p_id - 1); -} - -int BroadPhase2DBVH::get_subindex(ID p_id) const { - return bvh.get_subindex(p_id - 1); -} - -int BroadPhase2DBVH::cull_segment(const Vector2 &p_from, const Vector2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices) { - return bvh.cull_segment(p_from, p_to, p_results, p_max_results, p_result_indices); -} - -int BroadPhase2DBVH::cull_aabb(const Rect2 &p_aabb, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices) { - return bvh.cull_aabb(p_aabb, p_results, p_max_results, p_result_indices); -} - -void *BroadPhase2DBVH::_pair_callback(void *self, uint32_t p_A, CollisionObject2DSW *p_object_A, int subindex_A, uint32_t p_B, CollisionObject2DSW *p_object_B, int subindex_B) { - BroadPhase2DBVH *bpo = (BroadPhase2DBVH *)(self); - if (!bpo->pair_callback) { - return nullptr; - } - - return bpo->pair_callback(p_object_A, subindex_A, p_object_B, subindex_B, bpo->pair_userdata); -} - -void BroadPhase2DBVH::_unpair_callback(void *self, uint32_t p_A, CollisionObject2DSW *p_object_A, int subindex_A, uint32_t p_B, CollisionObject2DSW *p_object_B, int subindex_B, void *pairdata) { - BroadPhase2DBVH *bpo = (BroadPhase2DBVH *)(self); - if (!bpo->unpair_callback) { - return; - } - - bpo->unpair_callback(p_object_A, subindex_A, p_object_B, subindex_B, pairdata, bpo->unpair_userdata); -} - -void BroadPhase2DBVH::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) { - pair_callback = p_pair_callback; - pair_userdata = p_userdata; -} - -void BroadPhase2DBVH::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) { - unpair_callback = p_unpair_callback; - unpair_userdata = p_userdata; -} - -void BroadPhase2DBVH::update() { - bvh.update(); -} - -BroadPhase2DSW *BroadPhase2DBVH::_create() { - return memnew(BroadPhase2DBVH); -} - -BroadPhase2DBVH::BroadPhase2DBVH() { - bvh.set_pair_callback(_pair_callback, this); - bvh.set_unpair_callback(_unpair_callback, this); -} diff --git a/servers/physics_2d/broad_phase_2d_bvh.h b/servers/physics_2d/broad_phase_2d_bvh.h deleted file mode 100644 index ea02a98417..0000000000 --- a/servers/physics_2d/broad_phase_2d_bvh.h +++ /dev/null @@ -1,73 +0,0 @@ -/*************************************************************************/ -/* broad_phase_2d_bvh.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 BROAD_PHASE_2D_BVH_H -#define BROAD_PHASE_2D_BVH_H - -#include "broad_phase_2d_sw.h" -#include "core/math/bvh.h" -#include "core/math/rect2.h" -#include "core/math/vector2.h" - -class BroadPhase2DBVH : public BroadPhase2DSW { - BVH_Manager bvh; - - static void *_pair_callback(void *, uint32_t, CollisionObject2DSW *, int, uint32_t, CollisionObject2DSW *, int); - static void _unpair_callback(void *, uint32_t, CollisionObject2DSW *, int, uint32_t, CollisionObject2DSW *, int, void *); - - PairCallback pair_callback = nullptr; - void *pair_userdata = nullptr; - UnpairCallback unpair_callback = nullptr; - void *unpair_userdata = nullptr; - -public: - // 0 is an invalid ID - virtual ID create(CollisionObject2DSW *p_object, int p_subindex = 0, const Rect2 &p_aabb = Rect2(), bool p_static = false); - virtual void move(ID p_id, const Rect2 &p_aabb); - virtual void set_static(ID p_id, bool p_static); - virtual void remove(ID p_id); - - virtual CollisionObject2DSW *get_object(ID p_id) const; - virtual bool is_static(ID p_id) const; - virtual int get_subindex(ID p_id) const; - - virtual int cull_segment(const Vector2 &p_from, const Vector2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices = nullptr); - virtual int cull_aabb(const Rect2 &p_aabb, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices = nullptr); - - virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata); - virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata); - - virtual void update(); - - static BroadPhase2DSW *_create(); - BroadPhase2DBVH(); -}; - -#endif // BROAD_PHASE_2D_BVH_H diff --git a/servers/physics_2d/broad_phase_2d_sw.cpp b/servers/physics_2d/broad_phase_2d_sw.cpp deleted file mode 100644 index 7f0af48b1f..0000000000 --- a/servers/physics_2d/broad_phase_2d_sw.cpp +++ /dev/null @@ -1,36 +0,0 @@ -/*************************************************************************/ -/* broad_phase_2d_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/*************************************************************************/ - -#include "broad_phase_2d_sw.h" - -BroadPhase2DSW::CreateFunction BroadPhase2DSW::create_func = nullptr; - -BroadPhase2DSW::~BroadPhase2DSW() { -} diff --git a/servers/physics_2d/broad_phase_2d_sw.h b/servers/physics_2d/broad_phase_2d_sw.h deleted file mode 100644 index 0f82f06b9c..0000000000 --- a/servers/physics_2d/broad_phase_2d_sw.h +++ /dev/null @@ -1,71 +0,0 @@ -/*************************************************************************/ -/* broad_phase_2d_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/*************************************************************************/ - -#ifndef BROAD_PHASE_2D_SW_H -#define BROAD_PHASE_2D_SW_H - -#include "core/math/math_funcs.h" -#include "core/math/rect2.h" - -class CollisionObject2DSW; - -class BroadPhase2DSW { -public: - typedef BroadPhase2DSW *(*CreateFunction)(); - - static CreateFunction create_func; - - typedef uint32_t ID; - - typedef void *(*PairCallback)(CollisionObject2DSW *A, int p_subindex_A, CollisionObject2DSW *B, int p_subindex_B, void *p_userdata); - typedef void (*UnpairCallback)(CollisionObject2DSW *A, int p_subindex_A, CollisionObject2DSW *B, int p_subindex_B, void *p_data, void *p_userdata); - - // 0 is an invalid ID - virtual ID create(CollisionObject2DSW *p_object_, int p_subindex = 0, const Rect2 &p_aabb = Rect2(), bool p_static = false) = 0; - virtual void move(ID p_id, const Rect2 &p_aabb) = 0; - virtual void set_static(ID p_id, bool p_static) = 0; - virtual void remove(ID p_id) = 0; - - virtual CollisionObject2DSW *get_object(ID p_id) const = 0; - virtual bool is_static(ID p_id) const = 0; - virtual int get_subindex(ID p_id) const = 0; - - virtual int cull_segment(const Vector2 &p_from, const Vector2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices = nullptr) = 0; - virtual int cull_aabb(const Rect2 &p_aabb, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices = nullptr) = 0; - - virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata) = 0; - virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) = 0; - - virtual void update() = 0; - - virtual ~BroadPhase2DSW(); -}; - -#endif // BROAD_PHASE_2D_SW_H diff --git a/servers/physics_2d/collision_object_2d_sw.cpp b/servers/physics_2d/collision_object_2d_sw.cpp deleted file mode 100644 index bc7d277152..0000000000 --- a/servers/physics_2d/collision_object_2d_sw.cpp +++ /dev/null @@ -1,243 +0,0 @@ -/*************************************************************************/ -/* collision_object_2d_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/*************************************************************************/ - -#include "collision_object_2d_sw.h" -#include "servers/physics_2d/physics_server_2d_sw.h" -#include "space_2d_sw.h" - -void CollisionObject2DSW::add_shape(Shape2DSW *p_shape, const Transform2D &p_transform, bool p_disabled) { - Shape s; - s.shape = p_shape; - s.xform = p_transform; - s.xform_inv = s.xform.affine_inverse(); - s.bpid = 0; //needs update - s.disabled = p_disabled; - s.one_way_collision = false; - s.one_way_collision_margin = 0; - shapes.push_back(s); - p_shape->add_owner(this); - - if (!pending_shape_update_list.in_list()) { - PhysicsServer2DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list); - } -} - -void CollisionObject2DSW::set_shape(int p_index, Shape2DSW *p_shape) { - ERR_FAIL_INDEX(p_index, shapes.size()); - shapes[p_index].shape->remove_owner(this); - shapes.write[p_index].shape = p_shape; - - p_shape->add_owner(this); - - if (!pending_shape_update_list.in_list()) { - PhysicsServer2DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list); - } -} - -void CollisionObject2DSW::set_shape_transform(int p_index, const Transform2D &p_transform) { - ERR_FAIL_INDEX(p_index, shapes.size()); - - shapes.write[p_index].xform = p_transform; - shapes.write[p_index].xform_inv = p_transform.affine_inverse(); - - if (!pending_shape_update_list.in_list()) { - PhysicsServer2DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list); - } -} - -void CollisionObject2DSW::set_shape_disabled(int p_idx, bool p_disabled) { - ERR_FAIL_INDEX(p_idx, shapes.size()); - - CollisionObject2DSW::Shape &shape = shapes.write[p_idx]; - if (shape.disabled == p_disabled) { - return; - } - - shape.disabled = p_disabled; - - if (!space) { - return; - } - - if (p_disabled && shape.bpid != 0) { - space->get_broadphase()->remove(shape.bpid); - shape.bpid = 0; - if (!pending_shape_update_list.in_list()) { - PhysicsServer2DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list); - } - } else if (!p_disabled && shape.bpid == 0) { - if (!pending_shape_update_list.in_list()) { - PhysicsServer2DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list); - } - } -} - -void CollisionObject2DSW::remove_shape(Shape2DSW *p_shape) { - //remove a shape, all the times it appears - for (int i = 0; i < shapes.size(); i++) { - if (shapes[i].shape == p_shape) { - remove_shape(i); - i--; - } - } -} - -void CollisionObject2DSW::remove_shape(int p_index) { - //remove anything from shape to be erased to end, so subindices don't change - ERR_FAIL_INDEX(p_index, shapes.size()); - for (int i = p_index; i < shapes.size(); i++) { - if (shapes[i].bpid == 0) { - continue; - } - //should never get here with a null owner - space->get_broadphase()->remove(shapes[i].bpid); - shapes.write[i].bpid = 0; - } - shapes[p_index].shape->remove_owner(this); - shapes.remove(p_index); - - if (!pending_shape_update_list.in_list()) { - PhysicsServer2DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list); - } - // _update_shapes(); - // _shapes_changed(); -} - -void CollisionObject2DSW::_set_static(bool p_static) { - if (_static == p_static) { - return; - } - _static = p_static; - - if (!space) { - return; - } - for (int i = 0; i < get_shape_count(); i++) { - const Shape &s = shapes[i]; - if (s.bpid > 0) { - space->get_broadphase()->set_static(s.bpid, _static); - } - } -} - -void CollisionObject2DSW::_unregister_shapes() { - for (int i = 0; i < shapes.size(); i++) { - Shape &s = shapes.write[i]; - if (s.bpid > 0) { - space->get_broadphase()->remove(s.bpid); - s.bpid = 0; - } - } -} - -void CollisionObject2DSW::_update_shapes() { - if (!space) { - return; - } - - for (int i = 0; i < shapes.size(); i++) { - Shape &s = shapes.write[i]; - if (s.disabled) { - continue; - } - - //not quite correct, should compute the next matrix.. - Rect2 shape_aabb = s.shape->get_aabb(); - Transform2D xform = transform * s.xform; - shape_aabb = xform.xform(shape_aabb); - shape_aabb.grow_by((s.aabb_cache.size.x + s.aabb_cache.size.y) * 0.5 * 0.05); - s.aabb_cache = shape_aabb; - - if (s.bpid == 0) { - s.bpid = space->get_broadphase()->create(this, i, shape_aabb, _static); - space->get_broadphase()->set_static(s.bpid, _static); - } - - space->get_broadphase()->move(s.bpid, shape_aabb); - } -} - -void CollisionObject2DSW::_update_shapes_with_motion(const Vector2 &p_motion) { - if (!space) { - return; - } - - for (int i = 0; i < shapes.size(); i++) { - Shape &s = shapes.write[i]; - if (s.disabled) { - continue; - } - - //not quite correct, should compute the next matrix.. - Rect2 shape_aabb = s.shape->get_aabb(); - Transform2D xform = transform * s.xform; - shape_aabb = xform.xform(shape_aabb); - shape_aabb = shape_aabb.merge(Rect2(shape_aabb.position + p_motion, shape_aabb.size)); //use motion - s.aabb_cache = shape_aabb; - - if (s.bpid == 0) { - s.bpid = space->get_broadphase()->create(this, i, shape_aabb, _static); - space->get_broadphase()->set_static(s.bpid, _static); - } - - space->get_broadphase()->move(s.bpid, shape_aabb); - } -} - -void CollisionObject2DSW::_set_space(Space2DSW *p_space) { - if (space) { - space->remove_object(this); - - for (int i = 0; i < shapes.size(); i++) { - Shape &s = shapes.write[i]; - if (s.bpid) { - space->get_broadphase()->remove(s.bpid); - s.bpid = 0; - } - } - } - - space = p_space; - - if (space) { - space->add_object(this); - _update_shapes(); - } -} - -void CollisionObject2DSW::_shape_changed() { - _update_shapes(); - _shapes_changed(); -} - -CollisionObject2DSW::CollisionObject2DSW(Type p_type) : - pending_shape_update_list(this) { - type = p_type; -} diff --git a/servers/physics_2d/collision_object_2d_sw.h b/servers/physics_2d/collision_object_2d_sw.h deleted file mode 100644 index ca258a906a..0000000000 --- a/servers/physics_2d/collision_object_2d_sw.h +++ /dev/null @@ -1,189 +0,0 @@ -/*************************************************************************/ -/* collision_object_2d_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/*************************************************************************/ - -#ifndef COLLISION_OBJECT_2D_SW_H -#define COLLISION_OBJECT_2D_SW_H - -#include "broad_phase_2d_sw.h" -#include "core/templates/self_list.h" -#include "servers/physics_server_2d.h" -#include "shape_2d_sw.h" - -class Space2DSW; - -class CollisionObject2DSW : public ShapeOwner2DSW { -public: - enum Type { - TYPE_AREA, - TYPE_BODY - }; - -private: - Type type; - RID self; - ObjectID instance_id; - ObjectID canvas_instance_id; - bool pickable = true; - - struct Shape { - Transform2D xform; - Transform2D xform_inv; - BroadPhase2DSW::ID bpid = 0; - Rect2 aabb_cache; //for rayqueries - Shape2DSW *shape = nullptr; - bool disabled = false; - bool one_way_collision = false; - real_t one_way_collision_margin = 0.0; - }; - - Vector shapes; - Space2DSW *space = nullptr; - Transform2D transform; - Transform2D inv_transform; - uint32_t collision_mask = 1; - uint32_t collision_layer = 1; - bool _static = true; - - SelfList pending_shape_update_list; - - void _update_shapes(); - -protected: - void _update_shapes_with_motion(const Vector2 &p_motion); - void _unregister_shapes(); - - _FORCE_INLINE_ void _set_transform(const Transform2D &p_transform, bool p_update_shapes = true) { - transform = p_transform; - if (p_update_shapes) { - _update_shapes(); - } - } - _FORCE_INLINE_ void _set_inv_transform(const Transform2D &p_transform) { inv_transform = p_transform; } - void _set_static(bool p_static); - - virtual void _shapes_changed() = 0; - void _set_space(Space2DSW *p_space); - - CollisionObject2DSW(Type p_type); - -public: - _FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; } - _FORCE_INLINE_ RID get_self() const { return self; } - - _FORCE_INLINE_ void set_instance_id(const ObjectID &p_instance_id) { instance_id = p_instance_id; } - _FORCE_INLINE_ ObjectID get_instance_id() const { return instance_id; } - - _FORCE_INLINE_ void set_canvas_instance_id(const ObjectID &p_canvas_instance_id) { canvas_instance_id = p_canvas_instance_id; } - _FORCE_INLINE_ ObjectID get_canvas_instance_id() const { return canvas_instance_id; } - - void _shape_changed(); - - _FORCE_INLINE_ Type get_type() const { return type; } - void add_shape(Shape2DSW *p_shape, const Transform2D &p_transform = Transform2D(), bool p_disabled = false); - void set_shape(int p_index, Shape2DSW *p_shape); - void set_shape_transform(int p_index, const Transform2D &p_transform); - - _FORCE_INLINE_ int get_shape_count() const { return shapes.size(); } - _FORCE_INLINE_ Shape2DSW *get_shape(int p_index) const { - CRASH_BAD_INDEX(p_index, shapes.size()); - return shapes[p_index].shape; - } - _FORCE_INLINE_ const Transform2D &get_shape_transform(int p_index) const { - CRASH_BAD_INDEX(p_index, shapes.size()); - return shapes[p_index].xform; - } - _FORCE_INLINE_ const Transform2D &get_shape_inv_transform(int p_index) const { - CRASH_BAD_INDEX(p_index, shapes.size()); - return shapes[p_index].xform_inv; - } - _FORCE_INLINE_ const Rect2 &get_shape_aabb(int p_index) const { - CRASH_BAD_INDEX(p_index, shapes.size()); - return shapes[p_index].aabb_cache; - } - - _FORCE_INLINE_ const Transform2D &get_transform() const { return transform; } - _FORCE_INLINE_ const Transform2D &get_inv_transform() const { return inv_transform; } - _FORCE_INLINE_ Space2DSW *get_space() const { return space; } - - void set_shape_disabled(int p_idx, bool p_disabled); - _FORCE_INLINE_ bool is_shape_disabled(int p_idx) const { - ERR_FAIL_INDEX_V(p_idx, shapes.size(), false); - return shapes[p_idx].disabled; - } - - _FORCE_INLINE_ void set_shape_as_one_way_collision(int p_idx, bool p_one_way_collision, real_t p_margin) { - CRASH_BAD_INDEX(p_idx, shapes.size()); - shapes.write[p_idx].one_way_collision = p_one_way_collision; - shapes.write[p_idx].one_way_collision_margin = p_margin; - } - _FORCE_INLINE_ bool is_shape_set_as_one_way_collision(int p_idx) const { - CRASH_BAD_INDEX(p_idx, shapes.size()); - return shapes[p_idx].one_way_collision; - } - - _FORCE_INLINE_ real_t get_shape_one_way_collision_margin(int p_idx) const { - CRASH_BAD_INDEX(p_idx, shapes.size()); - return shapes[p_idx].one_way_collision_margin; - } - - 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; } - - 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; } - - void remove_shape(Shape2DSW *p_shape); - void remove_shape(int p_index); - - virtual void set_space(Space2DSW *p_space) = 0; - - _FORCE_INLINE_ bool is_static() const { return _static; } - - void set_pickable(bool p_pickable) { pickable = p_pickable; } - _FORCE_INLINE_ bool is_pickable() const { return pickable; } - - _FORCE_INLINE_ bool collides_with(CollisionObject2DSW *p_other) const { - return p_other->collision_layer & collision_mask; - } - - _FORCE_INLINE_ bool interacts_with(CollisionObject2DSW *p_other) const { - return collision_layer & p_other->collision_mask || p_other->collision_layer & collision_mask; - } - - virtual ~CollisionObject2DSW() {} -}; - -#endif // COLLISION_OBJECT_2D_SW_H diff --git a/servers/physics_2d/collision_solver_2d_sat.cpp b/servers/physics_2d/collision_solver_2d_sat.cpp deleted file mode 100644 index 2e67cc6520..0000000000 --- a/servers/physics_2d/collision_solver_2d_sat.cpp +++ /dev/null @@ -1,1408 +0,0 @@ -/*************************************************************************/ -/* collision_solver_2d_sat.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 "collision_solver_2d_sat.h" - -#include "core/math/geometry_2d.h" - -struct _CollectorCallback2D { - CollisionSolver2DSW::CallbackResult callback; - void *userdata = nullptr; - bool swap = false; - bool collided = false; - Vector2 normal; - Vector2 *sep_axis = nullptr; - - _FORCE_INLINE_ void call(const Vector2 &p_point_A, const Vector2 &p_point_B) { - /* - if (normal.dot(p_point_A) >= normal.dot(p_point_B)) - return; - */ - if (swap) { - callback(p_point_B, p_point_A, userdata); - } else { - callback(p_point_A, p_point_B, userdata); - } - } -}; - -typedef void (*GenerateContactsFunc)(const Vector2 *, int, const Vector2 *, int, _CollectorCallback2D *); - -_FORCE_INLINE_ static void _generate_contacts_point_point(const Vector2 *p_points_A, int p_point_count_A, const Vector2 *p_points_B, int p_point_count_B, _CollectorCallback2D *p_collector) { -#ifdef DEBUG_ENABLED - ERR_FAIL_COND(p_point_count_A != 1); - ERR_FAIL_COND(p_point_count_B != 1); -#endif - - p_collector->call(*p_points_A, *p_points_B); -} - -_FORCE_INLINE_ static void _generate_contacts_point_edge(const Vector2 *p_points_A, int p_point_count_A, const Vector2 *p_points_B, int p_point_count_B, _CollectorCallback2D *p_collector) { -#ifdef DEBUG_ENABLED - ERR_FAIL_COND(p_point_count_A != 1); - ERR_FAIL_COND(p_point_count_B != 2); -#endif - - Vector2 closest_B = Geometry2D::get_closest_point_to_segment_uncapped(*p_points_A, p_points_B); - p_collector->call(*p_points_A, closest_B); -} - -struct _generate_contacts_Pair { - bool a = false; - int idx = 0; - real_t d = 0.0; - _FORCE_INLINE_ bool operator<(const _generate_contacts_Pair &l) const { return d < l.d; } -}; - -_FORCE_INLINE_ static void _generate_contacts_edge_edge(const Vector2 *p_points_A, int p_point_count_A, const Vector2 *p_points_B, int p_point_count_B, _CollectorCallback2D *p_collector) { -#ifdef DEBUG_ENABLED - ERR_FAIL_COND(p_point_count_A != 2); - ERR_FAIL_COND(p_point_count_B != 2); // circle is actually a 4x3 matrix -#endif - - Vector2 n = p_collector->normal; - Vector2 t = n.orthogonal(); - real_t dA = n.dot(p_points_A[0]); - real_t dB = n.dot(p_points_B[0]); - - _generate_contacts_Pair dvec[4]; - - dvec[0].d = t.dot(p_points_A[0]); - dvec[0].a = true; - dvec[0].idx = 0; - dvec[1].d = t.dot(p_points_A[1]); - dvec[1].a = true; - dvec[1].idx = 1; - dvec[2].d = t.dot(p_points_B[0]); - dvec[2].a = false; - dvec[2].idx = 0; - dvec[3].d = t.dot(p_points_B[1]); - dvec[3].a = false; - dvec[3].idx = 1; - - SortArray<_generate_contacts_Pair> sa; - sa.sort(dvec, 4); - - for (int i = 1; i <= 2; i++) { - if (dvec[i].a) { - Vector2 a = p_points_A[dvec[i].idx]; - Vector2 b = n.plane_project(dB, a); - if (n.dot(a) > n.dot(b) - CMP_EPSILON) { - continue; - } - p_collector->call(a, b); - } else { - Vector2 b = p_points_B[dvec[i].idx]; - Vector2 a = n.plane_project(dA, b); - if (n.dot(a) > n.dot(b) - CMP_EPSILON) { - continue; - } - p_collector->call(a, b); - } - } -} - -static void _generate_contacts_from_supports(const Vector2 *p_points_A, int p_point_count_A, const Vector2 *p_points_B, int p_point_count_B, _CollectorCallback2D *p_collector) { -#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[2][2] = { - { - _generate_contacts_point_point, - _generate_contacts_point_edge, - }, - { - nullptr, - _generate_contacts_edge_edge, - } - }; - - int pointcount_B = 0; - int pointcount_A = 0; - const Vector2 *points_A = nullptr; - const Vector2 *points_B = nullptr; - - if (p_point_count_A > p_point_count_B) { - //swap - p_collector->swap = !p_collector->swap; - p_collector->normal = -p_collector->normal; - - pointcount_B = p_point_count_A; - pointcount_A = p_point_count_B; - points_A = p_points_B; - points_B = p_points_A; - } else { - pointcount_B = p_point_count_B; - pointcount_A = p_point_count_A; - points_A = p_points_A; - points_B = p_points_B; - } - - int version_A = (pointcount_A > 2 ? 2 : pointcount_A) - 1; - int version_B = (pointcount_B > 2 ? 2 : 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_collector); -} - -template -class SeparatorAxisTest2D { - const ShapeA *shape_A = nullptr; - const ShapeB *shape_B = nullptr; - const Transform2D *transform_A = nullptr; - const Transform2D *transform_B = nullptr; - real_t best_depth = 1e15; - Vector2 best_axis; -#ifdef DEBUG_ENABLED - int best_axis_count = 0; - int best_axis_index = -1; -#endif - Vector2 motion_A; - Vector2 motion_B; - real_t margin_A = 0.0; - real_t margin_B = 0.0; - _CollectorCallback2D *callback; - -public: - _FORCE_INLINE_ bool test_previous_axis() { - if (callback && callback->sep_axis && *callback->sep_axis != Vector2()) { - return test_axis(*callback->sep_axis); - } else { -#ifdef DEBUG_ENABLED - best_axis_count++; -#endif - } - return true; - } - - _FORCE_INLINE_ bool test_cast() { - if (castA) { - Vector2 na = motion_A.normalized(); - if (!test_axis(na)) { - return false; - } - if (!test_axis(na.orthogonal())) { - return false; - } - } - - if (castB) { - Vector2 nb = motion_B.normalized(); - if (!test_axis(nb)) { - return false; - } - if (!test_axis(nb.orthogonal())) { - return false; - } - } - - return true; - } - - _FORCE_INLINE_ bool test_axis(const Vector2 &p_axis) { - Vector2 axis = p_axis; - - if (Math::is_zero_approx(axis.x) && - Math::is_zero_approx(axis.y)) { - // strange case, try an upwards separator - axis = Vector2(0.0, 1.0); - } - - real_t min_A, max_A, min_B, max_B; - - if (castA) { - shape_A->project_range_cast(motion_A, axis, *transform_A, min_A, max_A); - } else { - shape_A->project_range(axis, *transform_A, min_A, max_A); - } - - if (castB) { - shape_B->project_range_cast(motion_B, axis, *transform_B, min_B, max_B); - } else { - shape_B->project_range(axis, *transform_B, min_B, max_B); - } - - if (withMargin) { - min_A -= margin_A; - max_A += margin_A; - min_B -= margin_B; - max_B += margin_B; - } - - min_B -= (max_A - min_A) * 0.5; - max_B += (max_A - min_A) * 0.5; - - real_t dmin = min_B - (min_A + max_A) * 0.5; - real_t dmax = max_B - (min_A + max_A) * 0.5; - - if (dmin > 0.0 || dmax < 0.0) { - if (callback && callback->sep_axis) { - *callback->sep_axis = axis; - } -#ifdef DEBUG_ENABLED - best_axis_count++; -#endif - - return false; // doesn't contain 0 - } - - //use the smallest depth - - dmin = Math::abs(dmin); - - if (dmax < dmin) { - if (dmax < best_depth) { - best_depth = dmax; - best_axis = axis; -#ifdef DEBUG_ENABLED - best_axis_index = best_axis_count; -#endif - } - } else { - if (dmin < best_depth) { - best_depth = dmin; - best_axis = -axis; // keep it as A axis -#ifdef DEBUG_ENABLED - best_axis_index = best_axis_count; -#endif - } - } - -#ifdef DEBUG_ENABLED - best_axis_count++; -#endif - - return true; - } - - _FORCE_INLINE_ void generate_contacts() { - // nothing to do, don't generate - if (best_axis == Vector2(0.0, 0.0)) { - return; - } - - if (callback) { - callback->collided = true; - - if (!callback->callback) { - return; //only collide, no callback - } - } - static const int max_supports = 2; - - Vector2 supports_A[max_supports]; - int support_count_A; - if (castA) { - shape_A->get_supports_transformed_cast(motion_A, -best_axis, *transform_A, supports_A, support_count_A); - } else { - shape_A->get_supports(transform_A->basis_xform_inv(-best_axis).normalized(), supports_A, support_count_A); - for (int i = 0; i < support_count_A; i++) { - supports_A[i] = transform_A->xform(supports_A[i]); - } - } - - if (withMargin) { - for (int i = 0; i < support_count_A; i++) { - supports_A[i] += -best_axis * margin_A; - } - } - - Vector2 supports_B[max_supports]; - int support_count_B; - if (castB) { - shape_B->get_supports_transformed_cast(motion_B, best_axis, *transform_B, supports_B, support_count_B); - } else { - shape_B->get_supports(transform_B->basis_xform_inv(best_axis).normalized(), supports_B, support_count_B); - for (int i = 0; i < support_count_B; i++) { - supports_B[i] = transform_B->xform(supports_B[i]); - } - } - - if (withMargin) { - for (int i = 0; i < support_count_B; i++) { - supports_B[i] += best_axis * margin_B; - } - } - if (callback) { - callback->normal = best_axis; - _generate_contacts_from_supports(supports_A, support_count_A, supports_B, support_count_B, callback); - - if (callback->sep_axis && *callback->sep_axis != Vector2()) { - *callback->sep_axis = Vector2(); //invalidate previous axis (no test) - } - } - } - - _FORCE_INLINE_ SeparatorAxisTest2D(const ShapeA *p_shape_A, const Transform2D &p_transform_a, const ShapeB *p_shape_B, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_A = Vector2(), const Vector2 &p_motion_B = Vector2(), real_t p_margin_A = 0, real_t p_margin_B = 0) { - margin_A = p_margin_A; - margin_B = p_margin_B; - shape_A = p_shape_A; - shape_B = p_shape_B; - transform_A = &p_transform_a; - transform_B = &p_transform_b; - motion_A = p_motion_A; - motion_B = p_motion_B; - callback = p_collector; - } -}; - -/****** SAT TESTS *******/ - -#define TEST_POINT(m_a, m_b) \ - ((!separator.test_axis(((m_a) - (m_b)).normalized())) || \ - (castA && !separator.test_axis(((m_a) + p_motion_a - (m_b)).normalized())) || \ - (castB && !separator.test_axis(((m_a) - ((m_b) + p_motion_b)).normalized())) || \ - (castA && castB && !separator.test_axis(((m_a) + p_motion_a - ((m_b) + p_motion_b)).normalized()))) - -typedef void (*CollisionFunc)(const Shape2DSW *, const Transform2D &, const Shape2DSW *, const Transform2D &, _CollectorCallback2D *p_collector, const Vector2 &, const Vector2 &, real_t, real_t); - -template -static void _collision_segment_segment(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const SegmentShape2DSW *segment_A = static_cast(p_a); - const SegmentShape2DSW *segment_B = static_cast(p_b); - - SeparatorAxisTest2D separator(segment_A, p_transform_a, segment_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); - - if (!separator.test_previous_axis()) { - return; - } - //this collision is kind of pointless - - if (!separator.test_cast()) { - return; - } - - if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a))) { - return; - } - if (!separator.test_axis(segment_B->get_xformed_normal(p_transform_b))) { - return; - } - - if (withMargin) { - //points grow to circles - - if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), p_transform_b.xform(segment_B->get_a()))) { - return; - } - if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), p_transform_b.xform(segment_B->get_b()))) { - return; - } - if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), p_transform_b.xform(segment_B->get_a()))) { - return; - } - if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), p_transform_b.xform(segment_B->get_b()))) { - return; - } - } - - separator.generate_contacts(); -} - -template -static void _collision_segment_circle(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const SegmentShape2DSW *segment_A = static_cast(p_a); - const CircleShape2DSW *circle_B = static_cast(p_b); - - SeparatorAxisTest2D separator(segment_A, p_transform_a, circle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); - - if (!separator.test_previous_axis()) { - return; - } - - if (!separator.test_cast()) { - return; - } - - //segment normal - if (!separator.test_axis( - (p_transform_a.xform(segment_A->get_b()) - p_transform_a.xform(segment_A->get_a())).normalized().orthogonal())) { - return; - } - - //endpoint a vs circle - if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), p_transform_b.get_origin())) { - return; - } - //endpoint b vs circle - if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), p_transform_b.get_origin())) { - return; - } - - separator.generate_contacts(); -} - -template -static void _collision_segment_rectangle(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const SegmentShape2DSW *segment_A = static_cast(p_a); - const RectangleShape2DSW *rectangle_B = static_cast(p_b); - - SeparatorAxisTest2D separator(segment_A, p_transform_a, rectangle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); - - if (!separator.test_previous_axis()) { - return; - } - - if (!separator.test_cast()) { - return; - } - - if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a))) { - return; - } - - if (!separator.test_axis(p_transform_b.elements[0].normalized())) { - return; - } - - if (!separator.test_axis(p_transform_b.elements[1].normalized())) { - return; - } - - if (withMargin) { - Transform2D inv = p_transform_b.affine_inverse(); - - Vector2 a = p_transform_a.xform(segment_A->get_a()); - Vector2 b = p_transform_a.xform(segment_A->get_b()); - - if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a))) { - return; - } - if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, b))) { - return; - } - - if (castA) { - if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a + p_motion_a))) { - return; - } - if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, b + p_motion_a))) { - return; - } - } - - if (castB) { - if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a - p_motion_b))) { - return; - } - if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, b - p_motion_b))) { - return; - } - } - - if (castA && castB) { - if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a - p_motion_b + p_motion_a))) { - return; - } - if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, b - p_motion_b + p_motion_a))) { - return; - } - } - } - - separator.generate_contacts(); -} - -template -static void _collision_segment_capsule(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const SegmentShape2DSW *segment_A = static_cast(p_a); - const CapsuleShape2DSW *capsule_B = static_cast(p_b); - - SeparatorAxisTest2D separator(segment_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); - - if (!separator.test_previous_axis()) { - return; - } - - if (!separator.test_cast()) { - return; - } - - if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a))) { - return; - } - - if (!separator.test_axis(p_transform_b.elements[0].normalized())) { - return; - } - - real_t capsule_dir = capsule_B->get_height() * 0.5 - capsule_B->get_radius(); - - if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir))) { - return; - } - if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), (p_transform_b.get_origin() - p_transform_b.elements[1] * capsule_dir))) { - return; - } - if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir))) { - return; - } - if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), (p_transform_b.get_origin() - p_transform_b.elements[1] * capsule_dir))) { - return; - } - - separator.generate_contacts(); -} - -template -static void _collision_segment_convex_polygon(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const SegmentShape2DSW *segment_A = static_cast(p_a); - const ConvexPolygonShape2DSW *convex_B = static_cast(p_b); - - SeparatorAxisTest2D separator(segment_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); - - if (!separator.test_previous_axis()) { - return; - } - - if (!separator.test_cast()) { - return; - } - - if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a))) { - return; - } - - for (int i = 0; i < convex_B->get_point_count(); i++) { - if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) { - return; - } - - if (withMargin) { - if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), p_transform_b.xform(convex_B->get_point(i)))) { - return; - } - if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), p_transform_b.xform(convex_B->get_point(i)))) { - return; - } - } - } - - separator.generate_contacts(); -} - -///////// - -template -static void _collision_circle_circle(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const CircleShape2DSW *circle_A = static_cast(p_a); - const CircleShape2DSW *circle_B = static_cast(p_b); - - SeparatorAxisTest2D separator(circle_A, p_transform_a, circle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); - - if (!separator.test_previous_axis()) { - return; - } - - if (!separator.test_cast()) { - return; - } - - if (TEST_POINT(p_transform_a.get_origin(), p_transform_b.get_origin())) { - return; - } - - separator.generate_contacts(); -} - -template -static void _collision_circle_rectangle(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const CircleShape2DSW *circle_A = static_cast(p_a); - const RectangleShape2DSW *rectangle_B = static_cast(p_b); - - SeparatorAxisTest2D separator(circle_A, p_transform_a, rectangle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); - - if (!separator.test_previous_axis()) { - return; - } - - if (!separator.test_cast()) { - return; - } - - const Vector2 &sphere = p_transform_a.elements[2]; - const Vector2 *axis = &p_transform_b.elements[0]; - //const Vector2& half_extents = rectangle_B->get_half_extents(); - - if (!separator.test_axis(axis[0].normalized())) { - return; - } - - if (!separator.test_axis(axis[1].normalized())) { - return; - } - - Transform2D binv = p_transform_b.affine_inverse(); - { - if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphere))) { - return; - } - } - - if (castA) { - Vector2 sphereofs = sphere + p_motion_a; - if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphereofs))) { - return; - } - } - - if (castB) { - Vector2 sphereofs = sphere - p_motion_b; - if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphereofs))) { - return; - } - } - - if (castA && castB) { - Vector2 sphereofs = sphere - p_motion_b + p_motion_a; - if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphereofs))) { - return; - } - } - - separator.generate_contacts(); -} - -template -static void _collision_circle_capsule(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const CircleShape2DSW *circle_A = static_cast(p_a); - const CapsuleShape2DSW *capsule_B = static_cast(p_b); - - SeparatorAxisTest2D separator(circle_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); - - if (!separator.test_previous_axis()) { - return; - } - - if (!separator.test_cast()) { - return; - } - - //capsule axis - if (!separator.test_axis(p_transform_b.elements[0].normalized())) { - return; - } - - real_t capsule_dir = capsule_B->get_height() * 0.5 - capsule_B->get_radius(); - - //capsule endpoints - if (TEST_POINT(p_transform_a.get_origin(), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir))) { - return; - } - if (TEST_POINT(p_transform_a.get_origin(), (p_transform_b.get_origin() - p_transform_b.elements[1] * capsule_dir))) { - return; - } - - separator.generate_contacts(); -} - -template -static void _collision_circle_convex_polygon(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const CircleShape2DSW *circle_A = static_cast(p_a); - const ConvexPolygonShape2DSW *convex_B = static_cast(p_b); - - SeparatorAxisTest2D separator(circle_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); - - if (!separator.test_previous_axis()) { - return; - } - - if (!separator.test_cast()) { - return; - } - - //poly faces and poly points vs circle - for (int i = 0; i < convex_B->get_point_count(); i++) { - if (TEST_POINT(p_transform_a.get_origin(), p_transform_b.xform(convex_B->get_point(i)))) { - return; - } - - if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) { - return; - } - } - - separator.generate_contacts(); -} - -///////// - -template -static void _collision_rectangle_rectangle(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const RectangleShape2DSW *rectangle_A = static_cast(p_a); - const RectangleShape2DSW *rectangle_B = static_cast(p_b); - - SeparatorAxisTest2D separator(rectangle_A, p_transform_a, rectangle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); - - if (!separator.test_previous_axis()) { - return; - } - - if (!separator.test_cast()) { - return; - } - - //box faces A - if (!separator.test_axis(p_transform_a.elements[0].normalized())) { - return; - } - - if (!separator.test_axis(p_transform_a.elements[1].normalized())) { - return; - } - - //box faces B - if (!separator.test_axis(p_transform_b.elements[0].normalized())) { - return; - } - - if (!separator.test_axis(p_transform_b.elements[1].normalized())) { - return; - } - - if (withMargin) { - Transform2D invA = p_transform_a.affine_inverse(); - Transform2D invB = p_transform_b.affine_inverse(); - - if (!separator.test_axis(rectangle_A->get_box_axis(p_transform_a, invA, rectangle_B, p_transform_b, invB))) { - return; - } - - if (castA || castB) { - Transform2D aofs = p_transform_a; - aofs.elements[2] += p_motion_a; - - Transform2D bofs = p_transform_b; - bofs.elements[2] += p_motion_b; - - Transform2D aofsinv = aofs.affine_inverse(); - Transform2D bofsinv = bofs.affine_inverse(); - - if (castA) { - if (!separator.test_axis(rectangle_A->get_box_axis(aofs, aofsinv, rectangle_B, p_transform_b, invB))) { - return; - } - } - - if (castB) { - if (!separator.test_axis(rectangle_A->get_box_axis(p_transform_a, invA, rectangle_B, bofs, bofsinv))) { - return; - } - } - - if (castA && castB) { - if (!separator.test_axis(rectangle_A->get_box_axis(aofs, aofsinv, rectangle_B, bofs, bofsinv))) { - return; - } - } - } - } - - separator.generate_contacts(); -} - -template -static void _collision_rectangle_capsule(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const RectangleShape2DSW *rectangle_A = static_cast(p_a); - const CapsuleShape2DSW *capsule_B = static_cast(p_b); - - SeparatorAxisTest2D separator(rectangle_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); - - if (!separator.test_previous_axis()) { - return; - } - - if (!separator.test_cast()) { - return; - } - - //box faces - if (!separator.test_axis(p_transform_a.elements[0].normalized())) { - return; - } - - if (!separator.test_axis(p_transform_a.elements[1].normalized())) { - return; - } - - //capsule axis - if (!separator.test_axis(p_transform_b.elements[0].normalized())) { - return; - } - - //box endpoints to capsule circles - - Transform2D boxinv = p_transform_a.affine_inverse(); - - real_t capsule_dir = capsule_B->get_height() * 0.5 - capsule_B->get_radius(); - - for (int i = 0; i < 2; i++) { - { - Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir; - - if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) { - return; - } - } - - if (castA) { - Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir; - capsule_endpoint -= p_motion_a; - - if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) { - return; - } - } - - if (castB) { - Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir; - capsule_endpoint += p_motion_b; - - if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) { - return; - } - } - - if (castA && castB) { - Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir; - capsule_endpoint -= p_motion_a; - capsule_endpoint += p_motion_b; - - if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) { - return; - } - } - - capsule_dir *= -1.0; - } - - separator.generate_contacts(); -} - -template -static void _collision_rectangle_convex_polygon(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const RectangleShape2DSW *rectangle_A = static_cast(p_a); - const ConvexPolygonShape2DSW *convex_B = static_cast(p_b); - - SeparatorAxisTest2D separator(rectangle_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); - - if (!separator.test_previous_axis()) { - return; - } - - if (!separator.test_cast()) { - return; - } - - //box faces - if (!separator.test_axis(p_transform_a.elements[0].normalized())) { - return; - } - - if (!separator.test_axis(p_transform_a.elements[1].normalized())) { - return; - } - - //convex faces - Transform2D boxinv; - if (withMargin) { - boxinv = p_transform_a.affine_inverse(); - } - for (int i = 0; i < convex_B->get_point_count(); i++) { - if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) { - return; - } - - if (withMargin) { - //all points vs all points need to be tested if margin exist - if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, p_transform_b.xform(convex_B->get_point(i))))) { - return; - } - if (castA) { - if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, p_transform_b.xform(convex_B->get_point(i)) - p_motion_a))) { - return; - } - } - if (castB) { - if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, p_transform_b.xform(convex_B->get_point(i)) + p_motion_b))) { - return; - } - } - if (castA && castB) { - if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, p_transform_b.xform(convex_B->get_point(i)) + p_motion_b - p_motion_a))) { - return; - } - } - } - } - - separator.generate_contacts(); -} - -///////// - -template -static void _collision_capsule_capsule(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const CapsuleShape2DSW *capsule_A = static_cast(p_a); - const CapsuleShape2DSW *capsule_B = static_cast(p_b); - - SeparatorAxisTest2D separator(capsule_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); - - if (!separator.test_previous_axis()) { - return; - } - - if (!separator.test_cast()) { - return; - } - - //capsule axis - - if (!separator.test_axis(p_transform_b.elements[0].normalized())) { - return; - } - - if (!separator.test_axis(p_transform_a.elements[0].normalized())) { - return; - } - - //capsule endpoints - - real_t capsule_dir_A = capsule_A->get_height() * 0.5 - capsule_A->get_radius(); - for (int i = 0; i < 2; i++) { - Vector2 capsule_endpoint_A = p_transform_a.get_origin() + p_transform_a.elements[1] * capsule_dir_A; - - real_t capsule_dir_B = capsule_B->get_height() * 0.5 - capsule_B->get_radius(); - for (int j = 0; j < 2; j++) { - Vector2 capsule_endpoint_B = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir_B; - - if (TEST_POINT(capsule_endpoint_A, capsule_endpoint_B)) { - return; - } - - capsule_dir_B *= -1.0; - } - - capsule_dir_A *= -1.0; - } - - separator.generate_contacts(); -} - -template -static void _collision_capsule_convex_polygon(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const CapsuleShape2DSW *capsule_A = static_cast(p_a); - const ConvexPolygonShape2DSW *convex_B = static_cast(p_b); - - SeparatorAxisTest2D separator(capsule_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); - - if (!separator.test_previous_axis()) { - return; - } - - if (!separator.test_cast()) { - return; - } - - //capsule axis - - if (!separator.test_axis(p_transform_a.elements[0].normalized())) { - return; - } - - //poly vs capsule - for (int i = 0; i < convex_B->get_point_count(); i++) { - Vector2 cpoint = p_transform_b.xform(convex_B->get_point(i)); - - real_t capsule_dir = capsule_A->get_height() * 0.5 - capsule_A->get_radius(); - for (int j = 0; j < 2; j++) { - Vector2 capsule_endpoint_A = p_transform_a.get_origin() + p_transform_a.elements[1] * capsule_dir; - - if (TEST_POINT(capsule_endpoint_A, cpoint)) { - return; - } - - capsule_dir *= -1.0; - } - - if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) { - return; - } - } - - separator.generate_contacts(); -} - -///////// - -template -static void _collision_convex_polygon_convex_polygon(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const ConvexPolygonShape2DSW *convex_A = static_cast(p_a); - const ConvexPolygonShape2DSW *convex_B = static_cast(p_b); - - SeparatorAxisTest2D separator(convex_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); - - if (!separator.test_previous_axis()) { - return; - } - - if (!separator.test_cast()) { - return; - } - - for (int i = 0; i < convex_A->get_point_count(); i++) { - if (!separator.test_axis(convex_A->get_xformed_segment_normal(p_transform_a, i))) { - return; - } - } - - for (int i = 0; i < convex_B->get_point_count(); i++) { - if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) { - return; - } - } - - if (withMargin) { - for (int i = 0; i < convex_A->get_point_count(); i++) { - for (int j = 0; j < convex_B->get_point_count(); j++) { - if (TEST_POINT(p_transform_a.xform(convex_A->get_point(i)), p_transform_b.xform(convex_B->get_point(j)))) { - return; - } - } - } - } - - separator.generate_contacts(); -} - -//////// - -bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CollisionSolver2DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap, Vector2 *sep_axis, real_t p_margin_A, real_t p_margin_B) { - PhysicsServer2D::ShapeType type_A = p_shape_A->get_type(); - - ERR_FAIL_COND_V(type_A == PhysicsServer2D::SHAPE_WORLD_BOUNDARY, false); - ERR_FAIL_COND_V(type_A == PhysicsServer2D::SHAPE_SEPARATION_RAY, false); - ERR_FAIL_COND_V(p_shape_A->is_concave(), false); - - PhysicsServer2D::ShapeType type_B = p_shape_B->get_type(); - - ERR_FAIL_COND_V(type_B == PhysicsServer2D::SHAPE_WORLD_BOUNDARY, false); - ERR_FAIL_COND_V(type_B == PhysicsServer2D::SHAPE_SEPARATION_RAY, false); - ERR_FAIL_COND_V(p_shape_B->is_concave(), false); - - static const CollisionFunc collision_table[5][5] = { - { _collision_segment_segment, - _collision_segment_circle, - _collision_segment_rectangle, - _collision_segment_capsule, - _collision_segment_convex_polygon }, - { nullptr, - _collision_circle_circle, - _collision_circle_rectangle, - _collision_circle_capsule, - _collision_circle_convex_polygon }, - { nullptr, - nullptr, - _collision_rectangle_rectangle, - _collision_rectangle_capsule, - _collision_rectangle_convex_polygon }, - { nullptr, - nullptr, - nullptr, - _collision_capsule_capsule, - _collision_capsule_convex_polygon }, - { nullptr, - nullptr, - nullptr, - nullptr, - _collision_convex_polygon_convex_polygon } - - }; - - static const CollisionFunc collision_table_castA[5][5] = { - { _collision_segment_segment, - _collision_segment_circle, - _collision_segment_rectangle, - _collision_segment_capsule, - _collision_segment_convex_polygon }, - { nullptr, - _collision_circle_circle, - _collision_circle_rectangle, - _collision_circle_capsule, - _collision_circle_convex_polygon }, - { nullptr, - nullptr, - _collision_rectangle_rectangle, - _collision_rectangle_capsule, - _collision_rectangle_convex_polygon }, - { nullptr, - nullptr, - nullptr, - _collision_capsule_capsule, - _collision_capsule_convex_polygon }, - { nullptr, - nullptr, - nullptr, - nullptr, - _collision_convex_polygon_convex_polygon } - - }; - - static const CollisionFunc collision_table_castB[5][5] = { - { _collision_segment_segment, - _collision_segment_circle, - _collision_segment_rectangle, - _collision_segment_capsule, - _collision_segment_convex_polygon }, - { nullptr, - _collision_circle_circle, - _collision_circle_rectangle, - _collision_circle_capsule, - _collision_circle_convex_polygon }, - { nullptr, - nullptr, - _collision_rectangle_rectangle, - _collision_rectangle_capsule, - _collision_rectangle_convex_polygon }, - { nullptr, - nullptr, - nullptr, - _collision_capsule_capsule, - _collision_capsule_convex_polygon }, - { nullptr, - nullptr, - nullptr, - nullptr, - _collision_convex_polygon_convex_polygon } - - }; - - static const CollisionFunc collision_table_castA_castB[5][5] = { - { _collision_segment_segment, - _collision_segment_circle, - _collision_segment_rectangle, - _collision_segment_capsule, - _collision_segment_convex_polygon }, - { nullptr, - _collision_circle_circle, - _collision_circle_rectangle, - _collision_circle_capsule, - _collision_circle_convex_polygon }, - { nullptr, - nullptr, - _collision_rectangle_rectangle, - _collision_rectangle_capsule, - _collision_rectangle_convex_polygon }, - { nullptr, - nullptr, - nullptr, - _collision_capsule_capsule, - _collision_capsule_convex_polygon }, - { nullptr, - nullptr, - nullptr, - nullptr, - _collision_convex_polygon_convex_polygon } - - }; - - static const CollisionFunc collision_table_margin[5][5] = { - { _collision_segment_segment, - _collision_segment_circle, - _collision_segment_rectangle, - _collision_segment_capsule, - _collision_segment_convex_polygon }, - { nullptr, - _collision_circle_circle, - _collision_circle_rectangle, - _collision_circle_capsule, - _collision_circle_convex_polygon }, - { nullptr, - nullptr, - _collision_rectangle_rectangle, - _collision_rectangle_capsule, - _collision_rectangle_convex_polygon }, - { nullptr, - nullptr, - nullptr, - _collision_capsule_capsule, - _collision_capsule_convex_polygon }, - { nullptr, - nullptr, - nullptr, - nullptr, - _collision_convex_polygon_convex_polygon } - - }; - - static const CollisionFunc collision_table_castA_margin[5][5] = { - { _collision_segment_segment, - _collision_segment_circle, - _collision_segment_rectangle, - _collision_segment_capsule, - _collision_segment_convex_polygon }, - { nullptr, - _collision_circle_circle, - _collision_circle_rectangle, - _collision_circle_capsule, - _collision_circle_convex_polygon }, - { nullptr, - nullptr, - _collision_rectangle_rectangle, - _collision_rectangle_capsule, - _collision_rectangle_convex_polygon }, - { nullptr, - nullptr, - nullptr, - _collision_capsule_capsule, - _collision_capsule_convex_polygon }, - { nullptr, - nullptr, - nullptr, - nullptr, - _collision_convex_polygon_convex_polygon } - - }; - - static const CollisionFunc collision_table_castB_margin[5][5] = { - { _collision_segment_segment, - _collision_segment_circle, - _collision_segment_rectangle, - _collision_segment_capsule, - _collision_segment_convex_polygon }, - { nullptr, - _collision_circle_circle, - _collision_circle_rectangle, - _collision_circle_capsule, - _collision_circle_convex_polygon }, - { nullptr, - nullptr, - _collision_rectangle_rectangle, - _collision_rectangle_capsule, - _collision_rectangle_convex_polygon }, - { nullptr, - nullptr, - nullptr, - _collision_capsule_capsule, - _collision_capsule_convex_polygon }, - { nullptr, - nullptr, - nullptr, - nullptr, - _collision_convex_polygon_convex_polygon } - - }; - - static const CollisionFunc collision_table_castA_castB_margin[5][5] = { - { _collision_segment_segment, - _collision_segment_circle, - _collision_segment_rectangle, - _collision_segment_capsule, - _collision_segment_convex_polygon }, - { nullptr, - _collision_circle_circle, - _collision_circle_rectangle, - _collision_circle_capsule, - _collision_circle_convex_polygon }, - { nullptr, - nullptr, - _collision_rectangle_rectangle, - _collision_rectangle_capsule, - _collision_rectangle_convex_polygon }, - { nullptr, - nullptr, - nullptr, - _collision_capsule_capsule, - _collision_capsule_convex_polygon }, - { nullptr, - nullptr, - nullptr, - nullptr, - _collision_convex_polygon_convex_polygon } - - }; - - _CollectorCallback2D callback; - callback.callback = p_result_callback; - callback.swap = p_swap; - callback.userdata = p_userdata; - callback.collided = false; - callback.sep_axis = sep_axis; - - const Shape2DSW *A = p_shape_A; - const Shape2DSW *B = p_shape_B; - const Transform2D *transform_A = &p_transform_A; - const Transform2D *transform_B = &p_transform_B; - const Vector2 *motion_A = &p_motion_A; - const Vector2 *motion_B = &p_motion_B; - real_t margin_A = p_margin_A, margin_B = p_margin_B; - - if (type_A > type_B) { - SWAP(A, B); - SWAP(transform_A, transform_B); - SWAP(type_A, type_B); - SWAP(motion_A, motion_B); - SWAP(margin_A, margin_B); - callback.swap = !callback.swap; - } - - CollisionFunc collision_func; - - if (p_margin_A || p_margin_B) { - if (*motion_A == Vector2() && *motion_B == Vector2()) { - collision_func = collision_table_margin[type_A - 2][type_B - 2]; - } else if (*motion_A != Vector2() && *motion_B == Vector2()) { - collision_func = collision_table_castA_margin[type_A - 2][type_B - 2]; - } else if (*motion_A == Vector2() && *motion_B != Vector2()) { - collision_func = collision_table_castB_margin[type_A - 2][type_B - 2]; - } else { - collision_func = collision_table_castA_castB_margin[type_A - 2][type_B - 2]; - } - } else { - if (*motion_A == Vector2() && *motion_B == Vector2()) { - collision_func = collision_table[type_A - 2][type_B - 2]; - } else if (*motion_A != Vector2() && *motion_B == Vector2()) { - collision_func = collision_table_castA[type_A - 2][type_B - 2]; - } else if (*motion_A == Vector2() && *motion_B != Vector2()) { - collision_func = collision_table_castB[type_A - 2][type_B - 2]; - } else { - collision_func = collision_table_castA_castB[type_A - 2][type_B - 2]; - } - } - - ERR_FAIL_COND_V(!collision_func, false); - - collision_func(A, *transform_A, B, *transform_B, &callback, *motion_A, *motion_B, margin_A, margin_B); - - return callback.collided; -} diff --git a/servers/physics_2d/collision_solver_2d_sat.h b/servers/physics_2d/collision_solver_2d_sat.h deleted file mode 100644 index 49cc5176f9..0000000000 --- a/servers/physics_2d/collision_solver_2d_sat.h +++ /dev/null @@ -1,38 +0,0 @@ -/*************************************************************************/ -/* collision_solver_2d_sat.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 COLLISION_SOLVER_2D_SAT_H -#define COLLISION_SOLVER_2D_SAT_H - -#include "collision_solver_2d_sw.h" - -bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CollisionSolver2DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap = false, Vector2 *sep_axis = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0); - -#endif // COLLISION_SOLVER_2D_SAT_H diff --git a/servers/physics_2d/collision_solver_2d_sw.cpp b/servers/physics_2d/collision_solver_2d_sw.cpp deleted file mode 100644 index 527bb1b0b2..0000000000 --- a/servers/physics_2d/collision_solver_2d_sw.cpp +++ /dev/null @@ -1,264 +0,0 @@ -/*************************************************************************/ -/* collision_solver_2d_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/*************************************************************************/ - -#include "collision_solver_2d_sw.h" -#include "collision_solver_2d_sat.h" - -#define collision_solver sat_2d_calculate_penetration -//#define collision_solver gjk_epa_calculate_penetration - -bool CollisionSolver2DSW::solve_static_world_boundary(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) { - const WorldBoundaryShape2DSW *world_boundary = static_cast(p_shape_A); - if (p_shape_B->get_type() == PhysicsServer2D::SHAPE_WORLD_BOUNDARY) { - return false; - } - - Vector2 n = p_transform_A.basis_xform(world_boundary->get_normal()).normalized(); - Vector2 p = p_transform_A.xform(world_boundary->get_normal() * world_boundary->get_d()); - real_t d = n.dot(p); - - Vector2 supports[2]; - int support_count; - - p_shape_B->get_supports(p_transform_B.affine_inverse().basis_xform(-n).normalized(), supports, support_count); - - bool found = false; - - for (int i = 0; i < support_count; i++) { - supports[i] = p_transform_B.xform(supports[i]); - real_t pd = n.dot(supports[i]); - if (pd >= d) { - continue; - } - found = true; - - Vector2 support_A = supports[i] - n * (pd - d); - - if (p_result_callback) { - if (p_swap_result) { - p_result_callback(supports[i], support_A, p_userdata); - } else { - p_result_callback(support_A, supports[i], p_userdata); - } - } - } - - return found; -} - -bool CollisionSolver2DSW::solve_separation_ray(const Shape2DSW *p_shape_A, const Vector2 &p_motion_A, const Transform2D &p_transform_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *r_sep_axis, real_t p_margin) { - const SeparationRayShape2DSW *ray = static_cast(p_shape_A); - if (p_shape_B->get_type() == PhysicsServer2D::SHAPE_SEPARATION_RAY) { - return false; - } - - Vector2 from = p_transform_A.get_origin(); - Vector2 to = from + p_transform_A[1] * (ray->get_length() + p_margin); - if (p_motion_A != Vector2()) { - //not the best but should be enough - Vector2 normal = (to - from).normalized(); - to += normal * MAX(0.0, normal.dot(p_motion_A)); - } - Vector2 support_A = to; - - Transform2D invb = p_transform_B.affine_inverse(); - from = invb.xform(from); - to = invb.xform(to); - - Vector2 p, n; - if (!p_shape_B->intersect_segment(from, to, p, n)) { - if (r_sep_axis) { - *r_sep_axis = p_transform_A[1].normalized(); - } - return false; - } - - // Discard contacts when the ray is fully contained inside the shape. - if (n == Vector2()) { - if (r_sep_axis) { - *r_sep_axis = p_transform_A[1].normalized(); - } - return false; - } - - // Discard contacts in the wrong direction. - if (n.dot(from - to) < CMP_EPSILON) { - if (r_sep_axis) { - *r_sep_axis = p_transform_A[1].normalized(); - } - return false; - } - - Vector2 support_B = p_transform_B.xform(p); - if (ray->get_slide_on_slope()) { - Vector2 global_n = invb.basis_xform_inv(n).normalized(); - support_B = support_A + (support_B - support_A).length() * global_n; - } - - if (p_result_callback) { - if (p_swap_result) { - p_result_callback(support_B, support_A, p_userdata); - } else { - p_result_callback(support_A, support_B, p_userdata); - } - } - return true; -} - -struct _ConcaveCollisionInfo2D { - const Transform2D *transform_A = nullptr; - const Shape2DSW *shape_A = nullptr; - const Transform2D *transform_B = nullptr; - Vector2 motion_A; - Vector2 motion_B; - real_t margin_A = 0.0; - real_t margin_B = 0.0; - CollisionSolver2DSW::CallbackResult result_callback; - void *userdata = nullptr; - bool swap_result = false; - bool collided = false; - int aabb_tests = 0; - int collisions = 0; - Vector2 *sep_axis = nullptr; -}; - -bool CollisionSolver2DSW::concave_callback(void *p_userdata, Shape2DSW *p_convex) { - _ConcaveCollisionInfo2D &cinfo = *(_ConcaveCollisionInfo2D *)(p_userdata); - cinfo.aabb_tests++; - - bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, cinfo.motion_A, p_convex, *cinfo.transform_B, cinfo.motion_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result, cinfo.sep_axis, cinfo.margin_A, cinfo.margin_B); - if (!collided) { - return false; - } - - cinfo.collided = true; - cinfo.collisions++; - - // Stop at first collision if contacts are not needed. - return !cinfo.result_callback; -} - -bool CollisionSolver2DSW::solve_concave(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *r_sep_axis, real_t p_margin_A, real_t p_margin_B) { - const ConcaveShape2DSW *concave_B = static_cast(p_shape_B); - - _ConcaveCollisionInfo2D cinfo; - cinfo.transform_A = &p_transform_A; - cinfo.shape_A = p_shape_A; - cinfo.transform_B = &p_transform_B; - cinfo.motion_A = p_motion_A; - cinfo.result_callback = p_result_callback; - cinfo.userdata = p_userdata; - cinfo.swap_result = p_swap_result; - cinfo.collided = false; - cinfo.collisions = 0; - cinfo.sep_axis = r_sep_axis; - cinfo.margin_A = p_margin_A; - cinfo.margin_B = p_margin_B; - - cinfo.aabb_tests = 0; - - Transform2D rel_transform = p_transform_A; - rel_transform.elements[2] -= p_transform_B.get_origin(); - - //quickly compute a local Rect2 - - Rect2 local_aabb; - for (int i = 0; i < 2; i++) { - Vector2 axis(p_transform_B.elements[i]); - real_t axis_scale = 1.0 / axis.length(); - axis *= axis_scale; - - real_t smin, smax; - p_shape_A->project_rangev(axis, rel_transform, smin, smax); - smin *= axis_scale; - smax *= axis_scale; - - local_aabb.position[i] = smin; - local_aabb.size[i] = smax - smin; - } - - concave_B->cull(local_aabb, concave_callback, &cinfo); - - return cinfo.collided; -} - -bool CollisionSolver2DSW::solve(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, Vector2 *r_sep_axis, real_t p_margin_A, real_t p_margin_B) { - PhysicsServer2D::ShapeType type_A = p_shape_A->get_type(); - PhysicsServer2D::ShapeType type_B = p_shape_B->get_type(); - bool concave_A = p_shape_A->is_concave(); - bool concave_B = p_shape_B->is_concave(); - real_t margin_A = p_margin_A, margin_B = p_margin_B; - - bool swap = false; - - if (type_A > type_B) { - SWAP(type_A, type_B); - SWAP(concave_A, concave_B); - SWAP(margin_A, margin_B); - swap = true; - } - - if (type_A == PhysicsServer2D::SHAPE_WORLD_BOUNDARY) { - if (type_B == PhysicsServer2D::SHAPE_WORLD_BOUNDARY) { - return false; - } - - if (swap) { - return solve_static_world_boundary(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true); - } else { - return solve_static_world_boundary(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false); - } - - } else if (type_A == PhysicsServer2D::SHAPE_SEPARATION_RAY) { - if (type_B == PhysicsServer2D::SHAPE_SEPARATION_RAY) { - return false; //no ray-ray - } - - if (swap) { - return solve_separation_ray(p_shape_B, p_motion_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true, r_sep_axis, p_margin_B); - } else { - return solve_separation_ray(p_shape_A, p_motion_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false, r_sep_axis, p_margin_A); - } - - } else if (concave_B) { - if (concave_A) { - return false; - } - - if (!swap) { - return solve_concave(p_shape_A, p_transform_A, p_motion_A, p_shape_B, p_transform_B, p_motion_B, p_result_callback, p_userdata, false, r_sep_axis, margin_A, margin_B); - } else { - return solve_concave(p_shape_B, p_transform_B, p_motion_B, p_shape_A, p_transform_A, p_motion_A, p_result_callback, p_userdata, true, r_sep_axis, margin_A, margin_B); - } - - } else { - return collision_solver(p_shape_A, p_transform_A, p_motion_A, p_shape_B, p_transform_B, p_motion_B, p_result_callback, p_userdata, false, r_sep_axis, margin_A, margin_B); - } -} diff --git a/servers/physics_2d/collision_solver_2d_sw.h b/servers/physics_2d/collision_solver_2d_sw.h deleted file mode 100644 index b87247b89a..0000000000 --- a/servers/physics_2d/collision_solver_2d_sw.h +++ /dev/null @@ -1,50 +0,0 @@ -/*************************************************************************/ -/* collision_solver_2d_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/*************************************************************************/ - -#ifndef COLLISION_SOLVER_2D_SW_H -#define COLLISION_SOLVER_2D_SW_H - -#include "shape_2d_sw.h" - -class CollisionSolver2DSW { -public: - typedef void (*CallbackResult)(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_userdata); - -private: - static bool solve_static_world_boundary(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); - static bool concave_callback(void *p_userdata, Shape2DSW *p_convex); - static bool solve_concave(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *r_sep_axis = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0); - static bool solve_separation_ray(const Shape2DSW *p_shape_A, const Vector2 &p_motion_A, const Transform2D &p_transform_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *r_sep_axis = nullptr, real_t p_margin = 0); - -public: - static bool solve(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, Vector2 *r_sep_axis = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0); -}; - -#endif // COLLISION_SOLVER_2D_SW_H diff --git a/servers/physics_2d/constraint_2d_sw.h b/servers/physics_2d/constraint_2d_sw.h deleted file mode 100644 index df300d666d..0000000000 --- a/servers/physics_2d/constraint_2d_sw.h +++ /dev/null @@ -1,70 +0,0 @@ -/*************************************************************************/ -/* constraint_2d_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/*************************************************************************/ - -#ifndef CONSTRAINT_2D_SW_H -#define CONSTRAINT_2D_SW_H - -#include "body_2d_sw.h" - -class Constraint2DSW { - Body2DSW **_body_ptr; - int _body_count; - uint64_t island_step = 0; - bool disabled_collisions_between_bodies = true; - - RID self; - -protected: - Constraint2DSW(Body2DSW **p_body_ptr = nullptr, int p_body_count = 0) { - _body_ptr = p_body_ptr; - _body_count = p_body_count; - } - -public: - _FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; } - _FORCE_INLINE_ RID get_self() const { return self; } - - _FORCE_INLINE_ uint64_t get_island_step() const { return island_step; } - _FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step = p_step; } - - _FORCE_INLINE_ Body2DSW **get_body_ptr() const { return _body_ptr; } - _FORCE_INLINE_ int get_body_count() const { return _body_count; } - - _FORCE_INLINE_ void disable_collisions_between_bodies(const bool p_disabled) { disabled_collisions_between_bodies = p_disabled; } - _FORCE_INLINE_ bool is_disabled_collisions_between_bodies() const { return disabled_collisions_between_bodies; } - - virtual bool setup(real_t p_step) = 0; - virtual bool pre_solve(real_t p_step) = 0; - virtual void solve(real_t p_step) = 0; - - virtual ~Constraint2DSW() {} -}; - -#endif // CONSTRAINT_2D_SW_H diff --git a/servers/physics_2d/godot_area_2d.cpp b/servers/physics_2d/godot_area_2d.cpp new file mode 100644 index 0000000000..7cb202dd1f --- /dev/null +++ b/servers/physics_2d/godot_area_2d.cpp @@ -0,0 +1,305 @@ +/*************************************************************************/ +/* godot_area_2d.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 "godot_area_2d.h" +#include "godot_body_2d.h" +#include "godot_space_2d.h" + +GodotArea2D::BodyKey::BodyKey(GodotBody2D *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { + rid = p_body->get_self(); + instance_id = p_body->get_instance_id(); + body_shape = p_body_shape; + area_shape = p_area_shape; +} + +GodotArea2D::BodyKey::BodyKey(GodotArea2D *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { + rid = p_body->get_self(); + instance_id = p_body->get_instance_id(); + body_shape = p_body_shape; + area_shape = p_area_shape; +} + +void GodotArea2D::_shapes_changed() { + if (!moved_list.in_list() && get_space()) { + get_space()->area_add_to_moved_list(&moved_list); + } +} + +void GodotArea2D::set_transform(const Transform2D &p_transform) { + if (!moved_list.in_list() && get_space()) { + get_space()->area_add_to_moved_list(&moved_list); + } + + _set_transform(p_transform); + _set_inv_transform(p_transform.affine_inverse()); +} + +void GodotArea2D::set_space(GodotSpace2D *p_space) { + if (get_space()) { + if (monitor_query_list.in_list()) { + get_space()->area_remove_from_monitor_query_list(&monitor_query_list); + } + if (moved_list.in_list()) { + get_space()->area_remove_from_moved_list(&moved_list); + } + } + + monitored_bodies.clear(); + monitored_areas.clear(); + + _set_space(p_space); +} + +void GodotArea2D::set_monitor_callback(ObjectID p_id, const StringName &p_method) { + if (p_id == monitor_callback_id) { + monitor_callback_method = p_method; + return; + } + + _unregister_shapes(); + + monitor_callback_id = p_id; + monitor_callback_method = p_method; + + monitored_bodies.clear(); + monitored_areas.clear(); + + _shape_changed(); + + if (!moved_list.in_list() && get_space()) { + get_space()->area_add_to_moved_list(&moved_list); + } +} + +void GodotArea2D::set_area_monitor_callback(ObjectID p_id, const StringName &p_method) { + if (p_id == area_monitor_callback_id) { + area_monitor_callback_method = p_method; + return; + } + + _unregister_shapes(); + + area_monitor_callback_id = p_id; + area_monitor_callback_method = p_method; + + monitored_bodies.clear(); + monitored_areas.clear(); + + _shape_changed(); + + if (!moved_list.in_list() && get_space()) { + get_space()->area_add_to_moved_list(&moved_list); + } +} + +void GodotArea2D::set_space_override_mode(PhysicsServer2D::AreaSpaceOverrideMode p_mode) { + bool do_override = p_mode != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED; + if (do_override == (space_override_mode != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED)) { + return; + } + _unregister_shapes(); + space_override_mode = p_mode; + _shape_changed(); +} + +void GodotArea2D::set_param(PhysicsServer2D::AreaParameter p_param, const Variant &p_value) { + switch (p_param) { + case PhysicsServer2D::AREA_PARAM_GRAVITY: + gravity = p_value; + break; + case PhysicsServer2D::AREA_PARAM_GRAVITY_VECTOR: + gravity_vector = p_value; + break; + case PhysicsServer2D::AREA_PARAM_GRAVITY_IS_POINT: + gravity_is_point = p_value; + break; + case PhysicsServer2D::AREA_PARAM_GRAVITY_DISTANCE_SCALE: + gravity_distance_scale = p_value; + break; + case PhysicsServer2D::AREA_PARAM_GRAVITY_POINT_ATTENUATION: + point_attenuation = p_value; + break; + case PhysicsServer2D::AREA_PARAM_LINEAR_DAMP: + linear_damp = p_value; + break; + case PhysicsServer2D::AREA_PARAM_ANGULAR_DAMP: + angular_damp = p_value; + break; + case PhysicsServer2D::AREA_PARAM_PRIORITY: + priority = p_value; + break; + } +} + +Variant GodotArea2D::get_param(PhysicsServer2D::AreaParameter p_param) const { + switch (p_param) { + case PhysicsServer2D::AREA_PARAM_GRAVITY: + return gravity; + case PhysicsServer2D::AREA_PARAM_GRAVITY_VECTOR: + return gravity_vector; + case PhysicsServer2D::AREA_PARAM_GRAVITY_IS_POINT: + return gravity_is_point; + case PhysicsServer2D::AREA_PARAM_GRAVITY_DISTANCE_SCALE: + return gravity_distance_scale; + case PhysicsServer2D::AREA_PARAM_GRAVITY_POINT_ATTENUATION: + return point_attenuation; + case PhysicsServer2D::AREA_PARAM_LINEAR_DAMP: + return linear_damp; + case PhysicsServer2D::AREA_PARAM_ANGULAR_DAMP: + return angular_damp; + case PhysicsServer2D::AREA_PARAM_PRIORITY: + return priority; + } + + return Variant(); +} + +void GodotArea2D::_queue_monitor_update() { + ERR_FAIL_COND(!get_space()); + + if (!monitor_query_list.in_list()) { + get_space()->area_add_to_monitor_query_list(&monitor_query_list); + } +} + +void GodotArea2D::set_monitorable(bool p_monitorable) { + if (monitorable == p_monitorable) { + return; + } + + monitorable = p_monitorable; + _set_static(!monitorable); +} + +void GodotArea2D::call_queries() { + if (monitor_callback_id.is_valid() && !monitored_bodies.is_empty()) { + Variant res[5]; + Variant *resptr[5]; + for (int i = 0; i < 5; i++) { + resptr[i] = &res[i]; + } + + Object *obj = ObjectDB::get_instance(monitor_callback_id); + if (!obj) { + monitored_bodies.clear(); + monitor_callback_id = ObjectID(); + return; + } + + for (Map::Element *E = monitored_bodies.front(); E;) { + if (E->get().state == 0) { // Nothing happened + Map::Element *next = E->next(); + monitored_bodies.erase(E); + E = next; + continue; + } + + res[0] = E->get().state > 0 ? PhysicsServer2D::AREA_BODY_ADDED : PhysicsServer2D::AREA_BODY_REMOVED; + res[1] = E->key().rid; + res[2] = E->key().instance_id; + res[3] = E->key().body_shape; + res[4] = E->key().area_shape; + + Map::Element *next = E->next(); + monitored_bodies.erase(E); + E = next; + + Callable::CallError ce; + obj->call(monitor_callback_method, (const Variant **)resptr, 5, ce); + } + } + + if (area_monitor_callback_id.is_valid() && !monitored_areas.is_empty()) { + Variant res[5]; + Variant *resptr[5]; + for (int i = 0; i < 5; i++) { + resptr[i] = &res[i]; + } + + Object *obj = ObjectDB::get_instance(area_monitor_callback_id); + if (!obj) { + monitored_areas.clear(); + area_monitor_callback_id = ObjectID(); + return; + } + + for (Map::Element *E = monitored_areas.front(); E;) { + if (E->get().state == 0) { // Nothing happened + Map::Element *next = E->next(); + monitored_areas.erase(E); + E = next; + continue; + } + + res[0] = E->get().state > 0 ? PhysicsServer2D::AREA_BODY_ADDED : PhysicsServer2D::AREA_BODY_REMOVED; + res[1] = E->key().rid; + res[2] = E->key().instance_id; + res[3] = E->key().body_shape; + res[4] = E->key().area_shape; + + Map::Element *next = E->next(); + monitored_areas.erase(E); + E = next; + + Callable::CallError ce; + obj->call(area_monitor_callback_method, (const Variant **)resptr, 5, ce); + } + } +} + +void GodotArea2D::compute_gravity(const Vector2 &p_position, Vector2 &r_gravity) const { + if (is_gravity_point()) { + const real_t gravity_distance_scale = get_gravity_distance_scale(); + Vector2 v = get_transform().xform(get_gravity_vector()) - p_position; + if (gravity_distance_scale > 0) { + const real_t v_length = v.length(); + if (v_length > 0) { + const real_t v_scaled = v_length * gravity_distance_scale; + r_gravity = (v.normalized() * (get_gravity() / (v_scaled * v_scaled))); + } else { + r_gravity = Vector2(); + } + } else { + r_gravity = v.normalized() * get_gravity(); + } + } else { + r_gravity = get_gravity_vector() * get_gravity(); + } +} + +GodotArea2D::GodotArea2D() : + GodotCollisionObject2D(TYPE_AREA), + monitor_query_list(this), + moved_list(this) { + _set_static(true); //areas are not active by default +} + +GodotArea2D::~GodotArea2D() { +} diff --git a/servers/physics_2d/godot_area_2d.h b/servers/physics_2d/godot_area_2d.h new file mode 100644 index 0000000000..daa03d39e3 --- /dev/null +++ b/servers/physics_2d/godot_area_2d.h @@ -0,0 +1,196 @@ +/*************************************************************************/ +/* godot_area_2d.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 GODOT_AREA_2D_H +#define GODOT_AREA_2D_H + +#include "godot_collision_object_2d.h" + +#include "core/templates/self_list.h" +#include "servers/physics_server_2d.h" + +class GodotSpace2D; +class GodotBody2D; +class GodotConstraint2D; + +class GodotArea2D : public GodotCollisionObject2D { + PhysicsServer2D::AreaSpaceOverrideMode space_override_mode = PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED; + real_t gravity = 9.80665; + Vector2 gravity_vector = Vector2(0, -1); + bool gravity_is_point = false; + real_t gravity_distance_scale = 0.0; + real_t point_attenuation = 1.0; + real_t linear_damp = 0.1; + real_t angular_damp = 1.0; + int priority = 0; + bool monitorable = false; + + ObjectID monitor_callback_id; + StringName monitor_callback_method; + + ObjectID area_monitor_callback_id; + StringName area_monitor_callback_method; + + SelfList monitor_query_list; + SelfList moved_list; + + struct BodyKey { + RID rid; + ObjectID instance_id; + uint32_t body_shape = 0; + uint32_t area_shape = 0; + + _FORCE_INLINE_ bool operator<(const BodyKey &p_key) const { + if (rid == p_key.rid) { + if (body_shape == p_key.body_shape) { + return area_shape < p_key.area_shape; + } else { + return body_shape < p_key.body_shape; + } + } else { + return rid < p_key.rid; + } + } + + _FORCE_INLINE_ BodyKey() {} + BodyKey(GodotBody2D *p_body, uint32_t p_body_shape, uint32_t p_area_shape); + BodyKey(GodotArea2D *p_body, uint32_t p_body_shape, uint32_t p_area_shape); + }; + + struct BodyState { + int state = 0; + _FORCE_INLINE_ void inc() { state++; } + _FORCE_INLINE_ void dec() { state--; } + }; + + Map monitored_bodies; + Map monitored_areas; + + Set constraints; + + virtual void _shapes_changed(); + void _queue_monitor_update(); + +public: + void set_monitor_callback(ObjectID p_id, const StringName &p_method); + _FORCE_INLINE_ bool has_monitor_callback() const { return monitor_callback_id.is_valid(); } + + void set_area_monitor_callback(ObjectID p_id, const StringName &p_method); + _FORCE_INLINE_ bool has_area_monitor_callback() const { return area_monitor_callback_id.is_valid(); } + + _FORCE_INLINE_ void add_body_to_query(GodotBody2D *p_body, uint32_t p_body_shape, uint32_t p_area_shape); + _FORCE_INLINE_ void remove_body_from_query(GodotBody2D *p_body, uint32_t p_body_shape, uint32_t p_area_shape); + + _FORCE_INLINE_ void add_area_to_query(GodotArea2D *p_area, uint32_t p_area_shape, uint32_t p_self_shape); + _FORCE_INLINE_ void remove_area_from_query(GodotArea2D *p_area, uint32_t p_area_shape, uint32_t p_self_shape); + + void set_param(PhysicsServer2D::AreaParameter p_param, const Variant &p_value); + Variant get_param(PhysicsServer2D::AreaParameter p_param) const; + + void set_space_override_mode(PhysicsServer2D::AreaSpaceOverrideMode p_mode); + PhysicsServer2D::AreaSpaceOverrideMode get_space_override_mode() const { return space_override_mode; } + + _FORCE_INLINE_ void set_gravity(real_t p_gravity) { gravity = p_gravity; } + _FORCE_INLINE_ real_t get_gravity() const { return gravity; } + + _FORCE_INLINE_ void set_gravity_vector(const Vector2 &p_gravity) { gravity_vector = p_gravity; } + _FORCE_INLINE_ Vector2 get_gravity_vector() const { return gravity_vector; } + + _FORCE_INLINE_ void set_gravity_as_point(bool p_enable) { gravity_is_point = p_enable; } + _FORCE_INLINE_ bool is_gravity_point() const { return gravity_is_point; } + + _FORCE_INLINE_ void set_gravity_distance_scale(real_t scale) { gravity_distance_scale = scale; } + _FORCE_INLINE_ real_t get_gravity_distance_scale() const { return gravity_distance_scale; } + + _FORCE_INLINE_ void set_point_attenuation(real_t p_point_attenuation) { point_attenuation = p_point_attenuation; } + _FORCE_INLINE_ real_t get_point_attenuation() const { return point_attenuation; } + + _FORCE_INLINE_ void set_linear_damp(real_t p_linear_damp) { linear_damp = p_linear_damp; } + _FORCE_INLINE_ real_t get_linear_damp() const { return linear_damp; } + + _FORCE_INLINE_ void set_angular_damp(real_t p_angular_damp) { angular_damp = p_angular_damp; } + _FORCE_INLINE_ real_t get_angular_damp() const { return angular_damp; } + + _FORCE_INLINE_ void set_priority(int p_priority) { priority = p_priority; } + _FORCE_INLINE_ int get_priority() const { return priority; } + + _FORCE_INLINE_ void add_constraint(GodotConstraint2D *p_constraint) { constraints.insert(p_constraint); } + _FORCE_INLINE_ void remove_constraint(GodotConstraint2D *p_constraint) { constraints.erase(p_constraint); } + _FORCE_INLINE_ const Set &get_constraints() const { return constraints; } + _FORCE_INLINE_ void clear_constraints() { constraints.clear(); } + + void set_monitorable(bool p_monitorable); + _FORCE_INLINE_ bool is_monitorable() const { return monitorable; } + + void set_transform(const Transform2D &p_transform); + + void set_space(GodotSpace2D *p_space); + + void call_queries(); + + void compute_gravity(const Vector2 &p_position, Vector2 &r_gravity) const; + + GodotArea2D(); + ~GodotArea2D(); +}; + +void GodotArea2D::add_body_to_query(GodotBody2D *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { + BodyKey bk(p_body, p_body_shape, p_area_shape); + monitored_bodies[bk].inc(); + if (!monitor_query_list.in_list()) { + _queue_monitor_update(); + } +} + +void GodotArea2D::remove_body_from_query(GodotBody2D *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { + BodyKey bk(p_body, p_body_shape, p_area_shape); + monitored_bodies[bk].dec(); + if (!monitor_query_list.in_list()) { + _queue_monitor_update(); + } +} + +void GodotArea2D::add_area_to_query(GodotArea2D *p_area, uint32_t p_area_shape, uint32_t p_self_shape) { + BodyKey bk(p_area, p_area_shape, p_self_shape); + monitored_areas[bk].inc(); + if (!monitor_query_list.in_list()) { + _queue_monitor_update(); + } +} + +void GodotArea2D::remove_area_from_query(GodotArea2D *p_area, uint32_t p_area_shape, uint32_t p_self_shape) { + BodyKey bk(p_area, p_area_shape, p_self_shape); + monitored_areas[bk].dec(); + if (!monitor_query_list.in_list()) { + _queue_monitor_update(); + } +} + +#endif // GODOT_AREA_2D_H diff --git a/servers/physics_2d/godot_area_pair_2d.cpp b/servers/physics_2d/godot_area_pair_2d.cpp new file mode 100644 index 0000000000..fdb95aa262 --- /dev/null +++ b/servers/physics_2d/godot_area_pair_2d.cpp @@ -0,0 +1,190 @@ +/*************************************************************************/ +/* godot_area_pair_2d.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 "godot_area_pair_2d.h" +#include "godot_collision_solver_2d.h" + +bool GodotAreaPair2D::setup(real_t p_step) { + bool result = false; + if (area->collides_with(body) && GodotCollisionSolver2D::solve(body->get_shape(body_shape), body->get_transform() * body->get_shape_transform(body_shape), Vector2(), area->get_shape(area_shape), area->get_transform() * area->get_shape_transform(area_shape), Vector2(), nullptr, this)) { + result = true; + } + + process_collision = false; + if (result != colliding) { + if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) { + process_collision = true; + } else if (area->has_monitor_callback()) { + process_collision = true; + } + + colliding = result; + } + + return process_collision; +} + +bool GodotAreaPair2D::pre_solve(real_t p_step) { + if (!process_collision) { + return false; + } + + if (colliding) { + if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) { + body->add_area(area); + } + + if (area->has_monitor_callback()) { + area->add_body_to_query(body, body_shape, area_shape); + } + } else { + if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) { + body->remove_area(area); + } + + if (area->has_monitor_callback()) { + area->remove_body_from_query(body, body_shape, area_shape); + } + } + + return false; // Never do any post solving. +} + +void GodotAreaPair2D::solve(real_t p_step) { + // Nothing to do. +} + +GodotAreaPair2D::GodotAreaPair2D(GodotBody2D *p_body, int p_body_shape, GodotArea2D *p_area, int p_area_shape) { + body = p_body; + area = p_area; + body_shape = p_body_shape; + area_shape = p_area_shape; + body->add_constraint(this, 0); + area->add_constraint(this); + if (p_body->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC) { //need to be active to process pair + p_body->set_active(true); + } +} + +GodotAreaPair2D::~GodotAreaPair2D() { + if (colliding) { + if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) { + body->remove_area(area); + } + if (area->has_monitor_callback()) { + area->remove_body_from_query(body, body_shape, area_shape); + } + } + body->remove_constraint(this, 0); + area->remove_constraint(this); +} + +////////////////////////////////// + +bool GodotArea2Pair2D::setup(real_t p_step) { + bool result_a = area_a->collides_with(area_b); + bool result_b = area_b->collides_with(area_a); + if ((result_a || result_b) && !GodotCollisionSolver2D::solve(area_a->get_shape(shape_a), area_a->get_transform() * area_a->get_shape_transform(shape_a), Vector2(), area_b->get_shape(shape_b), area_b->get_transform() * area_b->get_shape_transform(shape_b), Vector2(), nullptr, this)) { + result_a = false; + result_b = false; + } + + bool process_collision = false; + + process_collision_a = false; + if (result_a != colliding_a) { + if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) { + process_collision_a = true; + process_collision = true; + } + colliding_a = result_a; + } + + process_collision_b = false; + if (result_b != colliding_b) { + if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) { + process_collision_b = true; + process_collision = true; + } + colliding_b = result_b; + } + + return process_collision; +} + +bool GodotArea2Pair2D::pre_solve(real_t p_step) { + if (process_collision_a) { + if (colliding_a) { + area_a->add_area_to_query(area_b, shape_b, shape_a); + } else { + area_a->remove_area_from_query(area_b, shape_b, shape_a); + } + } + + if (process_collision_b) { + if (colliding_b) { + area_b->add_area_to_query(area_a, shape_a, shape_b); + } else { + area_b->remove_area_from_query(area_a, shape_a, shape_b); + } + } + + return false; // Never do any post solving. +} + +void GodotArea2Pair2D::solve(real_t p_step) { + // Nothing to do. +} + +GodotArea2Pair2D::GodotArea2Pair2D(GodotArea2D *p_area_a, int p_shape_a, GodotArea2D *p_area_b, int p_shape_b) { + area_a = p_area_a; + area_b = p_area_b; + shape_a = p_shape_a; + shape_b = p_shape_b; + area_a->add_constraint(this); + area_b->add_constraint(this); +} + +GodotArea2Pair2D::~GodotArea2Pair2D() { + if (colliding_a) { + if (area_a->has_area_monitor_callback()) { + area_a->remove_area_from_query(area_b, shape_b, shape_a); + } + } + + if (colliding_b) { + if (area_b->has_area_monitor_callback()) { + area_b->remove_area_from_query(area_a, shape_a, shape_b); + } + } + + area_a->remove_constraint(this); + area_b->remove_constraint(this); +} diff --git a/servers/physics_2d/godot_area_pair_2d.h b/servers/physics_2d/godot_area_pair_2d.h new file mode 100644 index 0000000000..7a9677f714 --- /dev/null +++ b/servers/physics_2d/godot_area_pair_2d.h @@ -0,0 +1,74 @@ +/*************************************************************************/ +/* godot_area_pair_2d.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 GODOT_AREA_PAIR_2D_H +#define GODOT_AREA_PAIR_2D_H + +#include "godot_area_2d.h" +#include "godot_body_2d.h" +#include "godot_constraint_2d.h" + +class GodotAreaPair2D : public GodotConstraint2D { + GodotBody2D *body = nullptr; + GodotArea2D *area = nullptr; + int body_shape = 0; + int area_shape = 0; + bool colliding = false; + bool process_collision = false; + +public: + virtual bool setup(real_t p_step) override; + virtual bool pre_solve(real_t p_step) override; + virtual void solve(real_t p_step) override; + + GodotAreaPair2D(GodotBody2D *p_body, int p_body_shape, GodotArea2D *p_area, int p_area_shape); + ~GodotAreaPair2D(); +}; + +class GodotArea2Pair2D : public GodotConstraint2D { + GodotArea2D *area_a = nullptr; + GodotArea2D *area_b = nullptr; + int shape_a = 0; + int shape_b = 0; + bool colliding_a = false; + bool colliding_b = false; + bool process_collision_a = false; + bool process_collision_b = false; + +public: + virtual bool setup(real_t p_step) override; + virtual bool pre_solve(real_t p_step) override; + virtual void solve(real_t p_step) override; + + GodotArea2Pair2D(GodotArea2D *p_area_a, int p_shape_a, GodotArea2D *p_area_b, int p_shape_b); + ~GodotArea2Pair2D(); +}; + +#endif // GODOT_AREA_PAIR_2D_H diff --git a/servers/physics_2d/godot_body_2d.cpp b/servers/physics_2d/godot_body_2d.cpp new file mode 100644 index 0000000000..b0885a1f7b --- /dev/null +++ b/servers/physics_2d/godot_body_2d.cpp @@ -0,0 +1,677 @@ +/*************************************************************************/ +/* godot_body_2d.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 "godot_body_2d.h" + +#include "godot_area_2d.h" +#include "godot_body_direct_state_2d.h" +#include "godot_space_2d.h" + +void GodotBody2D::_mass_properties_changed() { + if (get_space() && !mass_properties_update_list.in_list() && (calculate_inertia || calculate_center_of_mass)) { + get_space()->body_add_to_mass_properties_update_list(&mass_properties_update_list); + } +} + +void GodotBody2D::update_mass_properties() { + //update shapes and motions + + switch (mode) { + case PhysicsServer2D::BODY_MODE_DYNAMIC: { + real_t total_area = 0; + for (int i = 0; i < get_shape_count(); i++) { + if (is_shape_disabled(i)) { + continue; + } + total_area += get_shape_aabb(i).get_area(); + } + + if (calculate_center_of_mass) { + // We have to recompute the center of mass. + center_of_mass = Vector2(); + + if (total_area != 0.0) { + for (int i = 0; i < get_shape_count(); i++) { + if (is_shape_disabled(i)) { + continue; + } + + real_t area = get_shape_aabb(i).get_area(); + + real_t mass = area * this->mass / total_area; + + // NOTE: we assume that the shape origin is also its center of mass. + center_of_mass += mass * get_shape_transform(i).get_origin(); + } + + center_of_mass /= mass; + } + } + + if (calculate_inertia) { + inertia = 0; + + for (int i = 0; i < get_shape_count(); i++) { + if (is_shape_disabled(i)) { + continue; + } + + const GodotShape2D *shape = get_shape(i); + + real_t area = get_shape_aabb(i).get_area(); + if (area == 0.0) { + continue; + } + + real_t mass = area * this->mass / total_area; + + Transform2D mtx = get_shape_transform(i); + Vector2 scale = mtx.get_scale(); + Vector2 shape_origin = mtx.get_origin() - center_of_mass; + inertia += shape->get_moment_of_inertia(mass, scale) + mass * shape_origin.length_squared(); + } + } + + _inv_inertia = inertia > 0.0 ? (1.0 / inertia) : 0.0; + + if (mass) { + _inv_mass = 1.0 / mass; + } else { + _inv_mass = 0; + } + + } break; + case PhysicsServer2D::BODY_MODE_KINEMATIC: + case PhysicsServer2D::BODY_MODE_STATIC: { + _inv_inertia = 0; + _inv_mass = 0; + } break; + case PhysicsServer2D::BODY_MODE_DYNAMIC_LINEAR: { + _inv_inertia = 0; + _inv_mass = 1.0 / mass; + + } break; + } +} + +void GodotBody2D::reset_mass_properties() { + calculate_inertia = true; + calculate_center_of_mass = true; + _mass_properties_changed(); +} + +void GodotBody2D::set_active(bool p_active) { + if (active == p_active) { + return; + } + + active = p_active; + + if (active) { + if (mode == PhysicsServer2D::BODY_MODE_STATIC) { + // Static bodies can't be active. + active = false; + } else if (get_space()) { + get_space()->body_add_to_active_list(&active_list); + } + } else if (get_space()) { + get_space()->body_remove_from_active_list(&active_list); + } +} + +void GodotBody2D::set_param(PhysicsServer2D::BodyParameter p_param, const Variant &p_value) { + switch (p_param) { + case PhysicsServer2D::BODY_PARAM_BOUNCE: { + bounce = p_value; + } break; + case PhysicsServer2D::BODY_PARAM_FRICTION: { + friction = p_value; + } break; + case PhysicsServer2D::BODY_PARAM_MASS: { + real_t mass_value = p_value; + ERR_FAIL_COND(mass_value <= 0); + mass = mass_value; + if (mode >= PhysicsServer2D::BODY_MODE_DYNAMIC) { + _mass_properties_changed(); + } + } break; + case PhysicsServer2D::BODY_PARAM_INERTIA: { + real_t inertia_value = p_value; + if (inertia_value <= 0.0) { + calculate_inertia = true; + if (mode == PhysicsServer2D::BODY_MODE_DYNAMIC) { + _mass_properties_changed(); + } + } else { + calculate_inertia = false; + inertia = inertia_value; + if (mode == PhysicsServer2D::BODY_MODE_DYNAMIC) { + _inv_inertia = 1.0 / inertia; + } + } + } break; + case PhysicsServer2D::BODY_PARAM_CENTER_OF_MASS: { + calculate_center_of_mass = false; + center_of_mass = p_value; + } break; + case PhysicsServer2D::BODY_PARAM_GRAVITY_SCALE: { + gravity_scale = p_value; + } break; + case PhysicsServer2D::BODY_PARAM_LINEAR_DAMP: { + linear_damp = p_value; + } break; + case PhysicsServer2D::BODY_PARAM_ANGULAR_DAMP: { + angular_damp = p_value; + } break; + default: { + } + } +} + +Variant GodotBody2D::get_param(PhysicsServer2D::BodyParameter p_param) const { + switch (p_param) { + case PhysicsServer2D::BODY_PARAM_BOUNCE: { + return bounce; + } + case PhysicsServer2D::BODY_PARAM_FRICTION: { + return friction; + } + case PhysicsServer2D::BODY_PARAM_MASS: { + return mass; + } + case PhysicsServer2D::BODY_PARAM_INERTIA: { + return inertia; + } + case PhysicsServer2D::BODY_PARAM_CENTER_OF_MASS: { + return center_of_mass; + } + case PhysicsServer2D::BODY_PARAM_GRAVITY_SCALE: { + return gravity_scale; + } + case PhysicsServer2D::BODY_PARAM_LINEAR_DAMP: { + return linear_damp; + } + case PhysicsServer2D::BODY_PARAM_ANGULAR_DAMP: { + return angular_damp; + } + default: { + } + } + + return 0; +} + +void GodotBody2D::set_mode(PhysicsServer2D::BodyMode p_mode) { + PhysicsServer2D::BodyMode prev = mode; + mode = p_mode; + + switch (p_mode) { + //CLEAR UP EVERYTHING IN CASE IT NOT WORKS! + case PhysicsServer2D::BODY_MODE_STATIC: + case PhysicsServer2D::BODY_MODE_KINEMATIC: { + _set_inv_transform(get_transform().affine_inverse()); + _inv_mass = 0; + _inv_inertia = 0; + _set_static(p_mode == PhysicsServer2D::BODY_MODE_STATIC); + set_active(p_mode == PhysicsServer2D::BODY_MODE_KINEMATIC && contacts.size()); + linear_velocity = Vector2(); + angular_velocity = 0; + if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC && prev != mode) { + first_time_kinematic = true; + } + } break; + case PhysicsServer2D::BODY_MODE_DYNAMIC: { + _inv_mass = mass > 0 ? (1.0 / mass) : 0; + if (!calculate_inertia) { + _inv_inertia = 1.0 / inertia; + } + _mass_properties_changed(); + _set_static(false); + set_active(true); + + } break; + case PhysicsServer2D::BODY_MODE_DYNAMIC_LINEAR: { + _inv_mass = mass > 0 ? (1.0 / mass) : 0; + _inv_inertia = 0; + angular_velocity = 0; + _set_static(false); + set_active(true); + } + } +} + +PhysicsServer2D::BodyMode GodotBody2D::get_mode() const { + return mode; +} + +void GodotBody2D::_shapes_changed() { + _mass_properties_changed(); + wakeup_neighbours(); +} + +void GodotBody2D::set_state(PhysicsServer2D::BodyState p_state, const Variant &p_variant) { + switch (p_state) { + case PhysicsServer2D::BODY_STATE_TRANSFORM: { + if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC) { + new_transform = p_variant; + //wakeup_neighbours(); + set_active(true); + if (first_time_kinematic) { + _set_transform(p_variant); + _set_inv_transform(get_transform().affine_inverse()); + first_time_kinematic = false; + } + } else if (mode == PhysicsServer2D::BODY_MODE_STATIC) { + _set_transform(p_variant); + _set_inv_transform(get_transform().affine_inverse()); + wakeup_neighbours(); + } else { + Transform2D t = p_variant; + t.orthonormalize(); + new_transform = get_transform(); //used as old to compute motion + if (t == new_transform) { + break; + } + _set_transform(t); + _set_inv_transform(get_transform().inverse()); + } + wakeup(); + + } break; + case PhysicsServer2D::BODY_STATE_LINEAR_VELOCITY: { + linear_velocity = p_variant; + constant_linear_velocity = linear_velocity; + wakeup(); + + } break; + case PhysicsServer2D::BODY_STATE_ANGULAR_VELOCITY: { + angular_velocity = p_variant; + constant_angular_velocity = angular_velocity; + wakeup(); + + } break; + case PhysicsServer2D::BODY_STATE_SLEEPING: { + if (mode == PhysicsServer2D::BODY_MODE_STATIC || mode == PhysicsServer2D::BODY_MODE_KINEMATIC) { + break; + } + bool do_sleep = p_variant; + if (do_sleep) { + linear_velocity = Vector2(); + //biased_linear_velocity=Vector3(); + angular_velocity = 0; + //biased_angular_velocity=Vector3(); + set_active(false); + } else { + if (mode != PhysicsServer2D::BODY_MODE_STATIC) { + set_active(true); + } + } + } break; + case PhysicsServer2D::BODY_STATE_CAN_SLEEP: { + can_sleep = p_variant; + if (mode >= PhysicsServer2D::BODY_MODE_DYNAMIC && !active && !can_sleep) { + set_active(true); + } + + } break; + } +} + +Variant GodotBody2D::get_state(PhysicsServer2D::BodyState p_state) const { + switch (p_state) { + case PhysicsServer2D::BODY_STATE_TRANSFORM: { + return get_transform(); + } + case PhysicsServer2D::BODY_STATE_LINEAR_VELOCITY: { + return linear_velocity; + } + case PhysicsServer2D::BODY_STATE_ANGULAR_VELOCITY: { + return angular_velocity; + } + case PhysicsServer2D::BODY_STATE_SLEEPING: { + return !is_active(); + } + case PhysicsServer2D::BODY_STATE_CAN_SLEEP: { + return can_sleep; + } + } + + return Variant(); +} + +void GodotBody2D::set_space(GodotSpace2D *p_space) { + if (get_space()) { + wakeup_neighbours(); + + if (mass_properties_update_list.in_list()) { + get_space()->body_remove_from_mass_properties_update_list(&mass_properties_update_list); + } + if (active_list.in_list()) { + get_space()->body_remove_from_active_list(&active_list); + } + if (direct_state_query_list.in_list()) { + get_space()->body_remove_from_state_query_list(&direct_state_query_list); + } + } + + _set_space(p_space); + + if (get_space()) { + _mass_properties_changed(); + if (active) { + get_space()->body_add_to_active_list(&active_list); + } + } +} + +void GodotBody2D::_compute_area_gravity_and_damping(const GodotArea2D *p_area) { + Vector2 area_gravity; + p_area->compute_gravity(get_transform().get_origin(), area_gravity); + gravity += area_gravity; + + area_linear_damp += p_area->get_linear_damp(); + area_angular_damp += p_area->get_angular_damp(); +} + +void GodotBody2D::integrate_forces(real_t p_step) { + if (mode == PhysicsServer2D::BODY_MODE_STATIC) { + return; + } + + GodotArea2D *def_area = get_space()->get_default_area(); + // GodotArea2D *damp_area = def_area; + ERR_FAIL_COND(!def_area); + + int ac = areas.size(); + bool stopped = false; + gravity = Vector2(0, 0); + area_angular_damp = 0; + area_linear_damp = 0; + if (ac) { + areas.sort(); + const AreaCMP *aa = &areas[0]; + // damp_area = aa[ac-1].area; + for (int i = ac - 1; i >= 0 && !stopped; i--) { + PhysicsServer2D::AreaSpaceOverrideMode mode = aa[i].area->get_space_override_mode(); + switch (mode) { + case PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE: + case PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: { + _compute_area_gravity_and_damping(aa[i].area); + stopped = mode == PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE; + } break; + case PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE: + case PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: { + gravity = Vector2(0, 0); + area_angular_damp = 0; + area_linear_damp = 0; + _compute_area_gravity_and_damping(aa[i].area); + stopped = mode == PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE; + } break; + default: { + } + } + } + } + if (!stopped) { + _compute_area_gravity_and_damping(def_area); + } + gravity *= gravity_scale; + + // If less than 0, override dampenings with that of the Body2D + if (angular_damp >= 0) { + area_angular_damp = angular_damp; + } + /* + else + area_angular_damp=damp_area->get_angular_damp(); + */ + + if (linear_damp >= 0) { + area_linear_damp = linear_damp; + } + /* + else + area_linear_damp=damp_area->get_linear_damp(); + */ + + Vector2 motion; + bool do_motion = false; + + if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC) { + //compute motion, angular and etc. velocities from prev transform + motion = new_transform.get_origin() - get_transform().get_origin(); + linear_velocity = constant_linear_velocity + motion / p_step; + + real_t rot = new_transform.get_rotation() - get_transform().get_rotation(); + angular_velocity = constant_angular_velocity + remainder(rot, 2.0 * Math_PI) / p_step; + + do_motion = true; + + /* + for(int i=0;ibody_add_to_state_query_list(&direct_state_query_list); + } + + if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC) { + _set_transform(new_transform, false); + _set_inv_transform(new_transform.affine_inverse()); + if (contacts.size() == 0 && linear_velocity == Vector2() && angular_velocity == 0) { + set_active(false); //stopped moving, deactivate + } + return; + } + + real_t total_angular_velocity = angular_velocity + biased_angular_velocity; + Vector2 total_linear_velocity = linear_velocity + biased_linear_velocity; + + real_t angle = get_transform().get_rotation() + total_angular_velocity * p_step; + Vector2 pos = get_transform().get_origin() + total_linear_velocity * p_step; + + real_t center_of_mass_distance = center_of_mass.length(); + if (center_of_mass_distance > CMP_EPSILON) { + // Calculate displacement due to center of mass offset. + real_t prev_angle = get_transform().get_rotation(); + real_t angle_base = Math::atan2(center_of_mass.y, center_of_mass.x); + Vector2 point1(Math::cos(angle_base + prev_angle), Math::sin(angle_base + prev_angle)); + Vector2 point2(Math::cos(angle_base + angle), Math::sin(angle_base + angle)); + pos += center_of_mass_distance * (point1 - point2); + } + + _set_transform(Transform2D(angle, pos), continuous_cd_mode == PhysicsServer2D::CCD_MODE_DISABLED); + _set_inv_transform(get_transform().inverse()); + + if (continuous_cd_mode != PhysicsServer2D::CCD_MODE_DISABLED) { + new_transform = get_transform(); + } +} + +void GodotBody2D::wakeup_neighbours() { + for (const Pair &E : constraint_list) { + const GodotConstraint2D *c = E.first; + GodotBody2D **n = c->get_body_ptr(); + int bc = c->get_body_count(); + + for (int i = 0; i < bc; i++) { + if (i == E.second) { + continue; + } + GodotBody2D *b = n[i]; + if (b->mode < PhysicsServer2D::BODY_MODE_DYNAMIC) { + continue; + } + + if (!b->is_active()) { + b->set_active(true); + } + } + } +} + +void GodotBody2D::call_queries() { + if (fi_callback_data) { + if (!fi_callback_data->callable.get_object()) { + set_force_integration_callback(Callable()); + } else { + Variant direct_state_variant = get_direct_state(); + const Variant *vp[2] = { &direct_state_variant, &fi_callback_data->udata }; + + Callable::CallError ce; + Variant rv; + if (fi_callback_data->udata.get_type() != Variant::NIL) { + fi_callback_data->callable.call(vp, 2, rv, ce); + + } else { + fi_callback_data->callable.call(vp, 1, rv, ce); + } + } + } + + if (body_state_callback) { + (body_state_callback)(body_state_callback_instance, get_direct_state()); + } +} + +bool GodotBody2D::sleep_test(real_t p_step) { + if (mode == PhysicsServer2D::BODY_MODE_STATIC || mode == PhysicsServer2D::BODY_MODE_KINEMATIC) { + return true; + } else if (!can_sleep) { + return false; + } + + if (Math::abs(angular_velocity) < get_space()->get_body_angular_velocity_sleep_threshold() && Math::abs(linear_velocity.length_squared()) < get_space()->get_body_linear_velocity_sleep_threshold() * get_space()->get_body_linear_velocity_sleep_threshold()) { + still_time += p_step; + + return still_time > get_space()->get_body_time_to_sleep(); + } else { + still_time = 0; //maybe this should be set to 0 on set_active? + return false; + } +} + +void GodotBody2D::set_state_sync_callback(void *p_instance, PhysicsServer2D::BodyStateCallback p_callback) { + body_state_callback_instance = p_instance; + body_state_callback = p_callback; +} + +void GodotBody2D::set_force_integration_callback(const Callable &p_callable, const Variant &p_udata) { + if (p_callable.get_object()) { + if (!fi_callback_data) { + fi_callback_data = memnew(ForceIntegrationCallbackData); + } + fi_callback_data->callable = p_callable; + fi_callback_data->udata = p_udata; + } else if (fi_callback_data) { + memdelete(fi_callback_data); + fi_callback_data = nullptr; + } +} + +GodotPhysicsDirectBodyState2D *GodotBody2D::get_direct_state() { + if (!direct_state) { + direct_state = memnew(GodotPhysicsDirectBodyState2D); + direct_state->body = this; + } + return direct_state; +} + +GodotBody2D::GodotBody2D() : + GodotCollisionObject2D(TYPE_BODY), + active_list(this), + mass_properties_update_list(this), + direct_state_query_list(this) { + _set_static(false); +} + +GodotBody2D::~GodotBody2D() { + if (fi_callback_data) { + memdelete(fi_callback_data); + } + if (direct_state) { + memdelete(direct_state); + } +} diff --git a/servers/physics_2d/godot_body_2d.h b/servers/physics_2d/godot_body_2d.h new file mode 100644 index 0000000000..c4f3578f1b --- /dev/null +++ b/servers/physics_2d/godot_body_2d.h @@ -0,0 +1,354 @@ +/*************************************************************************/ +/* godot_body_2d.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 GODOT_BODY_2D_H +#define GODOT_BODY_2D_H + +#include "godot_area_2d.h" +#include "godot_collision_object_2d.h" + +#include "core/templates/list.h" +#include "core/templates/pair.h" +#include "core/templates/vset.h" + +class GodotConstraint2D; +class GodotPhysicsDirectBodyState2D; + +class GodotBody2D : public GodotCollisionObject2D { + PhysicsServer2D::BodyMode mode = PhysicsServer2D::BODY_MODE_DYNAMIC; + + Vector2 biased_linear_velocity; + real_t biased_angular_velocity = 0.0; + + Vector2 linear_velocity; + real_t angular_velocity = 0.0; + + Vector2 constant_linear_velocity; + real_t constant_angular_velocity = 0.0; + + real_t linear_damp = -1.0; + real_t angular_damp = -1.0; + real_t gravity_scale = 1.0; + + real_t bounce = 0.0; + real_t friction = 1.0; + + real_t mass = 1.0; + real_t _inv_mass = 1.0; + + real_t inertia = 0.0; + real_t _inv_inertia = 0.0; + + Vector2 center_of_mass; + + bool calculate_inertia = true; + bool calculate_center_of_mass = true; + + Vector2 gravity; + real_t area_linear_damp = 0.0; + real_t area_angular_damp = 0.0; + + real_t still_time = 0.0; + + Vector2 applied_force; + real_t applied_torque = 0.0; + + SelfList active_list; + SelfList mass_properties_update_list; + SelfList direct_state_query_list; + + VSet exceptions; + PhysicsServer2D::CCDMode continuous_cd_mode = PhysicsServer2D::CCD_MODE_DISABLED; + bool omit_force_integration = false; + bool active = true; + bool can_sleep = true; + bool first_time_kinematic = false; + void _mass_properties_changed(); + virtual void _shapes_changed(); + Transform2D new_transform; + + List> constraint_list; + + struct AreaCMP { + GodotArea2D *area = nullptr; + int refCount = 0; + _FORCE_INLINE_ bool operator==(const AreaCMP &p_cmp) const { return area->get_self() == p_cmp.area->get_self(); } + _FORCE_INLINE_ bool operator<(const AreaCMP &p_cmp) const { return area->get_priority() < p_cmp.area->get_priority(); } + _FORCE_INLINE_ AreaCMP() {} + _FORCE_INLINE_ AreaCMP(GodotArea2D *p_area) { + area = p_area; + refCount = 1; + } + }; + + Vector areas; + + struct Contact { + Vector2 local_pos; + Vector2 local_normal; + real_t depth = 0.0; + int local_shape = 0; + Vector2 collider_pos; + int collider_shape = 0; + ObjectID collider_instance_id; + RID collider; + Vector2 collider_velocity_at_pos; + }; + + Vector contacts; //no contacts by default + int contact_count = 0; + + void *body_state_callback_instance = nullptr; + PhysicsServer2D::BodyStateCallback body_state_callback = nullptr; + + struct ForceIntegrationCallbackData { + Callable callable; + Variant udata; + }; + + ForceIntegrationCallbackData *fi_callback_data = nullptr; + + GodotPhysicsDirectBodyState2D *direct_state = nullptr; + + uint64_t island_step = 0; + + _FORCE_INLINE_ void _compute_area_gravity_and_damping(const GodotArea2D *p_area); + + friend class GodotPhysicsDirectBodyState2D; // i give up, too many functions to expose + +public: + void set_state_sync_callback(void *p_instance, PhysicsServer2D::BodyStateCallback p_callback); + void set_force_integration_callback(const Callable &p_callable, const Variant &p_udata = Variant()); + + GodotPhysicsDirectBodyState2D *get_direct_state(); + + _FORCE_INLINE_ void add_area(GodotArea2D *p_area) { + int index = areas.find(AreaCMP(p_area)); + if (index > -1) { + areas.write[index].refCount += 1; + } else { + areas.ordered_insert(AreaCMP(p_area)); + } + } + + _FORCE_INLINE_ void remove_area(GodotArea2D *p_area) { + int index = areas.find(AreaCMP(p_area)); + if (index > -1) { + areas.write[index].refCount -= 1; + if (areas[index].refCount < 1) { + areas.remove(index); + } + } + } + + _FORCE_INLINE_ void set_max_contacts_reported(int p_size) { + contacts.resize(p_size); + contact_count = 0; + if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC && p_size) { + set_active(true); + } + } + + _FORCE_INLINE_ int get_max_contacts_reported() const { return contacts.size(); } + + _FORCE_INLINE_ bool can_report_contacts() const { return !contacts.is_empty(); } + _FORCE_INLINE_ void add_contact(const Vector2 &p_local_pos, const Vector2 &p_local_normal, real_t p_depth, int p_local_shape, const Vector2 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector2 &p_collider_velocity_at_pos); + + _FORCE_INLINE_ void add_exception(const RID &p_exception) { exceptions.insert(p_exception); } + _FORCE_INLINE_ void remove_exception(const RID &p_exception) { exceptions.erase(p_exception); } + _FORCE_INLINE_ bool has_exception(const RID &p_exception) const { return exceptions.has(p_exception); } + _FORCE_INLINE_ const VSet &get_exceptions() const { return exceptions; } + + _FORCE_INLINE_ uint64_t get_island_step() const { return island_step; } + _FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step = p_step; } + + _FORCE_INLINE_ void add_constraint(GodotConstraint2D *p_constraint, int p_pos) { constraint_list.push_back({ p_constraint, p_pos }); } + _FORCE_INLINE_ void remove_constraint(GodotConstraint2D *p_constraint, int p_pos) { constraint_list.erase({ p_constraint, p_pos }); } + const List> &get_constraint_list() const { return constraint_list; } + _FORCE_INLINE_ void clear_constraint_list() { constraint_list.clear(); } + + _FORCE_INLINE_ void set_omit_force_integration(bool p_omit_force_integration) { omit_force_integration = p_omit_force_integration; } + _FORCE_INLINE_ bool get_omit_force_integration() const { return omit_force_integration; } + + _FORCE_INLINE_ void set_linear_velocity(const Vector2 &p_velocity) { linear_velocity = p_velocity; } + _FORCE_INLINE_ Vector2 get_linear_velocity() const { return linear_velocity; } + + _FORCE_INLINE_ void set_angular_velocity(real_t p_velocity) { angular_velocity = p_velocity; } + _FORCE_INLINE_ real_t get_angular_velocity() const { return angular_velocity; } + + _FORCE_INLINE_ void set_biased_linear_velocity(const Vector2 &p_velocity) { biased_linear_velocity = p_velocity; } + _FORCE_INLINE_ Vector2 get_biased_linear_velocity() const { return biased_linear_velocity; } + + _FORCE_INLINE_ void set_biased_angular_velocity(real_t p_velocity) { biased_angular_velocity = p_velocity; } + _FORCE_INLINE_ real_t get_biased_angular_velocity() const { return biased_angular_velocity; } + + _FORCE_INLINE_ void apply_central_impulse(const Vector2 &p_impulse) { + linear_velocity += p_impulse * _inv_mass; + } + + _FORCE_INLINE_ void apply_impulse(const Vector2 &p_impulse, const Vector2 &p_position = Vector2()) { + linear_velocity += p_impulse * _inv_mass; + angular_velocity += _inv_inertia * (p_position - center_of_mass).cross(p_impulse); + } + + _FORCE_INLINE_ void apply_torque_impulse(real_t p_torque) { + angular_velocity += _inv_inertia * p_torque; + } + + _FORCE_INLINE_ void apply_bias_impulse(const Vector2 &p_impulse, const Vector2 &p_position = Vector2()) { + biased_linear_velocity += p_impulse * _inv_mass; + biased_angular_velocity += _inv_inertia * (p_position - center_of_mass).cross(p_impulse); + } + + void set_active(bool p_active); + _FORCE_INLINE_ bool is_active() const { return active; } + + _FORCE_INLINE_ void wakeup() { + if ((!get_space()) || mode == PhysicsServer2D::BODY_MODE_STATIC || mode == PhysicsServer2D::BODY_MODE_KINEMATIC) { + return; + } + set_active(true); + } + + void set_param(PhysicsServer2D::BodyParameter p_param, const Variant &p_value); + Variant get_param(PhysicsServer2D::BodyParameter p_param) const; + + void set_mode(PhysicsServer2D::BodyMode p_mode); + PhysicsServer2D::BodyMode get_mode() const; + + void set_state(PhysicsServer2D::BodyState p_state, const Variant &p_variant); + Variant get_state(PhysicsServer2D::BodyState p_state) const; + + void set_applied_force(const Vector2 &p_force) { applied_force = p_force; } + Vector2 get_applied_force() const { return applied_force; } + + void set_applied_torque(real_t p_torque) { applied_torque = p_torque; } + real_t get_applied_torque() const { return applied_torque; } + + _FORCE_INLINE_ void add_central_force(const Vector2 &p_force) { + applied_force += p_force; + } + + _FORCE_INLINE_ void add_force(const Vector2 &p_force, const Vector2 &p_position = Vector2()) { + applied_force += p_force; + applied_torque += (p_position - center_of_mass).cross(p_force); + } + + _FORCE_INLINE_ void add_torque(real_t p_torque) { + applied_torque += p_torque; + } + + _FORCE_INLINE_ void set_continuous_collision_detection_mode(PhysicsServer2D::CCDMode p_mode) { continuous_cd_mode = p_mode; } + _FORCE_INLINE_ PhysicsServer2D::CCDMode get_continuous_collision_detection_mode() const { return continuous_cd_mode; } + + void set_space(GodotSpace2D *p_space); + + void update_mass_properties(); + void reset_mass_properties(); + + _FORCE_INLINE_ Vector2 get_center_of_mass() const { return center_of_mass; } + _FORCE_INLINE_ real_t get_inv_mass() const { return _inv_mass; } + _FORCE_INLINE_ real_t get_inv_inertia() const { return _inv_inertia; } + _FORCE_INLINE_ real_t get_friction() const { return friction; } + _FORCE_INLINE_ Vector2 get_gravity() const { return gravity; } + _FORCE_INLINE_ real_t get_bounce() const { return bounce; } + _FORCE_INLINE_ real_t get_linear_damp() const { return linear_damp; } + _FORCE_INLINE_ real_t get_angular_damp() const { return angular_damp; } + + void integrate_forces(real_t p_step); + void integrate_velocities(real_t p_step); + + _FORCE_INLINE_ Vector2 get_velocity_in_local_point(const Vector2 &rel_pos) const { + return linear_velocity + Vector2(-angular_velocity * rel_pos.y, angular_velocity * rel_pos.x); + } + + _FORCE_INLINE_ Vector2 get_motion() const { + if (mode > PhysicsServer2D::BODY_MODE_KINEMATIC) { + return new_transform.get_origin() - get_transform().get_origin(); + } else if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC) { + return get_transform().get_origin() - new_transform.get_origin(); //kinematic simulates forward + } + return Vector2(); + } + + void call_queries(); + void wakeup_neighbours(); + + bool sleep_test(real_t p_step); + + GodotBody2D(); + ~GodotBody2D(); +}; + +//add contact inline + +void GodotBody2D::add_contact(const Vector2 &p_local_pos, const Vector2 &p_local_normal, real_t p_depth, int p_local_shape, const Vector2 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector2 &p_collider_velocity_at_pos) { + int c_max = contacts.size(); + + if (c_max == 0) { + return; + } + + Contact *c = contacts.ptrw(); + + int idx = -1; + + if (contact_count < c_max) { + idx = contact_count++; + } else { + real_t least_depth = 1e20; + int least_deep = -1; + for (int i = 0; i < c_max; i++) { + if (i == 0 || c[i].depth < least_depth) { + least_deep = i; + least_depth = c[i].depth; + } + } + + if (least_deep >= 0 && least_depth < p_depth) { + idx = least_deep; + } + if (idx == -1) { + return; //none least deepe than this + } + } + + c[idx].local_pos = p_local_pos; + c[idx].local_normal = p_local_normal; + c[idx].depth = p_depth; + c[idx].local_shape = p_local_shape; + c[idx].collider_pos = p_collider_pos; + c[idx].collider_shape = p_collider_shape; + c[idx].collider_instance_id = p_collider_instance_id; + c[idx].collider = p_collider; + c[idx].collider_velocity_at_pos = p_collider_velocity_at_pos; +} + +#endif // GODOT_BODY_2D_H diff --git a/servers/physics_2d/godot_body_direct_state_2d.cpp b/servers/physics_2d/godot_body_direct_state_2d.cpp new file mode 100644 index 0000000000..300c302c79 --- /dev/null +++ b/servers/physics_2d/godot_body_direct_state_2d.cpp @@ -0,0 +1,178 @@ +/*************************************************************************/ +/* godot_body_direct_state_2d.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 "godot_body_direct_state_2d.h" + +#include "godot_body_2d.h" +#include "godot_physics_server_2d.h" +#include "godot_space_2d.h" + +Vector2 GodotPhysicsDirectBodyState2D::get_total_gravity() const { + return body->gravity; +} + +real_t GodotPhysicsDirectBodyState2D::get_total_angular_damp() const { + return body->area_angular_damp; +} + +real_t GodotPhysicsDirectBodyState2D::get_total_linear_damp() const { + return body->area_linear_damp; +} + +Vector2 GodotPhysicsDirectBodyState2D::get_center_of_mass() const { + return body->get_center_of_mass(); +} + +real_t GodotPhysicsDirectBodyState2D::get_inverse_mass() const { + return body->get_inv_mass(); +} + +real_t GodotPhysicsDirectBodyState2D::get_inverse_inertia() const { + return body->get_inv_inertia(); +} + +void GodotPhysicsDirectBodyState2D::set_linear_velocity(const Vector2 &p_velocity) { + body->wakeup(); + body->set_linear_velocity(p_velocity); +} + +Vector2 GodotPhysicsDirectBodyState2D::get_linear_velocity() const { + return body->get_linear_velocity(); +} + +void GodotPhysicsDirectBodyState2D::set_angular_velocity(real_t p_velocity) { + body->wakeup(); + body->set_angular_velocity(p_velocity); +} + +real_t GodotPhysicsDirectBodyState2D::get_angular_velocity() const { + return body->get_angular_velocity(); +} + +void GodotPhysicsDirectBodyState2D::set_transform(const Transform2D &p_transform) { + body->set_state(PhysicsServer2D::BODY_STATE_TRANSFORM, p_transform); +} + +Transform2D GodotPhysicsDirectBodyState2D::get_transform() const { + return body->get_transform(); +} + +Vector2 GodotPhysicsDirectBodyState2D::get_velocity_at_local_position(const Vector2 &p_position) const { + return body->get_velocity_in_local_point(p_position); +} + +void GodotPhysicsDirectBodyState2D::add_central_force(const Vector2 &p_force) { + body->wakeup(); + body->add_central_force(p_force); +} + +void GodotPhysicsDirectBodyState2D::add_force(const Vector2 &p_force, const Vector2 &p_position) { + body->wakeup(); + body->add_force(p_force, p_position); +} + +void GodotPhysicsDirectBodyState2D::add_torque(real_t p_torque) { + body->wakeup(); + body->add_torque(p_torque); +} + +void GodotPhysicsDirectBodyState2D::apply_central_impulse(const Vector2 &p_impulse) { + body->wakeup(); + body->apply_central_impulse(p_impulse); +} + +void GodotPhysicsDirectBodyState2D::apply_impulse(const Vector2 &p_impulse, const Vector2 &p_position) { + body->wakeup(); + body->apply_impulse(p_impulse, p_position); +} + +void GodotPhysicsDirectBodyState2D::apply_torque_impulse(real_t p_torque) { + body->wakeup(); + body->apply_torque_impulse(p_torque); +} + +void GodotPhysicsDirectBodyState2D::set_sleep_state(bool p_enable) { + body->set_active(!p_enable); +} + +bool GodotPhysicsDirectBodyState2D::is_sleeping() const { + return !body->is_active(); +} + +int GodotPhysicsDirectBodyState2D::get_contact_count() const { + return body->contact_count; +} + +Vector2 GodotPhysicsDirectBodyState2D::get_contact_local_position(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2()); + return body->contacts[p_contact_idx].local_pos; +} + +Vector2 GodotPhysicsDirectBodyState2D::get_contact_local_normal(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2()); + return body->contacts[p_contact_idx].local_normal; +} + +int GodotPhysicsDirectBodyState2D::get_contact_local_shape(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, -1); + return body->contacts[p_contact_idx].local_shape; +} + +RID GodotPhysicsDirectBodyState2D::get_contact_collider(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, RID()); + return body->contacts[p_contact_idx].collider; +} +Vector2 GodotPhysicsDirectBodyState2D::get_contact_collider_position(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2()); + return body->contacts[p_contact_idx].collider_pos; +} + +ObjectID GodotPhysicsDirectBodyState2D::get_contact_collider_id(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, ObjectID()); + return body->contacts[p_contact_idx].collider_instance_id; +} + +int GodotPhysicsDirectBodyState2D::get_contact_collider_shape(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, 0); + return body->contacts[p_contact_idx].collider_shape; +} + +Vector2 GodotPhysicsDirectBodyState2D::get_contact_collider_velocity_at_position(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2()); + return body->contacts[p_contact_idx].collider_velocity_at_pos; +} + +PhysicsDirectSpaceState2D *GodotPhysicsDirectBodyState2D::get_space_state() { + return body->get_space()->get_direct_state(); +} + +real_t GodotPhysicsDirectBodyState2D::get_step() const { + return body->get_space()->get_last_step(); +} diff --git a/servers/physics_2d/godot_body_direct_state_2d.h b/servers/physics_2d/godot_body_direct_state_2d.h new file mode 100644 index 0000000000..2f3e8e5095 --- /dev/null +++ b/servers/physics_2d/godot_body_direct_state_2d.h @@ -0,0 +1,91 @@ +/*************************************************************************/ +/* godot_body_direct_state_2d.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 GODOT_BODY_DIRECT_STATE_2D_H +#define GODOT_BODY_DIRECT_STATE_2D_H + +#include "servers/physics_server_2d.h" + +class GodotBody2D; + +class GodotPhysicsDirectBodyState2D : public PhysicsDirectBodyState2D { + GDCLASS(GodotPhysicsDirectBodyState2D, PhysicsDirectBodyState2D); + +public: + GodotBody2D *body = nullptr; + + virtual Vector2 get_total_gravity() const override; + virtual real_t get_total_angular_damp() const override; + virtual real_t get_total_linear_damp() const override; + + virtual Vector2 get_center_of_mass() const override; + virtual real_t get_inverse_mass() const override; + virtual real_t get_inverse_inertia() const override; + + virtual void set_linear_velocity(const Vector2 &p_velocity) override; + virtual Vector2 get_linear_velocity() const override; + + virtual void set_angular_velocity(real_t p_velocity) override; + virtual real_t get_angular_velocity() const override; + + virtual void set_transform(const Transform2D &p_transform) override; + virtual Transform2D get_transform() const override; + + virtual Vector2 get_velocity_at_local_position(const Vector2 &p_position) const override; + + virtual void add_central_force(const Vector2 &p_force) override; + virtual void add_force(const Vector2 &p_force, const Vector2 &p_position = Vector2()) override; + virtual void add_torque(real_t p_torque) override; + virtual void apply_central_impulse(const Vector2 &p_impulse) override; + virtual void apply_impulse(const Vector2 &p_impulse, const Vector2 &p_position = Vector2()) override; + virtual void apply_torque_impulse(real_t p_torque) override; + + virtual void set_sleep_state(bool p_enable) override; + virtual bool is_sleeping() const override; + + virtual int get_contact_count() const override; + + virtual Vector2 get_contact_local_position(int p_contact_idx) const override; + virtual Vector2 get_contact_local_normal(int p_contact_idx) const override; + virtual int get_contact_local_shape(int p_contact_idx) const override; + + virtual RID get_contact_collider(int p_contact_idx) const override; + virtual Vector2 get_contact_collider_position(int p_contact_idx) const override; + virtual ObjectID get_contact_collider_id(int p_contact_idx) const override; + virtual int get_contact_collider_shape(int p_contact_idx) const override; + + virtual Vector2 get_contact_collider_velocity_at_position(int p_contact_idx) const override; + + virtual PhysicsDirectSpaceState2D *get_space_state() override; + + virtual real_t get_step() const override; +}; + +#endif // GODOT_BODY_DIRECT_STATE_2D_H diff --git a/servers/physics_2d/godot_body_pair_2d.cpp b/servers/physics_2d/godot_body_pair_2d.cpp new file mode 100644 index 0000000000..97eeefbfe6 --- /dev/null +++ b/servers/physics_2d/godot_body_pair_2d.cpp @@ -0,0 +1,545 @@ +/*************************************************************************/ +/* godot_body_pair_2d.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 "godot_body_pair_2d.h" +#include "godot_collision_solver_2d.h" +#include "godot_space_2d.h" + +#define POSITION_CORRECTION +#define ACCUMULATE_IMPULSES + +void GodotBodyPair2D::_add_contact(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_self) { + GodotBodyPair2D *self = (GodotBodyPair2D *)p_self; + + self->_contact_added_callback(p_point_A, p_point_B); +} + +void GodotBodyPair2D::_contact_added_callback(const Vector2 &p_point_A, const Vector2 &p_point_B) { + // check if we already have the contact + + Vector2 local_A = A->get_inv_transform().basis_xform(p_point_A); + Vector2 local_B = B->get_inv_transform().basis_xform(p_point_B - offset_B); + + int new_index = contact_count; + + ERR_FAIL_COND(new_index >= (MAX_CONTACTS + 1)); + + Contact contact; + + contact.acc_normal_impulse = 0; + contact.acc_bias_impulse = 0; + contact.acc_tangent_impulse = 0; + contact.local_A = local_A; + contact.local_B = local_B; + contact.reused = true; + contact.normal = (p_point_A - p_point_B).normalized(); + contact.mass_normal = 0; // will be computed in setup() + + // attempt to determine if the contact will be reused + + real_t recycle_radius_2 = space->get_contact_recycle_radius() * space->get_contact_recycle_radius(); + + for (int i = 0; i < contact_count; i++) { + Contact &c = contacts[i]; + if ( + c.local_A.distance_squared_to(local_A) < (recycle_radius_2) && + c.local_B.distance_squared_to(local_B) < (recycle_radius_2)) { + contact.acc_normal_impulse = c.acc_normal_impulse; + contact.acc_tangent_impulse = c.acc_tangent_impulse; + contact.acc_bias_impulse = c.acc_bias_impulse; + new_index = i; + break; + } + } + + // figure out if the contact amount must be reduced to fit the new contact + + if (new_index == MAX_CONTACTS) { + // remove the contact with the minimum depth + + int least_deep = -1; + real_t min_depth = 1e10; + + const Transform2D &transform_A = A->get_transform(); + const Transform2D &transform_B = B->get_transform(); + + for (int i = 0; i <= contact_count; i++) { + Contact &c = (i == contact_count) ? contact : contacts[i]; + Vector2 global_A = transform_A.basis_xform(c.local_A); + Vector2 global_B = transform_B.basis_xform(c.local_B) + offset_B; + + Vector2 axis = global_A - global_B; + real_t depth = axis.dot(c.normal); + + if (depth < min_depth) { + min_depth = depth; + least_deep = i; + } + } + + ERR_FAIL_COND(least_deep == -1); + + if (least_deep < contact_count) { //replace the last deep contact by the new one + + contacts[least_deep] = contact; + } + + return; + } + + contacts[new_index] = contact; + + if (new_index == contact_count) { + contact_count++; + } +} + +void GodotBodyPair2D::_validate_contacts() { + //make sure to erase contacts that are no longer valid + + real_t max_separation = space->get_contact_max_separation(); + real_t max_separation2 = max_separation * max_separation; + + const Transform2D &transform_A = A->get_transform(); + const Transform2D &transform_B = B->get_transform(); + + for (int i = 0; i < contact_count; i++) { + Contact &c = contacts[i]; + + bool erase = false; + if (!c.reused) { + //was left behind in previous frame + erase = true; + } else { + c.reused = false; + + Vector2 global_A = transform_A.basis_xform(c.local_A); + Vector2 global_B = transform_B.basis_xform(c.local_B) + offset_B; + Vector2 axis = global_A - global_B; + real_t depth = axis.dot(c.normal); + + if (depth < -max_separation || (global_B + c.normal * depth - global_A).length_squared() > max_separation2) { + erase = true; + } + } + + if (erase) { + // contact no longer needed, remove + + if ((i + 1) < contact_count) { + // swap with the last one + SWAP(contacts[i], contacts[contact_count - 1]); + } + + i--; + contact_count--; + } + } +} + +bool GodotBodyPair2D::_test_ccd(real_t p_step, GodotBody2D *p_A, int p_shape_A, const Transform2D &p_xform_A, GodotBody2D *p_B, int p_shape_B, const Transform2D &p_xform_B, bool p_swap_result) { + Vector2 motion = p_A->get_linear_velocity() * p_step; + real_t mlen = motion.length(); + if (mlen < CMP_EPSILON) { + return false; + } + + Vector2 mnormal = motion / mlen; + + real_t min, max; + p_A->get_shape(p_shape_A)->project_rangev(mnormal, p_xform_A, min, max); + bool fast_object = mlen > (max - min) * 0.3; //going too fast in that direction + + if (!fast_object) { //did it move enough in this direction to even attempt raycast? let's say it should move more than 1/3 the size of the object in that axis + return false; + } + + //cast a segment from support in motion normal, in the same direction of motion by motion length + //support is the worst case collision point, so real collision happened before + int a; + Vector2 s[2]; + p_A->get_shape(p_shape_A)->get_supports(p_xform_A.basis_xform(mnormal).normalized(), s, a); + Vector2 from = p_xform_A.xform(s[0]); + Vector2 to = from + motion; + + Transform2D from_inv = p_xform_B.affine_inverse(); + + Vector2 local_from = from_inv.xform(from - mnormal * mlen * 0.1); //start from a little inside the bounding box + Vector2 local_to = from_inv.xform(to); + + Vector2 rpos, rnorm; + if (!p_B->get_shape(p_shape_B)->intersect_segment(local_from, local_to, rpos, rnorm)) { + return false; + } + + //ray hit something + + Vector2 hitpos = p_xform_B.xform(rpos); + + Vector2 contact_A = to; + Vector2 contact_B = hitpos; + + //create a contact + + if (p_swap_result) { + _contact_added_callback(contact_B, contact_A); + } else { + _contact_added_callback(contact_A, contact_B); + } + + return true; +} + +real_t combine_bounce(GodotBody2D *A, GodotBody2D *B) { + return CLAMP(A->get_bounce() + B->get_bounce(), 0, 1); +} + +real_t combine_friction(GodotBody2D *A, GodotBody2D *B) { + return ABS(MIN(A->get_friction(), B->get_friction())); +} + +bool GodotBodyPair2D::setup(real_t p_step) { + if (!A->interacts_with(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self())) { + collided = false; + return false; + } + + collide_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) && A->collides_with(B); + collide_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) && B->collides_with(A); + + report_contacts_only = false; + if (!collide_A && !collide_B) { + if ((A->get_max_contacts_reported() > 0) || (B->get_max_contacts_reported() > 0)) { + report_contacts_only = true; + } else { + collided = false; + return false; + } + } + + //use local A coordinates to avoid numerical issues on collision detection + offset_B = B->get_transform().get_origin() - A->get_transform().get_origin(); + + _validate_contacts(); + + const Vector2 &offset_A = A->get_transform().get_origin(); + Transform2D xform_Au = A->get_transform().untranslated(); + Transform2D xform_A = xform_Au * A->get_shape_transform(shape_A); + + Transform2D xform_Bu = B->get_transform(); + xform_Bu.elements[2] -= offset_A; + Transform2D xform_B = xform_Bu * B->get_shape_transform(shape_B); + + GodotShape2D *shape_A_ptr = A->get_shape(shape_A); + GodotShape2D *shape_B_ptr = B->get_shape(shape_B); + + Vector2 motion_A, motion_B; + + if (A->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_SHAPE) { + motion_A = A->get_motion(); + } + if (B->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_SHAPE) { + motion_B = B->get_motion(); + } + + bool prev_collided = collided; + + collided = GodotCollisionSolver2D::solve(shape_A_ptr, xform_A, motion_A, shape_B_ptr, xform_B, motion_B, _add_contact, this, &sep_axis); + if (!collided) { + //test ccd (currently just a raycast) + + if (A->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && collide_A) { + if (_test_ccd(p_step, A, shape_A, xform_A, B, shape_B, xform_B)) { + collided = true; + } + } + + if (B->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && collide_B) { + if (_test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A, true)) { + collided = true; + } + } + + if (!collided) { + oneway_disabled = false; + return false; + } + } + + if (oneway_disabled) { + return false; + } + + if (!prev_collided) { + if (shape_B_ptr->allows_one_way_collision() && A->is_shape_set_as_one_way_collision(shape_A)) { + Vector2 direction = xform_A.get_axis(1).normalized(); + bool valid = false; + for (int i = 0; i < contact_count; i++) { + Contact &c = contacts[i]; + if (!c.reused) { + continue; + } + if (c.normal.dot(direction) > -CMP_EPSILON) { //greater (normal inverted) + continue; + } + valid = true; + break; + } + if (!valid) { + collided = false; + oneway_disabled = true; + return false; + } + } + + if (shape_A_ptr->allows_one_way_collision() && B->is_shape_set_as_one_way_collision(shape_B)) { + Vector2 direction = xform_B.get_axis(1).normalized(); + bool valid = false; + for (int i = 0; i < contact_count; i++) { + Contact &c = contacts[i]; + if (!c.reused) { + continue; + } + if (c.normal.dot(direction) < CMP_EPSILON) { //less (normal ok) + continue; + } + valid = true; + break; + } + if (!valid) { + collided = false; + oneway_disabled = true; + return false; + } + } + } + + return true; +} + +bool GodotBodyPair2D::pre_solve(real_t p_step) { + if (!collided || oneway_disabled) { + return false; + } + + real_t max_penetration = space->get_contact_max_allowed_penetration(); + + real_t bias = 0.3; + + GodotShape2D *shape_A_ptr = A->get_shape(shape_A); + GodotShape2D *shape_B_ptr = B->get_shape(shape_B); + + if (shape_A_ptr->get_custom_bias() || shape_B_ptr->get_custom_bias()) { + if (shape_A_ptr->get_custom_bias() == 0) { + bias = shape_B_ptr->get_custom_bias(); + } else if (shape_B_ptr->get_custom_bias() == 0) { + bias = shape_A_ptr->get_custom_bias(); + } else { + bias = (shape_B_ptr->get_custom_bias() + shape_A_ptr->get_custom_bias()) * 0.5; + } + } + + real_t inv_dt = 1.0 / p_step; + + bool do_process = false; + + const Vector2 &offset_A = A->get_transform().get_origin(); + const Transform2D &transform_A = A->get_transform(); + const Transform2D &transform_B = B->get_transform(); + + real_t inv_inertia_A = collide_A ? A->get_inv_inertia() : 0.0; + real_t inv_inertia_B = collide_B ? B->get_inv_inertia() : 0.0; + + real_t inv_mass_A = collide_A ? A->get_inv_mass() : 0.0; + real_t inv_mass_B = collide_B ? B->get_inv_mass() : 0.0; + + for (int i = 0; i < contact_count; i++) { + Contact &c = contacts[i]; + c.active = false; + + Vector2 global_A = transform_A.basis_xform(c.local_A); + Vector2 global_B = transform_B.basis_xform(c.local_B) + offset_B; + + Vector2 axis = global_A - global_B; + real_t depth = axis.dot(c.normal); + + if (depth <= 0.0 || !c.reused) { + continue; + } + +#ifdef DEBUG_ENABLED + if (space->is_debugging_contacts()) { + space->add_debug_contact(global_A + offset_A); + space->add_debug_contact(global_B + offset_A); + } +#endif + + c.rA = global_A; + c.rB = global_B - offset_B; + + if (A->can_report_contacts()) { + Vector2 crB(-B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x); + A->add_contact(global_A + offset_A, -c.normal, depth, shape_A, global_B + offset_A, shape_B, B->get_instance_id(), B->get_self(), crB + B->get_linear_velocity()); + } + + if (B->can_report_contacts()) { + Vector2 crA(-A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x); + B->add_contact(global_B + offset_A, c.normal, depth, shape_B, global_A + offset_A, shape_A, A->get_instance_id(), A->get_self(), crA + A->get_linear_velocity()); + } + + if (report_contacts_only) { + collided = false; + continue; + } + + // Precompute normal mass, tangent mass, and bias. + real_t rnA = c.rA.dot(c.normal); + real_t rnB = c.rB.dot(c.normal); + real_t kNormal = inv_mass_A + inv_mass_B; + kNormal += inv_inertia_A * (c.rA.dot(c.rA) - rnA * rnA) + inv_inertia_B * (c.rB.dot(c.rB) - rnB * rnB); + c.mass_normal = 1.0f / kNormal; + + Vector2 tangent = c.normal.orthogonal(); + real_t rtA = c.rA.dot(tangent); + real_t rtB = c.rB.dot(tangent); + real_t kTangent = inv_mass_A + inv_mass_B; + kTangent += inv_inertia_A * (c.rA.dot(c.rA) - rtA * rtA) + inv_inertia_B * (c.rB.dot(c.rB) - rtB * rtB); + c.mass_tangent = 1.0f / kTangent; + + c.bias = -bias * inv_dt * MIN(0.0f, -depth + max_penetration); + c.depth = depth; + //c.acc_bias_impulse=0; + +#ifdef ACCUMULATE_IMPULSES + { + // Apply normal + friction impulse + Vector2 P = c.acc_normal_impulse * c.normal + c.acc_tangent_impulse * tangent; + + if (collide_A) { + A->apply_impulse(-P, c.rA); + } + if (collide_B) { + B->apply_impulse(P, c.rB); + } + } +#endif + + c.bounce = combine_bounce(A, B); + if (c.bounce) { + Vector2 crA(-A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x); + Vector2 crB(-B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x); + Vector2 dv = B->get_linear_velocity() + crB - A->get_linear_velocity() - crA; + c.bounce = c.bounce * dv.dot(c.normal); + } + + c.active = true; + do_process = true; + } + + return do_process; +} + +void GodotBodyPair2D::solve(real_t p_step) { + if (!collided || oneway_disabled) { + return; + } + + for (int i = 0; i < contact_count; ++i) { + Contact &c = contacts[i]; + + if (!c.active) { + continue; + } + + // Relative velocity at contact + + Vector2 crA(-A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x); + Vector2 crB(-B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x); + Vector2 dv = B->get_linear_velocity() + crB - A->get_linear_velocity() - crA; + + Vector2 crbA(-A->get_biased_angular_velocity() * c.rA.y, A->get_biased_angular_velocity() * c.rA.x); + Vector2 crbB(-B->get_biased_angular_velocity() * c.rB.y, B->get_biased_angular_velocity() * c.rB.x); + Vector2 dbv = B->get_biased_linear_velocity() + crbB - A->get_biased_linear_velocity() - crbA; + + real_t vn = dv.dot(c.normal); + real_t vbn = dbv.dot(c.normal); + Vector2 tangent = c.normal.orthogonal(); + real_t vt = dv.dot(tangent); + + real_t jbn = (c.bias - vbn) * c.mass_normal; + real_t jbnOld = c.acc_bias_impulse; + c.acc_bias_impulse = MAX(jbnOld + jbn, 0.0f); + + Vector2 jb = c.normal * (c.acc_bias_impulse - jbnOld); + + if (collide_A) { + A->apply_bias_impulse(-jb, c.rA); + } + if (collide_B) { + B->apply_bias_impulse(jb, c.rB); + } + + real_t jn = -(c.bounce + vn) * c.mass_normal; + real_t jnOld = c.acc_normal_impulse; + c.acc_normal_impulse = MAX(jnOld + jn, 0.0f); + + real_t friction = combine_friction(A, B); + + real_t jtMax = friction * c.acc_normal_impulse; + real_t jt = -vt * c.mass_tangent; + real_t jtOld = c.acc_tangent_impulse; + c.acc_tangent_impulse = CLAMP(jtOld + jt, -jtMax, jtMax); + + Vector2 j = c.normal * (c.acc_normal_impulse - jnOld) + tangent * (c.acc_tangent_impulse - jtOld); + + if (collide_A) { + A->apply_impulse(-j, c.rA); + } + if (collide_B) { + B->apply_impulse(j, c.rB); + } + } +} + +GodotBodyPair2D::GodotBodyPair2D(GodotBody2D *p_A, int p_shape_A, GodotBody2D *p_B, int p_shape_B) : + GodotConstraint2D(_arr, 2) { + A = p_A; + B = p_B; + shape_A = p_shape_A; + shape_B = p_shape_B; + space = A->get_space(); + A->add_constraint(this, 0); + B->add_constraint(this, 1); +} + +GodotBodyPair2D::~GodotBodyPair2D() { + A->remove_constraint(this, 0); + B->remove_constraint(this, 1); +} diff --git a/servers/physics_2d/godot_body_pair_2d.h b/servers/physics_2d/godot_body_pair_2d.h new file mode 100644 index 0000000000..0938ab542b --- /dev/null +++ b/servers/physics_2d/godot_body_pair_2d.h @@ -0,0 +1,98 @@ +/*************************************************************************/ +/* godot_body_pair_2d.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 GODOT_BODY_PAIR_2D_H +#define GODOT_BODY_PAIR_2D_H + +#include "godot_body_2d.h" +#include "godot_constraint_2d.h" + +class GodotBodyPair2D : public GodotConstraint2D { + enum { + MAX_CONTACTS = 2 + }; + union { + struct { + GodotBody2D *A; + GodotBody2D *B; + }; + + GodotBody2D *_arr[2] = { nullptr, nullptr }; + }; + + int shape_A = 0; + int shape_B = 0; + + bool collide_A = false; + bool collide_B = false; + + GodotSpace2D *space = nullptr; + + struct Contact { + Vector2 position; + Vector2 normal; + Vector2 local_A, local_B; + real_t acc_normal_impulse = 0.0; // accumulated normal impulse (Pn) + real_t acc_tangent_impulse = 0.0; // accumulated tangent impulse (Pt) + real_t acc_bias_impulse = 0.0; // accumulated normal impulse for position bias (Pnb) + real_t mass_normal, mass_tangent = 0.0; + real_t bias = 0.0; + + real_t depth = 0.0; + bool active = false; + Vector2 rA, rB; + bool reused = false; + real_t bounce = 0.0; + }; + + Vector2 offset_B; //use local A coordinates to avoid numerical issues on collision detection + + Vector2 sep_axis; + Contact contacts[MAX_CONTACTS]; + int contact_count = 0; + bool collided = false; + bool oneway_disabled = false; + bool report_contacts_only = false; + + bool _test_ccd(real_t p_step, GodotBody2D *p_A, int p_shape_A, const Transform2D &p_xform_A, GodotBody2D *p_B, int p_shape_B, const Transform2D &p_xform_B, bool p_swap_result = false); + void _validate_contacts(); + static void _add_contact(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_self); + _FORCE_INLINE_ void _contact_added_callback(const Vector2 &p_point_A, const Vector2 &p_point_B); + +public: + virtual bool setup(real_t p_step) override; + virtual bool pre_solve(real_t p_step) override; + virtual void solve(real_t p_step) override; + + GodotBodyPair2D(GodotBody2D *p_A, int p_shape_A, GodotBody2D *p_B, int p_shape_B); + ~GodotBodyPair2D(); +}; + +#endif // GODOT_BODY_PAIR_2D_H diff --git a/servers/physics_2d/godot_broad_phase_2d.cpp b/servers/physics_2d/godot_broad_phase_2d.cpp new file mode 100644 index 0000000000..4b35f8d996 --- /dev/null +++ b/servers/physics_2d/godot_broad_phase_2d.cpp @@ -0,0 +1,36 @@ +/*************************************************************************/ +/* godot_broad_phase_2d.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 "godot_broad_phase_2d.h" + +GodotBroadPhase2D::CreateFunction GodotBroadPhase2D::create_func = nullptr; + +GodotBroadPhase2D::~GodotBroadPhase2D() { +} diff --git a/servers/physics_2d/godot_broad_phase_2d.h b/servers/physics_2d/godot_broad_phase_2d.h new file mode 100644 index 0000000000..7017a6e41f --- /dev/null +++ b/servers/physics_2d/godot_broad_phase_2d.h @@ -0,0 +1,71 @@ +/*************************************************************************/ +/* godot_broad_phase_2d.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 GODOT_BROAD_PHASE_2D_H +#define GODOT_BROAD_PHASE_2D_H + +#include "core/math/math_funcs.h" +#include "core/math/rect2.h" + +class GodotCollisionObject2D; + +class GodotBroadPhase2D { +public: + typedef GodotBroadPhase2D *(*CreateFunction)(); + + static CreateFunction create_func; + + typedef uint32_t ID; + + typedef void *(*PairCallback)(GodotCollisionObject2D *A, int p_subindex_A, GodotCollisionObject2D *B, int p_subindex_B, void *p_userdata); + typedef void (*UnpairCallback)(GodotCollisionObject2D *A, int p_subindex_A, GodotCollisionObject2D *B, int p_subindex_B, void *p_data, void *p_userdata); + + // 0 is an invalid ID + virtual ID create(GodotCollisionObject2D *p_object_, int p_subindex = 0, const Rect2 &p_aabb = Rect2(), bool p_static = false) = 0; + virtual void move(ID p_id, const Rect2 &p_aabb) = 0; + virtual void set_static(ID p_id, bool p_static) = 0; + virtual void remove(ID p_id) = 0; + + virtual GodotCollisionObject2D *get_object(ID p_id) const = 0; + virtual bool is_static(ID p_id) const = 0; + virtual int get_subindex(ID p_id) const = 0; + + virtual int cull_segment(const Vector2 &p_from, const Vector2 &p_to, GodotCollisionObject2D **p_results, int p_max_results, int *p_result_indices = nullptr) = 0; + virtual int cull_aabb(const Rect2 &p_aabb, GodotCollisionObject2D **p_results, int p_max_results, int *p_result_indices = nullptr) = 0; + + virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata) = 0; + virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) = 0; + + virtual void update() = 0; + + virtual ~GodotBroadPhase2D(); +}; + +#endif // GODOT_BROAD_PHASE_2D_H diff --git a/servers/physics_2d/godot_broad_phase_2d_bvh.cpp b/servers/physics_2d/godot_broad_phase_2d_bvh.cpp new file mode 100644 index 0000000000..9ec6b0a6b7 --- /dev/null +++ b/servers/physics_2d/godot_broad_phase_2d_bvh.cpp @@ -0,0 +1,113 @@ +/*************************************************************************/ +/* godot_broad_phase_2d_bvh.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 "godot_broad_phase_2d_bvh.h" +#include "godot_collision_object_2d.h" + +GodotBroadPhase2D::ID GodotBroadPhase2DBVH::create(GodotCollisionObject2D *p_object, int p_subindex, const Rect2 &p_aabb, bool p_static) { + ID oid = bvh.create(p_object, true, p_aabb, p_subindex, !p_static, 1 << p_object->get_type(), p_static ? 0 : 0xFFFFF); // Pair everything, don't care? + return oid + 1; +} + +void GodotBroadPhase2DBVH::move(ID p_id, const Rect2 &p_aabb) { + bvh.move(p_id - 1, p_aabb); +} + +void GodotBroadPhase2DBVH::set_static(ID p_id, bool p_static) { + GodotCollisionObject2D *it = bvh.get(p_id - 1); + bvh.set_pairable(p_id - 1, !p_static, 1 << it->get_type(), p_static ? 0 : 0xFFFFF, false); // Pair everything, don't care? +} + +void GodotBroadPhase2DBVH::remove(ID p_id) { + bvh.erase(p_id - 1); +} + +GodotCollisionObject2D *GodotBroadPhase2DBVH::get_object(ID p_id) const { + GodotCollisionObject2D *it = bvh.get(p_id - 1); + ERR_FAIL_COND_V(!it, nullptr); + return it; +} + +bool GodotBroadPhase2DBVH::is_static(ID p_id) const { + return !bvh.is_pairable(p_id - 1); +} + +int GodotBroadPhase2DBVH::get_subindex(ID p_id) const { + return bvh.get_subindex(p_id - 1); +} + +int GodotBroadPhase2DBVH::cull_segment(const Vector2 &p_from, const Vector2 &p_to, GodotCollisionObject2D **p_results, int p_max_results, int *p_result_indices) { + return bvh.cull_segment(p_from, p_to, p_results, p_max_results, p_result_indices); +} + +int GodotBroadPhase2DBVH::cull_aabb(const Rect2 &p_aabb, GodotCollisionObject2D **p_results, int p_max_results, int *p_result_indices) { + return bvh.cull_aabb(p_aabb, p_results, p_max_results, p_result_indices); +} + +void *GodotBroadPhase2DBVH::_pair_callback(void *self, uint32_t p_A, GodotCollisionObject2D *p_object_A, int subindex_A, uint32_t p_B, GodotCollisionObject2D *p_object_B, int subindex_B) { + GodotBroadPhase2DBVH *bpo = (GodotBroadPhase2DBVH *)(self); + if (!bpo->pair_callback) { + return nullptr; + } + + return bpo->pair_callback(p_object_A, subindex_A, p_object_B, subindex_B, bpo->pair_userdata); +} + +void GodotBroadPhase2DBVH::_unpair_callback(void *self, uint32_t p_A, GodotCollisionObject2D *p_object_A, int subindex_A, uint32_t p_B, GodotCollisionObject2D *p_object_B, int subindex_B, void *pairdata) { + GodotBroadPhase2DBVH *bpo = (GodotBroadPhase2DBVH *)(self); + if (!bpo->unpair_callback) { + return; + } + + bpo->unpair_callback(p_object_A, subindex_A, p_object_B, subindex_B, pairdata, bpo->unpair_userdata); +} + +void GodotBroadPhase2DBVH::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) { + pair_callback = p_pair_callback; + pair_userdata = p_userdata; +} + +void GodotBroadPhase2DBVH::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) { + unpair_callback = p_unpair_callback; + unpair_userdata = p_userdata; +} + +void GodotBroadPhase2DBVH::update() { + bvh.update(); +} + +GodotBroadPhase2D *GodotBroadPhase2DBVH::_create() { + return memnew(GodotBroadPhase2DBVH); +} + +GodotBroadPhase2DBVH::GodotBroadPhase2DBVH() { + bvh.set_pair_callback(_pair_callback, this); + bvh.set_unpair_callback(_unpair_callback, this); +} diff --git a/servers/physics_2d/godot_broad_phase_2d_bvh.h b/servers/physics_2d/godot_broad_phase_2d_bvh.h new file mode 100644 index 0000000000..19b49f3499 --- /dev/null +++ b/servers/physics_2d/godot_broad_phase_2d_bvh.h @@ -0,0 +1,74 @@ +/*************************************************************************/ +/* godot_broad_phase_2d_bvh.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 GODOT_BROAD_PHASE_2D_BVH_H +#define GODOT_BROAD_PHASE_2D_BVH_H + +#include "godot_broad_phase_2d.h" + +#include "core/math/bvh.h" +#include "core/math/rect2.h" +#include "core/math/vector2.h" + +class GodotBroadPhase2DBVH : public GodotBroadPhase2D { + BVH_Manager bvh; + + static void *_pair_callback(void *, uint32_t, GodotCollisionObject2D *, int, uint32_t, GodotCollisionObject2D *, int); + static void _unpair_callback(void *, uint32_t, GodotCollisionObject2D *, int, uint32_t, GodotCollisionObject2D *, int, void *); + + PairCallback pair_callback = nullptr; + void *pair_userdata = nullptr; + UnpairCallback unpair_callback = nullptr; + void *unpair_userdata = nullptr; + +public: + // 0 is an invalid ID + virtual ID create(GodotCollisionObject2D *p_object, int p_subindex = 0, const Rect2 &p_aabb = Rect2(), bool p_static = false); + virtual void move(ID p_id, const Rect2 &p_aabb); + virtual void set_static(ID p_id, bool p_static); + virtual void remove(ID p_id); + + virtual GodotCollisionObject2D *get_object(ID p_id) const; + virtual bool is_static(ID p_id) const; + virtual int get_subindex(ID p_id) const; + + virtual int cull_segment(const Vector2 &p_from, const Vector2 &p_to, GodotCollisionObject2D **p_results, int p_max_results, int *p_result_indices = nullptr); + virtual int cull_aabb(const Rect2 &p_aabb, GodotCollisionObject2D **p_results, int p_max_results, int *p_result_indices = nullptr); + + virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata); + virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata); + + virtual void update(); + + static GodotBroadPhase2D *_create(); + GodotBroadPhase2DBVH(); +}; + +#endif // GODOT_BROAD_PHASE_2D_BVH_H diff --git a/servers/physics_2d/godot_collision_object_2d.cpp b/servers/physics_2d/godot_collision_object_2d.cpp new file mode 100644 index 0000000000..3d4ebbedcd --- /dev/null +++ b/servers/physics_2d/godot_collision_object_2d.cpp @@ -0,0 +1,243 @@ +/*************************************************************************/ +/* godot_collision_object_2d.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 "godot_collision_object_2d.h" +#include "godot_physics_server_2d.h" +#include "godot_space_2d.h" + +void GodotCollisionObject2D::add_shape(GodotShape2D *p_shape, const Transform2D &p_transform, bool p_disabled) { + Shape s; + s.shape = p_shape; + s.xform = p_transform; + s.xform_inv = s.xform.affine_inverse(); + s.bpid = 0; //needs update + s.disabled = p_disabled; + s.one_way_collision = false; + s.one_way_collision_margin = 0; + shapes.push_back(s); + p_shape->add_owner(this); + + if (!pending_shape_update_list.in_list()) { + GodotPhysicsServer2D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list); + } +} + +void GodotCollisionObject2D::set_shape(int p_index, GodotShape2D *p_shape) { + ERR_FAIL_INDEX(p_index, shapes.size()); + shapes[p_index].shape->remove_owner(this); + shapes.write[p_index].shape = p_shape; + + p_shape->add_owner(this); + + if (!pending_shape_update_list.in_list()) { + GodotPhysicsServer2D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list); + } +} + +void GodotCollisionObject2D::set_shape_transform(int p_index, const Transform2D &p_transform) { + ERR_FAIL_INDEX(p_index, shapes.size()); + + shapes.write[p_index].xform = p_transform; + shapes.write[p_index].xform_inv = p_transform.affine_inverse(); + + if (!pending_shape_update_list.in_list()) { + GodotPhysicsServer2D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list); + } +} + +void GodotCollisionObject2D::set_shape_disabled(int p_idx, bool p_disabled) { + ERR_FAIL_INDEX(p_idx, shapes.size()); + + GodotCollisionObject2D::Shape &shape = shapes.write[p_idx]; + if (shape.disabled == p_disabled) { + return; + } + + shape.disabled = p_disabled; + + if (!space) { + return; + } + + if (p_disabled && shape.bpid != 0) { + space->get_broadphase()->remove(shape.bpid); + shape.bpid = 0; + if (!pending_shape_update_list.in_list()) { + GodotPhysicsServer2D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list); + } + } else if (!p_disabled && shape.bpid == 0) { + if (!pending_shape_update_list.in_list()) { + GodotPhysicsServer2D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list); + } + } +} + +void GodotCollisionObject2D::remove_shape(GodotShape2D *p_shape) { + //remove a shape, all the times it appears + for (int i = 0; i < shapes.size(); i++) { + if (shapes[i].shape == p_shape) { + remove_shape(i); + i--; + } + } +} + +void GodotCollisionObject2D::remove_shape(int p_index) { + //remove anything from shape to be erased to end, so subindices don't change + ERR_FAIL_INDEX(p_index, shapes.size()); + for (int i = p_index; i < shapes.size(); i++) { + if (shapes[i].bpid == 0) { + continue; + } + //should never get here with a null owner + space->get_broadphase()->remove(shapes[i].bpid); + shapes.write[i].bpid = 0; + } + shapes[p_index].shape->remove_owner(this); + shapes.remove(p_index); + + if (!pending_shape_update_list.in_list()) { + GodotPhysicsServer2D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list); + } + // _update_shapes(); + // _shapes_changed(); +} + +void GodotCollisionObject2D::_set_static(bool p_static) { + if (_static == p_static) { + return; + } + _static = p_static; + + if (!space) { + return; + } + for (int i = 0; i < get_shape_count(); i++) { + const Shape &s = shapes[i]; + if (s.bpid > 0) { + space->get_broadphase()->set_static(s.bpid, _static); + } + } +} + +void GodotCollisionObject2D::_unregister_shapes() { + for (int i = 0; i < shapes.size(); i++) { + Shape &s = shapes.write[i]; + if (s.bpid > 0) { + space->get_broadphase()->remove(s.bpid); + s.bpid = 0; + } + } +} + +void GodotCollisionObject2D::_update_shapes() { + if (!space) { + return; + } + + for (int i = 0; i < shapes.size(); i++) { + Shape &s = shapes.write[i]; + if (s.disabled) { + continue; + } + + //not quite correct, should compute the next matrix.. + Rect2 shape_aabb = s.shape->get_aabb(); + Transform2D xform = transform * s.xform; + shape_aabb = xform.xform(shape_aabb); + shape_aabb.grow_by((s.aabb_cache.size.x + s.aabb_cache.size.y) * 0.5 * 0.05); + s.aabb_cache = shape_aabb; + + if (s.bpid == 0) { + s.bpid = space->get_broadphase()->create(this, i, shape_aabb, _static); + space->get_broadphase()->set_static(s.bpid, _static); + } + + space->get_broadphase()->move(s.bpid, shape_aabb); + } +} + +void GodotCollisionObject2D::_update_shapes_with_motion(const Vector2 &p_motion) { + if (!space) { + return; + } + + for (int i = 0; i < shapes.size(); i++) { + Shape &s = shapes.write[i]; + if (s.disabled) { + continue; + } + + //not quite correct, should compute the next matrix.. + Rect2 shape_aabb = s.shape->get_aabb(); + Transform2D xform = transform * s.xform; + shape_aabb = xform.xform(shape_aabb); + shape_aabb = shape_aabb.merge(Rect2(shape_aabb.position + p_motion, shape_aabb.size)); //use motion + s.aabb_cache = shape_aabb; + + if (s.bpid == 0) { + s.bpid = space->get_broadphase()->create(this, i, shape_aabb, _static); + space->get_broadphase()->set_static(s.bpid, _static); + } + + space->get_broadphase()->move(s.bpid, shape_aabb); + } +} + +void GodotCollisionObject2D::_set_space(GodotSpace2D *p_space) { + if (space) { + space->remove_object(this); + + for (int i = 0; i < shapes.size(); i++) { + Shape &s = shapes.write[i]; + if (s.bpid) { + space->get_broadphase()->remove(s.bpid); + s.bpid = 0; + } + } + } + + space = p_space; + + if (space) { + space->add_object(this); + _update_shapes(); + } +} + +void GodotCollisionObject2D::_shape_changed() { + _update_shapes(); + _shapes_changed(); +} + +GodotCollisionObject2D::GodotCollisionObject2D(Type p_type) : + pending_shape_update_list(this) { + type = p_type; +} diff --git a/servers/physics_2d/godot_collision_object_2d.h b/servers/physics_2d/godot_collision_object_2d.h new file mode 100644 index 0000000000..7233857808 --- /dev/null +++ b/servers/physics_2d/godot_collision_object_2d.h @@ -0,0 +1,190 @@ +/*************************************************************************/ +/* godot_collision_object_2d.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 GODOT_COLLISION_OBJECT_2D_H +#define GODOT_COLLISION_OBJECT_2D_H + +#include "godot_broad_phase_2d.h" +#include "godot_shape_2d.h" + +#include "core/templates/self_list.h" +#include "servers/physics_server_2d.h" + +class GodotSpace2D; + +class GodotCollisionObject2D : public GodotShapeOwner2D { +public: + enum Type { + TYPE_AREA, + TYPE_BODY + }; + +private: + Type type; + RID self; + ObjectID instance_id; + ObjectID canvas_instance_id; + bool pickable = true; + + struct Shape { + Transform2D xform; + Transform2D xform_inv; + GodotBroadPhase2D::ID bpid = 0; + Rect2 aabb_cache; //for rayqueries + GodotShape2D *shape = nullptr; + bool disabled = false; + bool one_way_collision = false; + real_t one_way_collision_margin = 0.0; + }; + + Vector shapes; + GodotSpace2D *space = nullptr; + Transform2D transform; + Transform2D inv_transform; + uint32_t collision_mask = 1; + uint32_t collision_layer = 1; + bool _static = true; + + SelfList pending_shape_update_list; + + void _update_shapes(); + +protected: + void _update_shapes_with_motion(const Vector2 &p_motion); + void _unregister_shapes(); + + _FORCE_INLINE_ void _set_transform(const Transform2D &p_transform, bool p_update_shapes = true) { + transform = p_transform; + if (p_update_shapes) { + _update_shapes(); + } + } + _FORCE_INLINE_ void _set_inv_transform(const Transform2D &p_transform) { inv_transform = p_transform; } + void _set_static(bool p_static); + + virtual void _shapes_changed() = 0; + void _set_space(GodotSpace2D *p_space); + + GodotCollisionObject2D(Type p_type); + +public: + _FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; } + _FORCE_INLINE_ RID get_self() const { return self; } + + _FORCE_INLINE_ void set_instance_id(const ObjectID &p_instance_id) { instance_id = p_instance_id; } + _FORCE_INLINE_ ObjectID get_instance_id() const { return instance_id; } + + _FORCE_INLINE_ void set_canvas_instance_id(const ObjectID &p_canvas_instance_id) { canvas_instance_id = p_canvas_instance_id; } + _FORCE_INLINE_ ObjectID get_canvas_instance_id() const { return canvas_instance_id; } + + void _shape_changed(); + + _FORCE_INLINE_ Type get_type() const { return type; } + void add_shape(GodotShape2D *p_shape, const Transform2D &p_transform = Transform2D(), bool p_disabled = false); + void set_shape(int p_index, GodotShape2D *p_shape); + void set_shape_transform(int p_index, const Transform2D &p_transform); + + _FORCE_INLINE_ int get_shape_count() const { return shapes.size(); } + _FORCE_INLINE_ GodotShape2D *get_shape(int p_index) const { + CRASH_BAD_INDEX(p_index, shapes.size()); + return shapes[p_index].shape; + } + _FORCE_INLINE_ const Transform2D &get_shape_transform(int p_index) const { + CRASH_BAD_INDEX(p_index, shapes.size()); + return shapes[p_index].xform; + } + _FORCE_INLINE_ const Transform2D &get_shape_inv_transform(int p_index) const { + CRASH_BAD_INDEX(p_index, shapes.size()); + return shapes[p_index].xform_inv; + } + _FORCE_INLINE_ const Rect2 &get_shape_aabb(int p_index) const { + CRASH_BAD_INDEX(p_index, shapes.size()); + return shapes[p_index].aabb_cache; + } + + _FORCE_INLINE_ const Transform2D &get_transform() const { return transform; } + _FORCE_INLINE_ const Transform2D &get_inv_transform() const { return inv_transform; } + _FORCE_INLINE_ GodotSpace2D *get_space() const { return space; } + + void set_shape_disabled(int p_idx, bool p_disabled); + _FORCE_INLINE_ bool is_shape_disabled(int p_idx) const { + ERR_FAIL_INDEX_V(p_idx, shapes.size(), false); + return shapes[p_idx].disabled; + } + + _FORCE_INLINE_ void set_shape_as_one_way_collision(int p_idx, bool p_one_way_collision, real_t p_margin) { + CRASH_BAD_INDEX(p_idx, shapes.size()); + shapes.write[p_idx].one_way_collision = p_one_way_collision; + shapes.write[p_idx].one_way_collision_margin = p_margin; + } + _FORCE_INLINE_ bool is_shape_set_as_one_way_collision(int p_idx) const { + CRASH_BAD_INDEX(p_idx, shapes.size()); + return shapes[p_idx].one_way_collision; + } + + _FORCE_INLINE_ real_t get_shape_one_way_collision_margin(int p_idx) const { + CRASH_BAD_INDEX(p_idx, shapes.size()); + return shapes[p_idx].one_way_collision_margin; + } + + 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; } + + 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; } + + void remove_shape(GodotShape2D *p_shape); + void remove_shape(int p_index); + + virtual void set_space(GodotSpace2D *p_space) = 0; + + _FORCE_INLINE_ bool is_static() const { return _static; } + + void set_pickable(bool p_pickable) { pickable = p_pickable; } + _FORCE_INLINE_ bool is_pickable() const { return pickable; } + + _FORCE_INLINE_ bool collides_with(GodotCollisionObject2D *p_other) const { + return p_other->collision_layer & collision_mask; + } + + _FORCE_INLINE_ bool interacts_with(GodotCollisionObject2D *p_other) const { + return collision_layer & p_other->collision_mask || p_other->collision_layer & collision_mask; + } + + virtual ~GodotCollisionObject2D() {} +}; + +#endif // GODOT_COLLISION_OBJECT_2D_H diff --git a/servers/physics_2d/godot_collision_solver_2d.cpp b/servers/physics_2d/godot_collision_solver_2d.cpp new file mode 100644 index 0000000000..25371b9885 --- /dev/null +++ b/servers/physics_2d/godot_collision_solver_2d.cpp @@ -0,0 +1,264 @@ +/*************************************************************************/ +/* godot_collision_solver_2d.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 "godot_collision_solver_2d.h" +#include "godot_collision_solver_2d_sat.h" + +#define collision_solver sat_2d_calculate_penetration +//#define collision_solver gjk_epa_calculate_penetration + +bool GodotCollisionSolver2D::solve_static_world_boundary(const GodotShape2D *p_shape_A, const Transform2D &p_transform_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) { + const GodotWorldBoundaryShape2D *world_boundary = static_cast(p_shape_A); + if (p_shape_B->get_type() == PhysicsServer2D::SHAPE_WORLD_BOUNDARY) { + return false; + } + + Vector2 n = p_transform_A.basis_xform(world_boundary->get_normal()).normalized(); + Vector2 p = p_transform_A.xform(world_boundary->get_normal() * world_boundary->get_d()); + real_t d = n.dot(p); + + Vector2 supports[2]; + int support_count; + + p_shape_B->get_supports(p_transform_B.affine_inverse().basis_xform(-n).normalized(), supports, support_count); + + bool found = false; + + for (int i = 0; i < support_count; i++) { + supports[i] = p_transform_B.xform(supports[i]); + real_t pd = n.dot(supports[i]); + if (pd >= d) { + continue; + } + found = true; + + Vector2 support_A = supports[i] - n * (pd - d); + + if (p_result_callback) { + if (p_swap_result) { + p_result_callback(supports[i], support_A, p_userdata); + } else { + p_result_callback(support_A, supports[i], p_userdata); + } + } + } + + return found; +} + +bool GodotCollisionSolver2D::solve_separation_ray(const GodotShape2D *p_shape_A, const Vector2 &p_motion_A, const Transform2D &p_transform_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *r_sep_axis, real_t p_margin) { + const GodotSeparationRayShape2D *ray = static_cast(p_shape_A); + if (p_shape_B->get_type() == PhysicsServer2D::SHAPE_SEPARATION_RAY) { + return false; + } + + Vector2 from = p_transform_A.get_origin(); + Vector2 to = from + p_transform_A[1] * (ray->get_length() + p_margin); + if (p_motion_A != Vector2()) { + //not the best but should be enough + Vector2 normal = (to - from).normalized(); + to += normal * MAX(0.0, normal.dot(p_motion_A)); + } + Vector2 support_A = to; + + Transform2D invb = p_transform_B.affine_inverse(); + from = invb.xform(from); + to = invb.xform(to); + + Vector2 p, n; + if (!p_shape_B->intersect_segment(from, to, p, n)) { + if (r_sep_axis) { + *r_sep_axis = p_transform_A[1].normalized(); + } + return false; + } + + // Discard contacts when the ray is fully contained inside the shape. + if (n == Vector2()) { + if (r_sep_axis) { + *r_sep_axis = p_transform_A[1].normalized(); + } + return false; + } + + // Discard contacts in the wrong direction. + if (n.dot(from - to) < CMP_EPSILON) { + if (r_sep_axis) { + *r_sep_axis = p_transform_A[1].normalized(); + } + return false; + } + + Vector2 support_B = p_transform_B.xform(p); + if (ray->get_slide_on_slope()) { + Vector2 global_n = invb.basis_xform_inv(n).normalized(); + support_B = support_A + (support_B - support_A).length() * global_n; + } + + if (p_result_callback) { + if (p_swap_result) { + p_result_callback(support_B, support_A, p_userdata); + } else { + p_result_callback(support_A, support_B, p_userdata); + } + } + return true; +} + +struct _ConcaveCollisionInfo2D { + const Transform2D *transform_A = nullptr; + const GodotShape2D *shape_A = nullptr; + const Transform2D *transform_B = nullptr; + Vector2 motion_A; + Vector2 motion_B; + real_t margin_A = 0.0; + real_t margin_B = 0.0; + GodotCollisionSolver2D::CallbackResult result_callback; + void *userdata = nullptr; + bool swap_result = false; + bool collided = false; + int aabb_tests = 0; + int collisions = 0; + Vector2 *sep_axis = nullptr; +}; + +bool GodotCollisionSolver2D::concave_callback(void *p_userdata, GodotShape2D *p_convex) { + _ConcaveCollisionInfo2D &cinfo = *(_ConcaveCollisionInfo2D *)(p_userdata); + cinfo.aabb_tests++; + + bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, cinfo.motion_A, p_convex, *cinfo.transform_B, cinfo.motion_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result, cinfo.sep_axis, cinfo.margin_A, cinfo.margin_B); + if (!collided) { + return false; + } + + cinfo.collided = true; + cinfo.collisions++; + + // Stop at first collision if contacts are not needed. + return !cinfo.result_callback; +} + +bool GodotCollisionSolver2D::solve_concave(const GodotShape2D *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *r_sep_axis, real_t p_margin_A, real_t p_margin_B) { + const GodotConcaveShape2D *concave_B = static_cast(p_shape_B); + + _ConcaveCollisionInfo2D cinfo; + cinfo.transform_A = &p_transform_A; + cinfo.shape_A = p_shape_A; + cinfo.transform_B = &p_transform_B; + cinfo.motion_A = p_motion_A; + cinfo.result_callback = p_result_callback; + cinfo.userdata = p_userdata; + cinfo.swap_result = p_swap_result; + cinfo.collided = false; + cinfo.collisions = 0; + cinfo.sep_axis = r_sep_axis; + cinfo.margin_A = p_margin_A; + cinfo.margin_B = p_margin_B; + + cinfo.aabb_tests = 0; + + Transform2D rel_transform = p_transform_A; + rel_transform.elements[2] -= p_transform_B.get_origin(); + + //quickly compute a local Rect2 + + Rect2 local_aabb; + for (int i = 0; i < 2; i++) { + Vector2 axis(p_transform_B.elements[i]); + real_t axis_scale = 1.0 / axis.length(); + axis *= axis_scale; + + real_t smin, smax; + p_shape_A->project_rangev(axis, rel_transform, smin, smax); + smin *= axis_scale; + smax *= axis_scale; + + local_aabb.position[i] = smin; + local_aabb.size[i] = smax - smin; + } + + concave_B->cull(local_aabb, concave_callback, &cinfo); + + return cinfo.collided; +} + +bool GodotCollisionSolver2D::solve(const GodotShape2D *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, Vector2 *r_sep_axis, real_t p_margin_A, real_t p_margin_B) { + PhysicsServer2D::ShapeType type_A = p_shape_A->get_type(); + PhysicsServer2D::ShapeType type_B = p_shape_B->get_type(); + bool concave_A = p_shape_A->is_concave(); + bool concave_B = p_shape_B->is_concave(); + real_t margin_A = p_margin_A, margin_B = p_margin_B; + + bool swap = false; + + if (type_A > type_B) { + SWAP(type_A, type_B); + SWAP(concave_A, concave_B); + SWAP(margin_A, margin_B); + swap = true; + } + + if (type_A == PhysicsServer2D::SHAPE_WORLD_BOUNDARY) { + if (type_B == PhysicsServer2D::SHAPE_WORLD_BOUNDARY) { + return false; + } + + if (swap) { + return solve_static_world_boundary(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true); + } else { + return solve_static_world_boundary(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false); + } + + } else if (type_A == PhysicsServer2D::SHAPE_SEPARATION_RAY) { + if (type_B == PhysicsServer2D::SHAPE_SEPARATION_RAY) { + return false; //no ray-ray + } + + if (swap) { + return solve_separation_ray(p_shape_B, p_motion_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true, r_sep_axis, p_margin_B); + } else { + return solve_separation_ray(p_shape_A, p_motion_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false, r_sep_axis, p_margin_A); + } + + } else if (concave_B) { + if (concave_A) { + return false; + } + + if (!swap) { + return solve_concave(p_shape_A, p_transform_A, p_motion_A, p_shape_B, p_transform_B, p_motion_B, p_result_callback, p_userdata, false, r_sep_axis, margin_A, margin_B); + } else { + return solve_concave(p_shape_B, p_transform_B, p_motion_B, p_shape_A, p_transform_A, p_motion_A, p_result_callback, p_userdata, true, r_sep_axis, margin_A, margin_B); + } + + } else { + return collision_solver(p_shape_A, p_transform_A, p_motion_A, p_shape_B, p_transform_B, p_motion_B, p_result_callback, p_userdata, false, r_sep_axis, margin_A, margin_B); + } +} diff --git a/servers/physics_2d/godot_collision_solver_2d.h b/servers/physics_2d/godot_collision_solver_2d.h new file mode 100644 index 0000000000..f10815a444 --- /dev/null +++ b/servers/physics_2d/godot_collision_solver_2d.h @@ -0,0 +1,50 @@ +/*************************************************************************/ +/* godot_collision_solver_2d.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 GODOT_COLLISION_SOLVER_2D_H +#define GODOT_COLLISION_SOLVER_2D_H + +#include "godot_shape_2d.h" + +class GodotCollisionSolver2D { +public: + typedef void (*CallbackResult)(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_userdata); + +private: + static bool solve_static_world_boundary(const GodotShape2D *p_shape_A, const Transform2D &p_transform_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); + static bool concave_callback(void *p_userdata, GodotShape2D *p_convex); + static bool solve_concave(const GodotShape2D *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *r_sep_axis = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0); + static bool solve_separation_ray(const GodotShape2D *p_shape_A, const Vector2 &p_motion_A, const Transform2D &p_transform_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *r_sep_axis = nullptr, real_t p_margin = 0); + +public: + static bool solve(const GodotShape2D *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, Vector2 *r_sep_axis = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0); +}; + +#endif // GODOT_COLLISION_SOLVER_2D_H diff --git a/servers/physics_2d/godot_collision_solver_2d_sat.cpp b/servers/physics_2d/godot_collision_solver_2d_sat.cpp new file mode 100644 index 0000000000..63053e8259 --- /dev/null +++ b/servers/physics_2d/godot_collision_solver_2d_sat.cpp @@ -0,0 +1,1408 @@ +/*************************************************************************/ +/* godot_collision_solver_2d_sat.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 "godot_collision_solver_2d_sat.h" + +#include "core/math/geometry_2d.h" + +struct _CollectorCallback2D { + GodotCollisionSolver2D::CallbackResult callback; + void *userdata = nullptr; + bool swap = false; + bool collided = false; + Vector2 normal; + Vector2 *sep_axis = nullptr; + + _FORCE_INLINE_ void call(const Vector2 &p_point_A, const Vector2 &p_point_B) { + /* + if (normal.dot(p_point_A) >= normal.dot(p_point_B)) + return; + */ + if (swap) { + callback(p_point_B, p_point_A, userdata); + } else { + callback(p_point_A, p_point_B, userdata); + } + } +}; + +typedef void (*GenerateContactsFunc)(const Vector2 *, int, const Vector2 *, int, _CollectorCallback2D *); + +_FORCE_INLINE_ static void _generate_contacts_point_point(const Vector2 *p_points_A, int p_point_count_A, const Vector2 *p_points_B, int p_point_count_B, _CollectorCallback2D *p_collector) { +#ifdef DEBUG_ENABLED + ERR_FAIL_COND(p_point_count_A != 1); + ERR_FAIL_COND(p_point_count_B != 1); +#endif + + p_collector->call(*p_points_A, *p_points_B); +} + +_FORCE_INLINE_ static void _generate_contacts_point_edge(const Vector2 *p_points_A, int p_point_count_A, const Vector2 *p_points_B, int p_point_count_B, _CollectorCallback2D *p_collector) { +#ifdef DEBUG_ENABLED + ERR_FAIL_COND(p_point_count_A != 1); + ERR_FAIL_COND(p_point_count_B != 2); +#endif + + Vector2 closest_B = Geometry2D::get_closest_point_to_segment_uncapped(*p_points_A, p_points_B); + p_collector->call(*p_points_A, closest_B); +} + +struct _generate_contacts_Pair { + bool a = false; + int idx = 0; + real_t d = 0.0; + _FORCE_INLINE_ bool operator<(const _generate_contacts_Pair &l) const { return d < l.d; } +}; + +_FORCE_INLINE_ static void _generate_contacts_edge_edge(const Vector2 *p_points_A, int p_point_count_A, const Vector2 *p_points_B, int p_point_count_B, _CollectorCallback2D *p_collector) { +#ifdef DEBUG_ENABLED + ERR_FAIL_COND(p_point_count_A != 2); + ERR_FAIL_COND(p_point_count_B != 2); // circle is actually a 4x3 matrix +#endif + + Vector2 n = p_collector->normal; + Vector2 t = n.orthogonal(); + real_t dA = n.dot(p_points_A[0]); + real_t dB = n.dot(p_points_B[0]); + + _generate_contacts_Pair dvec[4]; + + dvec[0].d = t.dot(p_points_A[0]); + dvec[0].a = true; + dvec[0].idx = 0; + dvec[1].d = t.dot(p_points_A[1]); + dvec[1].a = true; + dvec[1].idx = 1; + dvec[2].d = t.dot(p_points_B[0]); + dvec[2].a = false; + dvec[2].idx = 0; + dvec[3].d = t.dot(p_points_B[1]); + dvec[3].a = false; + dvec[3].idx = 1; + + SortArray<_generate_contacts_Pair> sa; + sa.sort(dvec, 4); + + for (int i = 1; i <= 2; i++) { + if (dvec[i].a) { + Vector2 a = p_points_A[dvec[i].idx]; + Vector2 b = n.plane_project(dB, a); + if (n.dot(a) > n.dot(b) - CMP_EPSILON) { + continue; + } + p_collector->call(a, b); + } else { + Vector2 b = p_points_B[dvec[i].idx]; + Vector2 a = n.plane_project(dA, b); + if (n.dot(a) > n.dot(b) - CMP_EPSILON) { + continue; + } + p_collector->call(a, b); + } + } +} + +static void _generate_contacts_from_supports(const Vector2 *p_points_A, int p_point_count_A, const Vector2 *p_points_B, int p_point_count_B, _CollectorCallback2D *p_collector) { +#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[2][2] = { + { + _generate_contacts_point_point, + _generate_contacts_point_edge, + }, + { + nullptr, + _generate_contacts_edge_edge, + } + }; + + int pointcount_B = 0; + int pointcount_A = 0; + const Vector2 *points_A = nullptr; + const Vector2 *points_B = nullptr; + + if (p_point_count_A > p_point_count_B) { + //swap + p_collector->swap = !p_collector->swap; + p_collector->normal = -p_collector->normal; + + pointcount_B = p_point_count_A; + pointcount_A = p_point_count_B; + points_A = p_points_B; + points_B = p_points_A; + } else { + pointcount_B = p_point_count_B; + pointcount_A = p_point_count_A; + points_A = p_points_A; + points_B = p_points_B; + } + + int version_A = (pointcount_A > 2 ? 2 : pointcount_A) - 1; + int version_B = (pointcount_B > 2 ? 2 : 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_collector); +} + +template +class SeparatorAxisTest2D { + const ShapeA *shape_A = nullptr; + const ShapeB *shape_B = nullptr; + const Transform2D *transform_A = nullptr; + const Transform2D *transform_B = nullptr; + real_t best_depth = 1e15; + Vector2 best_axis; +#ifdef DEBUG_ENABLED + int best_axis_count = 0; + int best_axis_index = -1; +#endif + Vector2 motion_A; + Vector2 motion_B; + real_t margin_A = 0.0; + real_t margin_B = 0.0; + _CollectorCallback2D *callback; + +public: + _FORCE_INLINE_ bool test_previous_axis() { + if (callback && callback->sep_axis && *callback->sep_axis != Vector2()) { + return test_axis(*callback->sep_axis); + } else { +#ifdef DEBUG_ENABLED + best_axis_count++; +#endif + } + return true; + } + + _FORCE_INLINE_ bool test_cast() { + if (castA) { + Vector2 na = motion_A.normalized(); + if (!test_axis(na)) { + return false; + } + if (!test_axis(na.orthogonal())) { + return false; + } + } + + if (castB) { + Vector2 nb = motion_B.normalized(); + if (!test_axis(nb)) { + return false; + } + if (!test_axis(nb.orthogonal())) { + return false; + } + } + + return true; + } + + _FORCE_INLINE_ bool test_axis(const Vector2 &p_axis) { + Vector2 axis = p_axis; + + if (Math::is_zero_approx(axis.x) && + Math::is_zero_approx(axis.y)) { + // strange case, try an upwards separator + axis = Vector2(0.0, 1.0); + } + + real_t min_A, max_A, min_B, max_B; + + if (castA) { + shape_A->project_range_cast(motion_A, axis, *transform_A, min_A, max_A); + } else { + shape_A->project_range(axis, *transform_A, min_A, max_A); + } + + if (castB) { + shape_B->project_range_cast(motion_B, axis, *transform_B, min_B, max_B); + } else { + shape_B->project_range(axis, *transform_B, min_B, max_B); + } + + if (withMargin) { + min_A -= margin_A; + max_A += margin_A; + min_B -= margin_B; + max_B += margin_B; + } + + min_B -= (max_A - min_A) * 0.5; + max_B += (max_A - min_A) * 0.5; + + real_t dmin = min_B - (min_A + max_A) * 0.5; + real_t dmax = max_B - (min_A + max_A) * 0.5; + + if (dmin > 0.0 || dmax < 0.0) { + if (callback && callback->sep_axis) { + *callback->sep_axis = axis; + } +#ifdef DEBUG_ENABLED + best_axis_count++; +#endif + + return false; // doesn't contain 0 + } + + //use the smallest depth + + dmin = Math::abs(dmin); + + if (dmax < dmin) { + if (dmax < best_depth) { + best_depth = dmax; + best_axis = axis; +#ifdef DEBUG_ENABLED + best_axis_index = best_axis_count; +#endif + } + } else { + if (dmin < best_depth) { + best_depth = dmin; + best_axis = -axis; // keep it as A axis +#ifdef DEBUG_ENABLED + best_axis_index = best_axis_count; +#endif + } + } + +#ifdef DEBUG_ENABLED + best_axis_count++; +#endif + + return true; + } + + _FORCE_INLINE_ void generate_contacts() { + // nothing to do, don't generate + if (best_axis == Vector2(0.0, 0.0)) { + return; + } + + if (callback) { + callback->collided = true; + + if (!callback->callback) { + return; //only collide, no callback + } + } + static const int max_supports = 2; + + Vector2 supports_A[max_supports]; + int support_count_A; + if (castA) { + shape_A->get_supports_transformed_cast(motion_A, -best_axis, *transform_A, supports_A, support_count_A); + } else { + shape_A->get_supports(transform_A->basis_xform_inv(-best_axis).normalized(), supports_A, support_count_A); + for (int i = 0; i < support_count_A; i++) { + supports_A[i] = transform_A->xform(supports_A[i]); + } + } + + if (withMargin) { + for (int i = 0; i < support_count_A; i++) { + supports_A[i] += -best_axis * margin_A; + } + } + + Vector2 supports_B[max_supports]; + int support_count_B; + if (castB) { + shape_B->get_supports_transformed_cast(motion_B, best_axis, *transform_B, supports_B, support_count_B); + } else { + shape_B->get_supports(transform_B->basis_xform_inv(best_axis).normalized(), supports_B, support_count_B); + for (int i = 0; i < support_count_B; i++) { + supports_B[i] = transform_B->xform(supports_B[i]); + } + } + + if (withMargin) { + for (int i = 0; i < support_count_B; i++) { + supports_B[i] += best_axis * margin_B; + } + } + if (callback) { + callback->normal = best_axis; + _generate_contacts_from_supports(supports_A, support_count_A, supports_B, support_count_B, callback); + + if (callback->sep_axis && *callback->sep_axis != Vector2()) { + *callback->sep_axis = Vector2(); //invalidate previous axis (no test) + } + } + } + + _FORCE_INLINE_ SeparatorAxisTest2D(const ShapeA *p_shape_A, const Transform2D &p_transform_a, const ShapeB *p_shape_B, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_A = Vector2(), const Vector2 &p_motion_B = Vector2(), real_t p_margin_A = 0, real_t p_margin_B = 0) { + margin_A = p_margin_A; + margin_B = p_margin_B; + shape_A = p_shape_A; + shape_B = p_shape_B; + transform_A = &p_transform_a; + transform_B = &p_transform_b; + motion_A = p_motion_A; + motion_B = p_motion_B; + callback = p_collector; + } +}; + +/****** SAT TESTS *******/ + +#define TEST_POINT(m_a, m_b) \ + ((!separator.test_axis(((m_a) - (m_b)).normalized())) || \ + (castA && !separator.test_axis(((m_a) + p_motion_a - (m_b)).normalized())) || \ + (castB && !separator.test_axis(((m_a) - ((m_b) + p_motion_b)).normalized())) || \ + (castA && castB && !separator.test_axis(((m_a) + p_motion_a - ((m_b) + p_motion_b)).normalized()))) + +typedef void (*CollisionFunc)(const GodotShape2D *, const Transform2D &, const GodotShape2D *, const Transform2D &, _CollectorCallback2D *p_collector, const Vector2 &, const Vector2 &, real_t, real_t); + +template +static void _collision_segment_segment(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const GodotSegmentShape2D *segment_A = static_cast(p_a); + const GodotSegmentShape2D *segment_B = static_cast(p_b); + + SeparatorAxisTest2D separator(segment_A, p_transform_a, segment_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); + + if (!separator.test_previous_axis()) { + return; + } + //this collision is kind of pointless + + if (!separator.test_cast()) { + return; + } + + if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a))) { + return; + } + if (!separator.test_axis(segment_B->get_xformed_normal(p_transform_b))) { + return; + } + + if (withMargin) { + //points grow to circles + + if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), p_transform_b.xform(segment_B->get_a()))) { + return; + } + if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), p_transform_b.xform(segment_B->get_b()))) { + return; + } + if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), p_transform_b.xform(segment_B->get_a()))) { + return; + } + if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), p_transform_b.xform(segment_B->get_b()))) { + return; + } + } + + separator.generate_contacts(); +} + +template +static void _collision_segment_circle(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const GodotSegmentShape2D *segment_A = static_cast(p_a); + const GodotCircleShape2D *circle_B = static_cast(p_b); + + SeparatorAxisTest2D separator(segment_A, p_transform_a, circle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); + + if (!separator.test_previous_axis()) { + return; + } + + if (!separator.test_cast()) { + return; + } + + //segment normal + if (!separator.test_axis( + (p_transform_a.xform(segment_A->get_b()) - p_transform_a.xform(segment_A->get_a())).normalized().orthogonal())) { + return; + } + + //endpoint a vs circle + if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), p_transform_b.get_origin())) { + return; + } + //endpoint b vs circle + if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), p_transform_b.get_origin())) { + return; + } + + separator.generate_contacts(); +} + +template +static void _collision_segment_rectangle(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const GodotSegmentShape2D *segment_A = static_cast(p_a); + const GodotRectangleShape2D *rectangle_B = static_cast(p_b); + + SeparatorAxisTest2D separator(segment_A, p_transform_a, rectangle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); + + if (!separator.test_previous_axis()) { + return; + } + + if (!separator.test_cast()) { + return; + } + + if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a))) { + return; + } + + if (!separator.test_axis(p_transform_b.elements[0].normalized())) { + return; + } + + if (!separator.test_axis(p_transform_b.elements[1].normalized())) { + return; + } + + if (withMargin) { + Transform2D inv = p_transform_b.affine_inverse(); + + Vector2 a = p_transform_a.xform(segment_A->get_a()); + Vector2 b = p_transform_a.xform(segment_A->get_b()); + + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a))) { + return; + } + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, b))) { + return; + } + + if (castA) { + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a + p_motion_a))) { + return; + } + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, b + p_motion_a))) { + return; + } + } + + if (castB) { + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a - p_motion_b))) { + return; + } + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, b - p_motion_b))) { + return; + } + } + + if (castA && castB) { + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a - p_motion_b + p_motion_a))) { + return; + } + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, b - p_motion_b + p_motion_a))) { + return; + } + } + } + + separator.generate_contacts(); +} + +template +static void _collision_segment_capsule(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const GodotSegmentShape2D *segment_A = static_cast(p_a); + const GodotCapsuleShape2D *capsule_B = static_cast(p_b); + + SeparatorAxisTest2D separator(segment_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); + + if (!separator.test_previous_axis()) { + return; + } + + if (!separator.test_cast()) { + return; + } + + if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a))) { + return; + } + + if (!separator.test_axis(p_transform_b.elements[0].normalized())) { + return; + } + + real_t capsule_dir = capsule_B->get_height() * 0.5 - capsule_B->get_radius(); + + if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir))) { + return; + } + if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), (p_transform_b.get_origin() - p_transform_b.elements[1] * capsule_dir))) { + return; + } + if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir))) { + return; + } + if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), (p_transform_b.get_origin() - p_transform_b.elements[1] * capsule_dir))) { + return; + } + + separator.generate_contacts(); +} + +template +static void _collision_segment_convex_polygon(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const GodotSegmentShape2D *segment_A = static_cast(p_a); + const GodotConvexPolygonShape2D *convex_B = static_cast(p_b); + + SeparatorAxisTest2D separator(segment_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); + + if (!separator.test_previous_axis()) { + return; + } + + if (!separator.test_cast()) { + return; + } + + if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a))) { + return; + } + + for (int i = 0; i < convex_B->get_point_count(); i++) { + if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) { + return; + } + + if (withMargin) { + if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), p_transform_b.xform(convex_B->get_point(i)))) { + return; + } + if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), p_transform_b.xform(convex_B->get_point(i)))) { + return; + } + } + } + + separator.generate_contacts(); +} + +///////// + +template +static void _collision_circle_circle(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const GodotCircleShape2D *circle_A = static_cast(p_a); + const GodotCircleShape2D *circle_B = static_cast(p_b); + + SeparatorAxisTest2D separator(circle_A, p_transform_a, circle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); + + if (!separator.test_previous_axis()) { + return; + } + + if (!separator.test_cast()) { + return; + } + + if (TEST_POINT(p_transform_a.get_origin(), p_transform_b.get_origin())) { + return; + } + + separator.generate_contacts(); +} + +template +static void _collision_circle_rectangle(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const GodotCircleShape2D *circle_A = static_cast(p_a); + const GodotRectangleShape2D *rectangle_B = static_cast(p_b); + + SeparatorAxisTest2D separator(circle_A, p_transform_a, rectangle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); + + if (!separator.test_previous_axis()) { + return; + } + + if (!separator.test_cast()) { + return; + } + + const Vector2 &sphere = p_transform_a.elements[2]; + const Vector2 *axis = &p_transform_b.elements[0]; + //const Vector2& half_extents = rectangle_B->get_half_extents(); + + if (!separator.test_axis(axis[0].normalized())) { + return; + } + + if (!separator.test_axis(axis[1].normalized())) { + return; + } + + Transform2D binv = p_transform_b.affine_inverse(); + { + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphere))) { + return; + } + } + + if (castA) { + Vector2 sphereofs = sphere + p_motion_a; + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphereofs))) { + return; + } + } + + if (castB) { + Vector2 sphereofs = sphere - p_motion_b; + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphereofs))) { + return; + } + } + + if (castA && castB) { + Vector2 sphereofs = sphere - p_motion_b + p_motion_a; + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphereofs))) { + return; + } + } + + separator.generate_contacts(); +} + +template +static void _collision_circle_capsule(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const GodotCircleShape2D *circle_A = static_cast(p_a); + const GodotCapsuleShape2D *capsule_B = static_cast(p_b); + + SeparatorAxisTest2D separator(circle_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); + + if (!separator.test_previous_axis()) { + return; + } + + if (!separator.test_cast()) { + return; + } + + //capsule axis + if (!separator.test_axis(p_transform_b.elements[0].normalized())) { + return; + } + + real_t capsule_dir = capsule_B->get_height() * 0.5 - capsule_B->get_radius(); + + //capsule endpoints + if (TEST_POINT(p_transform_a.get_origin(), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir))) { + return; + } + if (TEST_POINT(p_transform_a.get_origin(), (p_transform_b.get_origin() - p_transform_b.elements[1] * capsule_dir))) { + return; + } + + separator.generate_contacts(); +} + +template +static void _collision_circle_convex_polygon(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const GodotCircleShape2D *circle_A = static_cast(p_a); + const GodotConvexPolygonShape2D *convex_B = static_cast(p_b); + + SeparatorAxisTest2D separator(circle_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); + + if (!separator.test_previous_axis()) { + return; + } + + if (!separator.test_cast()) { + return; + } + + //poly faces and poly points vs circle + for (int i = 0; i < convex_B->get_point_count(); i++) { + if (TEST_POINT(p_transform_a.get_origin(), p_transform_b.xform(convex_B->get_point(i)))) { + return; + } + + if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) { + return; + } + } + + separator.generate_contacts(); +} + +///////// + +template +static void _collision_rectangle_rectangle(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const GodotRectangleShape2D *rectangle_A = static_cast(p_a); + const GodotRectangleShape2D *rectangle_B = static_cast(p_b); + + SeparatorAxisTest2D separator(rectangle_A, p_transform_a, rectangle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); + + if (!separator.test_previous_axis()) { + return; + } + + if (!separator.test_cast()) { + return; + } + + //box faces A + if (!separator.test_axis(p_transform_a.elements[0].normalized())) { + return; + } + + if (!separator.test_axis(p_transform_a.elements[1].normalized())) { + return; + } + + //box faces B + if (!separator.test_axis(p_transform_b.elements[0].normalized())) { + return; + } + + if (!separator.test_axis(p_transform_b.elements[1].normalized())) { + return; + } + + if (withMargin) { + Transform2D invA = p_transform_a.affine_inverse(); + Transform2D invB = p_transform_b.affine_inverse(); + + if (!separator.test_axis(rectangle_A->get_box_axis(p_transform_a, invA, rectangle_B, p_transform_b, invB))) { + return; + } + + if (castA || castB) { + Transform2D aofs = p_transform_a; + aofs.elements[2] += p_motion_a; + + Transform2D bofs = p_transform_b; + bofs.elements[2] += p_motion_b; + + Transform2D aofsinv = aofs.affine_inverse(); + Transform2D bofsinv = bofs.affine_inverse(); + + if (castA) { + if (!separator.test_axis(rectangle_A->get_box_axis(aofs, aofsinv, rectangle_B, p_transform_b, invB))) { + return; + } + } + + if (castB) { + if (!separator.test_axis(rectangle_A->get_box_axis(p_transform_a, invA, rectangle_B, bofs, bofsinv))) { + return; + } + } + + if (castA && castB) { + if (!separator.test_axis(rectangle_A->get_box_axis(aofs, aofsinv, rectangle_B, bofs, bofsinv))) { + return; + } + } + } + } + + separator.generate_contacts(); +} + +template +static void _collision_rectangle_capsule(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const GodotRectangleShape2D *rectangle_A = static_cast(p_a); + const GodotCapsuleShape2D *capsule_B = static_cast(p_b); + + SeparatorAxisTest2D separator(rectangle_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); + + if (!separator.test_previous_axis()) { + return; + } + + if (!separator.test_cast()) { + return; + } + + //box faces + if (!separator.test_axis(p_transform_a.elements[0].normalized())) { + return; + } + + if (!separator.test_axis(p_transform_a.elements[1].normalized())) { + return; + } + + //capsule axis + if (!separator.test_axis(p_transform_b.elements[0].normalized())) { + return; + } + + //box endpoints to capsule circles + + Transform2D boxinv = p_transform_a.affine_inverse(); + + real_t capsule_dir = capsule_B->get_height() * 0.5 - capsule_B->get_radius(); + + for (int i = 0; i < 2; i++) { + { + Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir; + + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) { + return; + } + } + + if (castA) { + Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir; + capsule_endpoint -= p_motion_a; + + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) { + return; + } + } + + if (castB) { + Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir; + capsule_endpoint += p_motion_b; + + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) { + return; + } + } + + if (castA && castB) { + Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir; + capsule_endpoint -= p_motion_a; + capsule_endpoint += p_motion_b; + + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) { + return; + } + } + + capsule_dir *= -1.0; + } + + separator.generate_contacts(); +} + +template +static void _collision_rectangle_convex_polygon(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const GodotRectangleShape2D *rectangle_A = static_cast(p_a); + const GodotConvexPolygonShape2D *convex_B = static_cast(p_b); + + SeparatorAxisTest2D separator(rectangle_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); + + if (!separator.test_previous_axis()) { + return; + } + + if (!separator.test_cast()) { + return; + } + + //box faces + if (!separator.test_axis(p_transform_a.elements[0].normalized())) { + return; + } + + if (!separator.test_axis(p_transform_a.elements[1].normalized())) { + return; + } + + //convex faces + Transform2D boxinv; + if (withMargin) { + boxinv = p_transform_a.affine_inverse(); + } + for (int i = 0; i < convex_B->get_point_count(); i++) { + if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) { + return; + } + + if (withMargin) { + //all points vs all points need to be tested if margin exist + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, p_transform_b.xform(convex_B->get_point(i))))) { + return; + } + if (castA) { + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, p_transform_b.xform(convex_B->get_point(i)) - p_motion_a))) { + return; + } + } + if (castB) { + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, p_transform_b.xform(convex_B->get_point(i)) + p_motion_b))) { + return; + } + } + if (castA && castB) { + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, p_transform_b.xform(convex_B->get_point(i)) + p_motion_b - p_motion_a))) { + return; + } + } + } + } + + separator.generate_contacts(); +} + +///////// + +template +static void _collision_capsule_capsule(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const GodotCapsuleShape2D *capsule_A = static_cast(p_a); + const GodotCapsuleShape2D *capsule_B = static_cast(p_b); + + SeparatorAxisTest2D separator(capsule_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); + + if (!separator.test_previous_axis()) { + return; + } + + if (!separator.test_cast()) { + return; + } + + //capsule axis + + if (!separator.test_axis(p_transform_b.elements[0].normalized())) { + return; + } + + if (!separator.test_axis(p_transform_a.elements[0].normalized())) { + return; + } + + //capsule endpoints + + real_t capsule_dir_A = capsule_A->get_height() * 0.5 - capsule_A->get_radius(); + for (int i = 0; i < 2; i++) { + Vector2 capsule_endpoint_A = p_transform_a.get_origin() + p_transform_a.elements[1] * capsule_dir_A; + + real_t capsule_dir_B = capsule_B->get_height() * 0.5 - capsule_B->get_radius(); + for (int j = 0; j < 2; j++) { + Vector2 capsule_endpoint_B = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir_B; + + if (TEST_POINT(capsule_endpoint_A, capsule_endpoint_B)) { + return; + } + + capsule_dir_B *= -1.0; + } + + capsule_dir_A *= -1.0; + } + + separator.generate_contacts(); +} + +template +static void _collision_capsule_convex_polygon(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const GodotCapsuleShape2D *capsule_A = static_cast(p_a); + const GodotConvexPolygonShape2D *convex_B = static_cast(p_b); + + SeparatorAxisTest2D separator(capsule_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); + + if (!separator.test_previous_axis()) { + return; + } + + if (!separator.test_cast()) { + return; + } + + //capsule axis + + if (!separator.test_axis(p_transform_a.elements[0].normalized())) { + return; + } + + //poly vs capsule + for (int i = 0; i < convex_B->get_point_count(); i++) { + Vector2 cpoint = p_transform_b.xform(convex_B->get_point(i)); + + real_t capsule_dir = capsule_A->get_height() * 0.5 - capsule_A->get_radius(); + for (int j = 0; j < 2; j++) { + Vector2 capsule_endpoint_A = p_transform_a.get_origin() + p_transform_a.elements[1] * capsule_dir; + + if (TEST_POINT(capsule_endpoint_A, cpoint)) { + return; + } + + capsule_dir *= -1.0; + } + + if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) { + return; + } + } + + separator.generate_contacts(); +} + +///////// + +template +static void _collision_convex_polygon_convex_polygon(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const GodotConvexPolygonShape2D *convex_A = static_cast(p_a); + const GodotConvexPolygonShape2D *convex_B = static_cast(p_b); + + SeparatorAxisTest2D separator(convex_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); + + if (!separator.test_previous_axis()) { + return; + } + + if (!separator.test_cast()) { + return; + } + + for (int i = 0; i < convex_A->get_point_count(); i++) { + if (!separator.test_axis(convex_A->get_xformed_segment_normal(p_transform_a, i))) { + return; + } + } + + for (int i = 0; i < convex_B->get_point_count(); i++) { + if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) { + return; + } + } + + if (withMargin) { + for (int i = 0; i < convex_A->get_point_count(); i++) { + for (int j = 0; j < convex_B->get_point_count(); j++) { + if (TEST_POINT(p_transform_a.xform(convex_A->get_point(i)), p_transform_b.xform(convex_B->get_point(j)))) { + return; + } + } + } + } + + separator.generate_contacts(); +} + +//////// + +bool sat_2d_calculate_penetration(const GodotShape2D *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, GodotCollisionSolver2D::CallbackResult p_result_callback, void *p_userdata, bool p_swap, Vector2 *sep_axis, real_t p_margin_A, real_t p_margin_B) { + PhysicsServer2D::ShapeType type_A = p_shape_A->get_type(); + + ERR_FAIL_COND_V(type_A == PhysicsServer2D::SHAPE_WORLD_BOUNDARY, false); + ERR_FAIL_COND_V(type_A == PhysicsServer2D::SHAPE_SEPARATION_RAY, false); + ERR_FAIL_COND_V(p_shape_A->is_concave(), false); + + PhysicsServer2D::ShapeType type_B = p_shape_B->get_type(); + + ERR_FAIL_COND_V(type_B == PhysicsServer2D::SHAPE_WORLD_BOUNDARY, false); + ERR_FAIL_COND_V(type_B == PhysicsServer2D::SHAPE_SEPARATION_RAY, false); + ERR_FAIL_COND_V(p_shape_B->is_concave(), false); + + static const CollisionFunc collision_table[5][5] = { + { _collision_segment_segment, + _collision_segment_circle, + _collision_segment_rectangle, + _collision_segment_capsule, + _collision_segment_convex_polygon }, + { nullptr, + _collision_circle_circle, + _collision_circle_rectangle, + _collision_circle_capsule, + _collision_circle_convex_polygon }, + { nullptr, + nullptr, + _collision_rectangle_rectangle, + _collision_rectangle_capsule, + _collision_rectangle_convex_polygon }, + { nullptr, + nullptr, + nullptr, + _collision_capsule_capsule, + _collision_capsule_convex_polygon }, + { nullptr, + nullptr, + nullptr, + nullptr, + _collision_convex_polygon_convex_polygon } + + }; + + static const CollisionFunc collision_table_castA[5][5] = { + { _collision_segment_segment, + _collision_segment_circle, + _collision_segment_rectangle, + _collision_segment_capsule, + _collision_segment_convex_polygon }, + { nullptr, + _collision_circle_circle, + _collision_circle_rectangle, + _collision_circle_capsule, + _collision_circle_convex_polygon }, + { nullptr, + nullptr, + _collision_rectangle_rectangle, + _collision_rectangle_capsule, + _collision_rectangle_convex_polygon }, + { nullptr, + nullptr, + nullptr, + _collision_capsule_capsule, + _collision_capsule_convex_polygon }, + { nullptr, + nullptr, + nullptr, + nullptr, + _collision_convex_polygon_convex_polygon } + + }; + + static const CollisionFunc collision_table_castB[5][5] = { + { _collision_segment_segment, + _collision_segment_circle, + _collision_segment_rectangle, + _collision_segment_capsule, + _collision_segment_convex_polygon }, + { nullptr, + _collision_circle_circle, + _collision_circle_rectangle, + _collision_circle_capsule, + _collision_circle_convex_polygon }, + { nullptr, + nullptr, + _collision_rectangle_rectangle, + _collision_rectangle_capsule, + _collision_rectangle_convex_polygon }, + { nullptr, + nullptr, + nullptr, + _collision_capsule_capsule, + _collision_capsule_convex_polygon }, + { nullptr, + nullptr, + nullptr, + nullptr, + _collision_convex_polygon_convex_polygon } + + }; + + static const CollisionFunc collision_table_castA_castB[5][5] = { + { _collision_segment_segment, + _collision_segment_circle, + _collision_segment_rectangle, + _collision_segment_capsule, + _collision_segment_convex_polygon }, + { nullptr, + _collision_circle_circle, + _collision_circle_rectangle, + _collision_circle_capsule, + _collision_circle_convex_polygon }, + { nullptr, + nullptr, + _collision_rectangle_rectangle, + _collision_rectangle_capsule, + _collision_rectangle_convex_polygon }, + { nullptr, + nullptr, + nullptr, + _collision_capsule_capsule, + _collision_capsule_convex_polygon }, + { nullptr, + nullptr, + nullptr, + nullptr, + _collision_convex_polygon_convex_polygon } + + }; + + static const CollisionFunc collision_table_margin[5][5] = { + { _collision_segment_segment, + _collision_segment_circle, + _collision_segment_rectangle, + _collision_segment_capsule, + _collision_segment_convex_polygon }, + { nullptr, + _collision_circle_circle, + _collision_circle_rectangle, + _collision_circle_capsule, + _collision_circle_convex_polygon }, + { nullptr, + nullptr, + _collision_rectangle_rectangle, + _collision_rectangle_capsule, + _collision_rectangle_convex_polygon }, + { nullptr, + nullptr, + nullptr, + _collision_capsule_capsule, + _collision_capsule_convex_polygon }, + { nullptr, + nullptr, + nullptr, + nullptr, + _collision_convex_polygon_convex_polygon } + + }; + + static const CollisionFunc collision_table_castA_margin[5][5] = { + { _collision_segment_segment, + _collision_segment_circle, + _collision_segment_rectangle, + _collision_segment_capsule, + _collision_segment_convex_polygon }, + { nullptr, + _collision_circle_circle, + _collision_circle_rectangle, + _collision_circle_capsule, + _collision_circle_convex_polygon }, + { nullptr, + nullptr, + _collision_rectangle_rectangle, + _collision_rectangle_capsule, + _collision_rectangle_convex_polygon }, + { nullptr, + nullptr, + nullptr, + _collision_capsule_capsule, + _collision_capsule_convex_polygon }, + { nullptr, + nullptr, + nullptr, + nullptr, + _collision_convex_polygon_convex_polygon } + + }; + + static const CollisionFunc collision_table_castB_margin[5][5] = { + { _collision_segment_segment, + _collision_segment_circle, + _collision_segment_rectangle, + _collision_segment_capsule, + _collision_segment_convex_polygon }, + { nullptr, + _collision_circle_circle, + _collision_circle_rectangle, + _collision_circle_capsule, + _collision_circle_convex_polygon }, + { nullptr, + nullptr, + _collision_rectangle_rectangle, + _collision_rectangle_capsule, + _collision_rectangle_convex_polygon }, + { nullptr, + nullptr, + nullptr, + _collision_capsule_capsule, + _collision_capsule_convex_polygon }, + { nullptr, + nullptr, + nullptr, + nullptr, + _collision_convex_polygon_convex_polygon } + + }; + + static const CollisionFunc collision_table_castA_castB_margin[5][5] = { + { _collision_segment_segment, + _collision_segment_circle, + _collision_segment_rectangle, + _collision_segment_capsule, + _collision_segment_convex_polygon }, + { nullptr, + _collision_circle_circle, + _collision_circle_rectangle, + _collision_circle_capsule, + _collision_circle_convex_polygon }, + { nullptr, + nullptr, + _collision_rectangle_rectangle, + _collision_rectangle_capsule, + _collision_rectangle_convex_polygon }, + { nullptr, + nullptr, + nullptr, + _collision_capsule_capsule, + _collision_capsule_convex_polygon }, + { nullptr, + nullptr, + nullptr, + nullptr, + _collision_convex_polygon_convex_polygon } + + }; + + _CollectorCallback2D callback; + callback.callback = p_result_callback; + callback.swap = p_swap; + callback.userdata = p_userdata; + callback.collided = false; + callback.sep_axis = sep_axis; + + const GodotShape2D *A = p_shape_A; + const GodotShape2D *B = p_shape_B; + const Transform2D *transform_A = &p_transform_A; + const Transform2D *transform_B = &p_transform_B; + const Vector2 *motion_A = &p_motion_A; + const Vector2 *motion_B = &p_motion_B; + real_t margin_A = p_margin_A, margin_B = p_margin_B; + + if (type_A > type_B) { + SWAP(A, B); + SWAP(transform_A, transform_B); + SWAP(type_A, type_B); + SWAP(motion_A, motion_B); + SWAP(margin_A, margin_B); + callback.swap = !callback.swap; + } + + CollisionFunc collision_func; + + if (p_margin_A || p_margin_B) { + if (*motion_A == Vector2() && *motion_B == Vector2()) { + collision_func = collision_table_margin[type_A - 2][type_B - 2]; + } else if (*motion_A != Vector2() && *motion_B == Vector2()) { + collision_func = collision_table_castA_margin[type_A - 2][type_B - 2]; + } else if (*motion_A == Vector2() && *motion_B != Vector2()) { + collision_func = collision_table_castB_margin[type_A - 2][type_B - 2]; + } else { + collision_func = collision_table_castA_castB_margin[type_A - 2][type_B - 2]; + } + } else { + if (*motion_A == Vector2() && *motion_B == Vector2()) { + collision_func = collision_table[type_A - 2][type_B - 2]; + } else if (*motion_A != Vector2() && *motion_B == Vector2()) { + collision_func = collision_table_castA[type_A - 2][type_B - 2]; + } else if (*motion_A == Vector2() && *motion_B != Vector2()) { + collision_func = collision_table_castB[type_A - 2][type_B - 2]; + } else { + collision_func = collision_table_castA_castB[type_A - 2][type_B - 2]; + } + } + + ERR_FAIL_COND_V(!collision_func, false); + + collision_func(A, *transform_A, B, *transform_B, &callback, *motion_A, *motion_B, margin_A, margin_B); + + return callback.collided; +} diff --git a/servers/physics_2d/godot_collision_solver_2d_sat.h b/servers/physics_2d/godot_collision_solver_2d_sat.h new file mode 100644 index 0000000000..1517b90a19 --- /dev/null +++ b/servers/physics_2d/godot_collision_solver_2d_sat.h @@ -0,0 +1,38 @@ +/*************************************************************************/ +/* godot_collision_solver_2d_sat.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 GODOT_COLLISION_SOLVER_2D_SAT_H +#define GODOT_COLLISION_SOLVER_2D_SAT_H + +#include "godot_collision_solver_2d.h" + +bool sat_2d_calculate_penetration(const GodotShape2D *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, GodotCollisionSolver2D::CallbackResult p_result_callback, void *p_userdata, bool p_swap = false, Vector2 *sep_axis = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0); + +#endif // GODOT_COLLISION_SOLVER_2D_SAT_H diff --git a/servers/physics_2d/godot_constraint_2d.h b/servers/physics_2d/godot_constraint_2d.h new file mode 100644 index 0000000000..84f975e583 --- /dev/null +++ b/servers/physics_2d/godot_constraint_2d.h @@ -0,0 +1,70 @@ +/*************************************************************************/ +/* godot_constraint_2d.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 GODOT_CONSTRAINT_2D_H +#define GODOT_CONSTRAINT_2D_H + +#include "godot_body_2d.h" + +class GodotConstraint2D { + GodotBody2D **_body_ptr; + int _body_count; + uint64_t island_step = 0; + bool disabled_collisions_between_bodies = true; + + RID self; + +protected: + GodotConstraint2D(GodotBody2D **p_body_ptr = nullptr, int p_body_count = 0) { + _body_ptr = p_body_ptr; + _body_count = p_body_count; + } + +public: + _FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; } + _FORCE_INLINE_ RID get_self() const { return self; } + + _FORCE_INLINE_ uint64_t get_island_step() const { return island_step; } + _FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step = p_step; } + + _FORCE_INLINE_ GodotBody2D **get_body_ptr() const { return _body_ptr; } + _FORCE_INLINE_ int get_body_count() const { return _body_count; } + + _FORCE_INLINE_ void disable_collisions_between_bodies(const bool p_disabled) { disabled_collisions_between_bodies = p_disabled; } + _FORCE_INLINE_ bool is_disabled_collisions_between_bodies() const { return disabled_collisions_between_bodies; } + + virtual bool setup(real_t p_step) = 0; + virtual bool pre_solve(real_t p_step) = 0; + virtual void solve(real_t p_step) = 0; + + virtual ~GodotConstraint2D() {} +}; + +#endif // GODOT_CONSTRAINT_2D_H diff --git a/servers/physics_2d/godot_joints_2d.cpp b/servers/physics_2d/godot_joints_2d.cpp new file mode 100644 index 0000000000..7c08c2f4b4 --- /dev/null +++ b/servers/physics_2d/godot_joints_2d.cpp @@ -0,0 +1,486 @@ +/*************************************************************************/ +/* godot_joints_2d.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 "godot_joints_2d.h" + +#include "godot_space_2d.h" + +//based on chipmunk joint constraints + +/* Copyright (c) 2007 Scott Lembcke + * + * 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. + */ + +void GodotJoint2D::copy_settings_from(GodotJoint2D *p_joint) { + set_self(p_joint->get_self()); + set_max_force(p_joint->get_max_force()); + set_bias(p_joint->get_bias()); + set_max_bias(p_joint->get_max_bias()); + disable_collisions_between_bodies(p_joint->is_disabled_collisions_between_bodies()); +} + +static inline real_t k_scalar(GodotBody2D *a, GodotBody2D *b, const Vector2 &rA, const Vector2 &rB, const Vector2 &n) { + real_t value = 0.0; + + { + value += a->get_inv_mass(); + real_t rcn = (rA - a->get_center_of_mass()).cross(n); + value += a->get_inv_inertia() * rcn * rcn; + } + + if (b) { + value += b->get_inv_mass(); + real_t rcn = (rB - b->get_center_of_mass()).cross(n); + value += b->get_inv_inertia() * rcn * rcn; + } + + return value; +} + +static inline Vector2 +relative_velocity(GodotBody2D *a, GodotBody2D *b, Vector2 rA, Vector2 rB) { + Vector2 sum = a->get_linear_velocity() - (rA - a->get_center_of_mass()).orthogonal() * a->get_angular_velocity(); + if (b) { + return (b->get_linear_velocity() - (rB - b->get_center_of_mass()).orthogonal() * b->get_angular_velocity()) - sum; + } else { + return -sum; + } +} + +static inline real_t +normal_relative_velocity(GodotBody2D *a, GodotBody2D *b, Vector2 rA, Vector2 rB, Vector2 n) { + return relative_velocity(a, b, rA, rB).dot(n); +} + +bool GodotPinJoint2D::setup(real_t p_step) { + dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); + dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); + + if (!dynamic_A && !dynamic_B) { + return false; + } + + GodotSpace2D *space = A->get_space(); + ERR_FAIL_COND_V(!space, false); + + rA = A->get_transform().basis_xform(anchor_A); + rB = B ? B->get_transform().basis_xform(anchor_B) : anchor_B; + + real_t B_inv_mass = B ? B->get_inv_mass() : 0.0; + + Transform2D K1; + K1[0].x = A->get_inv_mass() + B_inv_mass; + K1[1].x = 0.0f; + K1[0].y = 0.0f; + K1[1].y = A->get_inv_mass() + B_inv_mass; + + Transform2D K2; + K2[0].x = A->get_inv_inertia() * rA.y * rA.y; + K2[1].x = -A->get_inv_inertia() * rA.x * rA.y; + K2[0].y = -A->get_inv_inertia() * rA.x * rA.y; + K2[1].y = A->get_inv_inertia() * rA.x * rA.x; + + Transform2D K; + K[0] = K1[0] + K2[0]; + K[1] = K1[1] + K2[1]; + + if (B) { + Transform2D K3; + K3[0].x = B->get_inv_inertia() * rB.y * rB.y; + K3[1].x = -B->get_inv_inertia() * rB.x * rB.y; + K3[0].y = -B->get_inv_inertia() * rB.x * rB.y; + K3[1].y = B->get_inv_inertia() * rB.x * rB.x; + + K[0] += K3[0]; + K[1] += K3[1]; + } + + K[0].x += softness; + K[1].y += softness; + + M = K.affine_inverse(); + + Vector2 gA = rA + A->get_transform().get_origin(); + Vector2 gB = B ? rB + B->get_transform().get_origin() : rB; + + Vector2 delta = gB - gA; + + bias = delta * -(get_bias() == 0 ? space->get_constraint_bias() : get_bias()) * (1.0 / p_step); + + return true; +} + +inline Vector2 custom_cross(const Vector2 &p_vec, real_t p_other) { + return Vector2(p_other * p_vec.y, -p_other * p_vec.x); +} + +bool GodotPinJoint2D::pre_solve(real_t p_step) { + // Apply accumulated impulse. + if (dynamic_A) { + A->apply_impulse(-P, rA); + } + if (B && dynamic_B) { + B->apply_impulse(P, rB); + } + + return true; +} + +void GodotPinJoint2D::solve(real_t p_step) { + // compute relative velocity + Vector2 vA = A->get_linear_velocity() - custom_cross(rA - A->get_center_of_mass(), A->get_angular_velocity()); + + Vector2 rel_vel; + if (B) { + rel_vel = B->get_linear_velocity() - custom_cross(rB - B->get_center_of_mass(), B->get_angular_velocity()) - vA; + } else { + rel_vel = -vA; + } + + Vector2 impulse = M.basis_xform(bias - rel_vel - Vector2(softness, softness) * P); + + if (dynamic_A) { + A->apply_impulse(-impulse, rA); + } + if (B && dynamic_B) { + B->apply_impulse(impulse, rB); + } + + P += impulse; +} + +void GodotPinJoint2D::set_param(PhysicsServer2D::PinJointParam p_param, real_t p_value) { + if (p_param == PhysicsServer2D::PIN_JOINT_SOFTNESS) { + softness = p_value; + } +} + +real_t GodotPinJoint2D::get_param(PhysicsServer2D::PinJointParam p_param) const { + if (p_param == PhysicsServer2D::PIN_JOINT_SOFTNESS) { + return softness; + } + ERR_FAIL_V(0); +} + +GodotPinJoint2D::GodotPinJoint2D(const Vector2 &p_pos, GodotBody2D *p_body_a, GodotBody2D *p_body_b) : + GodotJoint2D(_arr, p_body_b ? 2 : 1) { + A = p_body_a; + B = p_body_b; + anchor_A = p_body_a->get_inv_transform().xform(p_pos); + anchor_B = p_body_b ? p_body_b->get_inv_transform().xform(p_pos) : p_pos; + + p_body_a->add_constraint(this, 0); + if (p_body_b) { + p_body_b->add_constraint(this, 1); + } +} + +////////////////////////////////////////////// +////////////////////////////////////////////// +////////////////////////////////////////////// + +static inline void +k_tensor(GodotBody2D *a, GodotBody2D *b, Vector2 r1, Vector2 r2, Vector2 *k1, Vector2 *k2) { + // calculate mass matrix + // If I wasn't lazy and wrote a proper matrix class, this wouldn't be so gross... + real_t k11, k12, k21, k22; + real_t m_sum = a->get_inv_mass() + b->get_inv_mass(); + + // start with I*m_sum + k11 = m_sum; + k12 = 0.0f; + k21 = 0.0f; + k22 = m_sum; + + r1 -= a->get_center_of_mass(); + r2 -= b->get_center_of_mass(); + + // add the influence from r1 + real_t a_i_inv = a->get_inv_inertia(); + real_t r1xsq = r1.x * r1.x * a_i_inv; + real_t r1ysq = r1.y * r1.y * a_i_inv; + real_t r1nxy = -r1.x * r1.y * a_i_inv; + k11 += r1ysq; + k12 += r1nxy; + k21 += r1nxy; + k22 += r1xsq; + + // add the influnce from r2 + real_t b_i_inv = b->get_inv_inertia(); + real_t r2xsq = r2.x * r2.x * b_i_inv; + real_t r2ysq = r2.y * r2.y * b_i_inv; + real_t r2nxy = -r2.x * r2.y * b_i_inv; + k11 += r2ysq; + k12 += r2nxy; + k21 += r2nxy; + k22 += r2xsq; + + // invert + real_t determinant = k11 * k22 - k12 * k21; + ERR_FAIL_COND(determinant == 0.0); + + real_t det_inv = 1.0f / determinant; + *k1 = Vector2(k22 * det_inv, -k12 * det_inv); + *k2 = Vector2(-k21 * det_inv, k11 * det_inv); +} + +static _FORCE_INLINE_ Vector2 +mult_k(const Vector2 &vr, const Vector2 &k1, const Vector2 &k2) { + return Vector2(vr.dot(k1), vr.dot(k2)); +} + +bool GodotGrooveJoint2D::setup(real_t p_step) { + dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); + dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); + + if (!dynamic_A && !dynamic_B) { + return false; + } + + GodotSpace2D *space = A->get_space(); + ERR_FAIL_COND_V(!space, false); + + // calculate endpoints in worldspace + Vector2 ta = A->get_transform().xform(A_groove_1); + Vector2 tb = A->get_transform().xform(A_groove_2); + + // calculate axis + Vector2 n = -(tb - ta).orthogonal().normalized(); + real_t d = ta.dot(n); + + xf_normal = n; + rB = B->get_transform().basis_xform(B_anchor); + + // calculate tangential distance along the axis of rB + real_t td = (B->get_transform().get_origin() + rB).cross(n); + // calculate clamping factor and rB + if (td <= ta.cross(n)) { + clamp = 1.0f; + rA = ta - A->get_transform().get_origin(); + } else if (td >= tb.cross(n)) { + clamp = -1.0f; + rA = tb - A->get_transform().get_origin(); + } else { + clamp = 0.0f; + //joint->r1 = cpvsub(cpvadd(cpvmult(cpvperp(n), -td), cpvmult(n, d)), a->p); + rA = ((-n.orthogonal() * -td) + n * d) - A->get_transform().get_origin(); + } + + // Calculate mass tensor + k_tensor(A, B, rA, rB, &k1, &k2); + + // compute max impulse + jn_max = get_max_force() * p_step; + + // calculate bias velocity + //cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1)); + //joint->bias = cpvclamp(cpvmult(delta, -joint->constraint.biasCoef*dt_inv), joint->constraint.maxBias); + + Vector2 delta = (B->get_transform().get_origin() + rB) - (A->get_transform().get_origin() + rA); + + real_t _b = get_bias(); + gbias = (delta * -(_b == 0 ? space->get_constraint_bias() : _b) * (1.0 / p_step)).limit_length(get_max_bias()); + + correct = true; + return true; +} + +bool GodotGrooveJoint2D::pre_solve(real_t p_step) { + // Apply accumulated impulse. + if (dynamic_A) { + A->apply_impulse(-jn_acc, rA); + } + if (dynamic_B) { + B->apply_impulse(jn_acc, rB); + } + + return true; +} + +void GodotGrooveJoint2D::solve(real_t p_step) { + // compute impulse + Vector2 vr = relative_velocity(A, B, rA, rB); + + Vector2 j = mult_k(gbias - vr, k1, k2); + Vector2 jOld = jn_acc; + j += jOld; + + jn_acc = (((clamp * j.cross(xf_normal)) > 0) ? j : j.project(xf_normal)).limit_length(jn_max); + + j = jn_acc - jOld; + + if (dynamic_A) { + A->apply_impulse(-j, rA); + } + if (dynamic_B) { + B->apply_impulse(j, rB); + } +} + +GodotGrooveJoint2D::GodotGrooveJoint2D(const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, GodotBody2D *p_body_a, GodotBody2D *p_body_b) : + GodotJoint2D(_arr, 2) { + A = p_body_a; + B = p_body_b; + + A_groove_1 = A->get_inv_transform().xform(p_a_groove1); + A_groove_2 = A->get_inv_transform().xform(p_a_groove2); + B_anchor = B->get_inv_transform().xform(p_b_anchor); + A_groove_normal = -(A_groove_2 - A_groove_1).normalized().orthogonal(); + + A->add_constraint(this, 0); + B->add_constraint(this, 1); +} + +////////////////////////////////////////////// +////////////////////////////////////////////// +////////////////////////////////////////////// + +bool GodotDampedSpringJoint2D::setup(real_t p_step) { + dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); + dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); + + if (!dynamic_A && !dynamic_B) { + return false; + } + + rA = A->get_transform().basis_xform(anchor_A); + rB = B->get_transform().basis_xform(anchor_B); + + Vector2 delta = (B->get_transform().get_origin() + rB) - (A->get_transform().get_origin() + rA); + real_t dist = delta.length(); + + if (dist) { + n = delta / dist; + } else { + n = Vector2(); + } + + real_t k = k_scalar(A, B, rA, rB, n); + n_mass = 1.0f / k; + + target_vrn = 0.0f; + v_coef = 1.0f - Math::exp(-damping * (p_step)*k); + + // Calculate spring force. + real_t f_spring = (rest_length - dist) * stiffness; + j = n * f_spring * (p_step); + + return true; +} + +bool GodotDampedSpringJoint2D::pre_solve(real_t p_step) { + // Apply spring force. + if (dynamic_A) { + A->apply_impulse(-j, rA); + } + if (dynamic_B) { + B->apply_impulse(j, rB); + } + + return true; +} + +void GodotDampedSpringJoint2D::solve(real_t p_step) { + // compute relative velocity + real_t vrn = normal_relative_velocity(A, B, rA, rB, n) - target_vrn; + + // compute velocity loss from drag + // not 100% certain this is derived correctly, though it makes sense + real_t v_damp = -vrn * v_coef; + target_vrn = vrn + v_damp; + Vector2 j = n * v_damp * n_mass; + + if (dynamic_A) { + A->apply_impulse(-j, rA); + } + if (dynamic_B) { + B->apply_impulse(j, rB); + } +} + +void GodotDampedSpringJoint2D::set_param(PhysicsServer2D::DampedSpringParam p_param, real_t p_value) { + switch (p_param) { + case PhysicsServer2D::DAMPED_SPRING_REST_LENGTH: { + rest_length = p_value; + } break; + case PhysicsServer2D::DAMPED_SPRING_DAMPING: { + damping = p_value; + } break; + case PhysicsServer2D::DAMPED_SPRING_STIFFNESS: { + stiffness = p_value; + } break; + } +} + +real_t GodotDampedSpringJoint2D::get_param(PhysicsServer2D::DampedSpringParam p_param) const { + switch (p_param) { + case PhysicsServer2D::DAMPED_SPRING_REST_LENGTH: { + return rest_length; + } break; + case PhysicsServer2D::DAMPED_SPRING_DAMPING: { + return damping; + } break; + case PhysicsServer2D::DAMPED_SPRING_STIFFNESS: { + return stiffness; + } break; + } + + ERR_FAIL_V(0); +} + +GodotDampedSpringJoint2D::GodotDampedSpringJoint2D(const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, GodotBody2D *p_body_a, GodotBody2D *p_body_b) : + GodotJoint2D(_arr, 2) { + A = p_body_a; + B = p_body_b; + anchor_A = A->get_inv_transform().xform(p_anchor_a); + anchor_B = B->get_inv_transform().xform(p_anchor_b); + + rest_length = p_anchor_a.distance_to(p_anchor_b); + + A->add_constraint(this, 0); + B->add_constraint(this, 1); +} diff --git a/servers/physics_2d/godot_joints_2d.h b/servers/physics_2d/godot_joints_2d.h new file mode 100644 index 0000000000..4c97190d01 --- /dev/null +++ b/servers/physics_2d/godot_joints_2d.h @@ -0,0 +1,178 @@ +/*************************************************************************/ +/* godot_joints_2d.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 GODOT_JOINTS_2D_H +#define GODOT_JOINTS_2D_H + +#include "godot_body_2d.h" +#include "godot_constraint_2d.h" + +class GodotJoint2D : public GodotConstraint2D { + real_t bias = 0; + real_t max_bias = 3.40282e+38; + real_t max_force = 3.40282e+38; + +protected: + bool dynamic_A = false; + bool dynamic_B = false; + +public: + _FORCE_INLINE_ void set_max_force(real_t p_force) { max_force = p_force; } + _FORCE_INLINE_ real_t get_max_force() const { return max_force; } + + _FORCE_INLINE_ void set_bias(real_t p_bias) { bias = p_bias; } + _FORCE_INLINE_ real_t get_bias() const { return bias; } + + _FORCE_INLINE_ void set_max_bias(real_t p_bias) { max_bias = p_bias; } + _FORCE_INLINE_ real_t get_max_bias() const { return max_bias; } + + virtual bool setup(real_t p_step) override { return false; } + virtual bool pre_solve(real_t p_step) override { return false; } + virtual void solve(real_t p_step) override {} + + void copy_settings_from(GodotJoint2D *p_joint); + + virtual PhysicsServer2D::JointType get_type() const { return PhysicsServer2D::JOINT_TYPE_MAX; } + GodotJoint2D(GodotBody2D **p_body_ptr = nullptr, int p_body_count = 0) : + GodotConstraint2D(p_body_ptr, p_body_count) {} + + virtual ~GodotJoint2D() { + for (int i = 0; i < get_body_count(); i++) { + GodotBody2D *body = get_body_ptr()[i]; + if (body) { + body->remove_constraint(this, i); + } + } + }; +}; + +class GodotPinJoint2D : public GodotJoint2D { + union { + struct { + GodotBody2D *A; + GodotBody2D *B; + }; + + GodotBody2D *_arr[2] = { nullptr, nullptr }; + }; + + Transform2D M; + Vector2 rA, rB; + Vector2 anchor_A; + Vector2 anchor_B; + Vector2 bias; + Vector2 P; + real_t softness = 0.0; + +public: + virtual PhysicsServer2D::JointType get_type() const override { return PhysicsServer2D::JOINT_TYPE_PIN; } + + virtual bool setup(real_t p_step) override; + virtual bool pre_solve(real_t p_step) override; + virtual void solve(real_t p_step) override; + + void set_param(PhysicsServer2D::PinJointParam p_param, real_t p_value); + real_t get_param(PhysicsServer2D::PinJointParam p_param) const; + + GodotPinJoint2D(const Vector2 &p_pos, GodotBody2D *p_body_a, GodotBody2D *p_body_b = nullptr); +}; + +class GodotGrooveJoint2D : public GodotJoint2D { + union { + struct { + GodotBody2D *A; + GodotBody2D *B; + }; + + GodotBody2D *_arr[2] = { nullptr, nullptr }; + }; + + Vector2 A_groove_1; + Vector2 A_groove_2; + Vector2 A_groove_normal; + Vector2 B_anchor; + Vector2 jn_acc; + Vector2 gbias; + real_t jn_max = 0.0; + real_t clamp = 0.0; + Vector2 xf_normal; + Vector2 rA, rB; + Vector2 k1, k2; + + bool correct = false; + +public: + virtual PhysicsServer2D::JointType get_type() const override { return PhysicsServer2D::JOINT_TYPE_GROOVE; } + + virtual bool setup(real_t p_step) override; + virtual bool pre_solve(real_t p_step) override; + virtual void solve(real_t p_step) override; + + GodotGrooveJoint2D(const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, GodotBody2D *p_body_a, GodotBody2D *p_body_b); +}; + +class GodotDampedSpringJoint2D : public GodotJoint2D { + union { + struct { + GodotBody2D *A; + GodotBody2D *B; + }; + + GodotBody2D *_arr[2] = { nullptr, nullptr }; + }; + + Vector2 anchor_A; + Vector2 anchor_B; + + real_t rest_length = 0.0; + real_t damping = 1.5; + real_t stiffness = 20.0; + + Vector2 rA, rB; + Vector2 n; + Vector2 j; + real_t n_mass = 0.0; + real_t target_vrn = 0.0; + real_t v_coef = 0.0; + +public: + virtual PhysicsServer2D::JointType get_type() const override { return PhysicsServer2D::JOINT_TYPE_DAMPED_SPRING; } + + virtual bool setup(real_t p_step) override; + virtual bool pre_solve(real_t p_step) override; + virtual void solve(real_t p_step) override; + + void set_param(PhysicsServer2D::DampedSpringParam p_param, real_t p_value); + real_t get_param(PhysicsServer2D::DampedSpringParam p_param) const; + + GodotDampedSpringJoint2D(const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, GodotBody2D *p_body_a, GodotBody2D *p_body_b); +}; + +#endif // GODOT_JOINTS_2D_H diff --git a/servers/physics_2d/godot_physics_server_2d.cpp b/servers/physics_2d/godot_physics_server_2d.cpp new file mode 100644 index 0000000000..c86f87fc03 --- /dev/null +++ b/servers/physics_2d/godot_physics_server_2d.cpp @@ -0,0 +1,1350 @@ +/*************************************************************************/ +/* godot_physics_server_2d.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 "godot_physics_server_2d.h" + +#include "godot_body_direct_state_2d.h" +#include "godot_broad_phase_2d_bvh.h" +#include "godot_collision_solver_2d.h" + +#include "core/config/project_settings.h" +#include "core/debugger/engine_debugger.h" +#include "core/os/os.h" + +#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 GodotPhysicsServer2D::_shape_create(ShapeType p_shape) { + GodotShape2D *shape = nullptr; + switch (p_shape) { + case SHAPE_WORLD_BOUNDARY: { + shape = memnew(GodotWorldBoundaryShape2D); + } break; + case SHAPE_SEPARATION_RAY: { + shape = memnew(GodotSeparationRayShape2D); + } break; + case SHAPE_SEGMENT: { + shape = memnew(GodotSegmentShape2D); + } break; + case SHAPE_CIRCLE: { + shape = memnew(GodotCircleShape2D); + } break; + case SHAPE_RECTANGLE: { + shape = memnew(GodotRectangleShape2D); + } break; + case SHAPE_CAPSULE: { + shape = memnew(GodotCapsuleShape2D); + } break; + case SHAPE_CONVEX_POLYGON: { + shape = memnew(GodotConvexPolygonShape2D); + } break; + case SHAPE_CONCAVE_POLYGON: { + shape = memnew(GodotConcavePolygonShape2D); + } break; + case SHAPE_CUSTOM: { + ERR_FAIL_V(RID()); + + } break; + } + + RID id = shape_owner.make_rid(shape); + shape->set_self(id); + + return id; +} + +RID GodotPhysicsServer2D::world_boundary_shape_create() { + return _shape_create(SHAPE_WORLD_BOUNDARY); +} + +RID GodotPhysicsServer2D::separation_ray_shape_create() { + return _shape_create(SHAPE_SEPARATION_RAY); +} + +RID GodotPhysicsServer2D::segment_shape_create() { + return _shape_create(SHAPE_SEGMENT); +} + +RID GodotPhysicsServer2D::circle_shape_create() { + return _shape_create(SHAPE_CIRCLE); +} + +RID GodotPhysicsServer2D::rectangle_shape_create() { + return _shape_create(SHAPE_RECTANGLE); +} + +RID GodotPhysicsServer2D::capsule_shape_create() { + return _shape_create(SHAPE_CAPSULE); +} + +RID GodotPhysicsServer2D::convex_polygon_shape_create() { + return _shape_create(SHAPE_CONVEX_POLYGON); +} + +RID GodotPhysicsServer2D::concave_polygon_shape_create() { + return _shape_create(SHAPE_CONCAVE_POLYGON); +} + +void GodotPhysicsServer2D::shape_set_data(RID p_shape, const Variant &p_data) { + GodotShape2D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_COND(!shape); + shape->set_data(p_data); +}; + +void GodotPhysicsServer2D::shape_set_custom_solver_bias(RID p_shape, real_t p_bias) { + GodotShape2D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_COND(!shape); + shape->set_custom_bias(p_bias); +} + +PhysicsServer2D::ShapeType GodotPhysicsServer2D::shape_get_type(RID p_shape) const { + const GodotShape2D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_COND_V(!shape, SHAPE_CUSTOM); + return shape->get_type(); +}; + +Variant GodotPhysicsServer2D::shape_get_data(RID p_shape) const { + const GodotShape2D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_COND_V(!shape, Variant()); + ERR_FAIL_COND_V(!shape->is_configured(), Variant()); + return shape->get_data(); +}; + +real_t GodotPhysicsServer2D::shape_get_custom_solver_bias(RID p_shape) const { + const GodotShape2D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_COND_V(!shape, 0); + return shape->get_custom_bias(); +} + +void GodotPhysicsServer2D::_shape_col_cbk(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_userdata) { + CollCbkData *cbk = (CollCbkData *)p_userdata; + + if (cbk->max == 0) { + return; + } + + Vector2 rel_dir = (p_point_A - p_point_B); + real_t rel_length2 = rel_dir.length_squared(); + if (cbk->valid_dir != Vector2()) { + if (cbk->valid_depth < 10e20) { + if (rel_length2 > cbk->valid_depth * cbk->valid_depth || + (rel_length2 > CMP_EPSILON && cbk->valid_dir.dot(rel_dir.normalized()) < CMP_EPSILON)) { + cbk->invalid_by_dir++; + return; + } + } else { + if (rel_length2 > 0 && cbk->valid_dir.dot(rel_dir.normalized()) < CMP_EPSILON) { + return; + } + } + } + + if (cbk->amount == cbk->max) { + //find least deep + real_t min_depth = 1e20; + int min_depth_idx = 0; + for (int i = 0; i < cbk->amount; i++) { + real_t d = cbk->ptr[i * 2 + 0].distance_squared_to(cbk->ptr[i * 2 + 1]); + if (d < min_depth) { + min_depth = d; + min_depth_idx = i; + } + } + + if (rel_length2 < min_depth) { + return; + } + cbk->ptr[min_depth_idx * 2 + 0] = p_point_A; + cbk->ptr[min_depth_idx * 2 + 1] = p_point_B; + cbk->passed++; + + } else { + cbk->ptr[cbk->amount * 2 + 0] = p_point_A; + cbk->ptr[cbk->amount * 2 + 1] = p_point_B; + cbk->amount++; + cbk->passed++; + } +} + +bool GodotPhysicsServer2D::shape_collide(RID p_shape_A, const Transform2D &p_xform_A, const Vector2 &p_motion_A, RID p_shape_B, const Transform2D &p_xform_B, const Vector2 &p_motion_B, Vector2 *r_results, int p_result_max, int &r_result_count) { + GodotShape2D *shape_A = shape_owner.get_or_null(p_shape_A); + ERR_FAIL_COND_V(!shape_A, false); + GodotShape2D *shape_B = shape_owner.get_or_null(p_shape_B); + ERR_FAIL_COND_V(!shape_B, false); + + if (p_result_max == 0) { + return GodotCollisionSolver2D::solve(shape_A, p_xform_A, p_motion_A, shape_B, p_xform_B, p_motion_B, nullptr, nullptr); + } + + CollCbkData cbk; + cbk.max = p_result_max; + cbk.amount = 0; + cbk.passed = 0; + cbk.ptr = r_results; + + bool res = GodotCollisionSolver2D::solve(shape_A, p_xform_A, p_motion_A, shape_B, p_xform_B, p_motion_B, _shape_col_cbk, &cbk); + r_result_count = cbk.amount; + return res; +} + +RID GodotPhysicsServer2D::space_create() { + GodotSpace2D *space = memnew(GodotSpace2D); + RID id = space_owner.make_rid(space); + space->set_self(id); + RID area_id = area_create(); + GodotArea2D *area = area_owner.get_or_null(area_id); + ERR_FAIL_COND_V(!area, RID()); + space->set_default_area(area); + area->set_space(space); + area->set_priority(-1); + + return id; +}; + +void GodotPhysicsServer2D::space_set_active(RID p_space, bool p_active) { + GodotSpace2D *space = space_owner.get_or_null(p_space); + ERR_FAIL_COND(!space); + if (p_active) { + active_spaces.insert(space); + } else { + active_spaces.erase(space); + } +} + +bool GodotPhysicsServer2D::space_is_active(RID p_space) const { + const GodotSpace2D *space = space_owner.get_or_null(p_space); + ERR_FAIL_COND_V(!space, false); + + return active_spaces.has(space); +} + +void GodotPhysicsServer2D::space_set_param(RID p_space, SpaceParameter p_param, real_t p_value) { + GodotSpace2D *space = space_owner.get_or_null(p_space); + ERR_FAIL_COND(!space); + + space->set_param(p_param, p_value); +} + +real_t GodotPhysicsServer2D::space_get_param(RID p_space, SpaceParameter p_param) const { + const GodotSpace2D *space = space_owner.get_or_null(p_space); + ERR_FAIL_COND_V(!space, 0); + return space->get_param(p_param); +} + +void GodotPhysicsServer2D::space_set_debug_contacts(RID p_space, int p_max_contacts) { + GodotSpace2D *space = space_owner.get_or_null(p_space); + ERR_FAIL_COND(!space); + space->set_debug_contacts(p_max_contacts); +} + +Vector GodotPhysicsServer2D::space_get_contacts(RID p_space) const { + GodotSpace2D *space = space_owner.get_or_null(p_space); + ERR_FAIL_COND_V(!space, Vector()); + return space->get_debug_contacts(); +} + +int GodotPhysicsServer2D::space_get_contact_count(RID p_space) const { + GodotSpace2D *space = space_owner.get_or_null(p_space); + ERR_FAIL_COND_V(!space, 0); + return space->get_debug_contact_count(); +} + +PhysicsDirectSpaceState2D *GodotPhysicsServer2D::space_get_direct_state(RID p_space) { + GodotSpace2D *space = space_owner.get_or_null(p_space); + ERR_FAIL_COND_V(!space, nullptr); + 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(); +} + +RID GodotPhysicsServer2D::area_create() { + GodotArea2D *area = memnew(GodotArea2D); + RID rid = area_owner.make_rid(area); + area->set_self(rid); + return rid; +}; + +void GodotPhysicsServer2D::area_set_space(RID p_area, RID p_space) { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + + GodotSpace2D *space = nullptr; + if (p_space.is_valid()) { + space = space_owner.get_or_null(p_space); + ERR_FAIL_COND(!space); + } + + if (area->get_space() == space) { + return; //pointless + } + + area->clear_constraints(); + area->set_space(space); +}; + +RID GodotPhysicsServer2D::area_get_space(RID p_area) const { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND_V(!area, RID()); + + GodotSpace2D *space = area->get_space(); + if (!space) { + return RID(); + } + return space->get_self(); +}; + +void GodotPhysicsServer2D::area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode) { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + + area->set_space_override_mode(p_mode); +} + +PhysicsServer2D::AreaSpaceOverrideMode GodotPhysicsServer2D::area_get_space_override_mode(RID p_area) const { + const GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND_V(!area, AREA_SPACE_OVERRIDE_DISABLED); + + return area->get_space_override_mode(); +} + +void GodotPhysicsServer2D::area_add_shape(RID p_area, RID p_shape, const Transform2D &p_transform, bool p_disabled) { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + + GodotShape2D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_COND(!shape); + + area->add_shape(shape, p_transform, p_disabled); +} + +void GodotPhysicsServer2D::area_set_shape(RID p_area, int p_shape_idx, RID p_shape) { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + + GodotShape2D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_COND(!shape); + ERR_FAIL_COND(!shape->is_configured()); + + area->set_shape(p_shape_idx, shape); +} + +void GodotPhysicsServer2D::area_set_shape_transform(RID p_area, int p_shape_idx, const Transform2D &p_transform) { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + + area->set_shape_transform(p_shape_idx, p_transform); +} + +void GodotPhysicsServer2D::area_set_shape_disabled(RID p_area, int p_shape, bool p_disabled) { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + ERR_FAIL_INDEX(p_shape, area->get_shape_count()); + FLUSH_QUERY_CHECK(area); + + area->set_shape_disabled(p_shape, p_disabled); +} + +int GodotPhysicsServer2D::area_get_shape_count(RID p_area) const { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND_V(!area, -1); + + return area->get_shape_count(); +} + +RID GodotPhysicsServer2D::area_get_shape(RID p_area, int p_shape_idx) const { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND_V(!area, RID()); + + GodotShape2D *shape = area->get_shape(p_shape_idx); + ERR_FAIL_COND_V(!shape, RID()); + + return shape->get_self(); +} + +Transform2D GodotPhysicsServer2D::area_get_shape_transform(RID p_area, int p_shape_idx) const { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND_V(!area, Transform2D()); + + return area->get_shape_transform(p_shape_idx); +} + +void GodotPhysicsServer2D::area_remove_shape(RID p_area, int p_shape_idx) { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + + area->remove_shape(p_shape_idx); +} + +void GodotPhysicsServer2D::area_clear_shapes(RID p_area) { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + + while (area->get_shape_count()) { + area->remove_shape(0); + } +} + +void GodotPhysicsServer2D::area_attach_object_instance_id(RID p_area, ObjectID p_id) { + if (space_owner.owns(p_area)) { + GodotSpace2D *space = space_owner.get_or_null(p_area); + p_area = space->get_default_area()->get_self(); + } + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + area->set_instance_id(p_id); +} + +ObjectID GodotPhysicsServer2D::area_get_object_instance_id(RID p_area) const { + if (space_owner.owns(p_area)) { + GodotSpace2D *space = space_owner.get_or_null(p_area); + p_area = space->get_default_area()->get_self(); + } + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND_V(!area, ObjectID()); + return area->get_instance_id(); +} + +void GodotPhysicsServer2D::area_attach_canvas_instance_id(RID p_area, ObjectID p_id) { + if (space_owner.owns(p_area)) { + GodotSpace2D *space = space_owner.get_or_null(p_area); + p_area = space->get_default_area()->get_self(); + } + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + area->set_canvas_instance_id(p_id); +} + +ObjectID GodotPhysicsServer2D::area_get_canvas_instance_id(RID p_area) const { + if (space_owner.owns(p_area)) { + GodotSpace2D *space = space_owner.get_or_null(p_area); + p_area = space->get_default_area()->get_self(); + } + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND_V(!area, ObjectID()); + return area->get_canvas_instance_id(); +} + +void GodotPhysicsServer2D::area_set_param(RID p_area, AreaParameter p_param, const Variant &p_value) { + if (space_owner.owns(p_area)) { + GodotSpace2D *space = space_owner.get_or_null(p_area); + p_area = space->get_default_area()->get_self(); + } + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + area->set_param(p_param, p_value); +}; + +void GodotPhysicsServer2D::area_set_transform(RID p_area, const Transform2D &p_transform) { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + area->set_transform(p_transform); +}; + +Variant GodotPhysicsServer2D::area_get_param(RID p_area, AreaParameter p_param) const { + if (space_owner.owns(p_area)) { + GodotSpace2D *space = space_owner.get_or_null(p_area); + p_area = space->get_default_area()->get_self(); + } + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND_V(!area, Variant()); + + return area->get_param(p_param); +}; + +Transform2D GodotPhysicsServer2D::area_get_transform(RID p_area) const { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND_V(!area, Transform2D()); + + return area->get_transform(); +}; + +void GodotPhysicsServer2D::area_set_pickable(RID p_area, bool p_pickable) { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + area->set_pickable(p_pickable); +} + +void GodotPhysicsServer2D::area_set_monitorable(RID p_area, bool p_monitorable) { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + FLUSH_QUERY_CHECK(area); + + area->set_monitorable(p_monitorable); +} + +void GodotPhysicsServer2D::area_set_collision_mask(RID p_area, uint32_t p_mask) { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + + area->set_collision_mask(p_mask); +} + +void GodotPhysicsServer2D::area_set_collision_layer(RID p_area, uint32_t p_layer) { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + + area->set_collision_layer(p_layer); +} + +void GodotPhysicsServer2D::area_set_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + + area->set_monitor_callback(p_receiver ? p_receiver->get_instance_id() : ObjectID(), p_method); +} + +void GodotPhysicsServer2D::area_set_area_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + + area->set_area_monitor_callback(p_receiver ? p_receiver->get_instance_id() : ObjectID(), p_method); +} + +/* BODY API */ + +RID GodotPhysicsServer2D::body_create() { + GodotBody2D *body = memnew(GodotBody2D); + RID rid = body_owner.make_rid(body); + body->set_self(rid); + return rid; +} + +void GodotPhysicsServer2D::body_set_space(RID p_body, RID p_space) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + GodotSpace2D *space = nullptr; + if (p_space.is_valid()) { + space = space_owner.get_or_null(p_space); + ERR_FAIL_COND(!space); + } + + if (body->get_space() == space) { + return; //pointless + } + + body->clear_constraint_list(); + body->set_space(space); +}; + +RID GodotPhysicsServer2D::body_get_space(RID p_body) const { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, RID()); + + GodotSpace2D *space = body->get_space(); + if (!space) { + return RID(); + } + return space->get_self(); +}; + +void GodotPhysicsServer2D::body_set_mode(RID p_body, BodyMode p_mode) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + FLUSH_QUERY_CHECK(body); + + body->set_mode(p_mode); +}; + +PhysicsServer2D::BodyMode GodotPhysicsServer2D::body_get_mode(RID p_body) const { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, BODY_MODE_STATIC); + + return body->get_mode(); +}; + +void GodotPhysicsServer2D::body_add_shape(RID p_body, RID p_shape, const Transform2D &p_transform, bool p_disabled) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + GodotShape2D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_COND(!shape); + + body->add_shape(shape, p_transform, p_disabled); +} + +void GodotPhysicsServer2D::body_set_shape(RID p_body, int p_shape_idx, RID p_shape) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + GodotShape2D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_COND(!shape); + ERR_FAIL_COND(!shape->is_configured()); + + body->set_shape(p_shape_idx, shape); +} + +void GodotPhysicsServer2D::body_set_shape_transform(RID p_body, int p_shape_idx, const Transform2D &p_transform) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + body->set_shape_transform(p_shape_idx, p_transform); +} + +int GodotPhysicsServer2D::body_get_shape_count(RID p_body) const { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, -1); + + return body->get_shape_count(); +} + +RID GodotPhysicsServer2D::body_get_shape(RID p_body, int p_shape_idx) const { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, RID()); + + GodotShape2D *shape = body->get_shape(p_shape_idx); + ERR_FAIL_COND_V(!shape, RID()); + + return shape->get_self(); +} + +Transform2D GodotPhysicsServer2D::body_get_shape_transform(RID p_body, int p_shape_idx) const { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, Transform2D()); + + return body->get_shape_transform(p_shape_idx); +} + +void GodotPhysicsServer2D::body_remove_shape(RID p_body, int p_shape_idx) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + body->remove_shape(p_shape_idx); +} + +void GodotPhysicsServer2D::body_clear_shapes(RID p_body) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + while (body->get_shape_count()) { + body->remove_shape(0); + } +} + +void GodotPhysicsServer2D::body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + ERR_FAIL_INDEX(p_shape_idx, body->get_shape_count()); + FLUSH_QUERY_CHECK(body); + + body->set_shape_disabled(p_shape_idx, p_disabled); +} + +void GodotPhysicsServer2D::body_set_shape_as_one_way_collision(RID p_body, int p_shape_idx, bool p_enable, real_t p_margin) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + ERR_FAIL_INDEX(p_shape_idx, body->get_shape_count()); + FLUSH_QUERY_CHECK(body); + + body->set_shape_as_one_way_collision(p_shape_idx, p_enable, p_margin); +} + +void GodotPhysicsServer2D::body_set_continuous_collision_detection_mode(RID p_body, CCDMode p_mode) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + body->set_continuous_collision_detection_mode(p_mode); +} + +GodotPhysicsServer2D::CCDMode GodotPhysicsServer2D::body_get_continuous_collision_detection_mode(RID p_body) const { + const GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, CCD_MODE_DISABLED); + + return body->get_continuous_collision_detection_mode(); +} + +void GodotPhysicsServer2D::body_attach_object_instance_id(RID p_body, ObjectID p_id) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + body->set_instance_id(p_id); +}; + +ObjectID GodotPhysicsServer2D::body_get_object_instance_id(RID p_body) const { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, ObjectID()); + + return body->get_instance_id(); +}; + +void GodotPhysicsServer2D::body_attach_canvas_instance_id(RID p_body, ObjectID p_id) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + body->set_canvas_instance_id(p_id); +}; + +ObjectID GodotPhysicsServer2D::body_get_canvas_instance_id(RID p_body) const { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, ObjectID()); + + return body->get_canvas_instance_id(); +}; + +void GodotPhysicsServer2D::body_set_collision_layer(RID p_body, uint32_t p_layer) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + body->set_collision_layer(p_layer); +}; + +uint32_t GodotPhysicsServer2D::body_get_collision_layer(RID p_body) const { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, 0); + + return body->get_collision_layer(); +}; + +void GodotPhysicsServer2D::body_set_collision_mask(RID p_body, uint32_t p_mask) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + body->set_collision_mask(p_mask); +}; + +uint32_t GodotPhysicsServer2D::body_get_collision_mask(RID p_body) const { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, 0); + + return body->get_collision_mask(); +}; + +void GodotPhysicsServer2D::body_set_param(RID p_body, BodyParameter p_param, const Variant &p_value) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + body->set_param(p_param, p_value); +}; + +Variant GodotPhysicsServer2D::body_get_param(RID p_body, BodyParameter p_param) const { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, 0); + + return body->get_param(p_param); +}; + +void GodotPhysicsServer2D::body_reset_mass_properties(RID p_body) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + return body->reset_mass_properties(); +} + +void GodotPhysicsServer2D::body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + body->set_state(p_state, p_variant); +}; + +Variant GodotPhysicsServer2D::body_get_state(RID p_body, BodyState p_state) const { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, Variant()); + + return body->get_state(p_state); +}; + +void GodotPhysicsServer2D::body_set_applied_force(RID p_body, const Vector2 &p_force) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + body->set_applied_force(p_force); + body->wakeup(); +}; + +Vector2 GodotPhysicsServer2D::body_get_applied_force(RID p_body) const { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, Vector2()); + return body->get_applied_force(); +}; + +void GodotPhysicsServer2D::body_set_applied_torque(RID p_body, real_t p_torque) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + body->set_applied_torque(p_torque); + body->wakeup(); +}; + +real_t GodotPhysicsServer2D::body_get_applied_torque(RID p_body) const { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, 0); + + return body->get_applied_torque(); +}; + +void GodotPhysicsServer2D::body_apply_central_impulse(RID p_body, const Vector2 &p_impulse) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + body->apply_central_impulse(p_impulse); + body->wakeup(); +} + +void GodotPhysicsServer2D::body_apply_torque_impulse(RID p_body, real_t p_torque) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + _update_shapes(); + + body->apply_torque_impulse(p_torque); + body->wakeup(); +} + +void GodotPhysicsServer2D::body_apply_impulse(RID p_body, const Vector2 &p_impulse, const Vector2 &p_position) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + _update_shapes(); + + body->apply_impulse(p_impulse, p_position); + body->wakeup(); +}; + +void GodotPhysicsServer2D::body_add_central_force(RID p_body, const Vector2 &p_force) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + body->add_central_force(p_force); + body->wakeup(); +}; + +void GodotPhysicsServer2D::body_add_force(RID p_body, const Vector2 &p_force, const Vector2 &p_position) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + body->add_force(p_force, p_position); + body->wakeup(); +}; + +void GodotPhysicsServer2D::body_add_torque(RID p_body, real_t p_torque) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + body->add_torque(p_torque); + body->wakeup(); +}; + +void GodotPhysicsServer2D::body_set_axis_velocity(RID p_body, const Vector2 &p_axis_velocity) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + _update_shapes(); + + Vector2 v = body->get_linear_velocity(); + Vector2 axis = p_axis_velocity.normalized(); + v -= axis * axis.dot(v); + v += p_axis_velocity; + body->set_linear_velocity(v); + body->wakeup(); +}; + +void GodotPhysicsServer2D::body_add_collision_exception(RID p_body, RID p_body_b) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + body->add_exception(p_body_b); + body->wakeup(); +}; + +void GodotPhysicsServer2D::body_remove_collision_exception(RID p_body, RID p_body_b) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + body->remove_exception(p_body_b); + body->wakeup(); +}; + +void GodotPhysicsServer2D::body_get_collision_exceptions(RID p_body, List *p_exceptions) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + for (int i = 0; i < body->get_exceptions().size(); i++) { + p_exceptions->push_back(body->get_exceptions()[i]); + } +}; + +void GodotPhysicsServer2D::body_set_contacts_reported_depth_threshold(RID p_body, real_t p_threshold) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); +}; + +real_t GodotPhysicsServer2D::body_get_contacts_reported_depth_threshold(RID p_body) const { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, 0); + return 0; +}; + +void GodotPhysicsServer2D::body_set_omit_force_integration(RID p_body, bool p_omit) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + body->set_omit_force_integration(p_omit); +}; + +bool GodotPhysicsServer2D::body_is_omitting_force_integration(RID p_body) const { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, false); + return body->get_omit_force_integration(); +}; + +void GodotPhysicsServer2D::body_set_max_contacts_reported(RID p_body, int p_contacts) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + body->set_max_contacts_reported(p_contacts); +} + +int GodotPhysicsServer2D::body_get_max_contacts_reported(RID p_body) const { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, -1); + return body->get_max_contacts_reported(); +} + +void GodotPhysicsServer2D::body_set_state_sync_callback(RID p_body, void *p_instance, BodyStateCallback p_callback) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + body->set_state_sync_callback(p_instance, p_callback); +} + +void GodotPhysicsServer2D::body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + body->set_force_integration_callback(p_callable, p_udata); +} + +bool GodotPhysicsServer2D::body_collide_shape(RID p_body, int p_body_shape, RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, Vector2 *r_results, int p_result_max, int &r_result_count) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, false); + ERR_FAIL_INDEX_V(p_body_shape, body->get_shape_count(), false); + + return shape_collide(body->get_shape(p_body_shape)->get_self(), body->get_transform() * body->get_shape_transform(p_body_shape), Vector2(), p_shape, p_shape_xform, p_motion, r_results, p_result_max, r_result_count); +} + +void GodotPhysicsServer2D::body_set_pickable(RID p_body, bool p_pickable) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + body->set_pickable(p_pickable); +} + +bool GodotPhysicsServer2D::body_test_motion(RID p_body, const MotionParameters &p_parameters, MotionResult *r_result) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, false); + ERR_FAIL_COND_V(!body->get_space(), false); + ERR_FAIL_COND_V(body->get_space()->is_locked(), false); + + _update_shapes(); + + return body->get_space()->test_body_motion(body, p_parameters, r_result); +} + +PhysicsDirectBodyState2D *GodotPhysicsServer2D::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."); + + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, nullptr); + + ERR_FAIL_COND_V(!body->get_space(), nullptr); + ERR_FAIL_COND_V_MSG(body->get_space()->is_locked(), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification."); + + return body->get_direct_state(); +} + +/* JOINT API */ + +RID GodotPhysicsServer2D::joint_create() { + GodotJoint2D *joint = memnew(GodotJoint2D); + RID joint_rid = joint_owner.make_rid(joint); + joint->set_self(joint_rid); + return joint_rid; +} + +void GodotPhysicsServer2D::joint_clear(RID p_joint) { + GodotJoint2D *joint = joint_owner.get_or_null(p_joint); + if (joint->get_type() != JOINT_TYPE_MAX) { + GodotJoint2D *empty_joint = memnew(GodotJoint2D); + empty_joint->copy_settings_from(joint); + + joint_owner.replace(p_joint, empty_joint); + memdelete(joint); + } +} + +void GodotPhysicsServer2D::joint_set_param(RID p_joint, JointParam p_param, real_t p_value) { + GodotJoint2D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND(!joint); + + switch (p_param) { + case JOINT_PARAM_BIAS: + joint->set_bias(p_value); + break; + case JOINT_PARAM_MAX_BIAS: + joint->set_max_bias(p_value); + break; + case JOINT_PARAM_MAX_FORCE: + joint->set_max_force(p_value); + break; + } +} + +real_t GodotPhysicsServer2D::joint_get_param(RID p_joint, JointParam p_param) const { + const GodotJoint2D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND_V(!joint, -1); + + switch (p_param) { + case JOINT_PARAM_BIAS: + return joint->get_bias(); + break; + case JOINT_PARAM_MAX_BIAS: + return joint->get_max_bias(); + break; + case JOINT_PARAM_MAX_FORCE: + return joint->get_max_force(); + break; + } + + return 0; +} + +void GodotPhysicsServer2D::joint_disable_collisions_between_bodies(RID p_joint, const bool p_disable) { + GodotJoint2D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND(!joint); + + joint->disable_collisions_between_bodies(p_disable); + + if (2 == joint->get_body_count()) { + GodotBody2D *body_a = *joint->get_body_ptr(); + GodotBody2D *body_b = *(joint->get_body_ptr() + 1); + + if (p_disable) { + body_add_collision_exception(body_a->get_self(), body_b->get_self()); + body_add_collision_exception(body_b->get_self(), body_a->get_self()); + } else { + body_remove_collision_exception(body_a->get_self(), body_b->get_self()); + body_remove_collision_exception(body_b->get_self(), body_a->get_self()); + } + } +} + +bool GodotPhysicsServer2D::joint_is_disabled_collisions_between_bodies(RID p_joint) const { + const GodotJoint2D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND_V(!joint, true); + + return joint->is_disabled_collisions_between_bodies(); +} + +void GodotPhysicsServer2D::joint_make_pin(RID p_joint, const Vector2 &p_pos, RID p_body_a, RID p_body_b) { + GodotBody2D *A = body_owner.get_or_null(p_body_a); + ERR_FAIL_COND(!A); + GodotBody2D *B = nullptr; + if (body_owner.owns(p_body_b)) { + B = body_owner.get_or_null(p_body_b); + ERR_FAIL_COND(!B); + } + + GodotJoint2D *prev_joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND(prev_joint == nullptr); + + GodotJoint2D *joint = memnew(GodotPinJoint2D(p_pos, A, B)); + + joint_owner.replace(p_joint, joint); + joint->copy_settings_from(prev_joint); + memdelete(prev_joint); +} + +void GodotPhysicsServer2D::joint_make_groove(RID p_joint, const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, RID p_body_a, RID p_body_b) { + GodotBody2D *A = body_owner.get_or_null(p_body_a); + ERR_FAIL_COND(!A); + + GodotBody2D *B = body_owner.get_or_null(p_body_b); + ERR_FAIL_COND(!B); + + GodotJoint2D *prev_joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND(prev_joint == nullptr); + + GodotJoint2D *joint = memnew(GodotGrooveJoint2D(p_a_groove1, p_a_groove2, p_b_anchor, A, B)); + + joint_owner.replace(p_joint, joint); + joint->copy_settings_from(prev_joint); + memdelete(prev_joint); +} + +void GodotPhysicsServer2D::joint_make_damped_spring(RID p_joint, const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, RID p_body_a, RID p_body_b) { + GodotBody2D *A = body_owner.get_or_null(p_body_a); + ERR_FAIL_COND(!A); + + GodotBody2D *B = body_owner.get_or_null(p_body_b); + ERR_FAIL_COND(!B); + + GodotJoint2D *prev_joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND(prev_joint == nullptr); + + GodotJoint2D *joint = memnew(GodotDampedSpringJoint2D(p_anchor_a, p_anchor_b, A, B)); + + joint_owner.replace(p_joint, joint); + joint->copy_settings_from(prev_joint); + memdelete(prev_joint); +} + +void GodotPhysicsServer2D::pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value) { + GodotJoint2D *j = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND(!j); + ERR_FAIL_COND(j->get_type() != JOINT_TYPE_PIN); + + GodotPinJoint2D *pin_joint = static_cast(j); + pin_joint->set_param(p_param, p_value); +} + +real_t GodotPhysicsServer2D::pin_joint_get_param(RID p_joint, PinJointParam p_param) const { + GodotJoint2D *j = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND_V(!j, 0); + ERR_FAIL_COND_V(j->get_type() != JOINT_TYPE_PIN, 0); + + GodotPinJoint2D *pin_joint = static_cast(j); + return pin_joint->get_param(p_param); +} + +void GodotPhysicsServer2D::damped_spring_joint_set_param(RID p_joint, DampedSpringParam p_param, real_t p_value) { + GodotJoint2D *j = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND(!j); + ERR_FAIL_COND(j->get_type() != JOINT_TYPE_DAMPED_SPRING); + + GodotDampedSpringJoint2D *dsj = static_cast(j); + dsj->set_param(p_param, p_value); +} + +real_t GodotPhysicsServer2D::damped_spring_joint_get_param(RID p_joint, DampedSpringParam p_param) const { + GodotJoint2D *j = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND_V(!j, 0); + ERR_FAIL_COND_V(j->get_type() != JOINT_TYPE_DAMPED_SPRING, 0); + + GodotDampedSpringJoint2D *dsj = static_cast(j); + return dsj->get_param(p_param); +} + +PhysicsServer2D::JointType GodotPhysicsServer2D::joint_get_type(RID p_joint) const { + GodotJoint2D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND_V(!joint, JOINT_TYPE_PIN); + + return joint->get_type(); +} + +void GodotPhysicsServer2D::free(RID p_rid) { + _update_shapes(); // just in case + + if (shape_owner.owns(p_rid)) { + GodotShape2D *shape = shape_owner.get_or_null(p_rid); + + while (shape->get_owners().size()) { + GodotShapeOwner2D *so = shape->get_owners().front()->key(); + so->remove_shape(shape); + } + + shape_owner.free(p_rid); + memdelete(shape); + } else if (body_owner.owns(p_rid)) { + GodotBody2D *body = body_owner.get_or_null(p_rid); + + /* + if (body->get_state_query()) + _clear_query(body->get_state_query()); + + if (body->get_direct_state_query()) + _clear_query(body->get_direct_state_query()); + */ + + body_set_space(p_rid, RID()); + + while (body->get_shape_count()) { + body->remove_shape(0); + } + + body_owner.free(p_rid); + memdelete(body); + + } else if (area_owner.owns(p_rid)) { + GodotArea2D *area = area_owner.get_or_null(p_rid); + + /* + if (area->get_monitor_query()) + _clear_query(area->get_monitor_query()); + */ + + area->set_space(nullptr); + + while (area->get_shape_count()) { + area->remove_shape(0); + } + + area_owner.free(p_rid); + memdelete(area); + } else if (space_owner.owns(p_rid)) { + GodotSpace2D *space = space_owner.get_or_null(p_rid); + + while (space->get_objects().size()) { + GodotCollisionObject2D *co = (GodotCollisionObject2D *)space->get_objects().front()->get(); + co->set_space(nullptr); + } + + active_spaces.erase(space); + free(space->get_default_area()->get_self()); + space_owner.free(p_rid); + memdelete(space); + } else if (joint_owner.owns(p_rid)) { + GodotJoint2D *joint = joint_owner.get_or_null(p_rid); + + joint_owner.free(p_rid); + memdelete(joint); + + } else { + ERR_FAIL_MSG("Invalid ID."); + } +}; + +void GodotPhysicsServer2D::set_active(bool p_active) { + active = p_active; +}; + +void GodotPhysicsServer2D::set_collision_iterations(int p_iterations) { + iterations = p_iterations; +}; + +void GodotPhysicsServer2D::init() { + doing_sync = false; + iterations = 8; // 8? + stepper = memnew(GodotStep2D); +}; + +void GodotPhysicsServer2D::step(real_t p_step) { + if (!active) { + return; + } + + _update_shapes(); + + island_count = 0; + active_objects = 0; + collision_pairs = 0; + for (Set::Element *E = active_spaces.front(); E; E = E->next()) { + stepper->step((GodotSpace2D *)E->get(), p_step, iterations); + island_count += E->get()->get_island_count(); + active_objects += E->get()->get_active_objects(); + collision_pairs += E->get()->get_collision_pairs(); + } +}; + +void GodotPhysicsServer2D::sync() { + doing_sync = true; +}; + +void GodotPhysicsServer2D::flush_queries() { + if (!active) { + return; + } + + flushing_queries = true; + + uint64_t time_beg = OS::get_singleton()->get_ticks_usec(); + + for (Set::Element *E = active_spaces.front(); E; E = E->next()) { + GodotSpace2D *space = (GodotSpace2D *)E->get(); + space->call_queries(); + } + + flushing_queries = false; + + if (EngineDebugger::is_profiling("servers")) { + uint64_t total_time[GodotSpace2D::ELAPSED_TIME_MAX]; + static const char *time_name[GodotSpace2D::ELAPSED_TIME_MAX] = { + "integrate_forces", + "generate_islands", + "setup_constraints", + "solve_constraints", + "integrate_velocities" + }; + + for (int i = 0; i < GodotSpace2D::ELAPSED_TIME_MAX; i++) { + total_time[i] = 0; + } + + for (Set::Element *E = active_spaces.front(); E; E = E->next()) { + for (int i = 0; i < GodotSpace2D::ELAPSED_TIME_MAX; i++) { + total_time[i] += E->get()->get_elapsed_time(GodotSpace2D::ElapsedTime(i)); + } + } + + Array values; + values.resize(GodotSpace2D::ELAPSED_TIME_MAX * 2); + for (int i = 0; i < GodotSpace2D::ELAPSED_TIME_MAX; i++) { + values[i * 2 + 0] = time_name[i]; + values[i * 2 + 1] = USEC_TO_SEC(total_time[i]); + } + values.push_back("flush_queries"); + values.push_back(USEC_TO_SEC(OS::get_singleton()->get_ticks_usec() - time_beg)); + + values.push_front("physics_2d"); + EngineDebugger::profiler_add_frame_data("servers", values); + } +} + +void GodotPhysicsServer2D::end_sync() { + doing_sync = false; +} + +void GodotPhysicsServer2D::finish() { + memdelete(stepper); +}; + +void GodotPhysicsServer2D::_update_shapes() { + while (pending_shape_update_list.first()) { + pending_shape_update_list.first()->self()->_shape_changed(); + pending_shape_update_list.remove(pending_shape_update_list.first()); + } +} + +int GodotPhysicsServer2D::get_process_info(ProcessInfo p_info) { + switch (p_info) { + case INFO_ACTIVE_OBJECTS: { + return active_objects; + } break; + case INFO_COLLISION_PAIRS: { + return collision_pairs; + } break; + case INFO_ISLAND_COUNT: { + return island_count; + } break; + } + + return 0; +} + +GodotPhysicsServer2D *GodotPhysicsServer2D::godot_singleton = nullptr; + +GodotPhysicsServer2D::GodotPhysicsServer2D(bool p_using_threads) { + godot_singleton = this; + GodotBroadPhase2D::create_func = GodotBroadPhase2DBVH::_create; + + using_threads = p_using_threads; +}; diff --git a/servers/physics_2d/godot_physics_server_2d.h b/servers/physics_2d/godot_physics_server_2d.h new file mode 100644 index 0000000000..a8a1e71d13 --- /dev/null +++ b/servers/physics_2d/godot_physics_server_2d.h @@ -0,0 +1,299 @@ +/*************************************************************************/ +/* godot_physics_server_2d.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 GODOT_PHYSICS_SERVER_2D_H +#define GODOT_PHYSICS_SERVER_2D_H + +#include "godot_joints_2d.h" +#include "godot_shape_2d.h" +#include "godot_space_2d.h" +#include "godot_step_2d.h" + +#include "core/templates/rid_owner.h" +#include "servers/physics_server_2d.h" + +class GodotPhysicsServer2D : public PhysicsServer2D { + GDCLASS(GodotPhysicsServer2D, PhysicsServer2D); + + friend class GodotPhysicsDirectSpaceState2D; + friend class GodotPhysicsDirectBodyState2D; + bool active = true; + int iterations = 0; + bool doing_sync = false; + + int island_count = 0; + int active_objects = 0; + int collision_pairs = 0; + + bool using_threads = false; + + bool flushing_queries = false; + + GodotStep2D *stepper = nullptr; + Set active_spaces; + + mutable RID_PtrOwner shape_owner; + mutable RID_PtrOwner space_owner; + mutable RID_PtrOwner area_owner; + mutable RID_PtrOwner body_owner; + mutable RID_PtrOwner joint_owner; + + static GodotPhysicsServer2D *godot_singleton; + + friend class GodotCollisionObject2D; + SelfList::List pending_shape_update_list; + void _update_shapes(); + + RID _shape_create(ShapeType p_shape); + +public: + struct CollCbkData { + Vector2 valid_dir; + real_t valid_depth = 0.0; + int max = 0; + int amount = 0; + int passed = 0; + int invalid_by_dir = 0; + Vector2 *ptr = nullptr; + }; + + virtual RID world_boundary_shape_create() override; + virtual RID separation_ray_shape_create() override; + virtual RID segment_shape_create() override; + virtual RID circle_shape_create() override; + virtual RID rectangle_shape_create() override; + virtual RID capsule_shape_create() override; + virtual RID convex_polygon_shape_create() override; + virtual RID concave_polygon_shape_create() override; + + static void _shape_col_cbk(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_userdata); + + 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 override; + virtual Variant shape_get_data(RID p_shape) const override; + virtual real_t shape_get_custom_solver_bias(RID p_shape) const override; + + virtual bool shape_collide(RID p_shape_A, const Transform2D &p_xform_A, const Vector2 &p_motion_A, RID p_shape_B, const Transform2D &p_xform_B, const Vector2 &p_motion_B, Vector2 *r_results, int p_result_max, int &r_result_count) override; + + /* SPACE API */ + + 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) override; + virtual real_t space_get_param(RID p_space, SpaceParameter p_param) const override; + + virtual void space_set_debug_contacts(RID p_space, int p_max_contacts) override; + virtual Vector space_get_contacts(RID p_space) const override; + virtual int space_get_contact_count(RID p_space) const override; + + // this function only works on physics process, errors and returns null otherwise + virtual PhysicsDirectSpaceState2D *space_get_direct_state(RID p_space) override; + + /* AREA API */ + + virtual RID area_create() override; + + 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) override; + virtual RID area_get_space(RID p_area) const override; + + virtual void area_add_shape(RID p_area, RID p_shape, const Transform2D &p_transform = Transform2D(), 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 Transform2D &p_transform) override; + + 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 Transform2D area_get_shape_transform(RID p_area, int p_shape_idx) const override; + + virtual void area_set_shape_disabled(RID p_area, int p_shape, bool p_disabled) override; + + 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_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_attach_canvas_instance_id(RID p_area, ObjectID p_id) override; + virtual ObjectID area_get_canvas_instance_id(RID p_area) const override; + + 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 Transform2D &p_transform) override; + + virtual Variant area_get_param(RID p_area, AreaParameter p_param) const override; + virtual Transform2D area_get_transform(RID p_area) const override; + virtual void area_set_monitorable(RID p_area, bool p_monitorable) override; + 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_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; + + virtual void area_set_pickable(RID p_area, bool p_pickable) override; + + /* BODY API */ + + // create a body of a given type + virtual RID body_create() override; + + 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) override; + virtual BodyMode body_get_mode(RID p_body) const override; + + virtual void body_add_shape(RID p_body, RID p_shape, const Transform2D &p_transform = Transform2D(), 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 Transform2D &p_transform) override; + + 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 Transform2D body_get_shape_transform(RID p_body, int p_shape_idx) const override; + + 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_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled) override; + virtual void body_set_shape_as_one_way_collision(RID p_body, int p_shape_idx, bool p_enable, real_t p_margin) override; + + 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_attach_canvas_instance_id(RID p_body, ObjectID p_id) override; + virtual ObjectID body_get_canvas_instance_id(RID p_body) const override; + + virtual void body_set_continuous_collision_detection_mode(RID p_body, CCDMode p_mode) override; + virtual CCDMode body_get_continuous_collision_detection_mode(RID p_body) const override; + + 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) override; + virtual uint32_t body_get_collision_mask(RID p_body) const override; + + virtual void body_set_param(RID p_body, BodyParameter p_param, const Variant &p_value) override; + virtual Variant body_get_param(RID p_body, BodyParameter p_param) const override; + + virtual void body_reset_mass_properties(RID p_body) override; + + virtual void body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) override; + virtual Variant body_get_state(RID p_body, BodyState p_state) const override; + + virtual void body_set_applied_force(RID p_body, const Vector2 &p_force) override; + virtual Vector2 body_get_applied_force(RID p_body) const override; + + virtual void body_set_applied_torque(RID p_body, real_t p_torque) override; + virtual real_t body_get_applied_torque(RID p_body) const override; + + virtual void body_add_central_force(RID p_body, const Vector2 &p_force) override; + virtual void body_add_force(RID p_body, const Vector2 &p_force, const Vector2 &p_position = Vector2()) override; + virtual void body_add_torque(RID p_body, real_t p_torque) override; + + virtual void body_apply_central_impulse(RID p_body, const Vector2 &p_impulse) override; + virtual void body_apply_torque_impulse(RID p_body, real_t p_torque) override; + virtual void body_apply_impulse(RID p_body, const Vector2 &p_impulse, const Vector2 &p_position = Vector2()) override; + virtual void body_set_axis_velocity(RID p_body, const Vector2 &p_axis_velocity) override; + + 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 *p_exceptions) override; + + 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) 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) override; + virtual int body_get_max_contacts_reported(RID p_body) const override; + + virtual void body_set_state_sync_callback(RID p_body, void *p_instance, BodyStateCallback p_callback) override; + virtual void body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata = Variant()) override; + + virtual bool body_collide_shape(RID p_body, int p_body_shape, RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, Vector2 *r_results, int p_result_max, int &r_result_count) override; + + virtual void body_set_pickable(RID p_body, bool p_pickable) override; + + virtual bool body_test_motion(RID p_body, const MotionParameters &p_parameters, MotionResult *r_result = nullptr) override; + + // this function only works on physics process, errors and returns null otherwise + virtual PhysicsDirectBodyState2D *body_get_direct_state(RID p_body) override; + + /* JOINT API */ + + virtual RID joint_create() override; + + virtual void joint_clear(RID p_joint) override; + + virtual void joint_set_param(RID p_joint, JointParam p_param, real_t p_value) override; + virtual real_t joint_get_param(RID p_joint, JointParam p_param) const override; + + virtual void joint_disable_collisions_between_bodies(RID p_joint, const bool p_disabled) override; + virtual bool joint_is_disabled_collisions_between_bodies(RID p_joint) const override; + + virtual void joint_make_pin(RID p_joint, const Vector2 &p_anchor, RID p_body_a, RID p_body_b = RID()) override; + virtual void joint_make_groove(RID p_joint, const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, RID p_body_a, RID p_body_b) override; + virtual void joint_make_damped_spring(RID p_joint, const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, RID p_body_a, RID p_body_b = RID()) override; + + 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 damped_spring_joint_set_param(RID p_joint, DampedSpringParam p_param, real_t p_value) override; + virtual real_t damped_spring_joint_get_param(RID p_joint, DampedSpringParam p_param) const override; + + virtual JointType joint_get_type(RID p_joint) const override; + + /* MISC */ + + virtual void free(RID p_rid) override; + + 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 void set_collision_iterations(int p_iterations) override; + + virtual bool is_flushing_queries() const override { return flushing_queries; } + + int get_process_info(ProcessInfo p_info) override; + + GodotPhysicsServer2D(bool p_using_threads = false); + ~GodotPhysicsServer2D() {} +}; + +#endif // GODOT_PHYSICS_SERVER_2D_H diff --git a/servers/physics_2d/godot_shape_2d.cpp b/servers/physics_2d/godot_shape_2d.cpp new file mode 100644 index 0000000000..3604004324 --- /dev/null +++ b/servers/physics_2d/godot_shape_2d.cpp @@ -0,0 +1,995 @@ +/*************************************************************************/ +/* godot_shape_2d.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 "godot_shape_2d.h" + +#include "core/math/geometry_2d.h" +#include "core/templates/sort_array.h" + +void GodotShape2D::configure(const Rect2 &p_aabb) { + aabb = p_aabb; + configured = true; + for (const KeyValue &E : owners) { + GodotShapeOwner2D *co = (GodotShapeOwner2D *)E.key; + co->_shape_changed(); + } +} + +Vector2 GodotShape2D::get_support(const Vector2 &p_normal) const { + Vector2 res[2]; + int amnt; + get_supports(p_normal, res, amnt); + return res[0]; +} + +void GodotShape2D::add_owner(GodotShapeOwner2D *p_owner) { + Map::Element *E = owners.find(p_owner); + if (E) { + E->get()++; + } else { + owners[p_owner] = 1; + } +} + +void GodotShape2D::remove_owner(GodotShapeOwner2D *p_owner) { + Map::Element *E = owners.find(p_owner); + ERR_FAIL_COND(!E); + E->get()--; + if (E->get() == 0) { + owners.erase(E); + } +} + +bool GodotShape2D::is_owner(GodotShapeOwner2D *p_owner) const { + return owners.has(p_owner); +} + +const Map &GodotShape2D::get_owners() const { + return owners; +} + +GodotShape2D::~GodotShape2D() { + ERR_FAIL_COND(owners.size()); +} + +/*********************************************************/ +/*********************************************************/ +/*********************************************************/ + +void GodotWorldBoundaryShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { + r_amount = 0; +} + +bool GodotWorldBoundaryShape2D::contains_point(const Vector2 &p_point) const { + return normal.dot(p_point) < d; +} + +bool GodotWorldBoundaryShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { + Vector2 segment = p_begin - p_end; + real_t den = normal.dot(segment); + + //printf("den is %i\n",den); + if (Math::abs(den) <= CMP_EPSILON) { + return false; + } + + real_t dist = (normal.dot(p_begin) - d) / den; + //printf("dist is %i\n",dist); + + if (dist < -CMP_EPSILON || dist > (1.0 + CMP_EPSILON)) { + return false; + } + + r_point = p_begin + segment * -dist; + r_normal = normal; + + return true; +} + +real_t GodotWorldBoundaryShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { + return 0; +} + +void GodotWorldBoundaryShape2D::set_data(const Variant &p_data) { + ERR_FAIL_COND(p_data.get_type() != Variant::ARRAY); + Array arr = p_data; + ERR_FAIL_COND(arr.size() != 2); + normal = arr[0]; + d = arr[1]; + configure(Rect2(Vector2(-1e4, -1e4), Vector2(1e4 * 2, 1e4 * 2))); +} + +Variant GodotWorldBoundaryShape2D::get_data() const { + Array arr; + arr.resize(2); + arr[0] = normal; + arr[1] = d; + return arr; +} + +/*********************************************************/ +/*********************************************************/ +/*********************************************************/ + +void GodotSeparationRayShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { + r_amount = 1; + + if (p_normal.y > 0) { + *r_supports = Vector2(0, length); + } else { + *r_supports = Vector2(); + } +} + +bool GodotSeparationRayShape2D::contains_point(const Vector2 &p_point) const { + return false; +} + +bool GodotSeparationRayShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { + return false; //rays can't be intersected +} + +real_t GodotSeparationRayShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { + return 0; //rays are mass-less +} + +void GodotSeparationRayShape2D::set_data(const Variant &p_data) { + Dictionary d = p_data; + length = d["length"]; + slide_on_slope = d["slide_on_slope"]; + configure(Rect2(0, 0, 0.001, length)); +} + +Variant GodotSeparationRayShape2D::get_data() const { + Dictionary d; + d["length"] = length; + d["slide_on_slope"] = slide_on_slope; + return d; +} + +/*********************************************************/ +/*********************************************************/ +/*********************************************************/ + +void GodotSegmentShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { + if (Math::abs(p_normal.dot(n)) > _SEGMENT_IS_VALID_SUPPORT_THRESHOLD) { + r_supports[0] = a; + r_supports[1] = b; + r_amount = 2; + return; + } + + real_t dp = p_normal.dot(b - a); + if (dp > 0) { + *r_supports = b; + } else { + *r_supports = a; + } + r_amount = 1; +} + +bool GodotSegmentShape2D::contains_point(const Vector2 &p_point) const { + return false; +} + +bool GodotSegmentShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { + if (!Geometry2D::segment_intersects_segment(p_begin, p_end, a, b, &r_point)) { + return false; + } + + if (n.dot(p_begin) > n.dot(a)) { + r_normal = n; + } else { + r_normal = -n; + } + + return true; +} + +real_t GodotSegmentShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { + return p_mass * ((a * p_scale).distance_squared_to(b * p_scale)) / 12; +} + +void GodotSegmentShape2D::set_data(const Variant &p_data) { + ERR_FAIL_COND(p_data.get_type() != Variant::RECT2); + + Rect2 r = p_data; + a = r.position; + b = r.size; + n = (b - a).orthogonal(); + + Rect2 aabb; + aabb.position = a; + aabb.expand_to(b); + if (aabb.size.x == 0) { + aabb.size.x = 0.001; + } + if (aabb.size.y == 0) { + aabb.size.y = 0.001; + } + configure(aabb); +} + +Variant GodotSegmentShape2D::get_data() const { + Rect2 r; + r.position = a; + r.size = b; + return r; +} + +/*********************************************************/ +/*********************************************************/ +/*********************************************************/ + +void GodotCircleShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { + r_amount = 1; + *r_supports = p_normal * radius; +} + +bool GodotCircleShape2D::contains_point(const Vector2 &p_point) const { + return p_point.length_squared() < radius * radius; +} + +bool GodotCircleShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { + Vector2 line_vec = p_end - p_begin; + + real_t a, b, c; + + a = line_vec.dot(line_vec); + b = 2 * p_begin.dot(line_vec); + c = p_begin.dot(p_begin) - radius * radius; + + real_t sqrtterm = b * b - 4 * a * c; + + if (sqrtterm < 0) { + return false; + } + sqrtterm = Math::sqrt(sqrtterm); + real_t res = (-b - sqrtterm) / (2 * a); + + if (res < 0 || res > 1 + CMP_EPSILON) { + return false; + } + + r_point = p_begin + line_vec * res; + r_normal = r_point.normalized(); + return true; +} + +real_t GodotCircleShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { + real_t a = radius * p_scale.x; + real_t b = radius * p_scale.y; + return p_mass * (a * a + b * b) / 4; +} + +void GodotCircleShape2D::set_data(const Variant &p_data) { + ERR_FAIL_COND(!p_data.is_num()); + radius = p_data; + configure(Rect2(-radius, -radius, radius * 2, radius * 2)); +} + +Variant GodotCircleShape2D::get_data() const { + return radius; +} + +/*********************************************************/ +/*********************************************************/ +/*********************************************************/ + +void GodotRectangleShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { + for (int i = 0; i < 2; i++) { + Vector2 ag; + ag[i] = 1.0; + real_t dp = ag.dot(p_normal); + if (Math::abs(dp) < _SEGMENT_IS_VALID_SUPPORT_THRESHOLD) { + continue; + } + + real_t sgn = dp > 0 ? 1.0 : -1.0; + + r_amount = 2; + + r_supports[0][i] = half_extents[i] * sgn; + r_supports[0][i ^ 1] = half_extents[i ^ 1]; + + r_supports[1][i] = half_extents[i] * sgn; + r_supports[1][i ^ 1] = -half_extents[i ^ 1]; + + return; + } + + /* USE POINT */ + + r_amount = 1; + r_supports[0] = Vector2( + (p_normal.x < 0) ? -half_extents.x : half_extents.x, + (p_normal.y < 0) ? -half_extents.y : half_extents.y); +} + +bool GodotRectangleShape2D::contains_point(const Vector2 &p_point) const { + real_t x = p_point.x; + real_t y = p_point.y; + real_t edge_x = half_extents.x; + real_t edge_y = half_extents.y; + return (x >= -edge_x) && (x < edge_x) && (y >= -edge_y) && (y < edge_y); +} + +bool GodotRectangleShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { + return get_aabb().intersects_segment(p_begin, p_end, &r_point, &r_normal); +} + +real_t GodotRectangleShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { + Vector2 he2 = half_extents * 2 * p_scale; + return p_mass * he2.dot(he2) / 12.0; +} + +void GodotRectangleShape2D::set_data(const Variant &p_data) { + ERR_FAIL_COND(p_data.get_type() != Variant::VECTOR2); + + half_extents = p_data; + configure(Rect2(-half_extents, half_extents * 2.0)); +} + +Variant GodotRectangleShape2D::get_data() const { + return half_extents; +} + +/*********************************************************/ +/*********************************************************/ +/*********************************************************/ + +void GodotCapsuleShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { + Vector2 n = p_normal; + + real_t d = n.y; + + if (Math::abs(d) < (1.0 - _SEGMENT_IS_VALID_SUPPORT_THRESHOLD)) { + // make it flat + n.y = 0.0; + n.normalize(); + n *= radius; + + r_amount = 2; + r_supports[0] = n; + r_supports[0].y += height * 0.5 - radius; + r_supports[1] = n; + r_supports[1].y -= height * 0.5 - radius; + + } else { + real_t h = height * 0.5 - radius; + + n *= radius; + n.y += (d > 0) ? h : -h; + r_amount = 1; + *r_supports = n; + } +} + +bool GodotCapsuleShape2D::contains_point(const Vector2 &p_point) const { + Vector2 p = p_point; + p.y = Math::abs(p.y); + p.y -= height * 0.5 - radius; + if (p.y < 0) { + p.y = 0; + } + + return p.length_squared() < radius * radius; +} + +bool GodotCapsuleShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { + real_t d = 1e10; + Vector2 n = (p_end - p_begin).normalized(); + bool collided = false; + + //try spheres + for (int i = 0; i < 2; i++) { + Vector2 begin = p_begin; + Vector2 end = p_end; + real_t ofs = (i == 0) ? -height * 0.5 + radius : height * 0.5 - radius; + begin.y += ofs; + end.y += ofs; + + Vector2 line_vec = end - begin; + + real_t a, b, c; + + a = line_vec.dot(line_vec); + b = 2 * begin.dot(line_vec); + c = begin.dot(begin) - radius * radius; + + real_t sqrtterm = b * b - 4 * a * c; + + if (sqrtterm < 0) { + continue; + } + + sqrtterm = Math::sqrt(sqrtterm); + real_t res = (-b - sqrtterm) / (2 * a); + + if (res < 0 || res > 1 + CMP_EPSILON) { + continue; + } + + Vector2 point = begin + line_vec * res; + Vector2 pointf(point.x, point.y - ofs); + real_t pd = n.dot(pointf); + if (pd < d) { + r_point = pointf; + r_normal = point.normalized(); + d = pd; + collided = true; + } + } + + Vector2 rpos, rnorm; + if (Rect2(Point2(-radius, -height * 0.5 + radius), Size2(radius * 2.0, height - radius * 2)).intersects_segment(p_begin, p_end, &rpos, &rnorm)) { + real_t pd = n.dot(rpos); + if (pd < d) { + r_point = rpos; + r_normal = rnorm; + d = pd; + collided = true; + } + } + + //return get_aabb().intersects_segment(p_begin,p_end,&r_point,&r_normal); + return collided; //todo +} + +real_t GodotCapsuleShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { + Vector2 he2 = Vector2(radius * 2, height) * p_scale; + return p_mass * he2.dot(he2) / 12.0; +} + +void GodotCapsuleShape2D::set_data(const Variant &p_data) { + ERR_FAIL_COND(p_data.get_type() != Variant::ARRAY && p_data.get_type() != Variant::VECTOR2); + + if (p_data.get_type() == Variant::ARRAY) { + Array arr = p_data; + ERR_FAIL_COND(arr.size() != 2); + height = arr[0]; + radius = arr[1]; + } else { + Point2 p = p_data; + radius = p.x; + height = p.y; + } + + Point2 he(radius, height * 0.5); + configure(Rect2(-he, he * 2)); +} + +Variant GodotCapsuleShape2D::get_data() const { + return Point2(height, radius); +} + +/*********************************************************/ +/*********************************************************/ +/*********************************************************/ + +void GodotConvexPolygonShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { + int support_idx = -1; + real_t d = -1e10; + r_amount = 0; + + for (int i = 0; i < point_count; i++) { + //test point + real_t ld = p_normal.dot(points[i].pos); + if (ld > d) { + support_idx = i; + d = ld; + } + + //test segment + if (points[i].normal.dot(p_normal) > _SEGMENT_IS_VALID_SUPPORT_THRESHOLD) { + r_amount = 2; + r_supports[0] = points[i].pos; + r_supports[1] = points[(i + 1) % point_count].pos; + return; + } + } + + ERR_FAIL_COND_MSG(support_idx == -1, "Convex polygon shape support not found."); + + r_amount = 1; + r_supports[0] = points[support_idx].pos; +} + +bool GodotConvexPolygonShape2D::contains_point(const Vector2 &p_point) const { + bool out = false; + bool in = false; + + for (int i = 0; i < point_count; i++) { + real_t d = points[i].normal.dot(p_point) - points[i].normal.dot(points[i].pos); + if (d > 0) { + out = true; + } else { + in = true; + } + } + + return in != out; +} + +bool GodotConvexPolygonShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { + Vector2 n = (p_end - p_begin).normalized(); + real_t d = 1e10; + bool inters = false; + + for (int i = 0; i < point_count; i++) { + //hmm.. no can do.. + /* + if (d.dot(points[i].normal)>=0) + continue; + */ + + Vector2 res; + + if (!Geometry2D::segment_intersects_segment(p_begin, p_end, points[i].pos, points[(i + 1) % point_count].pos, &res)) { + continue; + } + + real_t nd = n.dot(res); + if (nd < d) { + d = nd; + r_point = res; + r_normal = points[i].normal; + inters = true; + } + } + + return inters; +} + +real_t GodotConvexPolygonShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { + ERR_FAIL_COND_V_MSG(point_count == 0, 0, "Convex polygon shape has no points."); + Rect2 aabb; + aabb.position = points[0].pos * p_scale; + for (int i = 0; i < point_count; i++) { + aabb.expand_to(points[i].pos * p_scale); + } + + return p_mass * aabb.size.dot(aabb.size) / 12.0; +} + +void GodotConvexPolygonShape2D::set_data(const Variant &p_data) { +#ifdef REAL_T_IS_DOUBLE + ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT64_ARRAY); +#else + ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT32_ARRAY); +#endif + + if (points) { + memdelete_arr(points); + } + points = nullptr; + point_count = 0; + + if (p_data.get_type() == Variant::PACKED_VECTOR2_ARRAY) { + Vector arr = p_data; + ERR_FAIL_COND(arr.size() == 0); + point_count = arr.size(); + points = memnew_arr(Point, point_count); + const Vector2 *r = arr.ptr(); + + for (int i = 0; i < point_count; i++) { + points[i].pos = r[i]; + } + + for (int i = 0; i < point_count; i++) { + Vector2 p = points[i].pos; + Vector2 pn = points[(i + 1) % point_count].pos; + points[i].normal = (pn - p).orthogonal().normalized(); + } + } else { + Vector dvr = p_data; + point_count = dvr.size() / 4; + ERR_FAIL_COND(point_count == 0); + + points = memnew_arr(Point, point_count); + const real_t *r = dvr.ptr(); + + for (int i = 0; i < point_count; i++) { + int idx = i << 2; + points[i].pos.x = r[idx + 0]; + points[i].pos.y = r[idx + 1]; + points[i].normal.x = r[idx + 2]; + points[i].normal.y = r[idx + 3]; + } + } + + ERR_FAIL_COND(point_count == 0); + Rect2 aabb; + aabb.position = points[0].pos; + for (int i = 1; i < point_count; i++) { + aabb.expand_to(points[i].pos); + } + + configure(aabb); +} + +Variant GodotConvexPolygonShape2D::get_data() const { + Vector dvr; + + dvr.resize(point_count); + + for (int i = 0; i < point_count; i++) { + dvr.set(i, points[i].pos); + } + + return dvr; +} + +GodotConvexPolygonShape2D::~GodotConvexPolygonShape2D() { + if (points) { + memdelete_arr(points); + } +} + +////////////////////////////////////////////////// + +void GodotConcavePolygonShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { + real_t d = -1e10; + int idx = -1; + for (int i = 0; i < points.size(); i++) { + real_t ld = p_normal.dot(points[i]); + if (ld > d) { + d = ld; + idx = i; + } + } + + r_amount = 1; + ERR_FAIL_COND(idx == -1); + *r_supports = points[idx]; +} + +bool GodotConcavePolygonShape2D::contains_point(const Vector2 &p_point) const { + return false; //sorry +} + +bool GodotConcavePolygonShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { + if (segments.size() == 0 || points.size() == 0) { + return false; + } + + uint32_t *stack = (uint32_t *)alloca(sizeof(int) * bvh_depth); + + enum { + TEST_AABB_BIT = 0, + VISIT_LEFT_BIT = 1, + VISIT_RIGHT_BIT = 2, + VISIT_DONE_BIT = 3, + VISITED_BIT_SHIFT = 29, + NODE_IDX_MASK = (1 << VISITED_BIT_SHIFT) - 1, + VISITED_BIT_MASK = ~NODE_IDX_MASK, + + }; + + Vector2 n = (p_end - p_begin).normalized(); + real_t d = 1e10; + bool inters = false; + + /* + for(int i=0;i> VISITED_BIT_SHIFT) { + case TEST_AABB_BIT: { + bool valid = bvh.aabb.intersects_segment(p_begin, p_end); + if (!valid) { + stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; + + } else { + if (bvh.left < 0) { + const Segment &s = segmentptr[bvh.right]; + Vector2 a = pointptr[s.points[0]]; + Vector2 b = pointptr[s.points[1]]; + + Vector2 res; + + if (Geometry2D::segment_intersects_segment(p_begin, p_end, a, b, &res)) { + real_t nd = n.dot(res); + if (nd < d) { + d = nd; + r_point = res; + r_normal = (b - a).orthogonal().normalized(); + inters = true; + } + } + + stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; + + } else { + stack[level] = (VISIT_LEFT_BIT << VISITED_BIT_SHIFT) | node; + } + } + } + continue; + case VISIT_LEFT_BIT: { + stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node; + stack[level + 1] = bvh.left | TEST_AABB_BIT; + level++; + } + continue; + case VISIT_RIGHT_BIT: { + stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; + stack[level + 1] = bvh.right | TEST_AABB_BIT; + level++; + } + continue; + case VISIT_DONE_BIT: { + if (level == 0) { + done = true; + break; + } else { + level--; + } + } + continue; + } + + if (done) { + break; + } + } + + if (inters) { + if (n.dot(r_normal) > 0) { + r_normal = -r_normal; + } + } + + return inters; +} + +int GodotConcavePolygonShape2D::_generate_bvh(BVH *p_bvh, int p_len, int p_depth) { + if (p_len == 1) { + bvh_depth = MAX(p_depth, bvh_depth); + bvh.push_back(*p_bvh); + return bvh.size() - 1; + } + + //else sort best + + Rect2 global_aabb = p_bvh[0].aabb; + for (int i = 1; i < p_len; i++) { + global_aabb = global_aabb.merge(p_bvh[i].aabb); + } + + if (global_aabb.size.x > global_aabb.size.y) { + SortArray sort; + sort.sort(p_bvh, p_len); + + } else { + SortArray sort; + sort.sort(p_bvh, p_len); + } + + int median = p_len / 2; + + BVH node; + node.aabb = global_aabb; + int node_idx = bvh.size(); + bvh.push_back(node); + + int l = _generate_bvh(p_bvh, median, p_depth + 1); + int r = _generate_bvh(&p_bvh[median], p_len - median, p_depth + 1); + bvh.write[node_idx].left = l; + bvh.write[node_idx].right = r; + + return node_idx; +} + +void GodotConcavePolygonShape2D::set_data(const Variant &p_data) { +#ifdef REAL_T_IS_DOUBLE + ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT64_ARRAY); +#else + ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT32_ARRAY); +#endif + + Rect2 aabb; + + if (p_data.get_type() == Variant::PACKED_VECTOR2_ARRAY) { + Vector p2arr = p_data; + int len = p2arr.size(); + ERR_FAIL_COND(len % 2); + + segments.clear(); + points.clear(); + bvh.clear(); + bvh_depth = 1; + + if (len == 0) { + configure(aabb); + return; + } + + const Vector2 *arr = p2arr.ptr(); + + Map pointmap; + for (int i = 0; i < len; i += 2) { + Point2 p1 = arr[i]; + Point2 p2 = arr[i + 1]; + int idx_p1, idx_p2; + + if (pointmap.has(p1)) { + idx_p1 = pointmap[p1]; + } else { + idx_p1 = pointmap.size(); + pointmap[p1] = idx_p1; + } + + if (pointmap.has(p2)) { + idx_p2 = pointmap[p2]; + } else { + idx_p2 = pointmap.size(); + pointmap[p2] = idx_p2; + } + + Segment s; + s.points[0] = idx_p1; + s.points[1] = idx_p2; + segments.push_back(s); + } + + points.resize(pointmap.size()); + aabb.position = pointmap.front()->key(); + for (const KeyValue &E : pointmap) { + aabb.expand_to(E.key); + points.write[E.value] = E.key; + } + + Vector main_vbh; + main_vbh.resize(segments.size()); + for (int i = 0; i < main_vbh.size(); i++) { + main_vbh.write[i].aabb.position = points[segments[i].points[0]]; + main_vbh.write[i].aabb.expand_to(points[segments[i].points[1]]); + main_vbh.write[i].left = -1; + main_vbh.write[i].right = i; + } + + _generate_bvh(main_vbh.ptrw(), main_vbh.size(), 1); + + } else { + //dictionary with arrays + } + + configure(aabb); +} + +Variant GodotConcavePolygonShape2D::get_data() const { + Vector rsegments; + int len = segments.size(); + rsegments.resize(len * 2); + Vector2 *w = rsegments.ptrw(); + for (int i = 0; i < len; i++) { + w[(i << 1) + 0] = points[segments[i].points[0]]; + w[(i << 1) + 1] = points[segments[i].points[1]]; + } + + return rsegments; +} + +void GodotConcavePolygonShape2D::cull(const Rect2 &p_local_aabb, QueryCallback p_callback, void *p_userdata) const { + uint32_t *stack = (uint32_t *)alloca(sizeof(int) * bvh_depth); + + enum { + TEST_AABB_BIT = 0, + VISIT_LEFT_BIT = 1, + VISIT_RIGHT_BIT = 2, + VISIT_DONE_BIT = 3, + VISITED_BIT_SHIFT = 29, + NODE_IDX_MASK = (1 << VISITED_BIT_SHIFT) - 1, + VISITED_BIT_MASK = ~NODE_IDX_MASK, + + }; + + /* + for(int i=0;i> VISITED_BIT_SHIFT) { + case TEST_AABB_BIT: { + bool valid = p_local_aabb.intersects(bvh.aabb); + if (!valid) { + stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; + + } else { + if (bvh.left < 0) { + const Segment &s = segmentptr[bvh.right]; + Vector2 a = pointptr[s.points[0]]; + Vector2 b = pointptr[s.points[1]]; + + GodotSegmentShape2D ss(a, b, (b - a).orthogonal().normalized()); + + if (p_callback(p_userdata, &ss)) { + return; + } + stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; + + } else { + stack[level] = (VISIT_LEFT_BIT << VISITED_BIT_SHIFT) | node; + } + } + } + continue; + case VISIT_LEFT_BIT: { + stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node; + stack[level + 1] = bvh.left | TEST_AABB_BIT; + level++; + } + continue; + case VISIT_RIGHT_BIT: { + stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; + stack[level + 1] = bvh.right | TEST_AABB_BIT; + level++; + } + continue; + case VISIT_DONE_BIT: { + if (level == 0) { + return; + } else { + level--; + } + } + continue; + } + } +} diff --git a/servers/physics_2d/godot_shape_2d.h b/servers/physics_2d/godot_shape_2d.h new file mode 100644 index 0000000000..25d113aafb --- /dev/null +++ b/servers/physics_2d/godot_shape_2d.h @@ -0,0 +1,536 @@ +/*************************************************************************/ +/* godot_shape_2d.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 GODOT_SHAPE_2D_H +#define GODOT_SHAPE_2D_H + +#include "servers/physics_server_2d.h" +#define _SEGMENT_IS_VALID_SUPPORT_THRESHOLD 0.99998 + +class GodotShape2D; + +class GodotShapeOwner2D { +public: + virtual void _shape_changed() = 0; + virtual void remove_shape(GodotShape2D *p_shape) = 0; + + virtual ~GodotShapeOwner2D() {} +}; + +class GodotShape2D { + RID self; + Rect2 aabb; + bool configured = false; + real_t custom_bias = 0.0; + + Map owners; + +protected: + void configure(const Rect2 &p_aabb); + +public: + _FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; } + _FORCE_INLINE_ RID get_self() const { return self; } + + virtual PhysicsServer2D::ShapeType get_type() const = 0; + + _FORCE_INLINE_ Rect2 get_aabb() const { return aabb; } + _FORCE_INLINE_ bool is_configured() const { return configured; } + + virtual bool allows_one_way_collision() const { return true; } + + virtual bool is_concave() const { return false; } + + virtual bool contains_point(const Vector2 &p_point) const = 0; + + virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const = 0; + virtual void project_range_castv(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const = 0; + virtual Vector2 get_support(const Vector2 &p_normal) const; + virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const = 0; + + virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const = 0; + virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const = 0; + virtual void set_data(const Variant &p_data) = 0; + virtual Variant get_data() const = 0; + + _FORCE_INLINE_ void set_custom_bias(real_t p_bias) { custom_bias = p_bias; } + _FORCE_INLINE_ real_t get_custom_bias() const { return custom_bias; } + + void add_owner(GodotShapeOwner2D *p_owner); + void remove_owner(GodotShapeOwner2D *p_owner); + bool is_owner(GodotShapeOwner2D *p_owner) const; + const Map &get_owners() const; + + _FORCE_INLINE_ void get_supports_transformed_cast(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_xform, Vector2 *r_supports, int &r_amount) const { + get_supports(p_xform.basis_xform_inv(p_normal).normalized(), r_supports, r_amount); + for (int i = 0; i < r_amount; i++) { + r_supports[i] = p_xform.xform(r_supports[i]); + } + + if (r_amount == 1) { + if (Math::abs(p_normal.dot(p_cast.normalized())) < (1.0 - _SEGMENT_IS_VALID_SUPPORT_THRESHOLD)) { + //make line because they are parallel + r_amount = 2; + r_supports[1] = r_supports[0] + p_cast; + } else if (p_cast.dot(p_normal) > 0) { + //normal points towards cast, add cast + r_supports[0] += p_cast; + } + + } else { + if (Math::abs(p_normal.dot(p_cast.normalized())) < (1.0 - _SEGMENT_IS_VALID_SUPPORT_THRESHOLD)) { + //optimize line and make it larger because they are parallel + if ((r_supports[1] - r_supports[0]).dot(p_cast) > 0) { + //larger towards 1 + r_supports[1] += p_cast; + } else { + //larger towards 0 + r_supports[0] += p_cast; + } + } else if (p_cast.dot(p_normal) > 0) { + //normal points towards cast, add cast + r_supports[0] += p_cast; + r_supports[1] += p_cast; + } + } + } + GodotShape2D() {} + virtual ~GodotShape2D(); +}; + +//let the optimizer do the magic +#define DEFAULT_PROJECT_RANGE_CAST \ + virtual void project_range_castv(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { \ + project_range_cast(p_cast, p_normal, p_transform, r_min, r_max); \ + } \ + _FORCE_INLINE_ void project_range_cast(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { \ + real_t mina, maxa; \ + real_t minb, maxb; \ + Transform2D ofsb = p_transform; \ + ofsb.elements[2] += p_cast; \ + project_range(p_normal, p_transform, mina, maxa); \ + project_range(p_normal, ofsb, minb, maxb); \ + r_min = MIN(mina, minb); \ + r_max = MAX(maxa, maxb); \ + } + +class GodotWorldBoundaryShape2D : public GodotShape2D { + Vector2 normal; + real_t d = 0.0; + +public: + _FORCE_INLINE_ Vector2 get_normal() const { return normal; } + _FORCE_INLINE_ real_t get_d() const { return d; } + + virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_WORLD_BOUNDARY; } + + virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); } + virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; + + virtual bool contains_point(const Vector2 &p_point) const override; + virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; + virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; + + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; + + _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { + //real large + r_min = -1e10; + r_max = 1e10; + } + + virtual void project_range_castv(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { + project_range_cast(p_cast, p_normal, p_transform, r_min, r_max); + } + + _FORCE_INLINE_ void project_range_cast(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { + //real large + r_min = -1e10; + r_max = 1e10; + } +}; + +class GodotSeparationRayShape2D : public GodotShape2D { + real_t length = 0.0; + bool slide_on_slope = false; + +public: + _FORCE_INLINE_ real_t get_length() const { return length; } + _FORCE_INLINE_ bool get_slide_on_slope() const { return slide_on_slope; } + + virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_SEPARATION_RAY; } + + virtual bool allows_one_way_collision() const override { return false; } + + virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); } + virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; + + virtual bool contains_point(const Vector2 &p_point) const override; + virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; + virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; + + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; + + _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { + //real large + r_max = p_normal.dot(p_transform.get_origin()); + r_min = p_normal.dot(p_transform.xform(Vector2(0, length))); + if (r_max < r_min) { + SWAP(r_max, r_min); + } + } + + DEFAULT_PROJECT_RANGE_CAST + + _FORCE_INLINE_ GodotSeparationRayShape2D() {} + _FORCE_INLINE_ GodotSeparationRayShape2D(real_t p_length) { length = p_length; } +}; + +class GodotSegmentShape2D : public GodotShape2D { + Vector2 a; + Vector2 b; + Vector2 n; + +public: + _FORCE_INLINE_ const Vector2 &get_a() const { return a; } + _FORCE_INLINE_ const Vector2 &get_b() const { return b; } + _FORCE_INLINE_ const Vector2 &get_normal() const { return n; } + + virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_SEGMENT; } + + _FORCE_INLINE_ Vector2 get_xformed_normal(const Transform2D &p_xform) const { + return (p_xform.xform(b) - p_xform.xform(a)).normalized().orthogonal(); + } + virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); } + virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; + + virtual bool contains_point(const Vector2 &p_point) const override; + virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; + virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; + + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; + + _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { + //real large + r_max = p_normal.dot(p_transform.xform(a)); + r_min = p_normal.dot(p_transform.xform(b)); + if (r_max < r_min) { + SWAP(r_max, r_min); + } + } + + DEFAULT_PROJECT_RANGE_CAST + + _FORCE_INLINE_ GodotSegmentShape2D() {} + _FORCE_INLINE_ GodotSegmentShape2D(const Vector2 &p_a, const Vector2 &p_b, const Vector2 &p_n) { + a = p_a; + b = p_b; + n = p_n; + } +}; + +class GodotCircleShape2D : public GodotShape2D { + real_t radius; + +public: + _FORCE_INLINE_ const real_t &get_radius() const { return radius; } + + virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_CIRCLE; } + + virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); } + virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; + + virtual bool contains_point(const Vector2 &p_point) const override; + virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; + virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; + + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; + + _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { + //real large + real_t d = p_normal.dot(p_transform.get_origin()); + + // figure out scale at point + Vector2 local_normal = p_transform.basis_xform_inv(p_normal); + real_t scale = local_normal.length(); + + r_min = d - (radius)*scale; + r_max = d + (radius)*scale; + } + + DEFAULT_PROJECT_RANGE_CAST +}; + +class GodotRectangleShape2D : public GodotShape2D { + Vector2 half_extents; + +public: + _FORCE_INLINE_ const Vector2 &get_half_extents() const { return half_extents; } + + virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_RECTANGLE; } + + virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); } + virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; + + virtual bool contains_point(const Vector2 &p_point) const override; + virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; + virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; + + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; + + _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { + // no matter the angle, the box is mirrored anyway + r_max = -1e20; + r_min = 1e20; + for (int i = 0; i < 4; i++) { + real_t d = p_normal.dot(p_transform.xform(Vector2(((i & 1) * 2 - 1) * half_extents.x, ((i >> 1) * 2 - 1) * half_extents.y))); + + if (d > r_max) { + r_max = d; + } + if (d < r_min) { + r_min = d; + } + } + } + + _FORCE_INLINE_ Vector2 get_circle_axis(const Transform2D &p_xform, const Transform2D &p_xform_inv, const Vector2 &p_circle) const { + Vector2 local_v = p_xform_inv.xform(p_circle); + + Vector2 he( + (local_v.x < 0) ? -half_extents.x : half_extents.x, + (local_v.y < 0) ? -half_extents.y : half_extents.y); + + return (p_xform.xform(he) - p_circle).normalized(); + } + + _FORCE_INLINE_ Vector2 get_box_axis(const Transform2D &p_xform, const Transform2D &p_xform_inv, const GodotRectangleShape2D *p_B, const Transform2D &p_B_xform, const Transform2D &p_B_xform_inv) const { + Vector2 a, b; + + { + Vector2 local_v = p_xform_inv.xform(p_B_xform.get_origin()); + + Vector2 he( + (local_v.x < 0) ? -half_extents.x : half_extents.x, + (local_v.y < 0) ? -half_extents.y : half_extents.y); + + a = p_xform.xform(he); + } + { + Vector2 local_v = p_B_xform_inv.xform(p_xform.get_origin()); + + Vector2 he( + (local_v.x < 0) ? -p_B->half_extents.x : p_B->half_extents.x, + (local_v.y < 0) ? -p_B->half_extents.y : p_B->half_extents.y); + + b = p_B_xform.xform(he); + } + + return (a - b).normalized(); + } + + DEFAULT_PROJECT_RANGE_CAST +}; + +class GodotCapsuleShape2D : public GodotShape2D { + real_t radius = 0.0; + real_t height = 0.0; + +public: + _FORCE_INLINE_ const real_t &get_radius() const { return radius; } + _FORCE_INLINE_ const real_t &get_height() const { return height; } + + virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_CAPSULE; } + + virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); } + virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; + + virtual bool contains_point(const Vector2 &p_point) const override; + virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; + virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; + + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; + + _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { + // no matter the angle, the box is mirrored anyway + Vector2 n = p_transform.basis_xform_inv(p_normal).normalized(); + real_t h = height * 0.5 - radius; + + n *= radius; + n.y += (n.y > 0) ? h : -h; + + r_max = p_normal.dot(p_transform.xform(n)); + r_min = p_normal.dot(p_transform.xform(-n)); + + if (r_max < r_min) { + SWAP(r_max, r_min); + } + + //ERR_FAIL_COND( r_max < r_min ); + } + + DEFAULT_PROJECT_RANGE_CAST +}; + +class GodotConvexPolygonShape2D : public GodotShape2D { + struct Point { + Vector2 pos; + Vector2 normal; //normal to next segment + }; + + Point *points = nullptr; + int point_count = 0; + +public: + _FORCE_INLINE_ int get_point_count() const { return point_count; } + _FORCE_INLINE_ const Vector2 &get_point(int p_idx) const { return points[p_idx].pos; } + _FORCE_INLINE_ const Vector2 &get_segment_normal(int p_idx) const { return points[p_idx].normal; } + _FORCE_INLINE_ Vector2 get_xformed_segment_normal(const Transform2D &p_xform, int p_idx) const { + Vector2 a = points[p_idx].pos; + p_idx++; + Vector2 b = points[p_idx == point_count ? 0 : p_idx].pos; + return (p_xform.xform(b) - p_xform.xform(a)).normalized().orthogonal(); + } + + virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_CONVEX_POLYGON; } + + virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); } + virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; + + virtual bool contains_point(const Vector2 &p_point) const override; + virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; + virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; + + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; + + _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { + if (!points || point_count <= 0) { + r_min = r_max = 0; + return; + } + + r_min = r_max = p_normal.dot(p_transform.xform(points[0].pos)); + for (int i = 1; i < point_count; i++) { + real_t d = p_normal.dot(p_transform.xform(points[i].pos)); + if (d > r_max) { + r_max = d; + } + if (d < r_min) { + r_min = d; + } + } + } + + DEFAULT_PROJECT_RANGE_CAST + + GodotConvexPolygonShape2D() {} + ~GodotConvexPolygonShape2D(); +}; + +class GodotConcaveShape2D : public GodotShape2D { +public: + virtual bool is_concave() const override { return true; } + + // Returns true to stop the query. + typedef bool (*QueryCallback)(void *p_userdata, GodotShape2D *p_convex); + + virtual void cull(const Rect2 &p_local_aabb, QueryCallback p_callback, void *p_userdata) const = 0; +}; + +class GodotConcavePolygonShape2D : public GodotConcaveShape2D { + struct Segment { + int points[2] = {}; + }; + + Vector segments; + Vector points; + + struct BVH { + Rect2 aabb; + int left = 0, right = 0; + }; + + Vector bvh; + int bvh_depth = 0; + + struct BVH_CompareX { + _FORCE_INLINE_ bool operator()(const BVH &a, const BVH &b) const { + return (a.aabb.position.x + a.aabb.size.x * 0.5) < (b.aabb.position.x + b.aabb.size.x * 0.5); + } + }; + + struct BVH_CompareY { + _FORCE_INLINE_ bool operator()(const BVH &a, const BVH &b) const { + return (a.aabb.position.y + a.aabb.size.y * 0.5) < (b.aabb.position.y + b.aabb.size.y * 0.5); + } + }; + + int _generate_bvh(BVH *p_bvh, int p_len, int p_depth); + +public: + virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_CONCAVE_POLYGON; } + + virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { + r_min = 0; + r_max = 0; + ERR_FAIL_MSG("Unsupported call to project_rangev in GodotConcavePolygonShape2D"); + } + + void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { + r_min = 0; + r_max = 0; + ERR_FAIL_MSG("Unsupported call to project_range in GodotConcavePolygonShape2D"); + } + + virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; + + virtual bool contains_point(const Vector2 &p_point) const override; + virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; + + virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override { return 0; } + + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; + + virtual void cull(const Rect2 &p_local_aabb, QueryCallback p_callback, void *p_userdata) const override; + + DEFAULT_PROJECT_RANGE_CAST +}; + +#undef DEFAULT_PROJECT_RANGE_CAST + +#endif // GODOT_SHAPE_2D_H diff --git a/servers/physics_2d/godot_space_2d.cpp b/servers/physics_2d/godot_space_2d.cpp new file mode 100644 index 0000000000..d72014a8ed --- /dev/null +++ b/servers/physics_2d/godot_space_2d.cpp @@ -0,0 +1,1213 @@ +/*************************************************************************/ +/* godot_space_2d.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 "godot_space_2d.h" + +#include "godot_collision_solver_2d.h" +#include "godot_physics_server_2d.h" + +#include "core/os/os.h" +#include "core/templates/pair.h" + +#define TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR 0.05 + +_FORCE_INLINE_ static bool _can_collide_with(GodotCollisionObject2D *p_object, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { + if (!(p_object->get_collision_layer() & p_collision_mask)) { + return false; + } + + if (p_object->get_type() == GodotCollisionObject2D::TYPE_AREA && !p_collide_with_areas) { + return false; + } + + if (p_object->get_type() == GodotCollisionObject2D::TYPE_BODY && !p_collide_with_bodies) { + return false; + } + + return true; +} + +int GodotPhysicsDirectSpaceState2D::_intersect_point_impl(const Vector2 &p_point, ShapeResult *r_results, int p_result_max, const Set &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, bool p_pick_point, bool p_filter_by_canvas, ObjectID p_canvas_instance_id) { + if (p_result_max <= 0) { + return 0; + } + + Rect2 aabb; + aabb.position = p_point - Vector2(0.00001, 0.00001); + aabb.size = Vector2(0.00002, 0.00002); + + int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace2D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); + + int cc = 0; + + for (int i = 0; i < amount; i++) { + if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { + continue; + } + + if (p_exclude.has(space->intersection_query_results[i]->get_self())) { + continue; + } + + const GodotCollisionObject2D *col_obj = space->intersection_query_results[i]; + + if (p_pick_point && !col_obj->is_pickable()) { + continue; + } + + if (p_filter_by_canvas && col_obj->get_canvas_instance_id() != p_canvas_instance_id) { + continue; + } + + int shape_idx = space->intersection_query_subindex_results[i]; + + GodotShape2D *shape = col_obj->get_shape(shape_idx); + + Vector2 local_point = (col_obj->get_transform() * col_obj->get_shape_transform(shape_idx)).affine_inverse().xform(p_point); + + if (!shape->contains_point(local_point)) { + continue; + } + + if (cc >= p_result_max) { + continue; + } + + r_results[cc].collider_id = col_obj->get_instance_id(); + if (r_results[cc].collider_id.is_valid()) { + r_results[cc].collider = ObjectDB::get_instance(r_results[cc].collider_id); + } + r_results[cc].rid = col_obj->get_self(); + r_results[cc].shape = shape_idx; + + cc++; + } + + return cc; +} + +int GodotPhysicsDirectSpaceState2D::intersect_point(const Vector2 &p_point, ShapeResult *r_results, int p_result_max, const Set &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, bool p_pick_point) { + return _intersect_point_impl(p_point, r_results, p_result_max, p_exclude, p_collision_mask, p_collide_with_bodies, p_collide_with_areas, p_pick_point); +} + +int GodotPhysicsDirectSpaceState2D::intersect_point_on_canvas(const Vector2 &p_point, ObjectID p_canvas_instance_id, ShapeResult *r_results, int p_result_max, const Set &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, bool p_pick_point) { + return _intersect_point_impl(p_point, r_results, p_result_max, p_exclude, p_collision_mask, p_collide_with_bodies, p_collide_with_areas, p_pick_point, true, p_canvas_instance_id); +} + +bool GodotPhysicsDirectSpaceState2D::intersect_ray(const Vector2 &p_from, const Vector2 &p_to, RayResult &r_result, const Set &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { + ERR_FAIL_COND_V(space->locked, false); + + Vector2 begin, end; + Vector2 normal; + begin = p_from; + end = p_to; + normal = (end - begin).normalized(); + + int amount = space->broadphase->cull_segment(begin, end, space->intersection_query_results, GodotSpace2D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); + + //todo, create another array that references results, compute AABBs and check closest point to ray origin, sort, and stop evaluating results when beyond first collision + + bool collided = false; + Vector2 res_point, res_normal; + int res_shape; + const GodotCollisionObject2D *res_obj; + real_t min_d = 1e10; + + for (int i = 0; i < amount; i++) { + if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { + continue; + } + + if (p_exclude.has(space->intersection_query_results[i]->get_self())) { + continue; + } + + const GodotCollisionObject2D *col_obj = space->intersection_query_results[i]; + + int shape_idx = space->intersection_query_subindex_results[i]; + Transform2D inv_xform = col_obj->get_shape_inv_transform(shape_idx) * col_obj->get_inv_transform(); + + Vector2 local_from = inv_xform.xform(begin); + Vector2 local_to = inv_xform.xform(end); + + /*local_from = col_obj->get_inv_transform().xform(begin); + local_from = col_obj->get_shape_inv_transform(shape_idx).xform(local_from); + + local_to = col_obj->get_inv_transform().xform(end); + local_to = col_obj->get_shape_inv_transform(shape_idx).xform(local_to);*/ + + const GodotShape2D *shape = col_obj->get_shape(shape_idx); + + Vector2 shape_point, shape_normal; + + if (shape->intersect_segment(local_from, local_to, shape_point, shape_normal)) { + Transform2D xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); + shape_point = xform.xform(shape_point); + + real_t ld = normal.dot(shape_point); + + if (ld < min_d) { + min_d = ld; + res_point = shape_point; + res_normal = inv_xform.basis_xform_inv(shape_normal).normalized(); + res_shape = shape_idx; + res_obj = col_obj; + collided = true; + } + } + } + + if (!collided) { + return false; + } + + r_result.collider_id = res_obj->get_instance_id(); + if (r_result.collider_id.is_valid()) { + r_result.collider = ObjectDB::get_instance(r_result.collider_id); + } + r_result.normal = res_normal; + r_result.position = res_point; + r_result.rid = res_obj->get_self(); + r_result.shape = res_shape; + + return true; +} + +int GodotPhysicsDirectSpaceState2D::intersect_shape(const RID &p_shape, const Transform2D &p_xform, const Vector2 &p_motion, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { + if (p_result_max <= 0) { + return 0; + } + + GodotShape2D *shape = GodotPhysicsServer2D::godot_singleton->shape_owner.get_or_null(p_shape); + ERR_FAIL_COND_V(!shape, 0); + + Rect2 aabb = p_xform.xform(shape->get_aabb()); + aabb = aabb.grow(p_margin); + + int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace2D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); + + int cc = 0; + + for (int i = 0; i < amount; i++) { + if (cc >= p_result_max) { + break; + } + + if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { + continue; + } + + if (p_exclude.has(space->intersection_query_results[i]->get_self())) { + continue; + } + + const GodotCollisionObject2D *col_obj = space->intersection_query_results[i]; + int shape_idx = space->intersection_query_subindex_results[i]; + + if (!GodotCollisionSolver2D::solve(shape, p_xform, p_motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), nullptr, nullptr, nullptr, p_margin)) { + continue; + } + + r_results[cc].collider_id = col_obj->get_instance_id(); + if (r_results[cc].collider_id.is_valid()) { + r_results[cc].collider = ObjectDB::get_instance(r_results[cc].collider_id); + } + r_results[cc].rid = col_obj->get_self(); + r_results[cc].shape = shape_idx; + + cc++; + } + + return cc; +} + +bool GodotPhysicsDirectSpaceState2D::cast_motion(const RID &p_shape, const Transform2D &p_xform, const Vector2 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { + GodotShape2D *shape = GodotPhysicsServer2D::godot_singleton->shape_owner.get_or_null(p_shape); + ERR_FAIL_COND_V(!shape, false); + + Rect2 aabb = p_xform.xform(shape->get_aabb()); + aabb = aabb.merge(Rect2(aabb.position + p_motion, aabb.size)); //motion + aabb = aabb.grow(p_margin); + + int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace2D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); + + real_t best_safe = 1; + real_t best_unsafe = 1; + + for (int i = 0; i < amount; i++) { + if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { + continue; + } + + if (p_exclude.has(space->intersection_query_results[i]->get_self())) { + continue; //ignore excluded + } + + const GodotCollisionObject2D *col_obj = space->intersection_query_results[i]; + int shape_idx = space->intersection_query_subindex_results[i]; + + Transform2D 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 (!GodotCollisionSolver2D::solve(shape, p_xform, p_motion, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, nullptr, p_margin)) { + continue; + } + + //test initial overlap, ignore objects it's inside of. + if (GodotCollisionSolver2D::solve(shape, p_xform, Vector2(), col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, nullptr, p_margin)) { + continue; + } + + Vector2 mnormal = p_motion.normalized(); + + //just do kinematic solving + real_t low = 0.0; + real_t hi = 1.0; + real_t fraction_coeff = 0.5; + for (int j = 0; j < 8; j++) { //steps should be customizable.. + real_t fraction = low + (hi - low) * fraction_coeff; + + Vector2 sep = mnormal; //important optimization for this to work fast enough + bool collided = GodotCollisionSolver2D::solve(shape, p_xform, p_motion * fraction, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, &sep, p_margin); + + if (collided) { + hi = fraction; + if ((j == 0) || (low > 0.0)) { // Did it not collide before? + // When alternating or first iteration, use dichotomy. + fraction_coeff = 0.5; + } else { + // When colliding again, converge faster towards low fraction + // for more accurate results with long motions that collide near the start. + fraction_coeff = 0.25; + } + } else { + low = fraction; + if ((j == 0) || (hi < 1.0)) { // Did it collide before? + // When alternating or first iteration, use dichotomy. + fraction_coeff = 0.5; + } else { + // When not colliding again, converge faster towards high fraction + // for more accurate results with long motions that collide near the end. + fraction_coeff = 0.75; + } + } + } + + if (low < best_safe) { + best_safe = low; + best_unsafe = hi; + } + } + + p_closest_safe = best_safe; + p_closest_unsafe = best_unsafe; + + return true; +} + +bool GodotPhysicsDirectSpaceState2D::collide_shape(RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, real_t p_margin, Vector2 *r_results, int p_result_max, int &r_result_count, const Set &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { + if (p_result_max <= 0) { + return false; + } + + GodotShape2D *shape = GodotPhysicsServer2D::godot_singleton->shape_owner.get_or_null(p_shape); + ERR_FAIL_COND_V(!shape, 0); + + Rect2 aabb = p_shape_xform.xform(shape->get_aabb()); + aabb = aabb.merge(Rect2(aabb.position + p_motion, aabb.size)); //motion + aabb = aabb.grow(p_margin); + + int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace2D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); + + bool collided = false; + r_result_count = 0; + + GodotPhysicsServer2D::CollCbkData cbk; + cbk.max = p_result_max; + cbk.amount = 0; + cbk.passed = 0; + cbk.ptr = r_results; + GodotCollisionSolver2D::CallbackResult cbkres = GodotPhysicsServer2D::_shape_col_cbk; + + GodotPhysicsServer2D::CollCbkData *cbkptr = &cbk; + + for (int i = 0; i < amount; i++) { + if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { + continue; + } + + const GodotCollisionObject2D *col_obj = space->intersection_query_results[i]; + + if (p_exclude.has(col_obj->get_self())) { + continue; + } + + int shape_idx = space->intersection_query_subindex_results[i]; + + cbk.valid_dir = Vector2(); + cbk.valid_depth = 0; + + if (GodotCollisionSolver2D::solve(shape, p_shape_xform, p_motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), cbkres, cbkptr, nullptr, p_margin)) { + collided = cbk.amount > 0; + } + } + + r_result_count = cbk.amount; + + return collided; +} + +struct _RestCallbackData2D { + const GodotCollisionObject2D *object = nullptr; + const GodotCollisionObject2D *best_object = nullptr; + int local_shape = 0; + int best_local_shape = 0; + int shape = 0; + int best_shape = 0; + Vector2 best_contact; + Vector2 best_normal; + real_t best_len = 0.0; + Vector2 valid_dir; + real_t valid_depth = 0.0; + real_t min_allowed_depth = 0.0; +}; + +static void _rest_cbk_result(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_userdata) { + _RestCallbackData2D *rd = (_RestCallbackData2D *)p_userdata; + + Vector2 contact_rel = p_point_B - p_point_A; + real_t len = contact_rel.length(); + + if (len < rd->min_allowed_depth) { + return; + } + + if (len <= rd->best_len) { + return; + } + + Vector2 normal = contact_rel / len; + + if (rd->valid_dir != Vector2()) { + if (len > rd->valid_depth) { + return; + } + + if (rd->valid_dir.dot(normal) > -CMP_EPSILON) { + return; + } + } + + rd->best_len = len; + rd->best_contact = p_point_B; + rd->best_normal = normal; + rd->best_object = rd->object; + rd->best_shape = rd->shape; + rd->best_local_shape = rd->local_shape; +} + +bool GodotPhysicsDirectSpaceState2D::rest_info(RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, real_t p_margin, ShapeRestInfo *r_info, const Set &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { + GodotShape2D *shape = GodotPhysicsServer2D::godot_singleton->shape_owner.get_or_null(p_shape); + ERR_FAIL_COND_V(!shape, 0); + + real_t min_contact_depth = p_margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR; + + Rect2 aabb = p_shape_xform.xform(shape->get_aabb()); + aabb = aabb.merge(Rect2(aabb.position + p_motion, aabb.size)); //motion + aabb = aabb.grow(p_margin); + + int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace2D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); + + _RestCallbackData2D rcd; + rcd.best_len = 0; + rcd.best_object = nullptr; + rcd.best_shape = 0; + rcd.min_allowed_depth = min_contact_depth; + + for (int i = 0; i < amount; i++) { + if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { + continue; + } + + const GodotCollisionObject2D *col_obj = space->intersection_query_results[i]; + + if (p_exclude.has(col_obj->get_self())) { + continue; + } + + int shape_idx = space->intersection_query_subindex_results[i]; + + rcd.valid_dir = Vector2(); + rcd.object = col_obj; + rcd.shape = shape_idx; + rcd.local_shape = 0; + bool sc = GodotCollisionSolver2D::solve(shape, p_shape_xform, p_motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), _rest_cbk_result, &rcd, nullptr, p_margin); + if (!sc) { + continue; + } + } + + if (rcd.best_len == 0 || !rcd.best_object) { + return false; + } + + r_info->collider_id = rcd.best_object->get_instance_id(); + r_info->shape = rcd.best_shape; + r_info->normal = rcd.best_normal; + r_info->point = rcd.best_contact; + r_info->rid = rcd.best_object->get_self(); + if (rcd.best_object->get_type() == GodotCollisionObject2D::TYPE_BODY) { + const GodotBody2D *body = static_cast(rcd.best_object); + Vector2 rel_vec = r_info->point - (body->get_transform().get_origin() + body->get_center_of_mass()); + r_info->linear_velocity = Vector2(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity(); + + } else { + r_info->linear_velocity = Vector2(); + } + + return true; +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////// + +int GodotSpace2D::_cull_aabb_for_body(GodotBody2D *p_body, const Rect2 &p_aabb) { + int amount = broadphase->cull_aabb(p_aabb, intersection_query_results, INTERSECTION_QUERY_MAX, intersection_query_subindex_results); + + for (int i = 0; i < amount; i++) { + bool keep = true; + + if (intersection_query_results[i] == p_body) { + keep = false; + } else if (intersection_query_results[i]->get_type() == GodotCollisionObject2D::TYPE_AREA) { + keep = false; + } else if (!p_body->collides_with(static_cast(intersection_query_results[i]))) { + keep = false; + } else if (static_cast(intersection_query_results[i])->has_exception(p_body->get_self()) || p_body->has_exception(intersection_query_results[i]->get_self())) { + keep = false; + } + + if (!keep) { + if (i < amount - 1) { + SWAP(intersection_query_results[i], intersection_query_results[amount - 1]); + SWAP(intersection_query_subindex_results[i], intersection_query_subindex_results[amount - 1]); + } + + amount--; + i--; + } + } + + return amount; +} + +bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D::MotionParameters &p_parameters, PhysicsServer2D::MotionResult *r_result) { + //give me back regular physics engine logic + //this is madness + //and most people using this function will think + //what it does is simpler than using physics + //this took about a week to get right.. + //but is it right? who knows at this point.. + + if (r_result) { + r_result->collider_id = ObjectID(); + r_result->collider_shape = 0; + } + Rect2 body_aabb; + + bool shapes_found = false; + + for (int i = 0; i < p_body->get_shape_count(); i++) { + if (p_body->is_shape_disabled(i)) { + continue; + } + + if (!shapes_found) { + body_aabb = p_body->get_shape_aabb(i); + shapes_found = true; + } else { + body_aabb = body_aabb.merge(p_body->get_shape_aabb(i)); + } + } + + if (!shapes_found) { + if (r_result) { + *r_result = PhysicsServer2D::MotionResult(); + r_result->travel = p_parameters.motion; + } + return false; + } + + // Undo the currently transform the physics server is aware of and apply the provided one + body_aabb = p_parameters.from.xform(p_body->get_inv_transform().xform(body_aabb)); + body_aabb = body_aabb.grow(p_parameters.margin); + + static const int max_excluded_shape_pairs = 32; + ExcludedShapeSW excluded_shape_pairs[max_excluded_shape_pairs]; + int excluded_shape_pair_count = 0; + + real_t min_contact_depth = p_parameters.margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR; + + real_t motion_length = p_parameters.motion.length(); + Vector2 motion_normal = p_parameters.motion / motion_length; + + Transform2D body_transform = p_parameters.from; + + bool recovered = false; + + { + //STEP 1, FREE BODY IF STUCK + + const int max_results = 32; + int recover_attempts = 4; + Vector2 sr[max_results * 2]; + + do { + GodotPhysicsServer2D::CollCbkData cbk; + cbk.max = max_results; + cbk.amount = 0; + cbk.passed = 0; + cbk.ptr = sr; + cbk.invalid_by_dir = 0; + excluded_shape_pair_count = 0; //last step is the one valid + + GodotPhysicsServer2D::CollCbkData *cbkptr = &cbk; + GodotCollisionSolver2D::CallbackResult cbkres = GodotPhysicsServer2D::_shape_col_cbk; + + bool collided = false; + + int amount = _cull_aabb_for_body(p_body, body_aabb); + + for (int j = 0; j < p_body->get_shape_count(); j++) { + if (p_body->is_shape_disabled(j)) { + continue; + } + + GodotShape2D *body_shape = p_body->get_shape(j); + Transform2D body_shape_xform = body_transform * p_body->get_shape_transform(j); + + for (int i = 0; i < amount; i++) { + const GodotCollisionObject2D *col_obj = intersection_query_results[i]; + if (p_parameters.exclude_bodies.has(col_obj->get_self())) { + continue; + } + if (p_parameters.exclude_objects.has(col_obj->get_instance_id())) { + continue; + } + + int shape_idx = intersection_query_subindex_results[i]; + + Transform2D col_obj_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); + + if (body_shape->allows_one_way_collision() && col_obj->is_shape_set_as_one_way_collision(shape_idx)) { + cbk.valid_dir = col_obj_shape_xform.get_axis(1).normalized(); + + real_t owc_margin = col_obj->get_shape_one_way_collision_margin(shape_idx); + cbk.valid_depth = MAX(owc_margin, p_parameters.margin); //user specified, but never less than actual margin or it won't work + cbk.invalid_by_dir = 0; + + if (col_obj->get_type() == GodotCollisionObject2D::TYPE_BODY) { + const GodotBody2D *b = static_cast(col_obj); + if (b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_DYNAMIC) { + //fix for moving platforms (kinematic and dynamic), margin is increased by how much it moved in the given direction + Vector2 lv = b->get_linear_velocity(); + //compute displacement from linear velocity + Vector2 motion = lv * last_step; + real_t motion_len = motion.length(); + motion.normalize(); + cbk.valid_depth += motion_len * MAX(motion.dot(-cbk.valid_dir), 0.0); + } + } + } else { + cbk.valid_dir = Vector2(); + cbk.valid_depth = 0; + cbk.invalid_by_dir = 0; + } + + int current_passed = cbk.passed; //save how many points passed collision + bool did_collide = false; + + GodotShape2D *against_shape = col_obj->get_shape(shape_idx); + if (GodotCollisionSolver2D::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), cbkres, cbkptr, nullptr, p_parameters.margin)) { + did_collide = cbk.passed > current_passed; //more passed, so collision actually existed + } + + if (!did_collide && cbk.invalid_by_dir > 0) { + //this shape must be excluded + if (excluded_shape_pair_count < max_excluded_shape_pairs) { + ExcludedShapeSW esp; + esp.local_shape = body_shape; + esp.against_object = col_obj; + esp.against_shape_index = shape_idx; + excluded_shape_pairs[excluded_shape_pair_count++] = esp; + } + } + + if (did_collide) { + collided = true; + } + } + } + + if (!collided) { + break; + } + + recovered = true; + + Vector2 recover_motion; + for (int i = 0; i < cbk.amount; i++) { + Vector2 a = sr[i * 2 + 0]; + Vector2 b = sr[i * 2 + 1]; + + // Compute plane on b towards a. + Vector2 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 > min_contact_depth + CMP_EPSILON) { + // Only recover if there is penetration. + recover_motion -= n * (depth - min_contact_depth) * 0.4; + } + } + + if (recover_motion == Vector2()) { + collided = false; + break; + } + + body_transform.elements[2] += recover_motion; + body_aabb.position += recover_motion; + + recover_attempts--; + + } while (recover_attempts); + } + + real_t safe = 1.0; + real_t unsafe = 1.0; + int best_shape = -1; + + { + // STEP 2 ATTEMPT MOTION + + Rect2 motion_aabb = body_aabb; + motion_aabb.position += p_parameters.motion; + motion_aabb = motion_aabb.merge(body_aabb); + + int amount = _cull_aabb_for_body(p_body, motion_aabb); + + for (int body_shape_idx = 0; body_shape_idx < p_body->get_shape_count(); body_shape_idx++) { + if (p_body->is_shape_disabled(body_shape_idx)) { + continue; + } + + GodotShape2D *body_shape = p_body->get_shape(body_shape_idx); + + // Colliding separation rays allows to properly snap to the ground, + // otherwise it's not needed in regular motion. + if (!p_parameters.collide_separation_ray && (body_shape->get_type() == PhysicsServer2D::SHAPE_SEPARATION_RAY)) { + // When slide on slope is on, separation ray shape acts like a regular shape. + if (!static_cast(body_shape)->get_slide_on_slope()) { + continue; + } + } + + Transform2D body_shape_xform = body_transform * p_body->get_shape_transform(body_shape_idx); + + bool stuck = false; + + real_t best_safe = 1; + real_t best_unsafe = 1; + + for (int i = 0; i < amount; i++) { + const GodotCollisionObject2D *col_obj = intersection_query_results[i]; + if (p_parameters.exclude_bodies.has(col_obj->get_self())) { + continue; + } + if (p_parameters.exclude_objects.has(col_obj->get_instance_id())) { + continue; + } + + int col_shape_idx = intersection_query_subindex_results[i]; + GodotShape2D *against_shape = col_obj->get_shape(col_shape_idx); + + bool excluded = false; + + for (int k = 0; k < excluded_shape_pair_count; k++) { + if (excluded_shape_pairs[k].local_shape == body_shape && excluded_shape_pairs[k].against_object == col_obj && excluded_shape_pairs[k].against_shape_index == col_shape_idx) { + excluded = true; + break; + } + } + + if (excluded) { + continue; + } + + Transform2D col_obj_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(col_shape_idx); + //test initial overlap, does it collide if going all the way? + if (!GodotCollisionSolver2D::solve(body_shape, body_shape_xform, p_parameters.motion, against_shape, col_obj_shape_xform, Vector2(), nullptr, nullptr, nullptr, 0)) { + continue; + } + + //test initial overlap + if (GodotCollisionSolver2D::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), nullptr, nullptr, nullptr, 0)) { + if (body_shape->allows_one_way_collision() && col_obj->is_shape_set_as_one_way_collision(col_shape_idx)) { + Vector2 direction = col_obj_shape_xform.get_axis(1).normalized(); + if (motion_normal.dot(direction) < 0) { + continue; + } + } + + stuck = true; + break; + } + + //just do kinematic solving + real_t low = 0.0; + real_t hi = 1.0; + real_t fraction_coeff = 0.5; + for (int k = 0; k < 8; k++) { //steps should be customizable.. + real_t fraction = low + (hi - low) * fraction_coeff; + + Vector2 sep = motion_normal; //important optimization for this to work fast enough + bool collided = GodotCollisionSolver2D::solve(body_shape, body_shape_xform, p_parameters.motion * fraction, against_shape, col_obj_shape_xform, Vector2(), nullptr, nullptr, &sep, 0); + + if (collided) { + hi = fraction; + if ((k == 0) || (low > 0.0)) { // Did it not collide before? + // When alternating or first iteration, use dichotomy. + fraction_coeff = 0.5; + } else { + // When colliding again, converge faster towards low fraction + // for more accurate results with long motions that collide near the start. + fraction_coeff = 0.25; + } + } else { + low = fraction; + if ((k == 0) || (hi < 1.0)) { // Did it collide before? + // When alternating or first iteration, use dichotomy. + fraction_coeff = 0.5; + } else { + // When not colliding again, converge faster towards high fraction + // for more accurate results with long motions that collide near the end. + fraction_coeff = 0.75; + } + } + } + + if (body_shape->allows_one_way_collision() && col_obj->is_shape_set_as_one_way_collision(col_shape_idx)) { + Vector2 cd[2]; + GodotPhysicsServer2D::CollCbkData cbk; + cbk.max = 1; + cbk.amount = 0; + cbk.passed = 0; + cbk.ptr = cd; + cbk.valid_dir = col_obj_shape_xform.get_axis(1).normalized(); + + cbk.valid_depth = 10e20; + + Vector2 sep = motion_normal; //important optimization for this to work fast enough + bool collided = GodotCollisionSolver2D::solve(body_shape, body_shape_xform, p_parameters.motion * (hi + contact_max_allowed_penetration), col_obj->get_shape(col_shape_idx), col_obj_shape_xform, Vector2(), GodotPhysicsServer2D::_shape_col_cbk, &cbk, &sep, 0); + if (!collided || cbk.amount == 0) { + continue; + } + } + + if (low < best_safe) { + best_safe = low; + best_unsafe = hi; + } + } + + if (stuck) { + safe = 0; + unsafe = 0; + best_shape = body_shape_idx; //sadly it's the best + break; + } + if (best_safe == 1.0) { + continue; + } + if (best_safe < safe) { + safe = best_safe; + unsafe = best_unsafe; + best_shape = body_shape_idx; + } + } + } + + bool collided = false; + + if (recovered || (safe < 1)) { + if (safe >= 1) { + best_shape = -1; //no best shape with cast, reset to -1 + } + + //it collided, let's get the rest info in unsafe advance + Transform2D ugt = body_transform; + ugt.elements[2] += p_parameters.motion * unsafe; + + _RestCallbackData2D rcd; + rcd.best_len = 0; + rcd.best_object = nullptr; + rcd.best_shape = 0; + + // Allowed depth can't be lower than motion length, in order to handle contacts at low speed. + rcd.min_allowed_depth = MIN(motion_length, min_contact_depth); + + int from_shape = best_shape != -1 ? best_shape : 0; + int to_shape = best_shape != -1 ? best_shape + 1 : p_body->get_shape_count(); + + for (int j = from_shape; j < to_shape; j++) { + if (p_body->is_shape_disabled(j)) { + continue; + } + + Transform2D body_shape_xform = ugt * p_body->get_shape_transform(j); + GodotShape2D *body_shape = p_body->get_shape(j); + + body_aabb.position += p_parameters.motion * unsafe; + + int amount = _cull_aabb_for_body(p_body, body_aabb); + + for (int i = 0; i < amount; i++) { + const GodotCollisionObject2D *col_obj = intersection_query_results[i]; + if (p_parameters.exclude_bodies.has(col_obj->get_self())) { + continue; + } + if (p_parameters.exclude_objects.has(col_obj->get_instance_id())) { + continue; + } + + int shape_idx = intersection_query_subindex_results[i]; + + GodotShape2D *against_shape = col_obj->get_shape(shape_idx); + + bool excluded = false; + for (int k = 0; k < excluded_shape_pair_count; k++) { + if (excluded_shape_pairs[k].local_shape == body_shape && excluded_shape_pairs[k].against_object == col_obj && excluded_shape_pairs[k].against_shape_index == shape_idx) { + excluded = true; + break; + } + } + if (excluded) { + continue; + } + + Transform2D col_obj_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); + + if (body_shape->allows_one_way_collision() && col_obj->is_shape_set_as_one_way_collision(shape_idx)) { + rcd.valid_dir = col_obj_shape_xform.get_axis(1).normalized(); + + real_t owc_margin = col_obj->get_shape_one_way_collision_margin(shape_idx); + rcd.valid_depth = MAX(owc_margin, p_parameters.margin); //user specified, but never less than actual margin or it won't work + + if (col_obj->get_type() == GodotCollisionObject2D::TYPE_BODY) { + const GodotBody2D *b = static_cast(col_obj); + if (b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_DYNAMIC) { + //fix for moving platforms (kinematic and dynamic), margin is increased by how much it moved in the given direction + Vector2 lv = b->get_linear_velocity(); + //compute displacement from linear velocity + Vector2 motion = lv * last_step; + real_t motion_len = motion.length(); + motion.normalize(); + rcd.valid_depth += motion_len * MAX(motion.dot(-rcd.valid_dir), 0.0); + } + } + } else { + rcd.valid_dir = Vector2(); + rcd.valid_depth = 0; + } + + rcd.object = col_obj; + rcd.shape = shape_idx; + rcd.local_shape = j; + bool sc = GodotCollisionSolver2D::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), _rest_cbk_result, &rcd, nullptr, p_parameters.margin); + if (!sc) { + continue; + } + } + } + + if (rcd.best_len != 0) { + if (r_result) { + 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 = rcd.best_local_shape; + r_result->collision_normal = rcd.best_normal; + r_result->collision_point = rcd.best_contact; + r_result->collision_depth = rcd.best_len; + r_result->collision_safe_fraction = safe; + r_result->collision_unsafe_fraction = unsafe; + + const GodotBody2D *body = static_cast(rcd.best_object); + Vector2 rel_vec = r_result->collision_point - (body->get_transform().get_origin() + body->get_center_of_mass()); + r_result->collider_velocity = Vector2(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity(); + + r_result->travel = safe * p_parameters.motion; + r_result->remainder = p_parameters.motion - safe * p_parameters.motion; + r_result->travel += (body_transform.get_origin() - p_parameters.from.get_origin()); + } + + collided = true; + } + } + + if (!collided && r_result) { + r_result->travel = p_parameters.motion; + r_result->remainder = Vector2(); + r_result->travel += (body_transform.get_origin() - p_parameters.from.get_origin()); + } + + return collided; +} + +void *GodotSpace2D::_broadphase_pair(GodotCollisionObject2D *A, int p_subindex_A, GodotCollisionObject2D *B, int p_subindex_B, void *p_self) { + if (!A->interacts_with(B)) { + return nullptr; + } + + GodotCollisionObject2D::Type type_A = A->get_type(); + GodotCollisionObject2D::Type type_B = B->get_type(); + if (type_A > type_B) { + SWAP(A, B); + SWAP(p_subindex_A, p_subindex_B); + SWAP(type_A, type_B); + } + + GodotSpace2D *self = (GodotSpace2D *)p_self; + self->collision_pairs++; + + if (type_A == GodotCollisionObject2D::TYPE_AREA) { + GodotArea2D *area = static_cast(A); + if (type_B == GodotCollisionObject2D::TYPE_AREA) { + GodotArea2D *area_b = static_cast(B); + GodotArea2Pair2D *area2_pair = memnew(GodotArea2Pair2D(area_b, p_subindex_B, area, p_subindex_A)); + return area2_pair; + } else { + GodotBody2D *body = static_cast(B); + GodotAreaPair2D *area_pair = memnew(GodotAreaPair2D(body, p_subindex_B, area, p_subindex_A)); + return area_pair; + } + + } else { + GodotBodyPair2D *b = memnew(GodotBodyPair2D((GodotBody2D *)A, p_subindex_A, (GodotBody2D *)B, p_subindex_B)); + return b; + } + + return nullptr; +} + +void GodotSpace2D::_broadphase_unpair(GodotCollisionObject2D *A, int p_subindex_A, GodotCollisionObject2D *B, int p_subindex_B, void *p_data, void *p_self) { + if (!p_data) { + return; + } + + GodotSpace2D *self = (GodotSpace2D *)p_self; + self->collision_pairs--; + GodotConstraint2D *c = (GodotConstraint2D *)p_data; + memdelete(c); +} + +const SelfList::List &GodotSpace2D::get_active_body_list() const { + return active_list; +} + +void GodotSpace2D::body_add_to_active_list(SelfList *p_body) { + active_list.add(p_body); +} + +void GodotSpace2D::body_remove_from_active_list(SelfList *p_body) { + active_list.remove(p_body); +} + +void GodotSpace2D::body_add_to_mass_properties_update_list(SelfList *p_body) { + mass_properties_update_list.add(p_body); +} + +void GodotSpace2D::body_remove_from_mass_properties_update_list(SelfList *p_body) { + mass_properties_update_list.remove(p_body); +} + +GodotBroadPhase2D *GodotSpace2D::get_broadphase() { + return broadphase; +} + +void GodotSpace2D::add_object(GodotCollisionObject2D *p_object) { + ERR_FAIL_COND(objects.has(p_object)); + objects.insert(p_object); +} + +void GodotSpace2D::remove_object(GodotCollisionObject2D *p_object) { + ERR_FAIL_COND(!objects.has(p_object)); + objects.erase(p_object); +} + +const Set &GodotSpace2D::get_objects() const { + return objects; +} + +void GodotSpace2D::body_add_to_state_query_list(SelfList *p_body) { + state_query_list.add(p_body); +} + +void GodotSpace2D::body_remove_from_state_query_list(SelfList *p_body) { + state_query_list.remove(p_body); +} + +void GodotSpace2D::area_add_to_monitor_query_list(SelfList *p_area) { + monitor_query_list.add(p_area); +} + +void GodotSpace2D::area_remove_from_monitor_query_list(SelfList *p_area) { + monitor_query_list.remove(p_area); +} + +void GodotSpace2D::area_add_to_moved_list(SelfList *p_area) { + area_moved_list.add(p_area); +} + +void GodotSpace2D::area_remove_from_moved_list(SelfList *p_area) { + area_moved_list.remove(p_area); +} + +const SelfList::List &GodotSpace2D::get_moved_area_list() const { + return area_moved_list; +} + +void GodotSpace2D::call_queries() { + while (state_query_list.first()) { + GodotBody2D *b = state_query_list.first()->self(); + state_query_list.remove(state_query_list.first()); + b->call_queries(); + } + + while (monitor_query_list.first()) { + GodotArea2D *a = monitor_query_list.first()->self(); + monitor_query_list.remove(monitor_query_list.first()); + a->call_queries(); + } +} + +void GodotSpace2D::setup() { + contact_debug_count = 0; + + while (mass_properties_update_list.first()) { + mass_properties_update_list.first()->self()->update_mass_properties(); + mass_properties_update_list.remove(mass_properties_update_list.first()); + } +} + +void GodotSpace2D::update() { + broadphase->update(); +} + +void GodotSpace2D::set_param(PhysicsServer2D::SpaceParameter p_param, real_t p_value) { + switch (p_param) { + case PhysicsServer2D::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: + contact_recycle_radius = p_value; + break; + case PhysicsServer2D::SPACE_PARAM_CONTACT_MAX_SEPARATION: + contact_max_separation = p_value; + break; + case PhysicsServer2D::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: + contact_max_allowed_penetration = p_value; + break; + case PhysicsServer2D::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD: + body_linear_velocity_sleep_threshold = p_value; + break; + case PhysicsServer2D::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD: + body_angular_velocity_sleep_threshold = p_value; + break; + case PhysicsServer2D::SPACE_PARAM_BODY_TIME_TO_SLEEP: + body_time_to_sleep = p_value; + break; + case PhysicsServer2D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: + constraint_bias = p_value; + break; + } +} + +real_t GodotSpace2D::get_param(PhysicsServer2D::SpaceParameter p_param) const { + switch (p_param) { + case PhysicsServer2D::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: + return contact_recycle_radius; + case PhysicsServer2D::SPACE_PARAM_CONTACT_MAX_SEPARATION: + return contact_max_separation; + case PhysicsServer2D::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: + return contact_max_allowed_penetration; + case PhysicsServer2D::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD: + return body_linear_velocity_sleep_threshold; + case PhysicsServer2D::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD: + return body_angular_velocity_sleep_threshold; + case PhysicsServer2D::SPACE_PARAM_BODY_TIME_TO_SLEEP: + return body_time_to_sleep; + case PhysicsServer2D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: + return constraint_bias; + } + return 0; +} + +void GodotSpace2D::lock() { + locked = true; +} + +void GodotSpace2D::unlock() { + locked = false; +} + +bool GodotSpace2D::is_locked() const { + return locked; +} + +GodotPhysicsDirectSpaceState2D *GodotSpace2D::get_direct_state() { + return direct_access; +} + +GodotSpace2D::GodotSpace2D() { + body_linear_velocity_sleep_threshold = GLOBAL_DEF("physics/2d/sleep_threshold_linear", 2.0); + body_angular_velocity_sleep_threshold = GLOBAL_DEF("physics/2d/sleep_threshold_angular", Math::deg2rad(8.0)); + body_time_to_sleep = GLOBAL_DEF("physics/2d/time_before_sleep", 0.5); + ProjectSettings::get_singleton()->set_custom_property_info("physics/2d/time_before_sleep", PropertyInfo(Variant::FLOAT, "physics/2d/time_before_sleep", PROPERTY_HINT_RANGE, "0,5,0.01,or_greater")); + + broadphase = GodotBroadPhase2D::create_func(); + broadphase->set_pair_callback(_broadphase_pair, this); + broadphase->set_unpair_callback(_broadphase_unpair, this); + + direct_access = memnew(GodotPhysicsDirectSpaceState2D); + direct_access->space = this; +} + +GodotSpace2D::~GodotSpace2D() { + memdelete(broadphase); + memdelete(direct_access); +} diff --git a/servers/physics_2d/godot_space_2d.h b/servers/physics_2d/godot_space_2d.h new file mode 100644 index 0000000000..97e2928a9d --- /dev/null +++ b/servers/physics_2d/godot_space_2d.h @@ -0,0 +1,212 @@ +/*************************************************************************/ +/* godot_space_2d.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 GODOT_SPACE_2D_H +#define GODOT_SPACE_2D_H + +#include "godot_area_2d.h" +#include "godot_area_pair_2d.h" +#include "godot_body_2d.h" +#include "godot_body_pair_2d.h" +#include "godot_broad_phase_2d.h" +#include "godot_collision_object_2d.h" + +#include "core/config/project_settings.h" +#include "core/templates/hash_map.h" +#include "core/typedefs.h" + +class GodotPhysicsDirectSpaceState2D : public PhysicsDirectSpaceState2D { + GDCLASS(GodotPhysicsDirectSpaceState2D, PhysicsDirectSpaceState2D); + + int _intersect_point_impl(const Vector2 &p_point, ShapeResult *r_results, int p_result_max, const Set &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, bool p_pick_point, bool p_filter_by_canvas = false, ObjectID p_canvas_instance_id = ObjectID()); + +public: + GodotSpace2D *space = nullptr; + + virtual int intersect_point(const Vector2 &p_point, ShapeResult *r_results, int p_result_max, const Set &p_exclude = Set(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, bool p_pick_point = false) override; + virtual int intersect_point_on_canvas(const Vector2 &p_point, ObjectID p_canvas_instance_id, ShapeResult *r_results, int p_result_max, const Set &p_exclude = Set(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, bool p_pick_point = false) override; + virtual bool intersect_ray(const Vector2 &p_from, const Vector2 &p_to, RayResult &r_result, const Set &p_exclude = Set(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; + virtual int intersect_shape(const RID &p_shape, const Transform2D &p_xform, const Vector2 &p_motion, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set &p_exclude = Set(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; + virtual bool cast_motion(const RID &p_shape, const Transform2D &p_xform, const Vector2 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set &p_exclude = Set(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; + virtual bool collide_shape(RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, real_t p_margin, Vector2 *r_results, int p_result_max, int &r_result_count, const Set &p_exclude = Set(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; + virtual bool rest_info(RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, real_t p_margin, ShapeRestInfo *r_info, const Set &p_exclude = Set(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; + + GodotPhysicsDirectSpaceState2D() {} +}; + +class GodotSpace2D { +public: + enum ElapsedTime { + ELAPSED_TIME_INTEGRATE_FORCES, + ELAPSED_TIME_GENERATE_ISLANDS, + ELAPSED_TIME_SETUP_CONSTRAINTS, + ELAPSED_TIME_SOLVE_CONSTRAINTS, + ELAPSED_TIME_INTEGRATE_VELOCITIES, + ELAPSED_TIME_MAX + + }; + +private: + struct ExcludedShapeSW { + GodotShape2D *local_shape = nullptr; + const GodotCollisionObject2D *against_object = nullptr; + int against_shape_index = 0; + }; + + uint64_t elapsed_time[ELAPSED_TIME_MAX] = {}; + + GodotPhysicsDirectSpaceState2D *direct_access = nullptr; + RID self; + + GodotBroadPhase2D *broadphase; + SelfList::List active_list; + SelfList::List mass_properties_update_list; + SelfList::List state_query_list; + SelfList::List monitor_query_list; + SelfList::List area_moved_list; + + static void *_broadphase_pair(GodotCollisionObject2D *A, int p_subindex_A, GodotCollisionObject2D *B, int p_subindex_B, void *p_self); + static void _broadphase_unpair(GodotCollisionObject2D *A, int p_subindex_A, GodotCollisionObject2D *B, int p_subindex_B, void *p_data, void *p_self); + + Set objects; + + GodotArea2D *area = nullptr; + + real_t contact_recycle_radius = 1.0; + real_t contact_max_separation = 1.5; + real_t contact_max_allowed_penetration = 0.3; + real_t constraint_bias = 0.2; + + enum { + INTERSECTION_QUERY_MAX = 2048 + }; + + GodotCollisionObject2D *intersection_query_results[INTERSECTION_QUERY_MAX]; + int intersection_query_subindex_results[INTERSECTION_QUERY_MAX]; + + real_t body_linear_velocity_sleep_threshold = 0.0; + real_t body_angular_velocity_sleep_threshold = 0.0; + real_t body_time_to_sleep = 0.0; + + bool locked = false; + + real_t last_step = 0.001; + + int island_count = 0; + int active_objects = 0; + int collision_pairs = 0; + + int _cull_aabb_for_body(GodotBody2D *p_body, const Rect2 &p_aabb); + + Vector contact_debug; + int contact_debug_count = 0; + + friend class GodotPhysicsDirectSpaceState2D; + +public: + _FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; } + _FORCE_INLINE_ RID get_self() const { return self; } + + void set_default_area(GodotArea2D *p_area) { area = p_area; } + GodotArea2D *get_default_area() const { return area; } + + const SelfList::List &get_active_body_list() const; + void body_add_to_active_list(SelfList *p_body); + void body_remove_from_active_list(SelfList *p_body); + void body_add_to_mass_properties_update_list(SelfList *p_body); + void body_remove_from_mass_properties_update_list(SelfList *p_body); + void area_add_to_moved_list(SelfList *p_area); + void area_remove_from_moved_list(SelfList *p_area); + const SelfList::List &get_moved_area_list() const; + + void body_add_to_state_query_list(SelfList *p_body); + void body_remove_from_state_query_list(SelfList *p_body); + + void area_add_to_monitor_query_list(SelfList *p_area); + void area_remove_from_monitor_query_list(SelfList *p_area); + + GodotBroadPhase2D *get_broadphase(); + + void add_object(GodotCollisionObject2D *p_object); + void remove_object(GodotCollisionObject2D *p_object); + const Set &get_objects() const; + + _FORCE_INLINE_ real_t get_contact_recycle_radius() const { return contact_recycle_radius; } + _FORCE_INLINE_ real_t get_contact_max_separation() const { return contact_max_separation; } + _FORCE_INLINE_ real_t get_contact_max_allowed_penetration() const { return contact_max_allowed_penetration; } + _FORCE_INLINE_ real_t get_constraint_bias() const { return constraint_bias; } + _FORCE_INLINE_ real_t get_body_linear_velocity_sleep_threshold() const { return body_linear_velocity_sleep_threshold; } + _FORCE_INLINE_ real_t get_body_angular_velocity_sleep_threshold() const { return body_angular_velocity_sleep_threshold; } + _FORCE_INLINE_ real_t get_body_time_to_sleep() const { return body_time_to_sleep; } + + void update(); + void setup(); + void call_queries(); + + bool is_locked() const; + void lock(); + void unlock(); + + real_t get_last_step() const { return last_step; } + void set_last_step(real_t p_step) { last_step = p_step; } + + void set_param(PhysicsServer2D::SpaceParameter p_param, real_t p_value); + real_t get_param(PhysicsServer2D::SpaceParameter p_param) const; + + void set_island_count(int p_island_count) { island_count = p_island_count; } + int get_island_count() const { return island_count; } + + void set_active_objects(int p_active_objects) { active_objects = p_active_objects; } + int get_active_objects() const { return active_objects; } + + int get_collision_pairs() const { return collision_pairs; } + + bool test_body_motion(GodotBody2D *p_body, const PhysicsServer2D::MotionParameters &p_parameters, PhysicsServer2D::MotionResult *r_result); + + void set_debug_contacts(int p_amount) { contact_debug.resize(p_amount); } + _FORCE_INLINE_ bool is_debugging_contacts() const { return !contact_debug.is_empty(); } + _FORCE_INLINE_ void add_debug_contact(const Vector2 &p_contact) { + if (contact_debug_count < contact_debug.size()) { + contact_debug.write[contact_debug_count++] = p_contact; + } + } + _FORCE_INLINE_ Vector get_debug_contacts() { return contact_debug; } + _FORCE_INLINE_ int get_debug_contact_count() { return contact_debug_count; } + + GodotPhysicsDirectSpaceState2D *get_direct_state(); + + void set_elapsed_time(ElapsedTime p_time, uint64_t p_msec) { elapsed_time[p_time] = p_msec; } + uint64_t get_elapsed_time(ElapsedTime p_time) const { return elapsed_time[p_time]; } + + GodotSpace2D(); + ~GodotSpace2D(); +}; + +#endif // GODOT_SPACE_2D_H diff --git a/servers/physics_2d/godot_step_2d.cpp b/servers/physics_2d/godot_step_2d.cpp new file mode 100644 index 0000000000..3010315571 --- /dev/null +++ b/servers/physics_2d/godot_step_2d.cpp @@ -0,0 +1,308 @@ +/*************************************************************************/ +/* godot_step_2d.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 "godot_step_2d.h" + +#include "core/os/os.h" + +#define BODY_ISLAND_COUNT_RESERVE 128 +#define BODY_ISLAND_SIZE_RESERVE 512 +#define ISLAND_COUNT_RESERVE 128 +#define ISLAND_SIZE_RESERVE 512 +#define CONSTRAINT_COUNT_RESERVE 1024 + +void GodotStep2D::_populate_island(GodotBody2D *p_body, LocalVector &p_body_island, LocalVector &p_constraint_island) { + p_body->set_island_step(_step); + + if (p_body->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) { + // Only dynamic bodies are tested for activation. + p_body_island.push_back(p_body); + } + + for (const Pair &E : p_body->get_constraint_list()) { + GodotConstraint2D *constraint = (GodotConstraint2D *)E.first; + if (constraint->get_island_step() == _step) { + continue; // Already processed. + } + constraint->set_island_step(_step); + p_constraint_island.push_back(constraint); + all_constraints.push_back(constraint); + + for (int i = 0; i < constraint->get_body_count(); i++) { + if (i == E.second) { + continue; + } + GodotBody2D *other_body = constraint->get_body_ptr()[i]; + if (other_body->get_island_step() == _step) { + continue; // Already processed. + } + if (other_body->get_mode() == PhysicsServer2D::BODY_MODE_STATIC) { + continue; // Static bodies don't connect islands. + } + _populate_island(other_body, p_body_island, p_constraint_island); + } + } +} + +void GodotStep2D::_setup_contraint(uint32_t p_constraint_index, void *p_userdata) { + GodotConstraint2D *constraint = all_constraints[p_constraint_index]; + constraint->setup(delta); +} + +void GodotStep2D::_pre_solve_island(LocalVector &p_constraint_island) const { + uint32_t constraint_count = p_constraint_island.size(); + uint32_t valid_constraint_count = 0; + for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) { + GodotConstraint2D *constraint = p_constraint_island[constraint_index]; + if (p_constraint_island[constraint_index]->pre_solve(delta)) { + // Keep this constraint for solving. + p_constraint_island[valid_constraint_count++] = constraint; + } + } + p_constraint_island.resize(valid_constraint_count); +} + +void GodotStep2D::_solve_island(uint32_t p_island_index, void *p_userdata) const { + const LocalVector &constraint_island = constraint_islands[p_island_index]; + + for (int i = 0; i < iterations; i++) { + uint32_t constraint_count = constraint_island.size(); + for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) { + constraint_island[constraint_index]->solve(delta); + } + } +} + +void GodotStep2D::_check_suspend(LocalVector &p_body_island) const { + bool can_sleep = true; + + uint32_t body_count = p_body_island.size(); + for (uint32_t body_index = 0; body_index < body_count; ++body_index) { + GodotBody2D *body = p_body_island[body_index]; + + if (!body->sleep_test(delta)) { + can_sleep = false; + } + } + + // Put all to sleep or wake up everyone. + for (uint32_t body_index = 0; body_index < body_count; ++body_index) { + GodotBody2D *body = p_body_island[body_index]; + + bool active = body->is_active(); + + if (active == can_sleep) { + body->set_active(!can_sleep); + } + } +} + +void GodotStep2D::step(GodotSpace2D *p_space, real_t p_delta, int p_iterations) { + p_space->lock(); // can't access space during this + + p_space->setup(); //update inertias, etc + + p_space->set_last_step(p_delta); + + iterations = p_iterations; + delta = p_delta; + + const SelfList::List *body_list = &p_space->get_active_body_list(); + + /* INTEGRATE FORCES */ + + uint64_t profile_begtime = OS::get_singleton()->get_ticks_usec(); + uint64_t profile_endtime = 0; + + int active_count = 0; + + const SelfList *b = body_list->first(); + while (b) { + b->self()->integrate_forces(p_delta); + b = b->next(); + active_count++; + } + + p_space->set_active_objects(active_count); + + { //profile + profile_endtime = OS::get_singleton()->get_ticks_usec(); + p_space->set_elapsed_time(GodotSpace2D::ELAPSED_TIME_INTEGRATE_FORCES, profile_endtime - profile_begtime); + profile_begtime = profile_endtime; + } + + /* GENERATE CONSTRAINT ISLANDS FOR MOVING AREAS */ + + uint32_t island_count = 0; + + const SelfList::List &aml = p_space->get_moved_area_list(); + + while (aml.first()) { + for (const Set::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) { + GodotConstraint2D *constraint = E->get(); + if (constraint->get_island_step() == _step) { + continue; + } + constraint->set_island_step(_step); + + // Each constraint can be on a separate island for areas as there's no solving phase. + ++island_count; + if (constraint_islands.size() < island_count) { + constraint_islands.resize(island_count); + } + LocalVector &constraint_island = constraint_islands[island_count - 1]; + constraint_island.clear(); + + all_constraints.push_back(constraint); + constraint_island.push_back(constraint); + } + p_space->area_remove_from_moved_list((SelfList *)aml.first()); //faster to remove here + } + + /* GENERATE CONSTRAINT ISLANDS FOR ACTIVE RIGID BODIES */ + + b = body_list->first(); + + uint32_t body_island_count = 0; + + while (b) { + GodotBody2D *body = b->self(); + + if (body->get_island_step() != _step) { + ++body_island_count; + if (body_islands.size() < body_island_count) { + body_islands.resize(body_island_count); + } + LocalVector &body_island = body_islands[body_island_count - 1]; + body_island.clear(); + body_island.reserve(BODY_ISLAND_SIZE_RESERVE); + + ++island_count; + if (constraint_islands.size() < island_count) { + constraint_islands.resize(island_count); + } + LocalVector &constraint_island = constraint_islands[island_count - 1]; + constraint_island.clear(); + constraint_island.reserve(ISLAND_SIZE_RESERVE); + + _populate_island(body, body_island, constraint_island); + + if (body_island.is_empty()) { + --body_island_count; + } + + if (constraint_island.is_empty()) { + --island_count; + } + } + b = b->next(); + } + + p_space->set_island_count((int)island_count); + + { //profile + profile_endtime = OS::get_singleton()->get_ticks_usec(); + p_space->set_elapsed_time(GodotSpace2D::ELAPSED_TIME_GENERATE_ISLANDS, profile_endtime - profile_begtime); + profile_begtime = profile_endtime; + } + + /* SETUP CONSTRAINTS / PROCESS COLLISIONS */ + + uint32_t total_contraint_count = all_constraints.size(); + work_pool.do_work(total_contraint_count, this, &GodotStep2D::_setup_contraint, nullptr); + + { //profile + profile_endtime = OS::get_singleton()->get_ticks_usec(); + p_space->set_elapsed_time(GodotSpace2D::ELAPSED_TIME_SETUP_CONSTRAINTS, profile_endtime - profile_begtime); + profile_begtime = profile_endtime; + } + + /* PRE-SOLVE CONSTRAINT ISLANDS */ + + // Warning: This doesn't run on threads, because it involves thread-unsafe processing. + for (uint32_t island_index = 0; island_index < island_count; ++island_index) { + _pre_solve_island(constraint_islands[island_index]); + } + + /* SOLVE CONSTRAINT ISLANDS */ + + // Warning: _solve_island modifies the constraint islands for optimization purpose, + // their content is not reliable after these calls and shouldn't be used anymore. + if (island_count > 1) { + work_pool.do_work(island_count, this, &GodotStep2D::_solve_island, nullptr); + } else if (island_count > 0) { + _solve_island(0); + } + + { //profile + profile_endtime = OS::get_singleton()->get_ticks_usec(); + p_space->set_elapsed_time(GodotSpace2D::ELAPSED_TIME_SOLVE_CONSTRAINTS, profile_endtime - profile_begtime); + profile_begtime = profile_endtime; + } + + /* INTEGRATE VELOCITIES */ + + b = body_list->first(); + while (b) { + const SelfList *n = b->next(); + b->self()->integrate_velocities(p_delta); + b = n; // in case it shuts itself down + } + + /* SLEEP / WAKE UP ISLANDS */ + + for (uint32_t island_index = 0; island_index < body_island_count; ++island_index) { + _check_suspend(body_islands[island_index]); + } + + { //profile + profile_endtime = OS::get_singleton()->get_ticks_usec(); + p_space->set_elapsed_time(GodotSpace2D::ELAPSED_TIME_INTEGRATE_VELOCITIES, profile_endtime - profile_begtime); + //profile_begtime=profile_endtime; + } + + all_constraints.clear(); + + p_space->update(); + p_space->unlock(); + _step++; +} + +GodotStep2D::GodotStep2D() { + body_islands.reserve(BODY_ISLAND_COUNT_RESERVE); + constraint_islands.reserve(ISLAND_COUNT_RESERVE); + all_constraints.reserve(CONSTRAINT_COUNT_RESERVE); + + work_pool.init(); +} + +GodotStep2D::~GodotStep2D() { + work_pool.finish(); +} diff --git a/servers/physics_2d/godot_step_2d.h b/servers/physics_2d/godot_step_2d.h new file mode 100644 index 0000000000..efec243632 --- /dev/null +++ b/servers/physics_2d/godot_step_2d.h @@ -0,0 +1,63 @@ +/*************************************************************************/ +/* godot_step_2d.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 GODOT_STEP_2D_H +#define GODOT_STEP_2D_H + +#include "godot_space_2d.h" + +#include "core/templates/local_vector.h" +#include "core/templates/thread_work_pool.h" + +class GodotStep2D { + uint64_t _step = 1; + + int iterations = 0; + real_t delta = 0.0; + + ThreadWorkPool work_pool; + + LocalVector> body_islands; + LocalVector> constraint_islands; + LocalVector all_constraints; + + void _populate_island(GodotBody2D *p_body, LocalVector &p_body_island, LocalVector &p_constraint_island); + void _setup_contraint(uint32_t p_constraint_index, void *p_userdata = nullptr); + void _pre_solve_island(LocalVector &p_constraint_island) const; + void _solve_island(uint32_t p_island_index, void *p_userdata = nullptr) const; + void _check_suspend(LocalVector &p_body_island) const; + +public: + void step(GodotSpace2D *p_space, real_t p_delta, int p_iterations); + GodotStep2D(); + ~GodotStep2D(); +}; + +#endif // GODOT_STEP_2D_H diff --git a/servers/physics_2d/joints_2d_sw.cpp b/servers/physics_2d/joints_2d_sw.cpp deleted file mode 100644 index b46397b8e6..0000000000 --- a/servers/physics_2d/joints_2d_sw.cpp +++ /dev/null @@ -1,486 +0,0 @@ -/*************************************************************************/ -/* joints_2d_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/*************************************************************************/ - -#include "joints_2d_sw.h" - -#include "space_2d_sw.h" - -//based on chipmunk joint constraints - -/* Copyright (c) 2007 Scott Lembcke - * - * 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. - */ - -void Joint2DSW::copy_settings_from(Joint2DSW *p_joint) { - set_self(p_joint->get_self()); - set_max_force(p_joint->get_max_force()); - set_bias(p_joint->get_bias()); - set_max_bias(p_joint->get_max_bias()); - disable_collisions_between_bodies(p_joint->is_disabled_collisions_between_bodies()); -} - -static inline real_t k_scalar(Body2DSW *a, Body2DSW *b, const Vector2 &rA, const Vector2 &rB, const Vector2 &n) { - real_t value = 0.0; - - { - value += a->get_inv_mass(); - real_t rcn = (rA - a->get_center_of_mass()).cross(n); - value += a->get_inv_inertia() * rcn * rcn; - } - - if (b) { - value += b->get_inv_mass(); - real_t rcn = (rB - b->get_center_of_mass()).cross(n); - value += b->get_inv_inertia() * rcn * rcn; - } - - return value; -} - -static inline Vector2 -relative_velocity(Body2DSW *a, Body2DSW *b, Vector2 rA, Vector2 rB) { - Vector2 sum = a->get_linear_velocity() - (rA - a->get_center_of_mass()).orthogonal() * a->get_angular_velocity(); - if (b) { - return (b->get_linear_velocity() - (rB - b->get_center_of_mass()).orthogonal() * b->get_angular_velocity()) - sum; - } else { - return -sum; - } -} - -static inline real_t -normal_relative_velocity(Body2DSW *a, Body2DSW *b, Vector2 rA, Vector2 rB, Vector2 n) { - return relative_velocity(a, b, rA, rB).dot(n); -} - -bool PinJoint2DSW::setup(real_t p_step) { - dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); - dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); - - if (!dynamic_A && !dynamic_B) { - return false; - } - - Space2DSW *space = A->get_space(); - ERR_FAIL_COND_V(!space, false); - - rA = A->get_transform().basis_xform(anchor_A); - rB = B ? B->get_transform().basis_xform(anchor_B) : anchor_B; - - real_t B_inv_mass = B ? B->get_inv_mass() : 0.0; - - Transform2D K1; - K1[0].x = A->get_inv_mass() + B_inv_mass; - K1[1].x = 0.0f; - K1[0].y = 0.0f; - K1[1].y = A->get_inv_mass() + B_inv_mass; - - Transform2D K2; - K2[0].x = A->get_inv_inertia() * rA.y * rA.y; - K2[1].x = -A->get_inv_inertia() * rA.x * rA.y; - K2[0].y = -A->get_inv_inertia() * rA.x * rA.y; - K2[1].y = A->get_inv_inertia() * rA.x * rA.x; - - Transform2D K; - K[0] = K1[0] + K2[0]; - K[1] = K1[1] + K2[1]; - - if (B) { - Transform2D K3; - K3[0].x = B->get_inv_inertia() * rB.y * rB.y; - K3[1].x = -B->get_inv_inertia() * rB.x * rB.y; - K3[0].y = -B->get_inv_inertia() * rB.x * rB.y; - K3[1].y = B->get_inv_inertia() * rB.x * rB.x; - - K[0] += K3[0]; - K[1] += K3[1]; - } - - K[0].x += softness; - K[1].y += softness; - - M = K.affine_inverse(); - - Vector2 gA = rA + A->get_transform().get_origin(); - Vector2 gB = B ? rB + B->get_transform().get_origin() : rB; - - Vector2 delta = gB - gA; - - bias = delta * -(get_bias() == 0 ? space->get_constraint_bias() : get_bias()) * (1.0 / p_step); - - return true; -} - -inline Vector2 custom_cross(const Vector2 &p_vec, real_t p_other) { - return Vector2(p_other * p_vec.y, -p_other * p_vec.x); -} - -bool PinJoint2DSW::pre_solve(real_t p_step) { - // Apply accumulated impulse. - if (dynamic_A) { - A->apply_impulse(-P, rA); - } - if (B && dynamic_B) { - B->apply_impulse(P, rB); - } - - return true; -} - -void PinJoint2DSW::solve(real_t p_step) { - // compute relative velocity - Vector2 vA = A->get_linear_velocity() - custom_cross(rA - A->get_center_of_mass(), A->get_angular_velocity()); - - Vector2 rel_vel; - if (B) { - rel_vel = B->get_linear_velocity() - custom_cross(rB - B->get_center_of_mass(), B->get_angular_velocity()) - vA; - } else { - rel_vel = -vA; - } - - Vector2 impulse = M.basis_xform(bias - rel_vel - Vector2(softness, softness) * P); - - if (dynamic_A) { - A->apply_impulse(-impulse, rA); - } - if (B && dynamic_B) { - B->apply_impulse(impulse, rB); - } - - P += impulse; -} - -void PinJoint2DSW::set_param(PhysicsServer2D::PinJointParam p_param, real_t p_value) { - if (p_param == PhysicsServer2D::PIN_JOINT_SOFTNESS) { - softness = p_value; - } -} - -real_t PinJoint2DSW::get_param(PhysicsServer2D::PinJointParam p_param) const { - if (p_param == PhysicsServer2D::PIN_JOINT_SOFTNESS) { - return softness; - } - ERR_FAIL_V(0); -} - -PinJoint2DSW::PinJoint2DSW(const Vector2 &p_pos, Body2DSW *p_body_a, Body2DSW *p_body_b) : - Joint2DSW(_arr, p_body_b ? 2 : 1) { - A = p_body_a; - B = p_body_b; - anchor_A = p_body_a->get_inv_transform().xform(p_pos); - anchor_B = p_body_b ? p_body_b->get_inv_transform().xform(p_pos) : p_pos; - - p_body_a->add_constraint(this, 0); - if (p_body_b) { - p_body_b->add_constraint(this, 1); - } -} - -////////////////////////////////////////////// -////////////////////////////////////////////// -////////////////////////////////////////////// - -static inline void -k_tensor(Body2DSW *a, Body2DSW *b, Vector2 r1, Vector2 r2, Vector2 *k1, Vector2 *k2) { - // calculate mass matrix - // If I wasn't lazy and wrote a proper matrix class, this wouldn't be so gross... - real_t k11, k12, k21, k22; - real_t m_sum = a->get_inv_mass() + b->get_inv_mass(); - - // start with I*m_sum - k11 = m_sum; - k12 = 0.0f; - k21 = 0.0f; - k22 = m_sum; - - r1 -= a->get_center_of_mass(); - r2 -= b->get_center_of_mass(); - - // add the influence from r1 - real_t a_i_inv = a->get_inv_inertia(); - real_t r1xsq = r1.x * r1.x * a_i_inv; - real_t r1ysq = r1.y * r1.y * a_i_inv; - real_t r1nxy = -r1.x * r1.y * a_i_inv; - k11 += r1ysq; - k12 += r1nxy; - k21 += r1nxy; - k22 += r1xsq; - - // add the influnce from r2 - real_t b_i_inv = b->get_inv_inertia(); - real_t r2xsq = r2.x * r2.x * b_i_inv; - real_t r2ysq = r2.y * r2.y * b_i_inv; - real_t r2nxy = -r2.x * r2.y * b_i_inv; - k11 += r2ysq; - k12 += r2nxy; - k21 += r2nxy; - k22 += r2xsq; - - // invert - real_t determinant = k11 * k22 - k12 * k21; - ERR_FAIL_COND(determinant == 0.0); - - real_t det_inv = 1.0f / determinant; - *k1 = Vector2(k22 * det_inv, -k12 * det_inv); - *k2 = Vector2(-k21 * det_inv, k11 * det_inv); -} - -static _FORCE_INLINE_ Vector2 -mult_k(const Vector2 &vr, const Vector2 &k1, const Vector2 &k2) { - return Vector2(vr.dot(k1), vr.dot(k2)); -} - -bool GrooveJoint2DSW::setup(real_t p_step) { - dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); - dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); - - if (!dynamic_A && !dynamic_B) { - return false; - } - - Space2DSW *space = A->get_space(); - ERR_FAIL_COND_V(!space, false); - - // calculate endpoints in worldspace - Vector2 ta = A->get_transform().xform(A_groove_1); - Vector2 tb = A->get_transform().xform(A_groove_2); - - // calculate axis - Vector2 n = -(tb - ta).orthogonal().normalized(); - real_t d = ta.dot(n); - - xf_normal = n; - rB = B->get_transform().basis_xform(B_anchor); - - // calculate tangential distance along the axis of rB - real_t td = (B->get_transform().get_origin() + rB).cross(n); - // calculate clamping factor and rB - if (td <= ta.cross(n)) { - clamp = 1.0f; - rA = ta - A->get_transform().get_origin(); - } else if (td >= tb.cross(n)) { - clamp = -1.0f; - rA = tb - A->get_transform().get_origin(); - } else { - clamp = 0.0f; - //joint->r1 = cpvsub(cpvadd(cpvmult(cpvperp(n), -td), cpvmult(n, d)), a->p); - rA = ((-n.orthogonal() * -td) + n * d) - A->get_transform().get_origin(); - } - - // Calculate mass tensor - k_tensor(A, B, rA, rB, &k1, &k2); - - // compute max impulse - jn_max = get_max_force() * p_step; - - // calculate bias velocity - //cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1)); - //joint->bias = cpvclamp(cpvmult(delta, -joint->constraint.biasCoef*dt_inv), joint->constraint.maxBias); - - Vector2 delta = (B->get_transform().get_origin() + rB) - (A->get_transform().get_origin() + rA); - - real_t _b = get_bias(); - gbias = (delta * -(_b == 0 ? space->get_constraint_bias() : _b) * (1.0 / p_step)).limit_length(get_max_bias()); - - correct = true; - return true; -} - -bool GrooveJoint2DSW::pre_solve(real_t p_step) { - // Apply accumulated impulse. - if (dynamic_A) { - A->apply_impulse(-jn_acc, rA); - } - if (dynamic_B) { - B->apply_impulse(jn_acc, rB); - } - - return true; -} - -void GrooveJoint2DSW::solve(real_t p_step) { - // compute impulse - Vector2 vr = relative_velocity(A, B, rA, rB); - - Vector2 j = mult_k(gbias - vr, k1, k2); - Vector2 jOld = jn_acc; - j += jOld; - - jn_acc = (((clamp * j.cross(xf_normal)) > 0) ? j : j.project(xf_normal)).limit_length(jn_max); - - j = jn_acc - jOld; - - if (dynamic_A) { - A->apply_impulse(-j, rA); - } - if (dynamic_B) { - B->apply_impulse(j, rB); - } -} - -GrooveJoint2DSW::GrooveJoint2DSW(const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, Body2DSW *p_body_a, Body2DSW *p_body_b) : - Joint2DSW(_arr, 2) { - A = p_body_a; - B = p_body_b; - - A_groove_1 = A->get_inv_transform().xform(p_a_groove1); - A_groove_2 = A->get_inv_transform().xform(p_a_groove2); - B_anchor = B->get_inv_transform().xform(p_b_anchor); - A_groove_normal = -(A_groove_2 - A_groove_1).normalized().orthogonal(); - - A->add_constraint(this, 0); - B->add_constraint(this, 1); -} - -////////////////////////////////////////////// -////////////////////////////////////////////// -////////////////////////////////////////////// - -bool DampedSpringJoint2DSW::setup(real_t p_step) { - dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); - dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); - - if (!dynamic_A && !dynamic_B) { - return false; - } - - rA = A->get_transform().basis_xform(anchor_A); - rB = B->get_transform().basis_xform(anchor_B); - - Vector2 delta = (B->get_transform().get_origin() + rB) - (A->get_transform().get_origin() + rA); - real_t dist = delta.length(); - - if (dist) { - n = delta / dist; - } else { - n = Vector2(); - } - - real_t k = k_scalar(A, B, rA, rB, n); - n_mass = 1.0f / k; - - target_vrn = 0.0f; - v_coef = 1.0f - Math::exp(-damping * (p_step)*k); - - // Calculate spring force. - real_t f_spring = (rest_length - dist) * stiffness; - j = n * f_spring * (p_step); - - return true; -} - -bool DampedSpringJoint2DSW::pre_solve(real_t p_step) { - // Apply spring force. - if (dynamic_A) { - A->apply_impulse(-j, rA); - } - if (dynamic_B) { - B->apply_impulse(j, rB); - } - - return true; -} - -void DampedSpringJoint2DSW::solve(real_t p_step) { - // compute relative velocity - real_t vrn = normal_relative_velocity(A, B, rA, rB, n) - target_vrn; - - // compute velocity loss from drag - // not 100% certain this is derived correctly, though it makes sense - real_t v_damp = -vrn * v_coef; - target_vrn = vrn + v_damp; - Vector2 j = n * v_damp * n_mass; - - if (dynamic_A) { - A->apply_impulse(-j, rA); - } - if (dynamic_B) { - B->apply_impulse(j, rB); - } -} - -void DampedSpringJoint2DSW::set_param(PhysicsServer2D::DampedSpringParam p_param, real_t p_value) { - switch (p_param) { - case PhysicsServer2D::DAMPED_SPRING_REST_LENGTH: { - rest_length = p_value; - } break; - case PhysicsServer2D::DAMPED_SPRING_DAMPING: { - damping = p_value; - } break; - case PhysicsServer2D::DAMPED_SPRING_STIFFNESS: { - stiffness = p_value; - } break; - } -} - -real_t DampedSpringJoint2DSW::get_param(PhysicsServer2D::DampedSpringParam p_param) const { - switch (p_param) { - case PhysicsServer2D::DAMPED_SPRING_REST_LENGTH: { - return rest_length; - } break; - case PhysicsServer2D::DAMPED_SPRING_DAMPING: { - return damping; - } break; - case PhysicsServer2D::DAMPED_SPRING_STIFFNESS: { - return stiffness; - } break; - } - - ERR_FAIL_V(0); -} - -DampedSpringJoint2DSW::DampedSpringJoint2DSW(const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, Body2DSW *p_body_a, Body2DSW *p_body_b) : - Joint2DSW(_arr, 2) { - A = p_body_a; - B = p_body_b; - anchor_A = A->get_inv_transform().xform(p_anchor_a); - anchor_B = B->get_inv_transform().xform(p_anchor_b); - - rest_length = p_anchor_a.distance_to(p_anchor_b); - - A->add_constraint(this, 0); - B->add_constraint(this, 1); -} diff --git a/servers/physics_2d/joints_2d_sw.h b/servers/physics_2d/joints_2d_sw.h deleted file mode 100644 index e2a7c0c91e..0000000000 --- a/servers/physics_2d/joints_2d_sw.h +++ /dev/null @@ -1,178 +0,0 @@ -/*************************************************************************/ -/* joints_2d_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/*************************************************************************/ - -#ifndef JOINTS_2D_SW_H -#define JOINTS_2D_SW_H - -#include "body_2d_sw.h" -#include "constraint_2d_sw.h" - -class Joint2DSW : public Constraint2DSW { - real_t bias = 0; - real_t max_bias = 3.40282e+38; - real_t max_force = 3.40282e+38; - -protected: - bool dynamic_A = false; - bool dynamic_B = false; - -public: - _FORCE_INLINE_ void set_max_force(real_t p_force) { max_force = p_force; } - _FORCE_INLINE_ real_t get_max_force() const { return max_force; } - - _FORCE_INLINE_ void set_bias(real_t p_bias) { bias = p_bias; } - _FORCE_INLINE_ real_t get_bias() const { return bias; } - - _FORCE_INLINE_ void set_max_bias(real_t p_bias) { max_bias = p_bias; } - _FORCE_INLINE_ real_t get_max_bias() const { return max_bias; } - - virtual bool setup(real_t p_step) override { return false; } - virtual bool pre_solve(real_t p_step) override { return false; } - virtual void solve(real_t p_step) override {} - - void copy_settings_from(Joint2DSW *p_joint); - - virtual PhysicsServer2D::JointType get_type() const { return PhysicsServer2D::JOINT_TYPE_MAX; } - Joint2DSW(Body2DSW **p_body_ptr = nullptr, int p_body_count = 0) : - Constraint2DSW(p_body_ptr, p_body_count) {} - - virtual ~Joint2DSW() { - for (int i = 0; i < get_body_count(); i++) { - Body2DSW *body = get_body_ptr()[i]; - if (body) { - body->remove_constraint(this, i); - } - } - }; -}; - -class PinJoint2DSW : public Joint2DSW { - union { - struct { - Body2DSW *A; - Body2DSW *B; - }; - - Body2DSW *_arr[2] = { nullptr, nullptr }; - }; - - Transform2D M; - Vector2 rA, rB; - Vector2 anchor_A; - Vector2 anchor_B; - Vector2 bias; - Vector2 P; - real_t softness = 0.0; - -public: - virtual PhysicsServer2D::JointType get_type() const override { return PhysicsServer2D::JOINT_TYPE_PIN; } - - virtual bool setup(real_t p_step) override; - virtual bool pre_solve(real_t p_step) override; - virtual void solve(real_t p_step) override; - - void set_param(PhysicsServer2D::PinJointParam p_param, real_t p_value); - real_t get_param(PhysicsServer2D::PinJointParam p_param) const; - - PinJoint2DSW(const Vector2 &p_pos, Body2DSW *p_body_a, Body2DSW *p_body_b = nullptr); -}; - -class GrooveJoint2DSW : public Joint2DSW { - union { - struct { - Body2DSW *A; - Body2DSW *B; - }; - - Body2DSW *_arr[2] = { nullptr, nullptr }; - }; - - Vector2 A_groove_1; - Vector2 A_groove_2; - Vector2 A_groove_normal; - Vector2 B_anchor; - Vector2 jn_acc; - Vector2 gbias; - real_t jn_max = 0.0; - real_t clamp = 0.0; - Vector2 xf_normal; - Vector2 rA, rB; - Vector2 k1, k2; - - bool correct = false; - -public: - virtual PhysicsServer2D::JointType get_type() const override { return PhysicsServer2D::JOINT_TYPE_GROOVE; } - - virtual bool setup(real_t p_step) override; - virtual bool pre_solve(real_t p_step) override; - virtual void solve(real_t p_step) override; - - GrooveJoint2DSW(const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, Body2DSW *p_body_a, Body2DSW *p_body_b); -}; - -class DampedSpringJoint2DSW : public Joint2DSW { - union { - struct { - Body2DSW *A; - Body2DSW *B; - }; - - Body2DSW *_arr[2] = { nullptr, nullptr }; - }; - - Vector2 anchor_A; - Vector2 anchor_B; - - real_t rest_length = 0.0; - real_t damping = 1.5; - real_t stiffness = 20.0; - - Vector2 rA, rB; - Vector2 n; - Vector2 j; - real_t n_mass = 0.0; - real_t target_vrn = 0.0; - real_t v_coef = 0.0; - -public: - virtual PhysicsServer2D::JointType get_type() const override { return PhysicsServer2D::JOINT_TYPE_DAMPED_SPRING; } - - virtual bool setup(real_t p_step) override; - virtual bool pre_solve(real_t p_step) override; - virtual void solve(real_t p_step) override; - - void set_param(PhysicsServer2D::DampedSpringParam p_param, real_t p_value); - real_t get_param(PhysicsServer2D::DampedSpringParam p_param) const; - - DampedSpringJoint2DSW(const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, Body2DSW *p_body_a, Body2DSW *p_body_b); -}; - -#endif // JOINTS_2D_SW_H diff --git a/servers/physics_2d/physics_server_2d_sw.cpp b/servers/physics_2d/physics_server_2d_sw.cpp deleted file mode 100644 index c2205e33b0..0000000000 --- a/servers/physics_2d/physics_server_2d_sw.cpp +++ /dev/null @@ -1,1349 +0,0 @@ -/*************************************************************************/ -/* physics_server_2d_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/*************************************************************************/ - -#include "physics_server_2d_sw.h" - -#include "body_direct_state_2d_sw.h" -#include "broad_phase_2d_bvh.h" -#include "collision_solver_2d_sw.h" -#include "core/config/project_settings.h" -#include "core/debugger/engine_debugger.h" -#include "core/os/os.h" - -#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 PhysicsServer2DSW::_shape_create(ShapeType p_shape) { - Shape2DSW *shape = nullptr; - switch (p_shape) { - case SHAPE_WORLD_BOUNDARY: { - shape = memnew(WorldBoundaryShape2DSW); - } break; - case SHAPE_SEPARATION_RAY: { - shape = memnew(SeparationRayShape2DSW); - } break; - case SHAPE_SEGMENT: { - shape = memnew(SegmentShape2DSW); - } break; - case SHAPE_CIRCLE: { - shape = memnew(CircleShape2DSW); - } break; - case SHAPE_RECTANGLE: { - shape = memnew(RectangleShape2DSW); - } break; - case SHAPE_CAPSULE: { - shape = memnew(CapsuleShape2DSW); - } break; - case SHAPE_CONVEX_POLYGON: { - shape = memnew(ConvexPolygonShape2DSW); - } break; - case SHAPE_CONCAVE_POLYGON: { - shape = memnew(ConcavePolygonShape2DSW); - } break; - case SHAPE_CUSTOM: { - ERR_FAIL_V(RID()); - - } break; - } - - RID id = shape_owner.make_rid(shape); - shape->set_self(id); - - return id; -} - -RID PhysicsServer2DSW::world_boundary_shape_create() { - return _shape_create(SHAPE_WORLD_BOUNDARY); -} - -RID PhysicsServer2DSW::separation_ray_shape_create() { - return _shape_create(SHAPE_SEPARATION_RAY); -} - -RID PhysicsServer2DSW::segment_shape_create() { - return _shape_create(SHAPE_SEGMENT); -} - -RID PhysicsServer2DSW::circle_shape_create() { - return _shape_create(SHAPE_CIRCLE); -} - -RID PhysicsServer2DSW::rectangle_shape_create() { - return _shape_create(SHAPE_RECTANGLE); -} - -RID PhysicsServer2DSW::capsule_shape_create() { - return _shape_create(SHAPE_CAPSULE); -} - -RID PhysicsServer2DSW::convex_polygon_shape_create() { - return _shape_create(SHAPE_CONVEX_POLYGON); -} - -RID PhysicsServer2DSW::concave_polygon_shape_create() { - return _shape_create(SHAPE_CONCAVE_POLYGON); -} - -void PhysicsServer2DSW::shape_set_data(RID p_shape, const Variant &p_data) { - Shape2DSW *shape = shape_owner.get_or_null(p_shape); - ERR_FAIL_COND(!shape); - shape->set_data(p_data); -}; - -void PhysicsServer2DSW::shape_set_custom_solver_bias(RID p_shape, real_t p_bias) { - Shape2DSW *shape = shape_owner.get_or_null(p_shape); - ERR_FAIL_COND(!shape); - shape->set_custom_bias(p_bias); -} - -PhysicsServer2D::ShapeType PhysicsServer2DSW::shape_get_type(RID p_shape) const { - const Shape2DSW *shape = shape_owner.get_or_null(p_shape); - ERR_FAIL_COND_V(!shape, SHAPE_CUSTOM); - return shape->get_type(); -}; - -Variant PhysicsServer2DSW::shape_get_data(RID p_shape) const { - const Shape2DSW *shape = shape_owner.get_or_null(p_shape); - ERR_FAIL_COND_V(!shape, Variant()); - ERR_FAIL_COND_V(!shape->is_configured(), Variant()); - return shape->get_data(); -}; - -real_t PhysicsServer2DSW::shape_get_custom_solver_bias(RID p_shape) const { - const Shape2DSW *shape = shape_owner.get_or_null(p_shape); - ERR_FAIL_COND_V(!shape, 0); - return shape->get_custom_bias(); -} - -void PhysicsServer2DSW::_shape_col_cbk(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_userdata) { - CollCbkData *cbk = (CollCbkData *)p_userdata; - - if (cbk->max == 0) { - return; - } - - Vector2 rel_dir = (p_point_A - p_point_B); - real_t rel_length2 = rel_dir.length_squared(); - if (cbk->valid_dir != Vector2()) { - if (cbk->valid_depth < 10e20) { - if (rel_length2 > cbk->valid_depth * cbk->valid_depth || - (rel_length2 > CMP_EPSILON && cbk->valid_dir.dot(rel_dir.normalized()) < CMP_EPSILON)) { - cbk->invalid_by_dir++; - return; - } - } else { - if (rel_length2 > 0 && cbk->valid_dir.dot(rel_dir.normalized()) < CMP_EPSILON) { - return; - } - } - } - - if (cbk->amount == cbk->max) { - //find least deep - real_t min_depth = 1e20; - int min_depth_idx = 0; - for (int i = 0; i < cbk->amount; i++) { - real_t d = cbk->ptr[i * 2 + 0].distance_squared_to(cbk->ptr[i * 2 + 1]); - if (d < min_depth) { - min_depth = d; - min_depth_idx = i; - } - } - - if (rel_length2 < min_depth) { - return; - } - cbk->ptr[min_depth_idx * 2 + 0] = p_point_A; - cbk->ptr[min_depth_idx * 2 + 1] = p_point_B; - cbk->passed++; - - } else { - cbk->ptr[cbk->amount * 2 + 0] = p_point_A; - cbk->ptr[cbk->amount * 2 + 1] = p_point_B; - cbk->amount++; - cbk->passed++; - } -} - -bool PhysicsServer2DSW::shape_collide(RID p_shape_A, const Transform2D &p_xform_A, const Vector2 &p_motion_A, RID p_shape_B, const Transform2D &p_xform_B, const Vector2 &p_motion_B, Vector2 *r_results, int p_result_max, int &r_result_count) { - Shape2DSW *shape_A = shape_owner.get_or_null(p_shape_A); - ERR_FAIL_COND_V(!shape_A, false); - Shape2DSW *shape_B = shape_owner.get_or_null(p_shape_B); - ERR_FAIL_COND_V(!shape_B, false); - - if (p_result_max == 0) { - return CollisionSolver2DSW::solve(shape_A, p_xform_A, p_motion_A, shape_B, p_xform_B, p_motion_B, nullptr, nullptr); - } - - CollCbkData cbk; - cbk.max = p_result_max; - cbk.amount = 0; - cbk.passed = 0; - cbk.ptr = r_results; - - bool res = CollisionSolver2DSW::solve(shape_A, p_xform_A, p_motion_A, shape_B, p_xform_B, p_motion_B, _shape_col_cbk, &cbk); - r_result_count = cbk.amount; - return res; -} - -RID PhysicsServer2DSW::space_create() { - Space2DSW *space = memnew(Space2DSW); - RID id = space_owner.make_rid(space); - space->set_self(id); - RID area_id = area_create(); - Area2DSW *area = area_owner.get_or_null(area_id); - ERR_FAIL_COND_V(!area, RID()); - space->set_default_area(area); - area->set_space(space); - area->set_priority(-1); - - return id; -}; - -void PhysicsServer2DSW::space_set_active(RID p_space, bool p_active) { - Space2DSW *space = space_owner.get_or_null(p_space); - ERR_FAIL_COND(!space); - if (p_active) { - active_spaces.insert(space); - } else { - active_spaces.erase(space); - } -} - -bool PhysicsServer2DSW::space_is_active(RID p_space) const { - const Space2DSW *space = space_owner.get_or_null(p_space); - ERR_FAIL_COND_V(!space, false); - - return active_spaces.has(space); -} - -void PhysicsServer2DSW::space_set_param(RID p_space, SpaceParameter p_param, real_t p_value) { - Space2DSW *space = space_owner.get_or_null(p_space); - ERR_FAIL_COND(!space); - - space->set_param(p_param, p_value); -} - -real_t PhysicsServer2DSW::space_get_param(RID p_space, SpaceParameter p_param) const { - const Space2DSW *space = space_owner.get_or_null(p_space); - ERR_FAIL_COND_V(!space, 0); - return space->get_param(p_param); -} - -void PhysicsServer2DSW::space_set_debug_contacts(RID p_space, int p_max_contacts) { - Space2DSW *space = space_owner.get_or_null(p_space); - ERR_FAIL_COND(!space); - space->set_debug_contacts(p_max_contacts); -} - -Vector PhysicsServer2DSW::space_get_contacts(RID p_space) const { - Space2DSW *space = space_owner.get_or_null(p_space); - ERR_FAIL_COND_V(!space, Vector()); - return space->get_debug_contacts(); -} - -int PhysicsServer2DSW::space_get_contact_count(RID p_space) const { - Space2DSW *space = space_owner.get_or_null(p_space); - ERR_FAIL_COND_V(!space, 0); - return space->get_debug_contact_count(); -} - -PhysicsDirectSpaceState2D *PhysicsServer2DSW::space_get_direct_state(RID p_space) { - Space2DSW *space = space_owner.get_or_null(p_space); - ERR_FAIL_COND_V(!space, nullptr); - 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(); -} - -RID PhysicsServer2DSW::area_create() { - Area2DSW *area = memnew(Area2DSW); - RID rid = area_owner.make_rid(area); - area->set_self(rid); - return rid; -}; - -void PhysicsServer2DSW::area_set_space(RID p_area, RID p_space) { - Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - - Space2DSW *space = nullptr; - if (p_space.is_valid()) { - space = space_owner.get_or_null(p_space); - ERR_FAIL_COND(!space); - } - - if (area->get_space() == space) { - return; //pointless - } - - area->clear_constraints(); - area->set_space(space); -}; - -RID PhysicsServer2DSW::area_get_space(RID p_area) const { - Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND_V(!area, RID()); - - Space2DSW *space = area->get_space(); - if (!space) { - return RID(); - } - return space->get_self(); -}; - -void PhysicsServer2DSW::area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode) { - Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - - area->set_space_override_mode(p_mode); -} - -PhysicsServer2D::AreaSpaceOverrideMode PhysicsServer2DSW::area_get_space_override_mode(RID p_area) const { - const Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND_V(!area, AREA_SPACE_OVERRIDE_DISABLED); - - return area->get_space_override_mode(); -} - -void PhysicsServer2DSW::area_add_shape(RID p_area, RID p_shape, const Transform2D &p_transform, bool p_disabled) { - Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - - Shape2DSW *shape = shape_owner.get_or_null(p_shape); - ERR_FAIL_COND(!shape); - - area->add_shape(shape, p_transform, p_disabled); -} - -void PhysicsServer2DSW::area_set_shape(RID p_area, int p_shape_idx, RID p_shape) { - Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - - Shape2DSW *shape = shape_owner.get_or_null(p_shape); - ERR_FAIL_COND(!shape); - ERR_FAIL_COND(!shape->is_configured()); - - area->set_shape(p_shape_idx, shape); -} - -void PhysicsServer2DSW::area_set_shape_transform(RID p_area, int p_shape_idx, const Transform2D &p_transform) { - Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - - area->set_shape_transform(p_shape_idx, p_transform); -} - -void PhysicsServer2DSW::area_set_shape_disabled(RID p_area, int p_shape, bool p_disabled) { - Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - ERR_FAIL_INDEX(p_shape, area->get_shape_count()); - FLUSH_QUERY_CHECK(area); - - area->set_shape_disabled(p_shape, p_disabled); -} - -int PhysicsServer2DSW::area_get_shape_count(RID p_area) const { - Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND_V(!area, -1); - - return area->get_shape_count(); -} - -RID PhysicsServer2DSW::area_get_shape(RID p_area, int p_shape_idx) const { - Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND_V(!area, RID()); - - Shape2DSW *shape = area->get_shape(p_shape_idx); - ERR_FAIL_COND_V(!shape, RID()); - - return shape->get_self(); -} - -Transform2D PhysicsServer2DSW::area_get_shape_transform(RID p_area, int p_shape_idx) const { - Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND_V(!area, Transform2D()); - - return area->get_shape_transform(p_shape_idx); -} - -void PhysicsServer2DSW::area_remove_shape(RID p_area, int p_shape_idx) { - Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - - area->remove_shape(p_shape_idx); -} - -void PhysicsServer2DSW::area_clear_shapes(RID p_area) { - Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - - while (area->get_shape_count()) { - area->remove_shape(0); - } -} - -void PhysicsServer2DSW::area_attach_object_instance_id(RID p_area, ObjectID p_id) { - if (space_owner.owns(p_area)) { - Space2DSW *space = space_owner.get_or_null(p_area); - p_area = space->get_default_area()->get_self(); - } - Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - area->set_instance_id(p_id); -} - -ObjectID PhysicsServer2DSW::area_get_object_instance_id(RID p_area) const { - if (space_owner.owns(p_area)) { - Space2DSW *space = space_owner.get_or_null(p_area); - p_area = space->get_default_area()->get_self(); - } - Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND_V(!area, ObjectID()); - return area->get_instance_id(); -} - -void PhysicsServer2DSW::area_attach_canvas_instance_id(RID p_area, ObjectID p_id) { - if (space_owner.owns(p_area)) { - Space2DSW *space = space_owner.get_or_null(p_area); - p_area = space->get_default_area()->get_self(); - } - Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - area->set_canvas_instance_id(p_id); -} - -ObjectID PhysicsServer2DSW::area_get_canvas_instance_id(RID p_area) const { - if (space_owner.owns(p_area)) { - Space2DSW *space = space_owner.get_or_null(p_area); - p_area = space->get_default_area()->get_self(); - } - Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND_V(!area, ObjectID()); - return area->get_canvas_instance_id(); -} - -void PhysicsServer2DSW::area_set_param(RID p_area, AreaParameter p_param, const Variant &p_value) { - if (space_owner.owns(p_area)) { - Space2DSW *space = space_owner.get_or_null(p_area); - p_area = space->get_default_area()->get_self(); - } - Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - area->set_param(p_param, p_value); -}; - -void PhysicsServer2DSW::area_set_transform(RID p_area, const Transform2D &p_transform) { - Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - area->set_transform(p_transform); -}; - -Variant PhysicsServer2DSW::area_get_param(RID p_area, AreaParameter p_param) const { - if (space_owner.owns(p_area)) { - Space2DSW *space = space_owner.get_or_null(p_area); - p_area = space->get_default_area()->get_self(); - } - Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND_V(!area, Variant()); - - return area->get_param(p_param); -}; - -Transform2D PhysicsServer2DSW::area_get_transform(RID p_area) const { - Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND_V(!area, Transform2D()); - - return area->get_transform(); -}; - -void PhysicsServer2DSW::area_set_pickable(RID p_area, bool p_pickable) { - Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - area->set_pickable(p_pickable); -} - -void PhysicsServer2DSW::area_set_monitorable(RID p_area, bool p_monitorable) { - Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - FLUSH_QUERY_CHECK(area); - - area->set_monitorable(p_monitorable); -} - -void PhysicsServer2DSW::area_set_collision_mask(RID p_area, uint32_t p_mask) { - Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - - area->set_collision_mask(p_mask); -} - -void PhysicsServer2DSW::area_set_collision_layer(RID p_area, uint32_t p_layer) { - Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - - area->set_collision_layer(p_layer); -} - -void PhysicsServer2DSW::area_set_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) { - Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - - area->set_monitor_callback(p_receiver ? p_receiver->get_instance_id() : ObjectID(), p_method); -} - -void PhysicsServer2DSW::area_set_area_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) { - Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - - area->set_area_monitor_callback(p_receiver ? p_receiver->get_instance_id() : ObjectID(), p_method); -} - -/* BODY API */ - -RID PhysicsServer2DSW::body_create() { - Body2DSW *body = memnew(Body2DSW); - RID rid = body_owner.make_rid(body); - body->set_self(rid); - return rid; -} - -void PhysicsServer2DSW::body_set_space(RID p_body, RID p_space) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - Space2DSW *space = nullptr; - if (p_space.is_valid()) { - space = space_owner.get_or_null(p_space); - ERR_FAIL_COND(!space); - } - - if (body->get_space() == space) { - return; //pointless - } - - body->clear_constraint_list(); - body->set_space(space); -}; - -RID PhysicsServer2DSW::body_get_space(RID p_body) const { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, RID()); - - Space2DSW *space = body->get_space(); - if (!space) { - return RID(); - } - return space->get_self(); -}; - -void PhysicsServer2DSW::body_set_mode(RID p_body, BodyMode p_mode) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - FLUSH_QUERY_CHECK(body); - - body->set_mode(p_mode); -}; - -PhysicsServer2D::BodyMode PhysicsServer2DSW::body_get_mode(RID p_body) const { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, BODY_MODE_STATIC); - - return body->get_mode(); -}; - -void PhysicsServer2DSW::body_add_shape(RID p_body, RID p_shape, const Transform2D &p_transform, bool p_disabled) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - Shape2DSW *shape = shape_owner.get_or_null(p_shape); - ERR_FAIL_COND(!shape); - - body->add_shape(shape, p_transform, p_disabled); -} - -void PhysicsServer2DSW::body_set_shape(RID p_body, int p_shape_idx, RID p_shape) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - Shape2DSW *shape = shape_owner.get_or_null(p_shape); - ERR_FAIL_COND(!shape); - ERR_FAIL_COND(!shape->is_configured()); - - body->set_shape(p_shape_idx, shape); -} - -void PhysicsServer2DSW::body_set_shape_transform(RID p_body, int p_shape_idx, const Transform2D &p_transform) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - body->set_shape_transform(p_shape_idx, p_transform); -} - -int PhysicsServer2DSW::body_get_shape_count(RID p_body) const { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, -1); - - return body->get_shape_count(); -} - -RID PhysicsServer2DSW::body_get_shape(RID p_body, int p_shape_idx) const { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, RID()); - - Shape2DSW *shape = body->get_shape(p_shape_idx); - ERR_FAIL_COND_V(!shape, RID()); - - return shape->get_self(); -} - -Transform2D PhysicsServer2DSW::body_get_shape_transform(RID p_body, int p_shape_idx) const { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, Transform2D()); - - return body->get_shape_transform(p_shape_idx); -} - -void PhysicsServer2DSW::body_remove_shape(RID p_body, int p_shape_idx) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - body->remove_shape(p_shape_idx); -} - -void PhysicsServer2DSW::body_clear_shapes(RID p_body) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - while (body->get_shape_count()) { - body->remove_shape(0); - } -} - -void PhysicsServer2DSW::body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - ERR_FAIL_INDEX(p_shape_idx, body->get_shape_count()); - FLUSH_QUERY_CHECK(body); - - body->set_shape_disabled(p_shape_idx, p_disabled); -} - -void PhysicsServer2DSW::body_set_shape_as_one_way_collision(RID p_body, int p_shape_idx, bool p_enable, real_t p_margin) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - ERR_FAIL_INDEX(p_shape_idx, body->get_shape_count()); - FLUSH_QUERY_CHECK(body); - - body->set_shape_as_one_way_collision(p_shape_idx, p_enable, p_margin); -} - -void PhysicsServer2DSW::body_set_continuous_collision_detection_mode(RID p_body, CCDMode p_mode) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - body->set_continuous_collision_detection_mode(p_mode); -} - -PhysicsServer2DSW::CCDMode PhysicsServer2DSW::body_get_continuous_collision_detection_mode(RID p_body) const { - const Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, CCD_MODE_DISABLED); - - return body->get_continuous_collision_detection_mode(); -} - -void PhysicsServer2DSW::body_attach_object_instance_id(RID p_body, ObjectID p_id) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - body->set_instance_id(p_id); -}; - -ObjectID PhysicsServer2DSW::body_get_object_instance_id(RID p_body) const { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, ObjectID()); - - return body->get_instance_id(); -}; - -void PhysicsServer2DSW::body_attach_canvas_instance_id(RID p_body, ObjectID p_id) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - body->set_canvas_instance_id(p_id); -}; - -ObjectID PhysicsServer2DSW::body_get_canvas_instance_id(RID p_body) const { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, ObjectID()); - - return body->get_canvas_instance_id(); -}; - -void PhysicsServer2DSW::body_set_collision_layer(RID p_body, uint32_t p_layer) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - body->set_collision_layer(p_layer); -}; - -uint32_t PhysicsServer2DSW::body_get_collision_layer(RID p_body) const { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, 0); - - return body->get_collision_layer(); -}; - -void PhysicsServer2DSW::body_set_collision_mask(RID p_body, uint32_t p_mask) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - body->set_collision_mask(p_mask); -}; - -uint32_t PhysicsServer2DSW::body_get_collision_mask(RID p_body) const { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, 0); - - return body->get_collision_mask(); -}; - -void PhysicsServer2DSW::body_set_param(RID p_body, BodyParameter p_param, const Variant &p_value) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - body->set_param(p_param, p_value); -}; - -Variant PhysicsServer2DSW::body_get_param(RID p_body, BodyParameter p_param) const { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, 0); - - return body->get_param(p_param); -}; - -void PhysicsServer2DSW::body_reset_mass_properties(RID p_body) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - return body->reset_mass_properties(); -} - -void PhysicsServer2DSW::body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - body->set_state(p_state, p_variant); -}; - -Variant PhysicsServer2DSW::body_get_state(RID p_body, BodyState p_state) const { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, Variant()); - - return body->get_state(p_state); -}; - -void PhysicsServer2DSW::body_set_applied_force(RID p_body, const Vector2 &p_force) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - body->set_applied_force(p_force); - body->wakeup(); -}; - -Vector2 PhysicsServer2DSW::body_get_applied_force(RID p_body) const { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, Vector2()); - return body->get_applied_force(); -}; - -void PhysicsServer2DSW::body_set_applied_torque(RID p_body, real_t p_torque) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - body->set_applied_torque(p_torque); - body->wakeup(); -}; - -real_t PhysicsServer2DSW::body_get_applied_torque(RID p_body) const { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, 0); - - return body->get_applied_torque(); -}; - -void PhysicsServer2DSW::body_apply_central_impulse(RID p_body, const Vector2 &p_impulse) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - body->apply_central_impulse(p_impulse); - body->wakeup(); -} - -void PhysicsServer2DSW::body_apply_torque_impulse(RID p_body, real_t p_torque) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - _update_shapes(); - - body->apply_torque_impulse(p_torque); - body->wakeup(); -} - -void PhysicsServer2DSW::body_apply_impulse(RID p_body, const Vector2 &p_impulse, const Vector2 &p_position) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - _update_shapes(); - - body->apply_impulse(p_impulse, p_position); - body->wakeup(); -}; - -void PhysicsServer2DSW::body_add_central_force(RID p_body, const Vector2 &p_force) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - body->add_central_force(p_force); - body->wakeup(); -}; - -void PhysicsServer2DSW::body_add_force(RID p_body, const Vector2 &p_force, const Vector2 &p_position) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - body->add_force(p_force, p_position); - body->wakeup(); -}; - -void PhysicsServer2DSW::body_add_torque(RID p_body, real_t p_torque) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - body->add_torque(p_torque); - body->wakeup(); -}; - -void PhysicsServer2DSW::body_set_axis_velocity(RID p_body, const Vector2 &p_axis_velocity) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - _update_shapes(); - - Vector2 v = body->get_linear_velocity(); - Vector2 axis = p_axis_velocity.normalized(); - v -= axis * axis.dot(v); - v += p_axis_velocity; - body->set_linear_velocity(v); - body->wakeup(); -}; - -void PhysicsServer2DSW::body_add_collision_exception(RID p_body, RID p_body_b) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - body->add_exception(p_body_b); - body->wakeup(); -}; - -void PhysicsServer2DSW::body_remove_collision_exception(RID p_body, RID p_body_b) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - body->remove_exception(p_body_b); - body->wakeup(); -}; - -void PhysicsServer2DSW::body_get_collision_exceptions(RID p_body, List *p_exceptions) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - for (int i = 0; i < body->get_exceptions().size(); i++) { - p_exceptions->push_back(body->get_exceptions()[i]); - } -}; - -void PhysicsServer2DSW::body_set_contacts_reported_depth_threshold(RID p_body, real_t p_threshold) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); -}; - -real_t PhysicsServer2DSW::body_get_contacts_reported_depth_threshold(RID p_body) const { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, 0); - return 0; -}; - -void PhysicsServer2DSW::body_set_omit_force_integration(RID p_body, bool p_omit) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - body->set_omit_force_integration(p_omit); -}; - -bool PhysicsServer2DSW::body_is_omitting_force_integration(RID p_body) const { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, false); - return body->get_omit_force_integration(); -}; - -void PhysicsServer2DSW::body_set_max_contacts_reported(RID p_body, int p_contacts) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - body->set_max_contacts_reported(p_contacts); -} - -int PhysicsServer2DSW::body_get_max_contacts_reported(RID p_body) const { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, -1); - return body->get_max_contacts_reported(); -} - -void PhysicsServer2DSW::body_set_state_sync_callback(RID p_body, void *p_instance, BodyStateCallback p_callback) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - body->set_state_sync_callback(p_instance, p_callback); -} - -void PhysicsServer2DSW::body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - body->set_force_integration_callback(p_callable, p_udata); -} - -bool PhysicsServer2DSW::body_collide_shape(RID p_body, int p_body_shape, RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, Vector2 *r_results, int p_result_max, int &r_result_count) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, false); - ERR_FAIL_INDEX_V(p_body_shape, body->get_shape_count(), false); - - return shape_collide(body->get_shape(p_body_shape)->get_self(), body->get_transform() * body->get_shape_transform(p_body_shape), Vector2(), p_shape, p_shape_xform, p_motion, r_results, p_result_max, r_result_count); -} - -void PhysicsServer2DSW::body_set_pickable(RID p_body, bool p_pickable) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - body->set_pickable(p_pickable); -} - -bool PhysicsServer2DSW::body_test_motion(RID p_body, const MotionParameters &p_parameters, MotionResult *r_result) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, false); - ERR_FAIL_COND_V(!body->get_space(), false); - ERR_FAIL_COND_V(body->get_space()->is_locked(), false); - - _update_shapes(); - - return body->get_space()->test_body_motion(body, p_parameters, r_result); -} - -PhysicsDirectBodyState2D *PhysicsServer2DSW::body_get_direct_state(RID p_body) { - ERR_FAIL_COND_V_MSG((using_threads && !doing_sync), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification."); - - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, nullptr); - - ERR_FAIL_COND_V(!body->get_space(), nullptr); - ERR_FAIL_COND_V_MSG(body->get_space()->is_locked(), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification."); - - return body->get_direct_state(); -} - -/* JOINT API */ - -RID PhysicsServer2DSW::joint_create() { - Joint2DSW *joint = memnew(Joint2DSW); - RID joint_rid = joint_owner.make_rid(joint); - joint->set_self(joint_rid); - return joint_rid; -} - -void PhysicsServer2DSW::joint_clear(RID p_joint) { - Joint2DSW *joint = joint_owner.get_or_null(p_joint); - if (joint->get_type() != JOINT_TYPE_MAX) { - Joint2DSW *empty_joint = memnew(Joint2DSW); - empty_joint->copy_settings_from(joint); - - joint_owner.replace(p_joint, empty_joint); - memdelete(joint); - } -} - -void PhysicsServer2DSW::joint_set_param(RID p_joint, JointParam p_param, real_t p_value) { - Joint2DSW *joint = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND(!joint); - - switch (p_param) { - case JOINT_PARAM_BIAS: - joint->set_bias(p_value); - break; - case JOINT_PARAM_MAX_BIAS: - joint->set_max_bias(p_value); - break; - case JOINT_PARAM_MAX_FORCE: - joint->set_max_force(p_value); - break; - } -} - -real_t PhysicsServer2DSW::joint_get_param(RID p_joint, JointParam p_param) const { - const Joint2DSW *joint = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND_V(!joint, -1); - - switch (p_param) { - case JOINT_PARAM_BIAS: - return joint->get_bias(); - break; - case JOINT_PARAM_MAX_BIAS: - return joint->get_max_bias(); - break; - case JOINT_PARAM_MAX_FORCE: - return joint->get_max_force(); - break; - } - - return 0; -} - -void PhysicsServer2DSW::joint_disable_collisions_between_bodies(RID p_joint, const bool p_disable) { - Joint2DSW *joint = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND(!joint); - - joint->disable_collisions_between_bodies(p_disable); - - if (2 == joint->get_body_count()) { - Body2DSW *body_a = *joint->get_body_ptr(); - Body2DSW *body_b = *(joint->get_body_ptr() + 1); - - if (p_disable) { - body_add_collision_exception(body_a->get_self(), body_b->get_self()); - body_add_collision_exception(body_b->get_self(), body_a->get_self()); - } else { - body_remove_collision_exception(body_a->get_self(), body_b->get_self()); - body_remove_collision_exception(body_b->get_self(), body_a->get_self()); - } - } -} - -bool PhysicsServer2DSW::joint_is_disabled_collisions_between_bodies(RID p_joint) const { - const Joint2DSW *joint = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND_V(!joint, true); - - return joint->is_disabled_collisions_between_bodies(); -} - -void PhysicsServer2DSW::joint_make_pin(RID p_joint, const Vector2 &p_pos, RID p_body_a, RID p_body_b) { - Body2DSW *A = body_owner.get_or_null(p_body_a); - ERR_FAIL_COND(!A); - Body2DSW *B = nullptr; - if (body_owner.owns(p_body_b)) { - B = body_owner.get_or_null(p_body_b); - ERR_FAIL_COND(!B); - } - - Joint2DSW *prev_joint = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND(prev_joint == nullptr); - - Joint2DSW *joint = memnew(PinJoint2DSW(p_pos, A, B)); - - joint_owner.replace(p_joint, joint); - joint->copy_settings_from(prev_joint); - memdelete(prev_joint); -} - -void PhysicsServer2DSW::joint_make_groove(RID p_joint, const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, RID p_body_a, RID p_body_b) { - Body2DSW *A = body_owner.get_or_null(p_body_a); - ERR_FAIL_COND(!A); - - Body2DSW *B = body_owner.get_or_null(p_body_b); - ERR_FAIL_COND(!B); - - Joint2DSW *prev_joint = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND(prev_joint == nullptr); - - Joint2DSW *joint = memnew(GrooveJoint2DSW(p_a_groove1, p_a_groove2, p_b_anchor, A, B)); - - joint_owner.replace(p_joint, joint); - joint->copy_settings_from(prev_joint); - memdelete(prev_joint); -} - -void PhysicsServer2DSW::joint_make_damped_spring(RID p_joint, const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, RID p_body_a, RID p_body_b) { - Body2DSW *A = body_owner.get_or_null(p_body_a); - ERR_FAIL_COND(!A); - - Body2DSW *B = body_owner.get_or_null(p_body_b); - ERR_FAIL_COND(!B); - - Joint2DSW *prev_joint = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND(prev_joint == nullptr); - - Joint2DSW *joint = memnew(DampedSpringJoint2DSW(p_anchor_a, p_anchor_b, A, B)); - - joint_owner.replace(p_joint, joint); - joint->copy_settings_from(prev_joint); - memdelete(prev_joint); -} - -void PhysicsServer2DSW::pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value) { - Joint2DSW *j = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND(!j); - ERR_FAIL_COND(j->get_type() != JOINT_TYPE_PIN); - - PinJoint2DSW *pin_joint = static_cast(j); - pin_joint->set_param(p_param, p_value); -} - -real_t PhysicsServer2DSW::pin_joint_get_param(RID p_joint, PinJointParam p_param) const { - Joint2DSW *j = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND_V(!j, 0); - ERR_FAIL_COND_V(j->get_type() != JOINT_TYPE_PIN, 0); - - PinJoint2DSW *pin_joint = static_cast(j); - return pin_joint->get_param(p_param); -} - -void PhysicsServer2DSW::damped_spring_joint_set_param(RID p_joint, DampedSpringParam p_param, real_t p_value) { - Joint2DSW *j = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND(!j); - ERR_FAIL_COND(j->get_type() != JOINT_TYPE_DAMPED_SPRING); - - DampedSpringJoint2DSW *dsj = static_cast(j); - dsj->set_param(p_param, p_value); -} - -real_t PhysicsServer2DSW::damped_spring_joint_get_param(RID p_joint, DampedSpringParam p_param) const { - Joint2DSW *j = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND_V(!j, 0); - ERR_FAIL_COND_V(j->get_type() != JOINT_TYPE_DAMPED_SPRING, 0); - - DampedSpringJoint2DSW *dsj = static_cast(j); - return dsj->get_param(p_param); -} - -PhysicsServer2D::JointType PhysicsServer2DSW::joint_get_type(RID p_joint) const { - Joint2DSW *joint = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND_V(!joint, JOINT_TYPE_PIN); - - return joint->get_type(); -} - -void PhysicsServer2DSW::free(RID p_rid) { - _update_shapes(); // just in case - - if (shape_owner.owns(p_rid)) { - Shape2DSW *shape = shape_owner.get_or_null(p_rid); - - while (shape->get_owners().size()) { - ShapeOwner2DSW *so = shape->get_owners().front()->key(); - so->remove_shape(shape); - } - - shape_owner.free(p_rid); - memdelete(shape); - } else if (body_owner.owns(p_rid)) { - Body2DSW *body = body_owner.get_or_null(p_rid); - - /* - if (body->get_state_query()) - _clear_query(body->get_state_query()); - - if (body->get_direct_state_query()) - _clear_query(body->get_direct_state_query()); - */ - - body_set_space(p_rid, RID()); - - while (body->get_shape_count()) { - body->remove_shape(0); - } - - body_owner.free(p_rid); - memdelete(body); - - } else if (area_owner.owns(p_rid)) { - Area2DSW *area = area_owner.get_or_null(p_rid); - - /* - if (area->get_monitor_query()) - _clear_query(area->get_monitor_query()); - */ - - area->set_space(nullptr); - - while (area->get_shape_count()) { - area->remove_shape(0); - } - - area_owner.free(p_rid); - memdelete(area); - } else if (space_owner.owns(p_rid)) { - Space2DSW *space = space_owner.get_or_null(p_rid); - - while (space->get_objects().size()) { - CollisionObject2DSW *co = (CollisionObject2DSW *)space->get_objects().front()->get(); - co->set_space(nullptr); - } - - active_spaces.erase(space); - free(space->get_default_area()->get_self()); - space_owner.free(p_rid); - memdelete(space); - } else if (joint_owner.owns(p_rid)) { - Joint2DSW *joint = joint_owner.get_or_null(p_rid); - - joint_owner.free(p_rid); - memdelete(joint); - - } else { - ERR_FAIL_MSG("Invalid ID."); - } -}; - -void PhysicsServer2DSW::set_active(bool p_active) { - active = p_active; -}; - -void PhysicsServer2DSW::set_collision_iterations(int p_iterations) { - iterations = p_iterations; -}; - -void PhysicsServer2DSW::init() { - doing_sync = false; - iterations = 8; // 8? - stepper = memnew(Step2DSW); -}; - -void PhysicsServer2DSW::step(real_t p_step) { - if (!active) { - return; - } - - _update_shapes(); - - island_count = 0; - active_objects = 0; - collision_pairs = 0; - for (Set::Element *E = active_spaces.front(); E; E = E->next()) { - stepper->step((Space2DSW *)E->get(), p_step, iterations); - island_count += E->get()->get_island_count(); - active_objects += E->get()->get_active_objects(); - collision_pairs += E->get()->get_collision_pairs(); - } -}; - -void PhysicsServer2DSW::sync() { - doing_sync = true; -}; - -void PhysicsServer2DSW::flush_queries() { - if (!active) { - return; - } - - flushing_queries = true; - - uint64_t time_beg = OS::get_singleton()->get_ticks_usec(); - - for (Set::Element *E = active_spaces.front(); E; E = E->next()) { - Space2DSW *space = (Space2DSW *)E->get(); - space->call_queries(); - } - - flushing_queries = false; - - if (EngineDebugger::is_profiling("servers")) { - uint64_t total_time[Space2DSW::ELAPSED_TIME_MAX]; - static const char *time_name[Space2DSW::ELAPSED_TIME_MAX] = { - "integrate_forces", - "generate_islands", - "setup_constraints", - "solve_constraints", - "integrate_velocities" - }; - - for (int i = 0; i < Space2DSW::ELAPSED_TIME_MAX; i++) { - total_time[i] = 0; - } - - for (Set::Element *E = active_spaces.front(); E; E = E->next()) { - for (int i = 0; i < Space2DSW::ELAPSED_TIME_MAX; i++) { - total_time[i] += E->get()->get_elapsed_time(Space2DSW::ElapsedTime(i)); - } - } - - Array values; - values.resize(Space2DSW::ELAPSED_TIME_MAX * 2); - for (int i = 0; i < Space2DSW::ELAPSED_TIME_MAX; i++) { - values[i * 2 + 0] = time_name[i]; - values[i * 2 + 1] = USEC_TO_SEC(total_time[i]); - } - values.push_back("flush_queries"); - values.push_back(USEC_TO_SEC(OS::get_singleton()->get_ticks_usec() - time_beg)); - - values.push_front("physics_2d"); - EngineDebugger::profiler_add_frame_data("servers", values); - } -} - -void PhysicsServer2DSW::end_sync() { - doing_sync = false; -} - -void PhysicsServer2DSW::finish() { - memdelete(stepper); -}; - -void PhysicsServer2DSW::_update_shapes() { - while (pending_shape_update_list.first()) { - pending_shape_update_list.first()->self()->_shape_changed(); - pending_shape_update_list.remove(pending_shape_update_list.first()); - } -} - -int PhysicsServer2DSW::get_process_info(ProcessInfo p_info) { - switch (p_info) { - case INFO_ACTIVE_OBJECTS: { - return active_objects; - } break; - case INFO_COLLISION_PAIRS: { - return collision_pairs; - } break; - case INFO_ISLAND_COUNT: { - return island_count; - } break; - } - - return 0; -} - -PhysicsServer2DSW *PhysicsServer2DSW::singletonsw = nullptr; - -PhysicsServer2DSW::PhysicsServer2DSW(bool p_using_threads) { - singletonsw = this; - BroadPhase2DSW::create_func = BroadPhase2DBVH::_create; - - using_threads = p_using_threads; -}; diff --git a/servers/physics_2d/physics_server_2d_sw.h b/servers/physics_2d/physics_server_2d_sw.h deleted file mode 100644 index b8e375a087..0000000000 --- a/servers/physics_2d/physics_server_2d_sw.h +++ /dev/null @@ -1,299 +0,0 @@ -/*************************************************************************/ -/* physics_server_2d_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/*************************************************************************/ - -#ifndef PHYSICS_2D_SERVER_SW -#define PHYSICS_2D_SERVER_SW - -#include "core/templates/rid_owner.h" -#include "joints_2d_sw.h" -#include "servers/physics_server_2d.h" -#include "shape_2d_sw.h" -#include "space_2d_sw.h" -#include "step_2d_sw.h" - -class PhysicsServer2DSW : public PhysicsServer2D { - GDCLASS(PhysicsServer2DSW, PhysicsServer2D); - - friend class PhysicsDirectSpaceState2DSW; - friend class PhysicsDirectBodyState2DSW; - bool active = true; - int iterations = 0; - bool doing_sync = false; - - int island_count = 0; - int active_objects = 0; - int collision_pairs = 0; - - bool using_threads = false; - - bool flushing_queries = false; - - Step2DSW *stepper = nullptr; - Set active_spaces; - - mutable RID_PtrOwner shape_owner; - mutable RID_PtrOwner space_owner; - mutable RID_PtrOwner area_owner; - mutable RID_PtrOwner body_owner; - mutable RID_PtrOwner joint_owner; - - static PhysicsServer2DSW *singletonsw; - - //void _clear_query(Query2DSW *p_query); - friend class CollisionObject2DSW; - SelfList::List pending_shape_update_list; - void _update_shapes(); - - RID _shape_create(ShapeType p_shape); - -public: - struct CollCbkData { - Vector2 valid_dir; - real_t valid_depth = 0.0; - int max = 0; - int amount = 0; - int passed = 0; - int invalid_by_dir = 0; - Vector2 *ptr = nullptr; - }; - - virtual RID world_boundary_shape_create() override; - virtual RID separation_ray_shape_create() override; - virtual RID segment_shape_create() override; - virtual RID circle_shape_create() override; - virtual RID rectangle_shape_create() override; - virtual RID capsule_shape_create() override; - virtual RID convex_polygon_shape_create() override; - virtual RID concave_polygon_shape_create() override; - - static void _shape_col_cbk(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_userdata); - - 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 override; - virtual Variant shape_get_data(RID p_shape) const override; - virtual real_t shape_get_custom_solver_bias(RID p_shape) const override; - - virtual bool shape_collide(RID p_shape_A, const Transform2D &p_xform_A, const Vector2 &p_motion_A, RID p_shape_B, const Transform2D &p_xform_B, const Vector2 &p_motion_B, Vector2 *r_results, int p_result_max, int &r_result_count) override; - - /* SPACE API */ - - 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) override; - virtual real_t space_get_param(RID p_space, SpaceParameter p_param) const override; - - virtual void space_set_debug_contacts(RID p_space, int p_max_contacts) override; - virtual Vector space_get_contacts(RID p_space) const override; - virtual int space_get_contact_count(RID p_space) const override; - - // this function only works on physics process, errors and returns null otherwise - virtual PhysicsDirectSpaceState2D *space_get_direct_state(RID p_space) override; - - /* AREA API */ - - virtual RID area_create() override; - - 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) override; - virtual RID area_get_space(RID p_area) const override; - - virtual void area_add_shape(RID p_area, RID p_shape, const Transform2D &p_transform = Transform2D(), 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 Transform2D &p_transform) override; - - 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 Transform2D area_get_shape_transform(RID p_area, int p_shape_idx) const override; - - virtual void area_set_shape_disabled(RID p_area, int p_shape, bool p_disabled) override; - - 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_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_attach_canvas_instance_id(RID p_area, ObjectID p_id) override; - virtual ObjectID area_get_canvas_instance_id(RID p_area) const override; - - 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 Transform2D &p_transform) override; - - virtual Variant area_get_param(RID p_area, AreaParameter p_param) const override; - virtual Transform2D area_get_transform(RID p_area) const override; - virtual void area_set_monitorable(RID p_area, bool p_monitorable) override; - 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_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; - - virtual void area_set_pickable(RID p_area, bool p_pickable) override; - - /* BODY API */ - - // create a body of a given type - virtual RID body_create() override; - - 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) override; - virtual BodyMode body_get_mode(RID p_body) const override; - - virtual void body_add_shape(RID p_body, RID p_shape, const Transform2D &p_transform = Transform2D(), 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 Transform2D &p_transform) override; - - 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 Transform2D body_get_shape_transform(RID p_body, int p_shape_idx) const override; - - 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_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled) override; - virtual void body_set_shape_as_one_way_collision(RID p_body, int p_shape_idx, bool p_enable, real_t p_margin) override; - - 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_attach_canvas_instance_id(RID p_body, ObjectID p_id) override; - virtual ObjectID body_get_canvas_instance_id(RID p_body) const override; - - virtual void body_set_continuous_collision_detection_mode(RID p_body, CCDMode p_mode) override; - virtual CCDMode body_get_continuous_collision_detection_mode(RID p_body) const override; - - 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) override; - virtual uint32_t body_get_collision_mask(RID p_body) const override; - - virtual void body_set_param(RID p_body, BodyParameter p_param, const Variant &p_value) override; - virtual Variant body_get_param(RID p_body, BodyParameter p_param) const override; - - virtual void body_reset_mass_properties(RID p_body) override; - - virtual void body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) override; - virtual Variant body_get_state(RID p_body, BodyState p_state) const override; - - virtual void body_set_applied_force(RID p_body, const Vector2 &p_force) override; - virtual Vector2 body_get_applied_force(RID p_body) const override; - - virtual void body_set_applied_torque(RID p_body, real_t p_torque) override; - virtual real_t body_get_applied_torque(RID p_body) const override; - - virtual void body_add_central_force(RID p_body, const Vector2 &p_force) override; - virtual void body_add_force(RID p_body, const Vector2 &p_force, const Vector2 &p_position = Vector2()) override; - virtual void body_add_torque(RID p_body, real_t p_torque) override; - - virtual void body_apply_central_impulse(RID p_body, const Vector2 &p_impulse) override; - virtual void body_apply_torque_impulse(RID p_body, real_t p_torque) override; - virtual void body_apply_impulse(RID p_body, const Vector2 &p_impulse, const Vector2 &p_position = Vector2()) override; - virtual void body_set_axis_velocity(RID p_body, const Vector2 &p_axis_velocity) override; - - 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 *p_exceptions) override; - - 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) 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) override; - virtual int body_get_max_contacts_reported(RID p_body) const override; - - virtual void body_set_state_sync_callback(RID p_body, void *p_instance, BodyStateCallback p_callback) override; - virtual void body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata = Variant()) override; - - virtual bool body_collide_shape(RID p_body, int p_body_shape, RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, Vector2 *r_results, int p_result_max, int &r_result_count) override; - - virtual void body_set_pickable(RID p_body, bool p_pickable) override; - - virtual bool body_test_motion(RID p_body, const MotionParameters &p_parameters, MotionResult *r_result = nullptr) override; - - // this function only works on physics process, errors and returns null otherwise - virtual PhysicsDirectBodyState2D *body_get_direct_state(RID p_body) override; - - /* JOINT API */ - - virtual RID joint_create() override; - - virtual void joint_clear(RID p_joint) override; - - virtual void joint_set_param(RID p_joint, JointParam p_param, real_t p_value) override; - virtual real_t joint_get_param(RID p_joint, JointParam p_param) const override; - - virtual void joint_disable_collisions_between_bodies(RID p_joint, const bool p_disabled) override; - virtual bool joint_is_disabled_collisions_between_bodies(RID p_joint) const override; - - virtual void joint_make_pin(RID p_joint, const Vector2 &p_anchor, RID p_body_a, RID p_body_b = RID()) override; - virtual void joint_make_groove(RID p_joint, const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, RID p_body_a, RID p_body_b) override; - virtual void joint_make_damped_spring(RID p_joint, const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, RID p_body_a, RID p_body_b = RID()) override; - - 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 damped_spring_joint_set_param(RID p_joint, DampedSpringParam p_param, real_t p_value) override; - virtual real_t damped_spring_joint_get_param(RID p_joint, DampedSpringParam p_param) const override; - - virtual JointType joint_get_type(RID p_joint) const override; - - /* MISC */ - - virtual void free(RID p_rid) override; - - 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 void set_collision_iterations(int p_iterations) override; - - virtual bool is_flushing_queries() const override { return flushing_queries; } - - int get_process_info(ProcessInfo p_info) override; - - PhysicsServer2DSW(bool p_using_threads = false); - ~PhysicsServer2DSW() {} -}; - -#endif diff --git a/servers/physics_2d/physics_server_2d_wrap_mt.cpp b/servers/physics_2d/physics_server_2d_wrap_mt.cpp deleted file mode 100644 index 33070bf42d..0000000000 --- a/servers/physics_2d/physics_server_2d_wrap_mt.cpp +++ /dev/null @@ -1,137 +0,0 @@ -/*************************************************************************/ -/* physics_server_2d_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_2d_wrap_mt.h" - -#include "core/os/os.h" - -void PhysicsServer2DWrapMT::thread_exit() { - exit.set(); -} - -void PhysicsServer2DWrapMT::thread_step(real_t p_delta) { - physics_2d_server->step(p_delta); - step_sem.post(); -} - -void PhysicsServer2DWrapMT::_thread_callback(void *_instance) { - PhysicsServer2DWrapMT *vsmt = reinterpret_cast(_instance); - - vsmt->thread_loop(); -} - -void PhysicsServer2DWrapMT::thread_loop() { - server_thread = Thread::get_caller_id(); - - physics_2d_server->init(); - - exit.clear(); - step_thread_up.set(); - while (!exit.is_set()) { - // flush commands one by one, until exit is requested - command_queue.wait_and_flush(); - } - - command_queue.flush_all(); // flush all - - physics_2d_server->finish(); -} - -/* EVENT QUEUING */ - -void PhysicsServer2DWrapMT::step(real_t p_step) { - if (create_thread) { - command_queue.push(this, &PhysicsServer2DWrapMT::thread_step, p_step); - } else { - command_queue.flush_all(); //flush all pending from other threads - physics_2d_server->step(p_step); - } -} - -void PhysicsServer2DWrapMT::sync() { - if (create_thread) { - if (first_frame) { - first_frame = false; - } else { - step_sem.wait(); //must not wait if a step was not issued - } - } - physics_2d_server->sync(); -} - -void PhysicsServer2DWrapMT::flush_queries() { - physics_2d_server->flush_queries(); -} - -void PhysicsServer2DWrapMT::end_sync() { - physics_2d_server->end_sync(); -} - -void PhysicsServer2DWrapMT::init() { - if (create_thread) { - //OS::get_singleton()->release_rendering_thread(); - thread.start(_thread_callback, this); - while (!step_thread_up.is_set()) { - OS::get_singleton()->delay_usec(1000); - } - } else { - physics_2d_server->init(); - } -} - -void PhysicsServer2DWrapMT::finish() { - if (thread.is_started()) { - command_queue.push(this, &PhysicsServer2DWrapMT::thread_exit); - thread.wait_to_finish(); - } else { - physics_2d_server->finish(); - } -} - -PhysicsServer2DWrapMT::PhysicsServer2DWrapMT(PhysicsServer2D *p_contained, bool p_create_thread) : - command_queue(p_create_thread) { - physics_2d_server = p_contained; - create_thread = p_create_thread; - - 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(); -} - -PhysicsServer2DWrapMT::~PhysicsServer2DWrapMT() { - memdelete(physics_2d_server); - //finish(); -} diff --git a/servers/physics_2d/physics_server_2d_wrap_mt.h b/servers/physics_2d/physics_server_2d_wrap_mt.h deleted file mode 100644 index 8d9e366661..0000000000 --- a/servers/physics_2d/physics_server_2d_wrap_mt.h +++ /dev/null @@ -1,333 +0,0 @@ -/*************************************************************************/ -/* physics_server_2d_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 PHYSICS2DSERVERWRAPMT_H -#define PHYSICS2DSERVERWRAPMT_H - -#include "core/config/project_settings.h" -#include "core/os/thread.h" -#include "core/templates/command_queue_mt.h" -#include "core/templates/safe_refcount.h" -#include "servers/physics_server_2d.h" - -#ifdef DEBUG_SYNC -#define SYNC_DEBUG print_line("sync on: " + String(__FUNCTION__)); -#else -#define SYNC_DEBUG -#endif - -class PhysicsServer2DWrapMT : public PhysicsServer2D { - mutable PhysicsServer2D *physics_2d_server; - - mutable CommandQueueMT command_queue; - - static void _thread_callback(void *_instance); - void thread_loop(); - - Thread::ID server_thread; - Thread::ID main_thread; - SafeFlag exit; - Thread thread; - SafeFlag step_thread_up; - bool create_thread = false; - - Semaphore step_sem; - int step_pending = 0; - 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 PhysicsServer2D -#define ServerNameWrapMT PhysicsServer2DWrapMT -#define server_name physics_2d_server -#define WRITE_ACTION - -#include "servers/server_wrap_mt_common.h" - - //FUNC1RID(shape,ShapeType); todo fix - FUNCRID(world_boundary_shape) - FUNCRID(separation_ray_shape) - FUNCRID(segment_shape) - FUNCRID(circle_shape) - FUNCRID(rectangle_shape) - FUNCRID(capsule_shape) - FUNCRID(convex_polygon_shape) - FUNCRID(concave_polygon_shape) - - FUNC2(shape_set_data, RID, const Variant &); - FUNC2(shape_set_custom_solver_bias, RID, real_t); - - FUNC1RC(ShapeType, shape_get_type, RID); - FUNC1RC(Variant, shape_get_data, RID); - FUNC1RC(real_t, shape_get_custom_solver_bias, RID); - - //these work well, but should be used from the main thread only - bool shape_collide(RID p_shape_A, const Transform2D &p_xform_A, const Vector2 &p_motion_A, RID p_shape_B, const Transform2D &p_xform_B, const Vector2 &p_motion_B, Vector2 *r_results, int p_result_max, int &r_result_count) override { - ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), false); - return physics_2d_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); - } - - /* 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 - PhysicsDirectSpaceState2D *space_get_direct_state(RID p_space) override { - ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), nullptr); - return physics_2d_server->space_get_direct_state(p_space); - } - - FUNC2(space_set_debug_contacts, RID, int); - virtual Vector space_get_contacts(RID p_space) const override { - ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), Vector()); - return physics_2d_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_2d_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 Transform2D &, bool); - FUNC3(area_set_shape, RID, int, RID); - FUNC3(area_set_shape_transform, RID, int, const Transform2D &); - FUNC3(area_set_shape_disabled, RID, int, bool); - - FUNC1RC(int, area_get_shape_count, RID); - FUNC2RC(RID, area_get_shape, RID, int); - FUNC2RC(Transform2D, 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); - - FUNC2(area_attach_canvas_instance_id, RID, ObjectID); - FUNC1RC(ObjectID, area_get_canvas_instance_id, RID); - - FUNC3(area_set_param, RID, AreaParameter, const Variant &); - FUNC2(area_set_transform, RID, const Transform2D &); - - FUNC2RC(Variant, area_get_param, RID, AreaParameter); - FUNC1RC(Transform2D, 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_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 Transform2D &, bool); - FUNC3(body_set_shape, RID, int, RID); - FUNC3(body_set_shape_transform, RID, int, const Transform2D &); - - FUNC1RC(int, body_get_shape_count, RID); - FUNC2RC(Transform2D, body_get_shape_transform, RID, int); - FUNC2RC(RID, body_get_shape, RID, int); - - FUNC3(body_set_shape_disabled, RID, int, bool); - FUNC4(body_set_shape_as_one_way_collision, RID, int, bool, real_t); - - 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_attach_canvas_instance_id, RID, ObjectID); - FUNC1RC(ObjectID, body_get_canvas_instance_id, RID); - - FUNC2(body_set_continuous_collision_detection_mode, RID, CCDMode); - FUNC1RC(CCDMode, body_get_continuous_collision_detection_mode, 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); - - FUNC3(body_set_param, RID, BodyParameter, const Variant &); - FUNC2RC(Variant, body_get_param, RID, BodyParameter); - - FUNC1(body_reset_mass_properties, RID); - - FUNC3(body_set_state, RID, BodyState, const Variant &); - FUNC2RC(Variant, body_get_state, RID, BodyState); - - FUNC2(body_set_applied_force, RID, const Vector2 &); - FUNC1RC(Vector2, body_get_applied_force, RID); - - FUNC2(body_set_applied_torque, RID, real_t); - FUNC1RC(real_t, body_get_applied_torque, RID); - - FUNC2(body_add_central_force, RID, const Vector2 &); - FUNC3(body_add_force, RID, const Vector2 &, const Vector2 &); - FUNC2(body_add_torque, RID, real_t); - FUNC2(body_apply_central_impulse, RID, const Vector2 &); - FUNC2(body_apply_torque_impulse, RID, real_t); - FUNC3(body_apply_impulse, RID, const Vector2 &, const Vector2 &); - FUNC2(body_set_axis_velocity, RID, const Vector2 &); - - FUNC2(body_add_collision_exception, RID, RID); - FUNC2(body_remove_collision_exception, RID, RID); - FUNC2S(body_get_collision_exceptions, RID, List *); - - 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); - - FUNC3(body_set_state_sync_callback, RID, void *, BodyStateCallback); - FUNC3(body_set_force_integration_callback, RID, const Callable &, const Variant &); - - bool body_collide_shape(RID p_body, int p_body_shape, RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, Vector2 *r_results, int p_result_max, int &r_result_count) override { - return physics_2d_server->body_collide_shape(p_body, p_body_shape, p_shape, p_shape_xform, p_motion, r_results, p_result_max, r_result_count); - } - - FUNC2(body_set_pickable, RID, bool); - - bool body_test_motion(RID p_body, const MotionParameters &p_parameters, MotionResult *r_result = nullptr) override { - ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), false); - return physics_2d_server->body_test_motion(p_body, p_parameters, r_result); - } - - // this function only works on physics process, errors and returns null otherwise - PhysicsDirectBodyState2D *body_get_direct_state(RID p_body) override { - ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), nullptr); - return physics_2d_server->body_get_direct_state(p_body); - } - - /* JOINT API */ - - FUNCRID(joint) - - FUNC1(joint_clear, RID) - - FUNC3(joint_set_param, RID, JointParam, real_t); - FUNC2RC(real_t, joint_get_param, RID, JointParam); - - FUNC2(joint_disable_collisions_between_bodies, RID, const bool); - FUNC1RC(bool, joint_is_disabled_collisions_between_bodies, RID); - - ///FUNC3RID(pin_joint,const Vector2&,RID,RID); - ///FUNC5RID(groove_joint,const Vector2&,const Vector2&,const Vector2&,RID,RID); - ///FUNC4RID(damped_spring_joint,const Vector2&,const Vector2&,RID,RID); - - //TODO need to convert this to FUNCRID, but it's a hassle.. - - FUNC4(joint_make_pin, RID, const Vector2 &, RID, RID); - FUNC6(joint_make_groove, RID, const Vector2 &, const Vector2 &, const Vector2 &, RID, RID); - FUNC5(joint_make_damped_spring, RID, const Vector2 &, const Vector2 &, RID, RID); - - FUNC3(pin_joint_set_param, RID, PinJointParam, real_t); - FUNC2RC(real_t, pin_joint_get_param, RID, PinJointParam); - - FUNC3(damped_spring_joint_set_param, RID, DampedSpringParam, real_t); - FUNC2RC(real_t, damped_spring_joint_get_param, RID, DampedSpringParam); - - FUNC1RC(JointType, joint_get_type, RID); - - /* MISC */ - - FUNC1(free, RID); - FUNC1(set_active, bool); - FUNC1(set_collision_iterations, int); - - 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_2d_server->is_flushing_queries(); - } - - int get_process_info(ProcessInfo p_info) override { - return physics_2d_server->get_process_info(p_info); - } - - PhysicsServer2DWrapMT(PhysicsServer2D *p_contained, bool p_create_thread); - ~PhysicsServer2DWrapMT(); - -#undef ServerNameWrapMT -#undef ServerName -#undef server_name -#undef WRITE_ACTION -}; - -#ifdef DEBUG_SYNC -#undef DEBUG_SYNC -#endif -#undef SYNC_DEBUG - -#endif // PHYSICS2DSERVERWRAPMT_H diff --git a/servers/physics_2d/shape_2d_sw.cpp b/servers/physics_2d/shape_2d_sw.cpp deleted file mode 100644 index bde882ac24..0000000000 --- a/servers/physics_2d/shape_2d_sw.cpp +++ /dev/null @@ -1,995 +0,0 @@ -/*************************************************************************/ -/* shape_2d_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/*************************************************************************/ - -#include "shape_2d_sw.h" - -#include "core/math/geometry_2d.h" -#include "core/templates/sort_array.h" - -void Shape2DSW::configure(const Rect2 &p_aabb) { - aabb = p_aabb; - configured = true; - for (const KeyValue &E : owners) { - ShapeOwner2DSW *co = (ShapeOwner2DSW *)E.key; - co->_shape_changed(); - } -} - -Vector2 Shape2DSW::get_support(const Vector2 &p_normal) const { - Vector2 res[2]; - int amnt; - get_supports(p_normal, res, amnt); - return res[0]; -} - -void Shape2DSW::add_owner(ShapeOwner2DSW *p_owner) { - Map::Element *E = owners.find(p_owner); - if (E) { - E->get()++; - } else { - owners[p_owner] = 1; - } -} - -void Shape2DSW::remove_owner(ShapeOwner2DSW *p_owner) { - Map::Element *E = owners.find(p_owner); - ERR_FAIL_COND(!E); - E->get()--; - if (E->get() == 0) { - owners.erase(E); - } -} - -bool Shape2DSW::is_owner(ShapeOwner2DSW *p_owner) const { - return owners.has(p_owner); -} - -const Map &Shape2DSW::get_owners() const { - return owners; -} - -Shape2DSW::~Shape2DSW() { - ERR_FAIL_COND(owners.size()); -} - -/*********************************************************/ -/*********************************************************/ -/*********************************************************/ - -void WorldBoundaryShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { - r_amount = 0; -} - -bool WorldBoundaryShape2DSW::contains_point(const Vector2 &p_point) const { - return normal.dot(p_point) < d; -} - -bool WorldBoundaryShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { - Vector2 segment = p_begin - p_end; - real_t den = normal.dot(segment); - - //printf("den is %i\n",den); - if (Math::abs(den) <= CMP_EPSILON) { - return false; - } - - real_t dist = (normal.dot(p_begin) - d) / den; - //printf("dist is %i\n",dist); - - if (dist < -CMP_EPSILON || dist > (1.0 + CMP_EPSILON)) { - return false; - } - - r_point = p_begin + segment * -dist; - r_normal = normal; - - return true; -} - -real_t WorldBoundaryShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { - return 0; -} - -void WorldBoundaryShape2DSW::set_data(const Variant &p_data) { - ERR_FAIL_COND(p_data.get_type() != Variant::ARRAY); - Array arr = p_data; - ERR_FAIL_COND(arr.size() != 2); - normal = arr[0]; - d = arr[1]; - configure(Rect2(Vector2(-1e4, -1e4), Vector2(1e4 * 2, 1e4 * 2))); -} - -Variant WorldBoundaryShape2DSW::get_data() const { - Array arr; - arr.resize(2); - arr[0] = normal; - arr[1] = d; - return arr; -} - -/*********************************************************/ -/*********************************************************/ -/*********************************************************/ - -void SeparationRayShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { - r_amount = 1; - - if (p_normal.y > 0) { - *r_supports = Vector2(0, length); - } else { - *r_supports = Vector2(); - } -} - -bool SeparationRayShape2DSW::contains_point(const Vector2 &p_point) const { - return false; -} - -bool SeparationRayShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { - return false; //rays can't be intersected -} - -real_t SeparationRayShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { - return 0; //rays are mass-less -} - -void SeparationRayShape2DSW::set_data(const Variant &p_data) { - Dictionary d = p_data; - length = d["length"]; - slide_on_slope = d["slide_on_slope"]; - configure(Rect2(0, 0, 0.001, length)); -} - -Variant SeparationRayShape2DSW::get_data() const { - Dictionary d; - d["length"] = length; - d["slide_on_slope"] = slide_on_slope; - return d; -} - -/*********************************************************/ -/*********************************************************/ -/*********************************************************/ - -void SegmentShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { - if (Math::abs(p_normal.dot(n)) > _SEGMENT_IS_VALID_SUPPORT_THRESHOLD) { - r_supports[0] = a; - r_supports[1] = b; - r_amount = 2; - return; - } - - real_t dp = p_normal.dot(b - a); - if (dp > 0) { - *r_supports = b; - } else { - *r_supports = a; - } - r_amount = 1; -} - -bool SegmentShape2DSW::contains_point(const Vector2 &p_point) const { - return false; -} - -bool SegmentShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { - if (!Geometry2D::segment_intersects_segment(p_begin, p_end, a, b, &r_point)) { - return false; - } - - if (n.dot(p_begin) > n.dot(a)) { - r_normal = n; - } else { - r_normal = -n; - } - - return true; -} - -real_t SegmentShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { - return p_mass * ((a * p_scale).distance_squared_to(b * p_scale)) / 12; -} - -void SegmentShape2DSW::set_data(const Variant &p_data) { - ERR_FAIL_COND(p_data.get_type() != Variant::RECT2); - - Rect2 r = p_data; - a = r.position; - b = r.size; - n = (b - a).orthogonal(); - - Rect2 aabb; - aabb.position = a; - aabb.expand_to(b); - if (aabb.size.x == 0) { - aabb.size.x = 0.001; - } - if (aabb.size.y == 0) { - aabb.size.y = 0.001; - } - configure(aabb); -} - -Variant SegmentShape2DSW::get_data() const { - Rect2 r; - r.position = a; - r.size = b; - return r; -} - -/*********************************************************/ -/*********************************************************/ -/*********************************************************/ - -void CircleShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { - r_amount = 1; - *r_supports = p_normal * radius; -} - -bool CircleShape2DSW::contains_point(const Vector2 &p_point) const { - return p_point.length_squared() < radius * radius; -} - -bool CircleShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { - Vector2 line_vec = p_end - p_begin; - - real_t a, b, c; - - a = line_vec.dot(line_vec); - b = 2 * p_begin.dot(line_vec); - c = p_begin.dot(p_begin) - radius * radius; - - real_t sqrtterm = b * b - 4 * a * c; - - if (sqrtterm < 0) { - return false; - } - sqrtterm = Math::sqrt(sqrtterm); - real_t res = (-b - sqrtterm) / (2 * a); - - if (res < 0 || res > 1 + CMP_EPSILON) { - return false; - } - - r_point = p_begin + line_vec * res; - r_normal = r_point.normalized(); - return true; -} - -real_t CircleShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { - real_t a = radius * p_scale.x; - real_t b = radius * p_scale.y; - return p_mass * (a * a + b * b) / 4; -} - -void CircleShape2DSW::set_data(const Variant &p_data) { - ERR_FAIL_COND(!p_data.is_num()); - radius = p_data; - configure(Rect2(-radius, -radius, radius * 2, radius * 2)); -} - -Variant CircleShape2DSW::get_data() const { - return radius; -} - -/*********************************************************/ -/*********************************************************/ -/*********************************************************/ - -void RectangleShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { - for (int i = 0; i < 2; i++) { - Vector2 ag; - ag[i] = 1.0; - real_t dp = ag.dot(p_normal); - if (Math::abs(dp) < _SEGMENT_IS_VALID_SUPPORT_THRESHOLD) { - continue; - } - - real_t sgn = dp > 0 ? 1.0 : -1.0; - - r_amount = 2; - - r_supports[0][i] = half_extents[i] * sgn; - r_supports[0][i ^ 1] = half_extents[i ^ 1]; - - r_supports[1][i] = half_extents[i] * sgn; - r_supports[1][i ^ 1] = -half_extents[i ^ 1]; - - return; - } - - /* USE POINT */ - - r_amount = 1; - r_supports[0] = Vector2( - (p_normal.x < 0) ? -half_extents.x : half_extents.x, - (p_normal.y < 0) ? -half_extents.y : half_extents.y); -} - -bool RectangleShape2DSW::contains_point(const Vector2 &p_point) const { - real_t x = p_point.x; - real_t y = p_point.y; - real_t edge_x = half_extents.x; - real_t edge_y = half_extents.y; - return (x >= -edge_x) && (x < edge_x) && (y >= -edge_y) && (y < edge_y); -} - -bool RectangleShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { - return get_aabb().intersects_segment(p_begin, p_end, &r_point, &r_normal); -} - -real_t RectangleShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { - Vector2 he2 = half_extents * 2 * p_scale; - return p_mass * he2.dot(he2) / 12.0; -} - -void RectangleShape2DSW::set_data(const Variant &p_data) { - ERR_FAIL_COND(p_data.get_type() != Variant::VECTOR2); - - half_extents = p_data; - configure(Rect2(-half_extents, half_extents * 2.0)); -} - -Variant RectangleShape2DSW::get_data() const { - return half_extents; -} - -/*********************************************************/ -/*********************************************************/ -/*********************************************************/ - -void CapsuleShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { - Vector2 n = p_normal; - - real_t d = n.y; - - if (Math::abs(d) < (1.0 - _SEGMENT_IS_VALID_SUPPORT_THRESHOLD)) { - // make it flat - n.y = 0.0; - n.normalize(); - n *= radius; - - r_amount = 2; - r_supports[0] = n; - r_supports[0].y += height * 0.5 - radius; - r_supports[1] = n; - r_supports[1].y -= height * 0.5 - radius; - - } else { - real_t h = height * 0.5 - radius; - - n *= radius; - n.y += (d > 0) ? h : -h; - r_amount = 1; - *r_supports = n; - } -} - -bool CapsuleShape2DSW::contains_point(const Vector2 &p_point) const { - Vector2 p = p_point; - p.y = Math::abs(p.y); - p.y -= height * 0.5 - radius; - if (p.y < 0) { - p.y = 0; - } - - return p.length_squared() < radius * radius; -} - -bool CapsuleShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { - real_t d = 1e10; - Vector2 n = (p_end - p_begin).normalized(); - bool collided = false; - - //try spheres - for (int i = 0; i < 2; i++) { - Vector2 begin = p_begin; - Vector2 end = p_end; - real_t ofs = (i == 0) ? -height * 0.5 + radius : height * 0.5 - radius; - begin.y += ofs; - end.y += ofs; - - Vector2 line_vec = end - begin; - - real_t a, b, c; - - a = line_vec.dot(line_vec); - b = 2 * begin.dot(line_vec); - c = begin.dot(begin) - radius * radius; - - real_t sqrtterm = b * b - 4 * a * c; - - if (sqrtterm < 0) { - continue; - } - - sqrtterm = Math::sqrt(sqrtterm); - real_t res = (-b - sqrtterm) / (2 * a); - - if (res < 0 || res > 1 + CMP_EPSILON) { - continue; - } - - Vector2 point = begin + line_vec * res; - Vector2 pointf(point.x, point.y - ofs); - real_t pd = n.dot(pointf); - if (pd < d) { - r_point = pointf; - r_normal = point.normalized(); - d = pd; - collided = true; - } - } - - Vector2 rpos, rnorm; - if (Rect2(Point2(-radius, -height * 0.5 + radius), Size2(radius * 2.0, height - radius * 2)).intersects_segment(p_begin, p_end, &rpos, &rnorm)) { - real_t pd = n.dot(rpos); - if (pd < d) { - r_point = rpos; - r_normal = rnorm; - d = pd; - collided = true; - } - } - - //return get_aabb().intersects_segment(p_begin,p_end,&r_point,&r_normal); - return collided; //todo -} - -real_t CapsuleShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { - Vector2 he2 = Vector2(radius * 2, height) * p_scale; - return p_mass * he2.dot(he2) / 12.0; -} - -void CapsuleShape2DSW::set_data(const Variant &p_data) { - ERR_FAIL_COND(p_data.get_type() != Variant::ARRAY && p_data.get_type() != Variant::VECTOR2); - - if (p_data.get_type() == Variant::ARRAY) { - Array arr = p_data; - ERR_FAIL_COND(arr.size() != 2); - height = arr[0]; - radius = arr[1]; - } else { - Point2 p = p_data; - radius = p.x; - height = p.y; - } - - Point2 he(radius, height * 0.5); - configure(Rect2(-he, he * 2)); -} - -Variant CapsuleShape2DSW::get_data() const { - return Point2(height, radius); -} - -/*********************************************************/ -/*********************************************************/ -/*********************************************************/ - -void ConvexPolygonShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { - int support_idx = -1; - real_t d = -1e10; - r_amount = 0; - - for (int i = 0; i < point_count; i++) { - //test point - real_t ld = p_normal.dot(points[i].pos); - if (ld > d) { - support_idx = i; - d = ld; - } - - //test segment - if (points[i].normal.dot(p_normal) > _SEGMENT_IS_VALID_SUPPORT_THRESHOLD) { - r_amount = 2; - r_supports[0] = points[i].pos; - r_supports[1] = points[(i + 1) % point_count].pos; - return; - } - } - - ERR_FAIL_COND_MSG(support_idx == -1, "Convex polygon shape support not found."); - - r_amount = 1; - r_supports[0] = points[support_idx].pos; -} - -bool ConvexPolygonShape2DSW::contains_point(const Vector2 &p_point) const { - bool out = false; - bool in = false; - - for (int i = 0; i < point_count; i++) { - real_t d = points[i].normal.dot(p_point) - points[i].normal.dot(points[i].pos); - if (d > 0) { - out = true; - } else { - in = true; - } - } - - return in != out; -} - -bool ConvexPolygonShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { - Vector2 n = (p_end - p_begin).normalized(); - real_t d = 1e10; - bool inters = false; - - for (int i = 0; i < point_count; i++) { - //hmm.. no can do.. - /* - if (d.dot(points[i].normal)>=0) - continue; - */ - - Vector2 res; - - if (!Geometry2D::segment_intersects_segment(p_begin, p_end, points[i].pos, points[(i + 1) % point_count].pos, &res)) { - continue; - } - - real_t nd = n.dot(res); - if (nd < d) { - d = nd; - r_point = res; - r_normal = points[i].normal; - inters = true; - } - } - - return inters; -} - -real_t ConvexPolygonShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { - ERR_FAIL_COND_V_MSG(point_count == 0, 0, "Convex polygon shape has no points."); - Rect2 aabb; - aabb.position = points[0].pos * p_scale; - for (int i = 0; i < point_count; i++) { - aabb.expand_to(points[i].pos * p_scale); - } - - return p_mass * aabb.size.dot(aabb.size) / 12.0; -} - -void ConvexPolygonShape2DSW::set_data(const Variant &p_data) { -#ifdef REAL_T_IS_DOUBLE - ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT64_ARRAY); -#else - ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT32_ARRAY); -#endif - - if (points) { - memdelete_arr(points); - } - points = nullptr; - point_count = 0; - - if (p_data.get_type() == Variant::PACKED_VECTOR2_ARRAY) { - Vector arr = p_data; - ERR_FAIL_COND(arr.size() == 0); - point_count = arr.size(); - points = memnew_arr(Point, point_count); - const Vector2 *r = arr.ptr(); - - for (int i = 0; i < point_count; i++) { - points[i].pos = r[i]; - } - - for (int i = 0; i < point_count; i++) { - Vector2 p = points[i].pos; - Vector2 pn = points[(i + 1) % point_count].pos; - points[i].normal = (pn - p).orthogonal().normalized(); - } - } else { - Vector dvr = p_data; - point_count = dvr.size() / 4; - ERR_FAIL_COND(point_count == 0); - - points = memnew_arr(Point, point_count); - const real_t *r = dvr.ptr(); - - for (int i = 0; i < point_count; i++) { - int idx = i << 2; - points[i].pos.x = r[idx + 0]; - points[i].pos.y = r[idx + 1]; - points[i].normal.x = r[idx + 2]; - points[i].normal.y = r[idx + 3]; - } - } - - ERR_FAIL_COND(point_count == 0); - Rect2 aabb; - aabb.position = points[0].pos; - for (int i = 1; i < point_count; i++) { - aabb.expand_to(points[i].pos); - } - - configure(aabb); -} - -Variant ConvexPolygonShape2DSW::get_data() const { - Vector dvr; - - dvr.resize(point_count); - - for (int i = 0; i < point_count; i++) { - dvr.set(i, points[i].pos); - } - - return dvr; -} - -ConvexPolygonShape2DSW::~ConvexPolygonShape2DSW() { - if (points) { - memdelete_arr(points); - } -} - -////////////////////////////////////////////////// - -void ConcavePolygonShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { - real_t d = -1e10; - int idx = -1; - for (int i = 0; i < points.size(); i++) { - real_t ld = p_normal.dot(points[i]); - if (ld > d) { - d = ld; - idx = i; - } - } - - r_amount = 1; - ERR_FAIL_COND(idx == -1); - *r_supports = points[idx]; -} - -bool ConcavePolygonShape2DSW::contains_point(const Vector2 &p_point) const { - return false; //sorry -} - -bool ConcavePolygonShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { - if (segments.size() == 0 || points.size() == 0) { - return false; - } - - uint32_t *stack = (uint32_t *)alloca(sizeof(int) * bvh_depth); - - enum { - TEST_AABB_BIT = 0, - VISIT_LEFT_BIT = 1, - VISIT_RIGHT_BIT = 2, - VISIT_DONE_BIT = 3, - VISITED_BIT_SHIFT = 29, - NODE_IDX_MASK = (1 << VISITED_BIT_SHIFT) - 1, - VISITED_BIT_MASK = ~NODE_IDX_MASK, - - }; - - Vector2 n = (p_end - p_begin).normalized(); - real_t d = 1e10; - bool inters = false; - - /* - for(int i=0;i> VISITED_BIT_SHIFT) { - case TEST_AABB_BIT: { - bool valid = bvh.aabb.intersects_segment(p_begin, p_end); - if (!valid) { - stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; - - } else { - if (bvh.left < 0) { - const Segment &s = segmentptr[bvh.right]; - Vector2 a = pointptr[s.points[0]]; - Vector2 b = pointptr[s.points[1]]; - - Vector2 res; - - if (Geometry2D::segment_intersects_segment(p_begin, p_end, a, b, &res)) { - real_t nd = n.dot(res); - if (nd < d) { - d = nd; - r_point = res; - r_normal = (b - a).orthogonal().normalized(); - inters = true; - } - } - - stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; - - } else { - stack[level] = (VISIT_LEFT_BIT << VISITED_BIT_SHIFT) | node; - } - } - } - continue; - case VISIT_LEFT_BIT: { - stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node; - stack[level + 1] = bvh.left | TEST_AABB_BIT; - level++; - } - continue; - case VISIT_RIGHT_BIT: { - stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; - stack[level + 1] = bvh.right | TEST_AABB_BIT; - level++; - } - continue; - case VISIT_DONE_BIT: { - if (level == 0) { - done = true; - break; - } else { - level--; - } - } - continue; - } - - if (done) { - break; - } - } - - if (inters) { - if (n.dot(r_normal) > 0) { - r_normal = -r_normal; - } - } - - return inters; -} - -int ConcavePolygonShape2DSW::_generate_bvh(BVH *p_bvh, int p_len, int p_depth) { - if (p_len == 1) { - bvh_depth = MAX(p_depth, bvh_depth); - bvh.push_back(*p_bvh); - return bvh.size() - 1; - } - - //else sort best - - Rect2 global_aabb = p_bvh[0].aabb; - for (int i = 1; i < p_len; i++) { - global_aabb = global_aabb.merge(p_bvh[i].aabb); - } - - if (global_aabb.size.x > global_aabb.size.y) { - SortArray sort; - sort.sort(p_bvh, p_len); - - } else { - SortArray sort; - sort.sort(p_bvh, p_len); - } - - int median = p_len / 2; - - BVH node; - node.aabb = global_aabb; - int node_idx = bvh.size(); - bvh.push_back(node); - - int l = _generate_bvh(p_bvh, median, p_depth + 1); - int r = _generate_bvh(&p_bvh[median], p_len - median, p_depth + 1); - bvh.write[node_idx].left = l; - bvh.write[node_idx].right = r; - - return node_idx; -} - -void ConcavePolygonShape2DSW::set_data(const Variant &p_data) { -#ifdef REAL_T_IS_DOUBLE - ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT64_ARRAY); -#else - ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT32_ARRAY); -#endif - - Rect2 aabb; - - if (p_data.get_type() == Variant::PACKED_VECTOR2_ARRAY) { - Vector p2arr = p_data; - int len = p2arr.size(); - ERR_FAIL_COND(len % 2); - - segments.clear(); - points.clear(); - bvh.clear(); - bvh_depth = 1; - - if (len == 0) { - configure(aabb); - return; - } - - const Vector2 *arr = p2arr.ptr(); - - Map pointmap; - for (int i = 0; i < len; i += 2) { - Point2 p1 = arr[i]; - Point2 p2 = arr[i + 1]; - int idx_p1, idx_p2; - - if (pointmap.has(p1)) { - idx_p1 = pointmap[p1]; - } else { - idx_p1 = pointmap.size(); - pointmap[p1] = idx_p1; - } - - if (pointmap.has(p2)) { - idx_p2 = pointmap[p2]; - } else { - idx_p2 = pointmap.size(); - pointmap[p2] = idx_p2; - } - - Segment s; - s.points[0] = idx_p1; - s.points[1] = idx_p2; - segments.push_back(s); - } - - points.resize(pointmap.size()); - aabb.position = pointmap.front()->key(); - for (const KeyValue &E : pointmap) { - aabb.expand_to(E.key); - points.write[E.value] = E.key; - } - - Vector main_vbh; - main_vbh.resize(segments.size()); - for (int i = 0; i < main_vbh.size(); i++) { - main_vbh.write[i].aabb.position = points[segments[i].points[0]]; - main_vbh.write[i].aabb.expand_to(points[segments[i].points[1]]); - main_vbh.write[i].left = -1; - main_vbh.write[i].right = i; - } - - _generate_bvh(main_vbh.ptrw(), main_vbh.size(), 1); - - } else { - //dictionary with arrays - } - - configure(aabb); -} - -Variant ConcavePolygonShape2DSW::get_data() const { - Vector rsegments; - int len = segments.size(); - rsegments.resize(len * 2); - Vector2 *w = rsegments.ptrw(); - for (int i = 0; i < len; i++) { - w[(i << 1) + 0] = points[segments[i].points[0]]; - w[(i << 1) + 1] = points[segments[i].points[1]]; - } - - return rsegments; -} - -void ConcavePolygonShape2DSW::cull(const Rect2 &p_local_aabb, QueryCallback p_callback, void *p_userdata) const { - uint32_t *stack = (uint32_t *)alloca(sizeof(int) * bvh_depth); - - enum { - TEST_AABB_BIT = 0, - VISIT_LEFT_BIT = 1, - VISIT_RIGHT_BIT = 2, - VISIT_DONE_BIT = 3, - VISITED_BIT_SHIFT = 29, - NODE_IDX_MASK = (1 << VISITED_BIT_SHIFT) - 1, - VISITED_BIT_MASK = ~NODE_IDX_MASK, - - }; - - /* - for(int i=0;i> VISITED_BIT_SHIFT) { - case TEST_AABB_BIT: { - bool valid = p_local_aabb.intersects(bvh.aabb); - if (!valid) { - stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; - - } else { - if (bvh.left < 0) { - const Segment &s = segmentptr[bvh.right]; - Vector2 a = pointptr[s.points[0]]; - Vector2 b = pointptr[s.points[1]]; - - SegmentShape2DSW ss(a, b, (b - a).orthogonal().normalized()); - - if (p_callback(p_userdata, &ss)) { - return; - } - stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; - - } else { - stack[level] = (VISIT_LEFT_BIT << VISITED_BIT_SHIFT) | node; - } - } - } - continue; - case VISIT_LEFT_BIT: { - stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node; - stack[level + 1] = bvh.left | TEST_AABB_BIT; - level++; - } - continue; - case VISIT_RIGHT_BIT: { - stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; - stack[level + 1] = bvh.right | TEST_AABB_BIT; - level++; - } - continue; - case VISIT_DONE_BIT: { - if (level == 0) { - return; - } else { - level--; - } - } - continue; - } - } -} diff --git a/servers/physics_2d/shape_2d_sw.h b/servers/physics_2d/shape_2d_sw.h deleted file mode 100644 index c118826284..0000000000 --- a/servers/physics_2d/shape_2d_sw.h +++ /dev/null @@ -1,536 +0,0 @@ -/*************************************************************************/ -/* shape_2d_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/*************************************************************************/ - -#ifndef SHAPE_2D_2DSW_H -#define SHAPE_2D_2DSW_H - -#include "servers/physics_server_2d.h" -#define _SEGMENT_IS_VALID_SUPPORT_THRESHOLD 0.99998 - -class Shape2DSW; - -class ShapeOwner2DSW { -public: - virtual void _shape_changed() = 0; - virtual void remove_shape(Shape2DSW *p_shape) = 0; - - virtual ~ShapeOwner2DSW() {} -}; - -class Shape2DSW { - RID self; - Rect2 aabb; - bool configured = false; - real_t custom_bias = 0.0; - - Map owners; - -protected: - void configure(const Rect2 &p_aabb); - -public: - _FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; } - _FORCE_INLINE_ RID get_self() const { return self; } - - virtual PhysicsServer2D::ShapeType get_type() const = 0; - - _FORCE_INLINE_ Rect2 get_aabb() const { return aabb; } - _FORCE_INLINE_ bool is_configured() const { return configured; } - - virtual bool allows_one_way_collision() const { return true; } - - virtual bool is_concave() const { return false; } - - virtual bool contains_point(const Vector2 &p_point) const = 0; - - virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const = 0; - virtual void project_range_castv(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const = 0; - virtual Vector2 get_support(const Vector2 &p_normal) const; - virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const = 0; - - virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const = 0; - virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const = 0; - virtual void set_data(const Variant &p_data) = 0; - virtual Variant get_data() const = 0; - - _FORCE_INLINE_ void set_custom_bias(real_t p_bias) { custom_bias = p_bias; } - _FORCE_INLINE_ real_t get_custom_bias() const { return custom_bias; } - - void add_owner(ShapeOwner2DSW *p_owner); - void remove_owner(ShapeOwner2DSW *p_owner); - bool is_owner(ShapeOwner2DSW *p_owner) const; - const Map &get_owners() const; - - _FORCE_INLINE_ void get_supports_transformed_cast(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_xform, Vector2 *r_supports, int &r_amount) const { - get_supports(p_xform.basis_xform_inv(p_normal).normalized(), r_supports, r_amount); - for (int i = 0; i < r_amount; i++) { - r_supports[i] = p_xform.xform(r_supports[i]); - } - - if (r_amount == 1) { - if (Math::abs(p_normal.dot(p_cast.normalized())) < (1.0 - _SEGMENT_IS_VALID_SUPPORT_THRESHOLD)) { - //make line because they are parallel - r_amount = 2; - r_supports[1] = r_supports[0] + p_cast; - } else if (p_cast.dot(p_normal) > 0) { - //normal points towards cast, add cast - r_supports[0] += p_cast; - } - - } else { - if (Math::abs(p_normal.dot(p_cast.normalized())) < (1.0 - _SEGMENT_IS_VALID_SUPPORT_THRESHOLD)) { - //optimize line and make it larger because they are parallel - if ((r_supports[1] - r_supports[0]).dot(p_cast) > 0) { - //larger towards 1 - r_supports[1] += p_cast; - } else { - //larger towards 0 - r_supports[0] += p_cast; - } - } else if (p_cast.dot(p_normal) > 0) { - //normal points towards cast, add cast - r_supports[0] += p_cast; - r_supports[1] += p_cast; - } - } - } - Shape2DSW() {} - virtual ~Shape2DSW(); -}; - -//let the optimizer do the magic -#define DEFAULT_PROJECT_RANGE_CAST \ - virtual void project_range_castv(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { \ - project_range_cast(p_cast, p_normal, p_transform, r_min, r_max); \ - } \ - _FORCE_INLINE_ void project_range_cast(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { \ - real_t mina, maxa; \ - real_t minb, maxb; \ - Transform2D ofsb = p_transform; \ - ofsb.elements[2] += p_cast; \ - project_range(p_normal, p_transform, mina, maxa); \ - project_range(p_normal, ofsb, minb, maxb); \ - r_min = MIN(mina, minb); \ - r_max = MAX(maxa, maxb); \ - } - -class WorldBoundaryShape2DSW : public Shape2DSW { - Vector2 normal; - real_t d = 0.0; - -public: - _FORCE_INLINE_ Vector2 get_normal() const { return normal; } - _FORCE_INLINE_ real_t get_d() const { return d; } - - virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_WORLD_BOUNDARY; } - - virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); } - virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; - - virtual bool contains_point(const Vector2 &p_point) const override; - virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; - virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; - - virtual void set_data(const Variant &p_data) override; - virtual Variant get_data() const override; - - _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { - //real large - r_min = -1e10; - r_max = 1e10; - } - - virtual void project_range_castv(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { - project_range_cast(p_cast, p_normal, p_transform, r_min, r_max); - } - - _FORCE_INLINE_ void project_range_cast(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { - //real large - r_min = -1e10; - r_max = 1e10; - } -}; - -class SeparationRayShape2DSW : public Shape2DSW { - real_t length = 0.0; - bool slide_on_slope = false; - -public: - _FORCE_INLINE_ real_t get_length() const { return length; } - _FORCE_INLINE_ bool get_slide_on_slope() const { return slide_on_slope; } - - virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_SEPARATION_RAY; } - - virtual bool allows_one_way_collision() const override { return false; } - - virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); } - virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; - - virtual bool contains_point(const Vector2 &p_point) const override; - virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; - virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; - - virtual void set_data(const Variant &p_data) override; - virtual Variant get_data() const override; - - _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { - //real large - r_max = p_normal.dot(p_transform.get_origin()); - r_min = p_normal.dot(p_transform.xform(Vector2(0, length))); - if (r_max < r_min) { - SWAP(r_max, r_min); - } - } - - DEFAULT_PROJECT_RANGE_CAST - - _FORCE_INLINE_ SeparationRayShape2DSW() {} - _FORCE_INLINE_ SeparationRayShape2DSW(real_t p_length) { length = p_length; } -}; - -class SegmentShape2DSW : public Shape2DSW { - Vector2 a; - Vector2 b; - Vector2 n; - -public: - _FORCE_INLINE_ const Vector2 &get_a() const { return a; } - _FORCE_INLINE_ const Vector2 &get_b() const { return b; } - _FORCE_INLINE_ const Vector2 &get_normal() const { return n; } - - virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_SEGMENT; } - - _FORCE_INLINE_ Vector2 get_xformed_normal(const Transform2D &p_xform) const { - return (p_xform.xform(b) - p_xform.xform(a)).normalized().orthogonal(); - } - virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); } - virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; - - virtual bool contains_point(const Vector2 &p_point) const override; - virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; - virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; - - virtual void set_data(const Variant &p_data) override; - virtual Variant get_data() const override; - - _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { - //real large - r_max = p_normal.dot(p_transform.xform(a)); - r_min = p_normal.dot(p_transform.xform(b)); - if (r_max < r_min) { - SWAP(r_max, r_min); - } - } - - DEFAULT_PROJECT_RANGE_CAST - - _FORCE_INLINE_ SegmentShape2DSW() {} - _FORCE_INLINE_ SegmentShape2DSW(const Vector2 &p_a, const Vector2 &p_b, const Vector2 &p_n) { - a = p_a; - b = p_b; - n = p_n; - } -}; - -class CircleShape2DSW : public Shape2DSW { - real_t radius; - -public: - _FORCE_INLINE_ const real_t &get_radius() const { return radius; } - - virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_CIRCLE; } - - virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); } - virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; - - virtual bool contains_point(const Vector2 &p_point) const override; - virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; - virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; - - virtual void set_data(const Variant &p_data) override; - virtual Variant get_data() const override; - - _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { - //real large - real_t d = p_normal.dot(p_transform.get_origin()); - - // figure out scale at point - Vector2 local_normal = p_transform.basis_xform_inv(p_normal); - real_t scale = local_normal.length(); - - r_min = d - (radius)*scale; - r_max = d + (radius)*scale; - } - - DEFAULT_PROJECT_RANGE_CAST -}; - -class RectangleShape2DSW : public Shape2DSW { - Vector2 half_extents; - -public: - _FORCE_INLINE_ const Vector2 &get_half_extents() const { return half_extents; } - - virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_RECTANGLE; } - - virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); } - virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; - - virtual bool contains_point(const Vector2 &p_point) const override; - virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; - virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; - - virtual void set_data(const Variant &p_data) override; - virtual Variant get_data() const override; - - _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { - // no matter the angle, the box is mirrored anyway - r_max = -1e20; - r_min = 1e20; - for (int i = 0; i < 4; i++) { - real_t d = p_normal.dot(p_transform.xform(Vector2(((i & 1) * 2 - 1) * half_extents.x, ((i >> 1) * 2 - 1) * half_extents.y))); - - if (d > r_max) { - r_max = d; - } - if (d < r_min) { - r_min = d; - } - } - } - - _FORCE_INLINE_ Vector2 get_circle_axis(const Transform2D &p_xform, const Transform2D &p_xform_inv, const Vector2 &p_circle) const { - Vector2 local_v = p_xform_inv.xform(p_circle); - - Vector2 he( - (local_v.x < 0) ? -half_extents.x : half_extents.x, - (local_v.y < 0) ? -half_extents.y : half_extents.y); - - return (p_xform.xform(he) - p_circle).normalized(); - } - - _FORCE_INLINE_ Vector2 get_box_axis(const Transform2D &p_xform, const Transform2D &p_xform_inv, const RectangleShape2DSW *p_B, const Transform2D &p_B_xform, const Transform2D &p_B_xform_inv) const { - Vector2 a, b; - - { - Vector2 local_v = p_xform_inv.xform(p_B_xform.get_origin()); - - Vector2 he( - (local_v.x < 0) ? -half_extents.x : half_extents.x, - (local_v.y < 0) ? -half_extents.y : half_extents.y); - - a = p_xform.xform(he); - } - { - Vector2 local_v = p_B_xform_inv.xform(p_xform.get_origin()); - - Vector2 he( - (local_v.x < 0) ? -p_B->half_extents.x : p_B->half_extents.x, - (local_v.y < 0) ? -p_B->half_extents.y : p_B->half_extents.y); - - b = p_B_xform.xform(he); - } - - return (a - b).normalized(); - } - - DEFAULT_PROJECT_RANGE_CAST -}; - -class CapsuleShape2DSW : public Shape2DSW { - real_t radius = 0.0; - real_t height = 0.0; - -public: - _FORCE_INLINE_ const real_t &get_radius() const { return radius; } - _FORCE_INLINE_ const real_t &get_height() const { return height; } - - virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_CAPSULE; } - - virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); } - virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; - - virtual bool contains_point(const Vector2 &p_point) const override; - virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; - virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; - - virtual void set_data(const Variant &p_data) override; - virtual Variant get_data() const override; - - _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { - // no matter the angle, the box is mirrored anyway - Vector2 n = p_transform.basis_xform_inv(p_normal).normalized(); - real_t h = height * 0.5 - radius; - - n *= radius; - n.y += (n.y > 0) ? h : -h; - - r_max = p_normal.dot(p_transform.xform(n)); - r_min = p_normal.dot(p_transform.xform(-n)); - - if (r_max < r_min) { - SWAP(r_max, r_min); - } - - //ERR_FAIL_COND( r_max < r_min ); - } - - DEFAULT_PROJECT_RANGE_CAST -}; - -class ConvexPolygonShape2DSW : public Shape2DSW { - struct Point { - Vector2 pos; - Vector2 normal; //normal to next segment - }; - - Point *points = nullptr; - int point_count = 0; - -public: - _FORCE_INLINE_ int get_point_count() const { return point_count; } - _FORCE_INLINE_ const Vector2 &get_point(int p_idx) const { return points[p_idx].pos; } - _FORCE_INLINE_ const Vector2 &get_segment_normal(int p_idx) const { return points[p_idx].normal; } - _FORCE_INLINE_ Vector2 get_xformed_segment_normal(const Transform2D &p_xform, int p_idx) const { - Vector2 a = points[p_idx].pos; - p_idx++; - Vector2 b = points[p_idx == point_count ? 0 : p_idx].pos; - return (p_xform.xform(b) - p_xform.xform(a)).normalized().orthogonal(); - } - - virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_CONVEX_POLYGON; } - - virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); } - virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; - - virtual bool contains_point(const Vector2 &p_point) const override; - virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; - virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; - - virtual void set_data(const Variant &p_data) override; - virtual Variant get_data() const override; - - _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { - if (!points || point_count <= 0) { - r_min = r_max = 0; - return; - } - - r_min = r_max = p_normal.dot(p_transform.xform(points[0].pos)); - for (int i = 1; i < point_count; i++) { - real_t d = p_normal.dot(p_transform.xform(points[i].pos)); - if (d > r_max) { - r_max = d; - } - if (d < r_min) { - r_min = d; - } - } - } - - DEFAULT_PROJECT_RANGE_CAST - - ConvexPolygonShape2DSW() {} - ~ConvexPolygonShape2DSW(); -}; - -class ConcaveShape2DSW : public Shape2DSW { -public: - virtual bool is_concave() const override { return true; } - - // Returns true to stop the query. - typedef bool (*QueryCallback)(void *p_userdata, Shape2DSW *p_convex); - - virtual void cull(const Rect2 &p_local_aabb, QueryCallback p_callback, void *p_userdata) const = 0; -}; - -class ConcavePolygonShape2DSW : public ConcaveShape2DSW { - struct Segment { - int points[2] = {}; - }; - - Vector segments; - Vector points; - - struct BVH { - Rect2 aabb; - int left = 0, right = 0; - }; - - Vector bvh; - int bvh_depth = 0; - - struct BVH_CompareX { - _FORCE_INLINE_ bool operator()(const BVH &a, const BVH &b) const { - return (a.aabb.position.x + a.aabb.size.x * 0.5) < (b.aabb.position.x + b.aabb.size.x * 0.5); - } - }; - - struct BVH_CompareY { - _FORCE_INLINE_ bool operator()(const BVH &a, const BVH &b) const { - return (a.aabb.position.y + a.aabb.size.y * 0.5) < (b.aabb.position.y + b.aabb.size.y * 0.5); - } - }; - - int _generate_bvh(BVH *p_bvh, int p_len, int p_depth); - -public: - virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_CONCAVE_POLYGON; } - - virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { - r_min = 0; - r_max = 0; - ERR_FAIL_MSG("Unsupported call to project_rangev in ConcavePolygonShape2DSW"); - } - - void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { - r_min = 0; - r_max = 0; - ERR_FAIL_MSG("Unsupported call to project_range in ConcavePolygonShape2DSW"); - } - - virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; - - virtual bool contains_point(const Vector2 &p_point) const override; - virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; - - virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override { return 0; } - - virtual void set_data(const Variant &p_data) override; - virtual Variant get_data() const override; - - virtual void cull(const Rect2 &p_local_aabb, QueryCallback p_callback, void *p_userdata) const override; - - DEFAULT_PROJECT_RANGE_CAST -}; - -#undef DEFAULT_PROJECT_RANGE_CAST - -#endif // SHAPE_2D_2DSW_H diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp deleted file mode 100644 index dd0780b5ff..0000000000 --- a/servers/physics_2d/space_2d_sw.cpp +++ /dev/null @@ -1,1212 +0,0 @@ -/*************************************************************************/ -/* space_2d_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/*************************************************************************/ - -#include "space_2d_sw.h" - -#include "collision_solver_2d_sw.h" -#include "core/os/os.h" -#include "core/templates/pair.h" -#include "physics_server_2d_sw.h" - -#define TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR 0.05 - -_FORCE_INLINE_ static bool _can_collide_with(CollisionObject2DSW *p_object, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { - if (!(p_object->get_collision_layer() & p_collision_mask)) { - return false; - } - - if (p_object->get_type() == CollisionObject2DSW::TYPE_AREA && !p_collide_with_areas) { - return false; - } - - if (p_object->get_type() == CollisionObject2DSW::TYPE_BODY && !p_collide_with_bodies) { - return false; - } - - return true; -} - -int PhysicsDirectSpaceState2DSW::_intersect_point_impl(const Vector2 &p_point, ShapeResult *r_results, int p_result_max, const Set &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, bool p_pick_point, bool p_filter_by_canvas, ObjectID p_canvas_instance_id) { - if (p_result_max <= 0) { - return 0; - } - - Rect2 aabb; - aabb.position = p_point - Vector2(0.00001, 0.00001); - aabb.size = Vector2(0.00002, 0.00002); - - int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, Space2DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); - - int cc = 0; - - for (int i = 0; i < amount; i++) { - if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { - continue; - } - - if (p_exclude.has(space->intersection_query_results[i]->get_self())) { - continue; - } - - const CollisionObject2DSW *col_obj = space->intersection_query_results[i]; - - if (p_pick_point && !col_obj->is_pickable()) { - continue; - } - - if (p_filter_by_canvas && col_obj->get_canvas_instance_id() != p_canvas_instance_id) { - continue; - } - - int shape_idx = space->intersection_query_subindex_results[i]; - - Shape2DSW *shape = col_obj->get_shape(shape_idx); - - Vector2 local_point = (col_obj->get_transform() * col_obj->get_shape_transform(shape_idx)).affine_inverse().xform(p_point); - - if (!shape->contains_point(local_point)) { - continue; - } - - if (cc >= p_result_max) { - continue; - } - - r_results[cc].collider_id = col_obj->get_instance_id(); - if (r_results[cc].collider_id.is_valid()) { - r_results[cc].collider = ObjectDB::get_instance(r_results[cc].collider_id); - } - r_results[cc].rid = col_obj->get_self(); - r_results[cc].shape = shape_idx; - - cc++; - } - - return cc; -} - -int PhysicsDirectSpaceState2DSW::intersect_point(const Vector2 &p_point, ShapeResult *r_results, int p_result_max, const Set &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, bool p_pick_point) { - return _intersect_point_impl(p_point, r_results, p_result_max, p_exclude, p_collision_mask, p_collide_with_bodies, p_collide_with_areas, p_pick_point); -} - -int PhysicsDirectSpaceState2DSW::intersect_point_on_canvas(const Vector2 &p_point, ObjectID p_canvas_instance_id, ShapeResult *r_results, int p_result_max, const Set &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, bool p_pick_point) { - return _intersect_point_impl(p_point, r_results, p_result_max, p_exclude, p_collision_mask, p_collide_with_bodies, p_collide_with_areas, p_pick_point, true, p_canvas_instance_id); -} - -bool PhysicsDirectSpaceState2DSW::intersect_ray(const Vector2 &p_from, const Vector2 &p_to, RayResult &r_result, const Set &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { - ERR_FAIL_COND_V(space->locked, false); - - Vector2 begin, end; - Vector2 normal; - begin = p_from; - end = p_to; - normal = (end - begin).normalized(); - - int amount = space->broadphase->cull_segment(begin, end, space->intersection_query_results, Space2DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); - - //todo, create another array that references results, compute AABBs and check closest point to ray origin, sort, and stop evaluating results when beyond first collision - - bool collided = false; - Vector2 res_point, res_normal; - int res_shape; - const CollisionObject2DSW *res_obj; - real_t min_d = 1e10; - - for (int i = 0; i < amount; i++) { - if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { - continue; - } - - if (p_exclude.has(space->intersection_query_results[i]->get_self())) { - continue; - } - - const CollisionObject2DSW *col_obj = space->intersection_query_results[i]; - - int shape_idx = space->intersection_query_subindex_results[i]; - Transform2D inv_xform = col_obj->get_shape_inv_transform(shape_idx) * col_obj->get_inv_transform(); - - Vector2 local_from = inv_xform.xform(begin); - Vector2 local_to = inv_xform.xform(end); - - /*local_from = col_obj->get_inv_transform().xform(begin); - local_from = col_obj->get_shape_inv_transform(shape_idx).xform(local_from); - - local_to = col_obj->get_inv_transform().xform(end); - local_to = col_obj->get_shape_inv_transform(shape_idx).xform(local_to);*/ - - const Shape2DSW *shape = col_obj->get_shape(shape_idx); - - Vector2 shape_point, shape_normal; - - if (shape->intersect_segment(local_from, local_to, shape_point, shape_normal)) { - Transform2D xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); - shape_point = xform.xform(shape_point); - - real_t ld = normal.dot(shape_point); - - if (ld < min_d) { - min_d = ld; - res_point = shape_point; - res_normal = inv_xform.basis_xform_inv(shape_normal).normalized(); - res_shape = shape_idx; - res_obj = col_obj; - collided = true; - } - } - } - - if (!collided) { - return false; - } - - r_result.collider_id = res_obj->get_instance_id(); - if (r_result.collider_id.is_valid()) { - r_result.collider = ObjectDB::get_instance(r_result.collider_id); - } - r_result.normal = res_normal; - r_result.position = res_point; - r_result.rid = res_obj->get_self(); - r_result.shape = res_shape; - - return true; -} - -int PhysicsDirectSpaceState2DSW::intersect_shape(const RID &p_shape, const Transform2D &p_xform, const Vector2 &p_motion, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { - if (p_result_max <= 0) { - return 0; - } - - Shape2DSW *shape = PhysicsServer2DSW::singletonsw->shape_owner.get_or_null(p_shape); - ERR_FAIL_COND_V(!shape, 0); - - Rect2 aabb = p_xform.xform(shape->get_aabb()); - aabb = aabb.grow(p_margin); - - int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, Space2DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); - - int cc = 0; - - for (int i = 0; i < amount; i++) { - if (cc >= p_result_max) { - break; - } - - if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { - continue; - } - - if (p_exclude.has(space->intersection_query_results[i]->get_self())) { - continue; - } - - const CollisionObject2DSW *col_obj = space->intersection_query_results[i]; - int shape_idx = space->intersection_query_subindex_results[i]; - - if (!CollisionSolver2DSW::solve(shape, p_xform, p_motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), nullptr, nullptr, nullptr, p_margin)) { - continue; - } - - r_results[cc].collider_id = col_obj->get_instance_id(); - if (r_results[cc].collider_id.is_valid()) { - r_results[cc].collider = ObjectDB::get_instance(r_results[cc].collider_id); - } - r_results[cc].rid = col_obj->get_self(); - r_results[cc].shape = shape_idx; - - cc++; - } - - return cc; -} - -bool PhysicsDirectSpaceState2DSW::cast_motion(const RID &p_shape, const Transform2D &p_xform, const Vector2 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { - Shape2DSW *shape = PhysicsServer2DSW::singletonsw->shape_owner.get_or_null(p_shape); - ERR_FAIL_COND_V(!shape, false); - - Rect2 aabb = p_xform.xform(shape->get_aabb()); - aabb = aabb.merge(Rect2(aabb.position + p_motion, aabb.size)); //motion - aabb = aabb.grow(p_margin); - - int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, Space2DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); - - real_t best_safe = 1; - real_t best_unsafe = 1; - - for (int i = 0; i < amount; i++) { - if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { - continue; - } - - if (p_exclude.has(space->intersection_query_results[i]->get_self())) { - continue; //ignore excluded - } - - const CollisionObject2DSW *col_obj = space->intersection_query_results[i]; - int shape_idx = space->intersection_query_subindex_results[i]; - - Transform2D 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 (!CollisionSolver2DSW::solve(shape, p_xform, p_motion, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, nullptr, p_margin)) { - continue; - } - - //test initial overlap, ignore objects it's inside of. - if (CollisionSolver2DSW::solve(shape, p_xform, Vector2(), col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, nullptr, p_margin)) { - continue; - } - - Vector2 mnormal = p_motion.normalized(); - - //just do kinematic solving - real_t low = 0.0; - real_t hi = 1.0; - real_t fraction_coeff = 0.5; - for (int j = 0; j < 8; j++) { //steps should be customizable.. - real_t fraction = low + (hi - low) * fraction_coeff; - - Vector2 sep = mnormal; //important optimization for this to work fast enough - bool collided = CollisionSolver2DSW::solve(shape, p_xform, p_motion * fraction, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, &sep, p_margin); - - if (collided) { - hi = fraction; - if ((j == 0) || (low > 0.0)) { // Did it not collide before? - // When alternating or first iteration, use dichotomy. - fraction_coeff = 0.5; - } else { - // When colliding again, converge faster towards low fraction - // for more accurate results with long motions that collide near the start. - fraction_coeff = 0.25; - } - } else { - low = fraction; - if ((j == 0) || (hi < 1.0)) { // Did it collide before? - // When alternating or first iteration, use dichotomy. - fraction_coeff = 0.5; - } else { - // When not colliding again, converge faster towards high fraction - // for more accurate results with long motions that collide near the end. - fraction_coeff = 0.75; - } - } - } - - if (low < best_safe) { - best_safe = low; - best_unsafe = hi; - } - } - - p_closest_safe = best_safe; - p_closest_unsafe = best_unsafe; - - return true; -} - -bool PhysicsDirectSpaceState2DSW::collide_shape(RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, real_t p_margin, Vector2 *r_results, int p_result_max, int &r_result_count, const Set &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { - if (p_result_max <= 0) { - return false; - } - - Shape2DSW *shape = PhysicsServer2DSW::singletonsw->shape_owner.get_or_null(p_shape); - ERR_FAIL_COND_V(!shape, 0); - - Rect2 aabb = p_shape_xform.xform(shape->get_aabb()); - aabb = aabb.merge(Rect2(aabb.position + p_motion, aabb.size)); //motion - aabb = aabb.grow(p_margin); - - int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, Space2DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); - - bool collided = false; - r_result_count = 0; - - PhysicsServer2DSW::CollCbkData cbk; - cbk.max = p_result_max; - cbk.amount = 0; - cbk.passed = 0; - cbk.ptr = r_results; - CollisionSolver2DSW::CallbackResult cbkres = PhysicsServer2DSW::_shape_col_cbk; - - PhysicsServer2DSW::CollCbkData *cbkptr = &cbk; - - for (int i = 0; i < amount; i++) { - if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { - continue; - } - - const CollisionObject2DSW *col_obj = space->intersection_query_results[i]; - - if (p_exclude.has(col_obj->get_self())) { - continue; - } - - int shape_idx = space->intersection_query_subindex_results[i]; - - cbk.valid_dir = Vector2(); - cbk.valid_depth = 0; - - if (CollisionSolver2DSW::solve(shape, p_shape_xform, p_motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), cbkres, cbkptr, nullptr, p_margin)) { - collided = cbk.amount > 0; - } - } - - r_result_count = cbk.amount; - - return collided; -} - -struct _RestCallbackData2D { - const CollisionObject2DSW *object = nullptr; - const CollisionObject2DSW *best_object = nullptr; - int local_shape = 0; - int best_local_shape = 0; - int shape = 0; - int best_shape = 0; - Vector2 best_contact; - Vector2 best_normal; - real_t best_len = 0.0; - Vector2 valid_dir; - real_t valid_depth = 0.0; - real_t min_allowed_depth = 0.0; -}; - -static void _rest_cbk_result(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_userdata) { - _RestCallbackData2D *rd = (_RestCallbackData2D *)p_userdata; - - Vector2 contact_rel = p_point_B - p_point_A; - real_t len = contact_rel.length(); - - if (len < rd->min_allowed_depth) { - return; - } - - if (len <= rd->best_len) { - return; - } - - Vector2 normal = contact_rel / len; - - if (rd->valid_dir != Vector2()) { - if (len > rd->valid_depth) { - return; - } - - if (rd->valid_dir.dot(normal) > -CMP_EPSILON) { - return; - } - } - - rd->best_len = len; - rd->best_contact = p_point_B; - rd->best_normal = normal; - rd->best_object = rd->object; - rd->best_shape = rd->shape; - rd->best_local_shape = rd->local_shape; -} - -bool PhysicsDirectSpaceState2DSW::rest_info(RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, real_t p_margin, ShapeRestInfo *r_info, const Set &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { - Shape2DSW *shape = PhysicsServer2DSW::singletonsw->shape_owner.get_or_null(p_shape); - ERR_FAIL_COND_V(!shape, 0); - - real_t min_contact_depth = p_margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR; - - Rect2 aabb = p_shape_xform.xform(shape->get_aabb()); - aabb = aabb.merge(Rect2(aabb.position + p_motion, aabb.size)); //motion - aabb = aabb.grow(p_margin); - - int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, Space2DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); - - _RestCallbackData2D rcd; - rcd.best_len = 0; - rcd.best_object = nullptr; - rcd.best_shape = 0; - rcd.min_allowed_depth = min_contact_depth; - - for (int i = 0; i < amount; i++) { - if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { - continue; - } - - const CollisionObject2DSW *col_obj = space->intersection_query_results[i]; - - if (p_exclude.has(col_obj->get_self())) { - continue; - } - - int shape_idx = space->intersection_query_subindex_results[i]; - - rcd.valid_dir = Vector2(); - rcd.object = col_obj; - rcd.shape = shape_idx; - rcd.local_shape = 0; - bool sc = CollisionSolver2DSW::solve(shape, p_shape_xform, p_motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), _rest_cbk_result, &rcd, nullptr, p_margin); - if (!sc) { - continue; - } - } - - if (rcd.best_len == 0 || !rcd.best_object) { - return false; - } - - r_info->collider_id = rcd.best_object->get_instance_id(); - r_info->shape = rcd.best_shape; - r_info->normal = rcd.best_normal; - r_info->point = rcd.best_contact; - r_info->rid = rcd.best_object->get_self(); - if (rcd.best_object->get_type() == CollisionObject2DSW::TYPE_BODY) { - const Body2DSW *body = static_cast(rcd.best_object); - Vector2 rel_vec = r_info->point - (body->get_transform().get_origin() + body->get_center_of_mass()); - r_info->linear_velocity = Vector2(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity(); - - } else { - r_info->linear_velocity = Vector2(); - } - - return true; -} - -//////////////////////////////////////////////////////////////////////////////////////////////////////////// - -int Space2DSW::_cull_aabb_for_body(Body2DSW *p_body, const Rect2 &p_aabb) { - int amount = broadphase->cull_aabb(p_aabb, intersection_query_results, INTERSECTION_QUERY_MAX, intersection_query_subindex_results); - - for (int i = 0; i < amount; i++) { - bool keep = true; - - if (intersection_query_results[i] == p_body) { - keep = false; - } else if (intersection_query_results[i]->get_type() == CollisionObject2DSW::TYPE_AREA) { - keep = false; - } else if (!p_body->collides_with(static_cast(intersection_query_results[i]))) { - keep = false; - } else if (static_cast(intersection_query_results[i])->has_exception(p_body->get_self()) || p_body->has_exception(intersection_query_results[i]->get_self())) { - keep = false; - } - - if (!keep) { - if (i < amount - 1) { - SWAP(intersection_query_results[i], intersection_query_results[amount - 1]); - SWAP(intersection_query_subindex_results[i], intersection_query_subindex_results[amount - 1]); - } - - amount--; - i--; - } - } - - return amount; -} - -bool Space2DSW::test_body_motion(Body2DSW *p_body, const PhysicsServer2D::MotionParameters &p_parameters, PhysicsServer2D::MotionResult *r_result) { - //give me back regular physics engine logic - //this is madness - //and most people using this function will think - //what it does is simpler than using physics - //this took about a week to get right.. - //but is it right? who knows at this point.. - - if (r_result) { - r_result->collider_id = ObjectID(); - r_result->collider_shape = 0; - } - Rect2 body_aabb; - - bool shapes_found = false; - - for (int i = 0; i < p_body->get_shape_count(); i++) { - if (p_body->is_shape_disabled(i)) { - continue; - } - - if (!shapes_found) { - body_aabb = p_body->get_shape_aabb(i); - shapes_found = true; - } else { - body_aabb = body_aabb.merge(p_body->get_shape_aabb(i)); - } - } - - if (!shapes_found) { - if (r_result) { - *r_result = PhysicsServer2D::MotionResult(); - r_result->travel = p_parameters.motion; - } - return false; - } - - // Undo the currently transform the physics server is aware of and apply the provided one - body_aabb = p_parameters.from.xform(p_body->get_inv_transform().xform(body_aabb)); - body_aabb = body_aabb.grow(p_parameters.margin); - - static const int max_excluded_shape_pairs = 32; - ExcludedShapeSW excluded_shape_pairs[max_excluded_shape_pairs]; - int excluded_shape_pair_count = 0; - - real_t min_contact_depth = p_parameters.margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR; - - real_t motion_length = p_parameters.motion.length(); - Vector2 motion_normal = p_parameters.motion / motion_length; - - Transform2D body_transform = p_parameters.from; - - bool recovered = false; - - { - //STEP 1, FREE BODY IF STUCK - - const int max_results = 32; - int recover_attempts = 4; - Vector2 sr[max_results * 2]; - - do { - PhysicsServer2DSW::CollCbkData cbk; - cbk.max = max_results; - cbk.amount = 0; - cbk.passed = 0; - cbk.ptr = sr; - cbk.invalid_by_dir = 0; - excluded_shape_pair_count = 0; //last step is the one valid - - PhysicsServer2DSW::CollCbkData *cbkptr = &cbk; - CollisionSolver2DSW::CallbackResult cbkres = PhysicsServer2DSW::_shape_col_cbk; - - bool collided = false; - - int amount = _cull_aabb_for_body(p_body, body_aabb); - - for (int j = 0; j < p_body->get_shape_count(); j++) { - if (p_body->is_shape_disabled(j)) { - continue; - } - - Shape2DSW *body_shape = p_body->get_shape(j); - Transform2D body_shape_xform = body_transform * p_body->get_shape_transform(j); - - for (int i = 0; i < amount; i++) { - const CollisionObject2DSW *col_obj = intersection_query_results[i]; - if (p_parameters.exclude_bodies.has(col_obj->get_self())) { - continue; - } - if (p_parameters.exclude_objects.has(col_obj->get_instance_id())) { - continue; - } - - int shape_idx = intersection_query_subindex_results[i]; - - Transform2D col_obj_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); - - if (body_shape->allows_one_way_collision() && col_obj->is_shape_set_as_one_way_collision(shape_idx)) { - cbk.valid_dir = col_obj_shape_xform.get_axis(1).normalized(); - - real_t owc_margin = col_obj->get_shape_one_way_collision_margin(shape_idx); - cbk.valid_depth = MAX(owc_margin, p_parameters.margin); //user specified, but never less than actual margin or it won't work - cbk.invalid_by_dir = 0; - - if (col_obj->get_type() == CollisionObject2DSW::TYPE_BODY) { - const Body2DSW *b = static_cast(col_obj); - if (b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_DYNAMIC) { - //fix for moving platforms (kinematic and dynamic), margin is increased by how much it moved in the given direction - Vector2 lv = b->get_linear_velocity(); - //compute displacement from linear velocity - Vector2 motion = lv * last_step; - real_t motion_len = motion.length(); - motion.normalize(); - cbk.valid_depth += motion_len * MAX(motion.dot(-cbk.valid_dir), 0.0); - } - } - } else { - cbk.valid_dir = Vector2(); - cbk.valid_depth = 0; - cbk.invalid_by_dir = 0; - } - - int current_passed = cbk.passed; //save how many points passed collision - bool did_collide = false; - - Shape2DSW *against_shape = col_obj->get_shape(shape_idx); - if (CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), cbkres, cbkptr, nullptr, p_parameters.margin)) { - did_collide = cbk.passed > current_passed; //more passed, so collision actually existed - } - - if (!did_collide && cbk.invalid_by_dir > 0) { - //this shape must be excluded - if (excluded_shape_pair_count < max_excluded_shape_pairs) { - ExcludedShapeSW esp; - esp.local_shape = body_shape; - esp.against_object = col_obj; - esp.against_shape_index = shape_idx; - excluded_shape_pairs[excluded_shape_pair_count++] = esp; - } - } - - if (did_collide) { - collided = true; - } - } - } - - if (!collided) { - break; - } - - recovered = true; - - Vector2 recover_motion; - for (int i = 0; i < cbk.amount; i++) { - Vector2 a = sr[i * 2 + 0]; - Vector2 b = sr[i * 2 + 1]; - - // Compute plane on b towards a. - Vector2 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 > min_contact_depth + CMP_EPSILON) { - // Only recover if there is penetration. - recover_motion -= n * (depth - min_contact_depth) * 0.4; - } - } - - if (recover_motion == Vector2()) { - collided = false; - break; - } - - body_transform.elements[2] += recover_motion; - body_aabb.position += recover_motion; - - recover_attempts--; - - } while (recover_attempts); - } - - real_t safe = 1.0; - real_t unsafe = 1.0; - int best_shape = -1; - - { - // STEP 2 ATTEMPT MOTION - - Rect2 motion_aabb = body_aabb; - motion_aabb.position += p_parameters.motion; - motion_aabb = motion_aabb.merge(body_aabb); - - int amount = _cull_aabb_for_body(p_body, motion_aabb); - - for (int body_shape_idx = 0; body_shape_idx < p_body->get_shape_count(); body_shape_idx++) { - if (p_body->is_shape_disabled(body_shape_idx)) { - continue; - } - - Shape2DSW *body_shape = p_body->get_shape(body_shape_idx); - - // Colliding separation rays allows to properly snap to the ground, - // otherwise it's not needed in regular motion. - if (!p_parameters.collide_separation_ray && (body_shape->get_type() == PhysicsServer2D::SHAPE_SEPARATION_RAY)) { - // When slide on slope is on, separation ray shape acts like a regular shape. - if (!static_cast(body_shape)->get_slide_on_slope()) { - continue; - } - } - - Transform2D body_shape_xform = body_transform * p_body->get_shape_transform(body_shape_idx); - - bool stuck = false; - - real_t best_safe = 1; - real_t best_unsafe = 1; - - for (int i = 0; i < amount; i++) { - const CollisionObject2DSW *col_obj = intersection_query_results[i]; - if (p_parameters.exclude_bodies.has(col_obj->get_self())) { - continue; - } - if (p_parameters.exclude_objects.has(col_obj->get_instance_id())) { - continue; - } - - int col_shape_idx = intersection_query_subindex_results[i]; - Shape2DSW *against_shape = col_obj->get_shape(col_shape_idx); - - bool excluded = false; - - for (int k = 0; k < excluded_shape_pair_count; k++) { - if (excluded_shape_pairs[k].local_shape == body_shape && excluded_shape_pairs[k].against_object == col_obj && excluded_shape_pairs[k].against_shape_index == col_shape_idx) { - excluded = true; - break; - } - } - - if (excluded) { - continue; - } - - Transform2D col_obj_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(col_shape_idx); - //test initial overlap, does it collide if going all the way? - if (!CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_parameters.motion, against_shape, col_obj_shape_xform, Vector2(), nullptr, nullptr, nullptr, 0)) { - continue; - } - - //test initial overlap - if (CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), nullptr, nullptr, nullptr, 0)) { - if (body_shape->allows_one_way_collision() && col_obj->is_shape_set_as_one_way_collision(col_shape_idx)) { - Vector2 direction = col_obj_shape_xform.get_axis(1).normalized(); - if (motion_normal.dot(direction) < 0) { - continue; - } - } - - stuck = true; - break; - } - - //just do kinematic solving - real_t low = 0.0; - real_t hi = 1.0; - real_t fraction_coeff = 0.5; - for (int k = 0; k < 8; k++) { //steps should be customizable.. - real_t fraction = low + (hi - low) * fraction_coeff; - - Vector2 sep = motion_normal; //important optimization for this to work fast enough - bool collided = CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_parameters.motion * fraction, against_shape, col_obj_shape_xform, Vector2(), nullptr, nullptr, &sep, 0); - - if (collided) { - hi = fraction; - if ((k == 0) || (low > 0.0)) { // Did it not collide before? - // When alternating or first iteration, use dichotomy. - fraction_coeff = 0.5; - } else { - // When colliding again, converge faster towards low fraction - // for more accurate results with long motions that collide near the start. - fraction_coeff = 0.25; - } - } else { - low = fraction; - if ((k == 0) || (hi < 1.0)) { // Did it collide before? - // When alternating or first iteration, use dichotomy. - fraction_coeff = 0.5; - } else { - // When not colliding again, converge faster towards high fraction - // for more accurate results with long motions that collide near the end. - fraction_coeff = 0.75; - } - } - } - - if (body_shape->allows_one_way_collision() && col_obj->is_shape_set_as_one_way_collision(col_shape_idx)) { - Vector2 cd[2]; - PhysicsServer2DSW::CollCbkData cbk; - cbk.max = 1; - cbk.amount = 0; - cbk.passed = 0; - cbk.ptr = cd; - cbk.valid_dir = col_obj_shape_xform.get_axis(1).normalized(); - - cbk.valid_depth = 10e20; - - Vector2 sep = motion_normal; //important optimization for this to work fast enough - bool collided = CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_parameters.motion * (hi + contact_max_allowed_penetration), col_obj->get_shape(col_shape_idx), col_obj_shape_xform, Vector2(), PhysicsServer2DSW::_shape_col_cbk, &cbk, &sep, 0); - if (!collided || cbk.amount == 0) { - continue; - } - } - - if (low < best_safe) { - best_safe = low; - best_unsafe = hi; - } - } - - if (stuck) { - safe = 0; - unsafe = 0; - best_shape = body_shape_idx; //sadly it's the best - break; - } - if (best_safe == 1.0) { - continue; - } - if (best_safe < safe) { - safe = best_safe; - unsafe = best_unsafe; - best_shape = body_shape_idx; - } - } - } - - bool collided = false; - - if (recovered || (safe < 1)) { - if (safe >= 1) { - best_shape = -1; //no best shape with cast, reset to -1 - } - - //it collided, let's get the rest info in unsafe advance - Transform2D ugt = body_transform; - ugt.elements[2] += p_parameters.motion * unsafe; - - _RestCallbackData2D rcd; - rcd.best_len = 0; - rcd.best_object = nullptr; - rcd.best_shape = 0; - - // Allowed depth can't be lower than motion length, in order to handle contacts at low speed. - rcd.min_allowed_depth = MIN(motion_length, min_contact_depth); - - int from_shape = best_shape != -1 ? best_shape : 0; - int to_shape = best_shape != -1 ? best_shape + 1 : p_body->get_shape_count(); - - for (int j = from_shape; j < to_shape; j++) { - if (p_body->is_shape_disabled(j)) { - continue; - } - - Transform2D body_shape_xform = ugt * p_body->get_shape_transform(j); - Shape2DSW *body_shape = p_body->get_shape(j); - - body_aabb.position += p_parameters.motion * unsafe; - - int amount = _cull_aabb_for_body(p_body, body_aabb); - - for (int i = 0; i < amount; i++) { - const CollisionObject2DSW *col_obj = intersection_query_results[i]; - if (p_parameters.exclude_bodies.has(col_obj->get_self())) { - continue; - } - if (p_parameters.exclude_objects.has(col_obj->get_instance_id())) { - continue; - } - - int shape_idx = intersection_query_subindex_results[i]; - - Shape2DSW *against_shape = col_obj->get_shape(shape_idx); - - bool excluded = false; - for (int k = 0; k < excluded_shape_pair_count; k++) { - if (excluded_shape_pairs[k].local_shape == body_shape && excluded_shape_pairs[k].against_object == col_obj && excluded_shape_pairs[k].against_shape_index == shape_idx) { - excluded = true; - break; - } - } - if (excluded) { - continue; - } - - Transform2D col_obj_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); - - if (body_shape->allows_one_way_collision() && col_obj->is_shape_set_as_one_way_collision(shape_idx)) { - rcd.valid_dir = col_obj_shape_xform.get_axis(1).normalized(); - - real_t owc_margin = col_obj->get_shape_one_way_collision_margin(shape_idx); - rcd.valid_depth = MAX(owc_margin, p_parameters.margin); //user specified, but never less than actual margin or it won't work - - if (col_obj->get_type() == CollisionObject2DSW::TYPE_BODY) { - const Body2DSW *b = static_cast(col_obj); - if (b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_DYNAMIC) { - //fix for moving platforms (kinematic and dynamic), margin is increased by how much it moved in the given direction - Vector2 lv = b->get_linear_velocity(); - //compute displacement from linear velocity - Vector2 motion = lv * last_step; - real_t motion_len = motion.length(); - motion.normalize(); - rcd.valid_depth += motion_len * MAX(motion.dot(-rcd.valid_dir), 0.0); - } - } - } else { - rcd.valid_dir = Vector2(); - rcd.valid_depth = 0; - } - - rcd.object = col_obj; - rcd.shape = shape_idx; - rcd.local_shape = j; - bool sc = CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), _rest_cbk_result, &rcd, nullptr, p_parameters.margin); - if (!sc) { - continue; - } - } - } - - if (rcd.best_len != 0) { - if (r_result) { - 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 = rcd.best_local_shape; - r_result->collision_normal = rcd.best_normal; - r_result->collision_point = rcd.best_contact; - r_result->collision_depth = rcd.best_len; - r_result->collision_safe_fraction = safe; - r_result->collision_unsafe_fraction = unsafe; - - const Body2DSW *body = static_cast(rcd.best_object); - Vector2 rel_vec = r_result->collision_point - (body->get_transform().get_origin() + body->get_center_of_mass()); - r_result->collider_velocity = Vector2(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity(); - - r_result->travel = safe * p_parameters.motion; - r_result->remainder = p_parameters.motion - safe * p_parameters.motion; - r_result->travel += (body_transform.get_origin() - p_parameters.from.get_origin()); - } - - collided = true; - } - } - - if (!collided && r_result) { - r_result->travel = p_parameters.motion; - r_result->remainder = Vector2(); - r_result->travel += (body_transform.get_origin() - p_parameters.from.get_origin()); - } - - return collided; -} - -void *Space2DSW::_broadphase_pair(CollisionObject2DSW *A, int p_subindex_A, CollisionObject2DSW *B, int p_subindex_B, void *p_self) { - if (!A->interacts_with(B)) { - return nullptr; - } - - CollisionObject2DSW::Type type_A = A->get_type(); - CollisionObject2DSW::Type type_B = B->get_type(); - if (type_A > type_B) { - SWAP(A, B); - SWAP(p_subindex_A, p_subindex_B); - SWAP(type_A, type_B); - } - - Space2DSW *self = (Space2DSW *)p_self; - self->collision_pairs++; - - if (type_A == CollisionObject2DSW::TYPE_AREA) { - Area2DSW *area = static_cast(A); - if (type_B == CollisionObject2DSW::TYPE_AREA) { - Area2DSW *area_b = static_cast(B); - Area2Pair2DSW *area2_pair = memnew(Area2Pair2DSW(area_b, p_subindex_B, area, p_subindex_A)); - return area2_pair; - } else { - Body2DSW *body = static_cast(B); - AreaPair2DSW *area_pair = memnew(AreaPair2DSW(body, p_subindex_B, area, p_subindex_A)); - return area_pair; - } - - } else { - BodyPair2DSW *b = memnew(BodyPair2DSW((Body2DSW *)A, p_subindex_A, (Body2DSW *)B, p_subindex_B)); - return b; - } - - return nullptr; -} - -void Space2DSW::_broadphase_unpair(CollisionObject2DSW *A, int p_subindex_A, CollisionObject2DSW *B, int p_subindex_B, void *p_data, void *p_self) { - if (!p_data) { - return; - } - - Space2DSW *self = (Space2DSW *)p_self; - self->collision_pairs--; - Constraint2DSW *c = (Constraint2DSW *)p_data; - memdelete(c); -} - -const SelfList::List &Space2DSW::get_active_body_list() const { - return active_list; -} - -void Space2DSW::body_add_to_active_list(SelfList *p_body) { - active_list.add(p_body); -} - -void Space2DSW::body_remove_from_active_list(SelfList *p_body) { - active_list.remove(p_body); -} - -void Space2DSW::body_add_to_mass_properties_update_list(SelfList *p_body) { - mass_properties_update_list.add(p_body); -} - -void Space2DSW::body_remove_from_mass_properties_update_list(SelfList *p_body) { - mass_properties_update_list.remove(p_body); -} - -BroadPhase2DSW *Space2DSW::get_broadphase() { - return broadphase; -} - -void Space2DSW::add_object(CollisionObject2DSW *p_object) { - ERR_FAIL_COND(objects.has(p_object)); - objects.insert(p_object); -} - -void Space2DSW::remove_object(CollisionObject2DSW *p_object) { - ERR_FAIL_COND(!objects.has(p_object)); - objects.erase(p_object); -} - -const Set &Space2DSW::get_objects() const { - return objects; -} - -void Space2DSW::body_add_to_state_query_list(SelfList *p_body) { - state_query_list.add(p_body); -} - -void Space2DSW::body_remove_from_state_query_list(SelfList *p_body) { - state_query_list.remove(p_body); -} - -void Space2DSW::area_add_to_monitor_query_list(SelfList *p_area) { - monitor_query_list.add(p_area); -} - -void Space2DSW::area_remove_from_monitor_query_list(SelfList *p_area) { - monitor_query_list.remove(p_area); -} - -void Space2DSW::area_add_to_moved_list(SelfList *p_area) { - area_moved_list.add(p_area); -} - -void Space2DSW::area_remove_from_moved_list(SelfList *p_area) { - area_moved_list.remove(p_area); -} - -const SelfList::List &Space2DSW::get_moved_area_list() const { - return area_moved_list; -} - -void Space2DSW::call_queries() { - while (state_query_list.first()) { - Body2DSW *b = state_query_list.first()->self(); - state_query_list.remove(state_query_list.first()); - b->call_queries(); - } - - while (monitor_query_list.first()) { - Area2DSW *a = monitor_query_list.first()->self(); - monitor_query_list.remove(monitor_query_list.first()); - a->call_queries(); - } -} - -void Space2DSW::setup() { - contact_debug_count = 0; - - while (mass_properties_update_list.first()) { - mass_properties_update_list.first()->self()->update_mass_properties(); - mass_properties_update_list.remove(mass_properties_update_list.first()); - } -} - -void Space2DSW::update() { - broadphase->update(); -} - -void Space2DSW::set_param(PhysicsServer2D::SpaceParameter p_param, real_t p_value) { - switch (p_param) { - case PhysicsServer2D::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: - contact_recycle_radius = p_value; - break; - case PhysicsServer2D::SPACE_PARAM_CONTACT_MAX_SEPARATION: - contact_max_separation = p_value; - break; - case PhysicsServer2D::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: - contact_max_allowed_penetration = p_value; - break; - case PhysicsServer2D::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD: - body_linear_velocity_sleep_threshold = p_value; - break; - case PhysicsServer2D::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD: - body_angular_velocity_sleep_threshold = p_value; - break; - case PhysicsServer2D::SPACE_PARAM_BODY_TIME_TO_SLEEP: - body_time_to_sleep = p_value; - break; - case PhysicsServer2D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: - constraint_bias = p_value; - break; - } -} - -real_t Space2DSW::get_param(PhysicsServer2D::SpaceParameter p_param) const { - switch (p_param) { - case PhysicsServer2D::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: - return contact_recycle_radius; - case PhysicsServer2D::SPACE_PARAM_CONTACT_MAX_SEPARATION: - return contact_max_separation; - case PhysicsServer2D::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: - return contact_max_allowed_penetration; - case PhysicsServer2D::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD: - return body_linear_velocity_sleep_threshold; - case PhysicsServer2D::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD: - return body_angular_velocity_sleep_threshold; - case PhysicsServer2D::SPACE_PARAM_BODY_TIME_TO_SLEEP: - return body_time_to_sleep; - case PhysicsServer2D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: - return constraint_bias; - } - return 0; -} - -void Space2DSW::lock() { - locked = true; -} - -void Space2DSW::unlock() { - locked = false; -} - -bool Space2DSW::is_locked() const { - return locked; -} - -PhysicsDirectSpaceState2DSW *Space2DSW::get_direct_state() { - return direct_access; -} - -Space2DSW::Space2DSW() { - body_linear_velocity_sleep_threshold = GLOBAL_DEF("physics/2d/sleep_threshold_linear", 2.0); - body_angular_velocity_sleep_threshold = GLOBAL_DEF("physics/2d/sleep_threshold_angular", Math::deg2rad(8.0)); - body_time_to_sleep = GLOBAL_DEF("physics/2d/time_before_sleep", 0.5); - ProjectSettings::get_singleton()->set_custom_property_info("physics/2d/time_before_sleep", PropertyInfo(Variant::FLOAT, "physics/2d/time_before_sleep", PROPERTY_HINT_RANGE, "0,5,0.01,or_greater")); - - broadphase = BroadPhase2DSW::create_func(); - broadphase->set_pair_callback(_broadphase_pair, this); - broadphase->set_unpair_callback(_broadphase_unpair, this); - - direct_access = memnew(PhysicsDirectSpaceState2DSW); - direct_access->space = this; -} - -Space2DSW::~Space2DSW() { - memdelete(broadphase); - memdelete(direct_access); -} diff --git a/servers/physics_2d/space_2d_sw.h b/servers/physics_2d/space_2d_sw.h deleted file mode 100644 index 746b5c6c9a..0000000000 --- a/servers/physics_2d/space_2d_sw.h +++ /dev/null @@ -1,211 +0,0 @@ -/*************************************************************************/ -/* space_2d_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/*************************************************************************/ - -#ifndef SPACE_2D_SW_H -#define SPACE_2D_SW_H - -#include "area_2d_sw.h" -#include "area_pair_2d_sw.h" -#include "body_2d_sw.h" -#include "body_pair_2d_sw.h" -#include "broad_phase_2d_sw.h" -#include "collision_object_2d_sw.h" -#include "core/config/project_settings.h" -#include "core/templates/hash_map.h" -#include "core/typedefs.h" - -class PhysicsDirectSpaceState2DSW : public PhysicsDirectSpaceState2D { - GDCLASS(PhysicsDirectSpaceState2DSW, PhysicsDirectSpaceState2D); - - int _intersect_point_impl(const Vector2 &p_point, ShapeResult *r_results, int p_result_max, const Set &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, bool p_pick_point, bool p_filter_by_canvas = false, ObjectID p_canvas_instance_id = ObjectID()); - -public: - Space2DSW *space = nullptr; - - virtual int intersect_point(const Vector2 &p_point, ShapeResult *r_results, int p_result_max, const Set &p_exclude = Set(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, bool p_pick_point = false) override; - virtual int intersect_point_on_canvas(const Vector2 &p_point, ObjectID p_canvas_instance_id, ShapeResult *r_results, int p_result_max, const Set &p_exclude = Set(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, bool p_pick_point = false) override; - virtual bool intersect_ray(const Vector2 &p_from, const Vector2 &p_to, RayResult &r_result, const Set &p_exclude = Set(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; - virtual int intersect_shape(const RID &p_shape, const Transform2D &p_xform, const Vector2 &p_motion, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set &p_exclude = Set(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; - virtual bool cast_motion(const RID &p_shape, const Transform2D &p_xform, const Vector2 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set &p_exclude = Set(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; - virtual bool collide_shape(RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, real_t p_margin, Vector2 *r_results, int p_result_max, int &r_result_count, const Set &p_exclude = Set(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; - virtual bool rest_info(RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, real_t p_margin, ShapeRestInfo *r_info, const Set &p_exclude = Set(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; - - PhysicsDirectSpaceState2DSW() {} -}; - -class Space2DSW { -public: - enum ElapsedTime { - ELAPSED_TIME_INTEGRATE_FORCES, - ELAPSED_TIME_GENERATE_ISLANDS, - ELAPSED_TIME_SETUP_CONSTRAINTS, - ELAPSED_TIME_SOLVE_CONSTRAINTS, - ELAPSED_TIME_INTEGRATE_VELOCITIES, - ELAPSED_TIME_MAX - - }; - -private: - struct ExcludedShapeSW { - Shape2DSW *local_shape = nullptr; - const CollisionObject2DSW *against_object = nullptr; - int against_shape_index = 0; - }; - - uint64_t elapsed_time[ELAPSED_TIME_MAX] = {}; - - PhysicsDirectSpaceState2DSW *direct_access = nullptr; - RID self; - - BroadPhase2DSW *broadphase; - SelfList::List active_list; - SelfList::List mass_properties_update_list; - SelfList::List state_query_list; - SelfList::List monitor_query_list; - SelfList::List area_moved_list; - - static void *_broadphase_pair(CollisionObject2DSW *A, int p_subindex_A, CollisionObject2DSW *B, int p_subindex_B, void *p_self); - static void _broadphase_unpair(CollisionObject2DSW *A, int p_subindex_A, CollisionObject2DSW *B, int p_subindex_B, void *p_data, void *p_self); - - Set objects; - - Area2DSW *area = nullptr; - - real_t contact_recycle_radius = 1.0; - real_t contact_max_separation = 1.5; - real_t contact_max_allowed_penetration = 0.3; - real_t constraint_bias = 0.2; - - enum { - INTERSECTION_QUERY_MAX = 2048 - }; - - CollisionObject2DSW *intersection_query_results[INTERSECTION_QUERY_MAX]; - int intersection_query_subindex_results[INTERSECTION_QUERY_MAX]; - - real_t body_linear_velocity_sleep_threshold = 0.0; - real_t body_angular_velocity_sleep_threshold = 0.0; - real_t body_time_to_sleep = 0.0; - - bool locked = false; - - real_t last_step = 0.001; - - int island_count = 0; - int active_objects = 0; - int collision_pairs = 0; - - int _cull_aabb_for_body(Body2DSW *p_body, const Rect2 &p_aabb); - - Vector contact_debug; - int contact_debug_count = 0; - - friend class PhysicsDirectSpaceState2DSW; - -public: - _FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; } - _FORCE_INLINE_ RID get_self() const { return self; } - - void set_default_area(Area2DSW *p_area) { area = p_area; } - Area2DSW *get_default_area() const { return area; } - - const SelfList::List &get_active_body_list() const; - void body_add_to_active_list(SelfList *p_body); - void body_remove_from_active_list(SelfList *p_body); - void body_add_to_mass_properties_update_list(SelfList *p_body); - void body_remove_from_mass_properties_update_list(SelfList *p_body); - void area_add_to_moved_list(SelfList *p_area); - void area_remove_from_moved_list(SelfList *p_area); - const SelfList::List &get_moved_area_list() const; - - void body_add_to_state_query_list(SelfList *p_body); - void body_remove_from_state_query_list(SelfList *p_body); - - void area_add_to_monitor_query_list(SelfList *p_area); - void area_remove_from_monitor_query_list(SelfList *p_area); - - BroadPhase2DSW *get_broadphase(); - - void add_object(CollisionObject2DSW *p_object); - void remove_object(CollisionObject2DSW *p_object); - const Set &get_objects() const; - - _FORCE_INLINE_ real_t get_contact_recycle_radius() const { return contact_recycle_radius; } - _FORCE_INLINE_ real_t get_contact_max_separation() const { return contact_max_separation; } - _FORCE_INLINE_ real_t get_contact_max_allowed_penetration() const { return contact_max_allowed_penetration; } - _FORCE_INLINE_ real_t get_constraint_bias() const { return constraint_bias; } - _FORCE_INLINE_ real_t get_body_linear_velocity_sleep_threshold() const { return body_linear_velocity_sleep_threshold; } - _FORCE_INLINE_ real_t get_body_angular_velocity_sleep_threshold() const { return body_angular_velocity_sleep_threshold; } - _FORCE_INLINE_ real_t get_body_time_to_sleep() const { return body_time_to_sleep; } - - void update(); - void setup(); - void call_queries(); - - bool is_locked() const; - void lock(); - void unlock(); - - real_t get_last_step() const { return last_step; } - void set_last_step(real_t p_step) { last_step = p_step; } - - void set_param(PhysicsServer2D::SpaceParameter p_param, real_t p_value); - real_t get_param(PhysicsServer2D::SpaceParameter p_param) const; - - void set_island_count(int p_island_count) { island_count = p_island_count; } - int get_island_count() const { return island_count; } - - void set_active_objects(int p_active_objects) { active_objects = p_active_objects; } - int get_active_objects() const { return active_objects; } - - int get_collision_pairs() const { return collision_pairs; } - - bool test_body_motion(Body2DSW *p_body, const PhysicsServer2D::MotionParameters &p_parameters, PhysicsServer2D::MotionResult *r_result); - - void set_debug_contacts(int p_amount) { contact_debug.resize(p_amount); } - _FORCE_INLINE_ bool is_debugging_contacts() const { return !contact_debug.is_empty(); } - _FORCE_INLINE_ void add_debug_contact(const Vector2 &p_contact) { - if (contact_debug_count < contact_debug.size()) { - contact_debug.write[contact_debug_count++] = p_contact; - } - } - _FORCE_INLINE_ Vector get_debug_contacts() { return contact_debug; } - _FORCE_INLINE_ int get_debug_contact_count() { return contact_debug_count; } - - PhysicsDirectSpaceState2DSW *get_direct_state(); - - void set_elapsed_time(ElapsedTime p_time, uint64_t p_msec) { elapsed_time[p_time] = p_msec; } - uint64_t get_elapsed_time(ElapsedTime p_time) const { return elapsed_time[p_time]; } - - Space2DSW(); - ~Space2DSW(); -}; - -#endif // SPACE_2D_SW_H diff --git a/servers/physics_2d/step_2d_sw.cpp b/servers/physics_2d/step_2d_sw.cpp deleted file mode 100644 index a03e30f850..0000000000 --- a/servers/physics_2d/step_2d_sw.cpp +++ /dev/null @@ -1,308 +0,0 @@ -/*************************************************************************/ -/* step_2d_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/*************************************************************************/ - -#include "step_2d_sw.h" - -#include "core/os/os.h" - -#define BODY_ISLAND_COUNT_RESERVE 128 -#define BODY_ISLAND_SIZE_RESERVE 512 -#define ISLAND_COUNT_RESERVE 128 -#define ISLAND_SIZE_RESERVE 512 -#define CONSTRAINT_COUNT_RESERVE 1024 - -void Step2DSW::_populate_island(Body2DSW *p_body, LocalVector &p_body_island, LocalVector &p_constraint_island) { - p_body->set_island_step(_step); - - if (p_body->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) { - // Only dynamic bodies are tested for activation. - p_body_island.push_back(p_body); - } - - for (const Pair &E : p_body->get_constraint_list()) { - Constraint2DSW *constraint = (Constraint2DSW *)E.first; - if (constraint->get_island_step() == _step) { - continue; // Already processed. - } - constraint->set_island_step(_step); - p_constraint_island.push_back(constraint); - all_constraints.push_back(constraint); - - for (int i = 0; i < constraint->get_body_count(); i++) { - if (i == E.second) { - continue; - } - Body2DSW *other_body = constraint->get_body_ptr()[i]; - if (other_body->get_island_step() == _step) { - continue; // Already processed. - } - if (other_body->get_mode() == PhysicsServer2D::BODY_MODE_STATIC) { - continue; // Static bodies don't connect islands. - } - _populate_island(other_body, p_body_island, p_constraint_island); - } - } -} - -void Step2DSW::_setup_contraint(uint32_t p_constraint_index, void *p_userdata) { - Constraint2DSW *constraint = all_constraints[p_constraint_index]; - constraint->setup(delta); -} - -void Step2DSW::_pre_solve_island(LocalVector &p_constraint_island) const { - uint32_t constraint_count = p_constraint_island.size(); - uint32_t valid_constraint_count = 0; - for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) { - Constraint2DSW *constraint = p_constraint_island[constraint_index]; - if (p_constraint_island[constraint_index]->pre_solve(delta)) { - // Keep this constraint for solving. - p_constraint_island[valid_constraint_count++] = constraint; - } - } - p_constraint_island.resize(valid_constraint_count); -} - -void Step2DSW::_solve_island(uint32_t p_island_index, void *p_userdata) const { - const LocalVector &constraint_island = constraint_islands[p_island_index]; - - for (int i = 0; i < iterations; i++) { - uint32_t constraint_count = constraint_island.size(); - for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) { - constraint_island[constraint_index]->solve(delta); - } - } -} - -void Step2DSW::_check_suspend(LocalVector &p_body_island) const { - bool can_sleep = true; - - uint32_t body_count = p_body_island.size(); - for (uint32_t body_index = 0; body_index < body_count; ++body_index) { - Body2DSW *body = p_body_island[body_index]; - - if (!body->sleep_test(delta)) { - can_sleep = false; - } - } - - // Put all to sleep or wake up everyone. - for (uint32_t body_index = 0; body_index < body_count; ++body_index) { - Body2DSW *body = p_body_island[body_index]; - - bool active = body->is_active(); - - if (active == can_sleep) { - body->set_active(!can_sleep); - } - } -} - -void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) { - p_space->lock(); // can't access space during this - - p_space->setup(); //update inertias, etc - - p_space->set_last_step(p_delta); - - iterations = p_iterations; - delta = p_delta; - - const SelfList::List *body_list = &p_space->get_active_body_list(); - - /* INTEGRATE FORCES */ - - uint64_t profile_begtime = OS::get_singleton()->get_ticks_usec(); - uint64_t profile_endtime = 0; - - int active_count = 0; - - const SelfList *b = body_list->first(); - while (b) { - b->self()->integrate_forces(p_delta); - b = b->next(); - active_count++; - } - - p_space->set_active_objects(active_count); - - { //profile - profile_endtime = OS::get_singleton()->get_ticks_usec(); - p_space->set_elapsed_time(Space2DSW::ELAPSED_TIME_INTEGRATE_FORCES, profile_endtime - profile_begtime); - profile_begtime = profile_endtime; - } - - /* GENERATE CONSTRAINT ISLANDS FOR MOVING AREAS */ - - uint32_t island_count = 0; - - const SelfList::List &aml = p_space->get_moved_area_list(); - - while (aml.first()) { - for (const Set::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) { - Constraint2DSW *constraint = E->get(); - if (constraint->get_island_step() == _step) { - continue; - } - constraint->set_island_step(_step); - - // Each constraint can be on a separate island for areas as there's no solving phase. - ++island_count; - if (constraint_islands.size() < island_count) { - constraint_islands.resize(island_count); - } - LocalVector &constraint_island = constraint_islands[island_count - 1]; - constraint_island.clear(); - - all_constraints.push_back(constraint); - constraint_island.push_back(constraint); - } - p_space->area_remove_from_moved_list((SelfList *)aml.first()); //faster to remove here - } - - /* GENERATE CONSTRAINT ISLANDS FOR ACTIVE RIGID BODIES */ - - b = body_list->first(); - - uint32_t body_island_count = 0; - - while (b) { - Body2DSW *body = b->self(); - - if (body->get_island_step() != _step) { - ++body_island_count; - if (body_islands.size() < body_island_count) { - body_islands.resize(body_island_count); - } - LocalVector &body_island = body_islands[body_island_count - 1]; - body_island.clear(); - body_island.reserve(BODY_ISLAND_SIZE_RESERVE); - - ++island_count; - if (constraint_islands.size() < island_count) { - constraint_islands.resize(island_count); - } - LocalVector &constraint_island = constraint_islands[island_count - 1]; - constraint_island.clear(); - constraint_island.reserve(ISLAND_SIZE_RESERVE); - - _populate_island(body, body_island, constraint_island); - - if (body_island.is_empty()) { - --body_island_count; - } - - if (constraint_island.is_empty()) { - --island_count; - } - } - b = b->next(); - } - - p_space->set_island_count((int)island_count); - - { //profile - profile_endtime = OS::get_singleton()->get_ticks_usec(); - p_space->set_elapsed_time(Space2DSW::ELAPSED_TIME_GENERATE_ISLANDS, profile_endtime - profile_begtime); - profile_begtime = profile_endtime; - } - - /* SETUP CONSTRAINTS / PROCESS COLLISIONS */ - - uint32_t total_contraint_count = all_constraints.size(); - work_pool.do_work(total_contraint_count, this, &Step2DSW::_setup_contraint, nullptr); - - { //profile - profile_endtime = OS::get_singleton()->get_ticks_usec(); - p_space->set_elapsed_time(Space2DSW::ELAPSED_TIME_SETUP_CONSTRAINTS, profile_endtime - profile_begtime); - profile_begtime = profile_endtime; - } - - /* PRE-SOLVE CONSTRAINT ISLANDS */ - - // Warning: This doesn't run on threads, because it involves thread-unsafe processing. - for (uint32_t island_index = 0; island_index < island_count; ++island_index) { - _pre_solve_island(constraint_islands[island_index]); - } - - /* SOLVE CONSTRAINT ISLANDS */ - - // Warning: _solve_island modifies the constraint islands for optimization purpose, - // their content is not reliable after these calls and shouldn't be used anymore. - if (island_count > 1) { - work_pool.do_work(island_count, this, &Step2DSW::_solve_island, nullptr); - } else if (island_count > 0) { - _solve_island(0); - } - - { //profile - profile_endtime = OS::get_singleton()->get_ticks_usec(); - p_space->set_elapsed_time(Space2DSW::ELAPSED_TIME_SOLVE_CONSTRAINTS, profile_endtime - profile_begtime); - profile_begtime = profile_endtime; - } - - /* INTEGRATE VELOCITIES */ - - b = body_list->first(); - while (b) { - const SelfList *n = b->next(); - b->self()->integrate_velocities(p_delta); - b = n; // in case it shuts itself down - } - - /* SLEEP / WAKE UP ISLANDS */ - - for (uint32_t island_index = 0; island_index < body_island_count; ++island_index) { - _check_suspend(body_islands[island_index]); - } - - { //profile - profile_endtime = OS::get_singleton()->get_ticks_usec(); - p_space->set_elapsed_time(Space2DSW::ELAPSED_TIME_INTEGRATE_VELOCITIES, profile_endtime - profile_begtime); - //profile_begtime=profile_endtime; - } - - all_constraints.clear(); - - p_space->update(); - p_space->unlock(); - _step++; -} - -Step2DSW::Step2DSW() { - body_islands.reserve(BODY_ISLAND_COUNT_RESERVE); - constraint_islands.reserve(ISLAND_COUNT_RESERVE); - all_constraints.reserve(CONSTRAINT_COUNT_RESERVE); - - work_pool.init(); -} - -Step2DSW::~Step2DSW() { - work_pool.finish(); -} diff --git a/servers/physics_2d/step_2d_sw.h b/servers/physics_2d/step_2d_sw.h deleted file mode 100644 index de8e76cc99..0000000000 --- a/servers/physics_2d/step_2d_sw.h +++ /dev/null @@ -1,63 +0,0 @@ -/*************************************************************************/ -/* step_2d_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/*************************************************************************/ - -#ifndef STEP_2D_SW_H -#define STEP_2D_SW_H - -#include "space_2d_sw.h" - -#include "core/templates/local_vector.h" -#include "core/templates/thread_work_pool.h" - -class Step2DSW { - uint64_t _step = 1; - - int iterations = 0; - real_t delta = 0.0; - - ThreadWorkPool work_pool; - - LocalVector> body_islands; - LocalVector> constraint_islands; - LocalVector all_constraints; - - void _populate_island(Body2DSW *p_body, LocalVector &p_body_island, LocalVector &p_constraint_island); - void _setup_contraint(uint32_t p_constraint_index, void *p_userdata = nullptr); - void _pre_solve_island(LocalVector &p_constraint_island) const; - void _solve_island(uint32_t p_island_index, void *p_userdata = nullptr) const; - void _check_suspend(LocalVector &p_body_island) const; - -public: - void step(Space2DSW *p_space, real_t p_delta, int p_iterations); - Step2DSW(); - ~Step2DSW(); -}; - -#endif // STEP_2D_SW_H -- cgit v1.2.3