diff options
Diffstat (limited to 'servers/physics')
45 files changed, 0 insertions, 15988 deletions
diff --git a/servers/physics/SCsub b/servers/physics/SCsub deleted file mode 100644 index c5cc889112..0000000000 --- a/servers/physics/SCsub +++ /dev/null @@ -1,7 +0,0 @@ -#!/usr/bin/env python - -Import('env') - -env.add_source_files(env.servers_sources, "*.cpp") - -SConscript("joints/SCsub") diff --git a/servers/physics/area_pair_sw.cpp b/servers/physics/area_pair_sw.cpp deleted file mode 100644 index 966a440930..0000000000 --- a/servers/physics/area_pair_sw.cpp +++ /dev/null @@ -1,159 +0,0 @@ -/*************************************************************************/ -/* area_pair_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 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_sw.h" -#include "collision_solver_sw.h" - -bool AreaPairSW::setup(real_t p_step) { - - bool result = false; - - if (area->is_shape_set_as_disabled(area_shape) || body->is_shape_set_as_disabled(body_shape)) { - result = false; - } else if (area->test_collision_mask(body) && CollisionSolverSW::solve_static(body->get_shape(body_shape), body->get_transform() * body->get_shape_transform(body_shape), area->get_shape(area_shape), area->get_transform() * area->get_shape_transform(area_shape), NULL, this)) { - result = true; - } - - if (result != colliding) { - - if (result) { - - if (area->get_space_override_mode() != PhysicsServer::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() != PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED) - body->remove_area(area); - if (area->has_monitor_callback()) - area->remove_body_from_query(body, body_shape, area_shape); - } - - colliding = result; - } - - return false; //never do any post solving -} - -void AreaPairSW::solve(real_t p_step) { -} - -AreaPairSW::AreaPairSW(BodySW *p_body, int p_body_shape, AreaSW *p_area, int p_area_shape) { - - body = p_body; - area = p_area; - body_shape = p_body_shape; - area_shape = p_area_shape; - colliding = false; - body->add_constraint(this, 0); - area->add_constraint(this); - if (p_body->get_mode() == PhysicsServer::BODY_MODE_KINEMATIC) - p_body->set_active(true); -} - -AreaPairSW::~AreaPairSW() { - - if (colliding) { - - if (area->get_space_override_mode() != PhysicsServer::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); - area->remove_constraint(this); -} - -//////////////////////////////////////////////////// - -bool Area2PairSW::setup(real_t p_step) { - - bool result = false; - if (area_a->is_shape_set_as_disabled(shape_a) || area_b->is_shape_set_as_disabled(shape_b)) { - result = false; - } else if (area_a->test_collision_mask(area_b) && CollisionSolverSW::solve_static(area_a->get_shape(shape_a), area_a->get_transform() * area_a->get_shape_transform(shape_a), area_b->get_shape(shape_b), area_b->get_transform() * area_b->get_shape_transform(shape_b), NULL, this)) { - result = true; - } - - if (result != colliding) { - - if (result) { - - if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) - area_b->add_area_to_query(area_a, shape_a, shape_b); - - if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) - area_a->add_area_to_query(area_b, shape_b, shape_a); - - } else { - - if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) - area_b->remove_area_from_query(area_a, shape_a, shape_b); - - if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) - area_a->remove_area_from_query(area_b, shape_b, shape_a); - } - - colliding = result; - } - - return false; //never do any post solving -} - -void Area2PairSW::solve(real_t p_step) { -} - -Area2PairSW::Area2PairSW(AreaSW *p_area_a, int p_shape_a, AreaSW *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; - colliding = false; - area_a->add_constraint(this); - area_b->add_constraint(this); -} - -Area2PairSW::~Area2PairSW() { - - if (colliding) { - - if (area_b->has_area_monitor_callback()) - area_b->remove_area_from_query(area_a, shape_a, shape_b); - - if (area_a->has_area_monitor_callback()) - area_a->remove_area_from_query(area_b, shape_b, shape_a); - } - - area_a->remove_constraint(this); - area_b->remove_constraint(this); -} diff --git a/servers/physics/area_pair_sw.h b/servers/physics/area_pair_sw.h deleted file mode 100644 index 97a37ebf90..0000000000 --- a/servers/physics/area_pair_sw.h +++ /dev/null @@ -1,70 +0,0 @@ -/*************************************************************************/ -/* area_pair_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 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_SW_H -#define AREA_PAIR_SW_H - -#include "area_sw.h" -#include "body_sw.h" -#include "constraint_sw.h" - -class AreaPairSW : public ConstraintSW { - - BodySW *body; - AreaSW *area; - int body_shape; - int area_shape; - bool colliding; - -public: - bool setup(real_t p_step); - void solve(real_t p_step); - - AreaPairSW(BodySW *p_body, int p_body_shape, AreaSW *p_area, int p_area_shape); - ~AreaPairSW(); -}; - -class Area2PairSW : public ConstraintSW { - - AreaSW *area_a; - AreaSW *area_b; - int shape_a; - int shape_b; - bool colliding; - -public: - bool setup(real_t p_step); - void solve(real_t p_step); - - Area2PairSW(AreaSW *p_area_a, int p_shape_a, AreaSW *p_area_b, int p_shape_b); - ~Area2PairSW(); -}; - -#endif // AREA_PAIR__SW_H diff --git a/servers/physics/area_sw.cpp b/servers/physics/area_sw.cpp deleted file mode 100644 index 4b54a56253..0000000000 --- a/servers/physics/area_sw.cpp +++ /dev/null @@ -1,264 +0,0 @@ -/*************************************************************************/ -/* area_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 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_sw.h" -#include "body_sw.h" -#include "space_sw.h" - -AreaSW::BodyKey::BodyKey(BodySW *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; -} -AreaSW::BodyKey::BodyKey(AreaSW *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 AreaSW::_shapes_changed() { - - if (!moved_list.in_list() && get_space()) - get_space()->area_add_to_moved_list(&moved_list); -} - -void AreaSW::set_transform(const Transform &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 AreaSW::set_space(SpaceSW *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 AreaSW::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 AreaSW::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 AreaSW::set_space_override_mode(PhysicsServer::AreaSpaceOverrideMode p_mode) { - bool do_override = p_mode != PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED; - if (do_override == (space_override_mode != PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED)) - return; - _unregister_shapes(); - space_override_mode = p_mode; - _shape_changed(); -} - -void AreaSW::set_param(PhysicsServer::AreaParameter p_param, const Variant &p_value) { - - switch (p_param) { - case PhysicsServer::AREA_PARAM_GRAVITY: gravity = p_value; break; - case PhysicsServer::AREA_PARAM_GRAVITY_VECTOR: gravity_vector = p_value; break; - case PhysicsServer::AREA_PARAM_GRAVITY_IS_POINT: gravity_is_point = p_value; break; - case PhysicsServer::AREA_PARAM_GRAVITY_DISTANCE_SCALE: gravity_distance_scale = p_value; break; - case PhysicsServer::AREA_PARAM_GRAVITY_POINT_ATTENUATION: point_attenuation = p_value; break; - case PhysicsServer::AREA_PARAM_LINEAR_DAMP: linear_damp = p_value; break; - case PhysicsServer::AREA_PARAM_ANGULAR_DAMP: angular_damp = p_value; break; - case PhysicsServer::AREA_PARAM_PRIORITY: priority = p_value; break; - } -} - -Variant AreaSW::get_param(PhysicsServer::AreaParameter p_param) const { - - switch (p_param) { - case PhysicsServer::AREA_PARAM_GRAVITY: return gravity; - case PhysicsServer::AREA_PARAM_GRAVITY_VECTOR: return gravity_vector; - case PhysicsServer::AREA_PARAM_GRAVITY_IS_POINT: return gravity_is_point; - case PhysicsServer::AREA_PARAM_GRAVITY_DISTANCE_SCALE: return gravity_distance_scale; - case PhysicsServer::AREA_PARAM_GRAVITY_POINT_ATTENUATION: return point_attenuation; - case PhysicsServer::AREA_PARAM_LINEAR_DAMP: return linear_damp; - case PhysicsServer::AREA_PARAM_ANGULAR_DAMP: return angular_damp; - case PhysicsServer::AREA_PARAM_PRIORITY: return priority; - } - - return Variant(); -} - -void AreaSW::_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 AreaSW::set_monitorable(bool p_monitorable) { - - if (monitorable == p_monitorable) - return; - - monitorable = p_monitorable; - _set_static(!monitorable); -} - -void AreaSW::call_queries() { - - if (monitor_callback_id.is_valid() && !monitored_bodies.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<BodyKey, BodyState>::Element *E = monitored_bodies.front(); E; E = E->next()) { - - if (E->get().state == 0) - continue; //nothing happened - - res[0] = E->get().state > 0 ? PhysicsServer::AREA_BODY_ADDED : PhysicsServer::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; - - Callable::CallError ce; - obj->call(monitor_callback_method, (const Variant **)resptr, 5, ce); - } - } - - monitored_bodies.clear(); - - if (area_monitor_callback_id.is_valid() && !monitored_areas.empty()) { - - 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<BodyKey, BodyState>::Element *E = monitored_areas.front(); E; E = E->next()) { - - if (E->get().state == 0) - continue; //nothing happened - - res[0] = E->get().state > 0 ? PhysicsServer::AREA_BODY_ADDED : PhysicsServer::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; - - Callable::CallError ce; - obj->call(area_monitor_callback_method, (const Variant **)resptr, 5, ce); - } - } - - monitored_areas.clear(); - //get_space()->area_remove_from_monitor_query_list(&monitor_query_list); -} - -AreaSW::AreaSW() : - CollisionObjectSW(TYPE_AREA), - monitor_query_list(this), - moved_list(this) { - - _set_static(true); //areas are never active - space_override_mode = PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED; - gravity = 9.80665; - gravity_vector = Vector3(0, -1, 0); - gravity_is_point = false; - gravity_distance_scale = 0; - point_attenuation = 1; - angular_damp = 0.1; - linear_damp = 0.1; - priority = 0; - set_ray_pickable(false); - monitorable = false; -} - -AreaSW::~AreaSW() { -} diff --git a/servers/physics/area_sw.h b/servers/physics/area_sw.h deleted file mode 100644 index 4da2b00d20..0000000000 --- a/servers/physics/area_sw.h +++ /dev/null @@ -1,203 +0,0 @@ -/*************************************************************************/ -/* area_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 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_SW_H -#define AREA_SW_H - -#include "collision_object_sw.h" -#include "core/self_list.h" -#include "servers/physics_server.h" -//#include "servers/physics/query_sw.h" - -class SpaceSW; -class BodySW; -class ConstraintSW; - -class AreaSW : public CollisionObjectSW { - - PhysicsServer::AreaSpaceOverrideMode space_override_mode; - real_t gravity; - Vector3 gravity_vector; - bool gravity_is_point; - real_t gravity_distance_scale; - real_t point_attenuation; - real_t linear_damp; - real_t angular_damp; - int priority; - bool monitorable; - - ObjectID monitor_callback_id; - StringName monitor_callback_method; - - ObjectID area_monitor_callback_id; - StringName area_monitor_callback_method; - - SelfList<AreaSW> monitor_query_list; - SelfList<AreaSW> moved_list; - - struct BodyKey { - - RID rid; - ObjectID instance_id; - uint32_t body_shape; - uint32_t area_shape; - - _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(BodySW *p_body, uint32_t p_body_shape, uint32_t p_area_shape); - BodyKey(AreaSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape); - }; - - struct BodyState { - - int state; - _FORCE_INLINE_ void inc() { state++; } - _FORCE_INLINE_ void dec() { state--; } - _FORCE_INLINE_ BodyState() { state = 0; } - }; - - Map<BodyKey, BodyState> monitored_bodies; - Map<BodyKey, BodyState> monitored_areas; - - //virtual void shape_changed_notify(ShapeSW *p_shape); - //virtual void shape_deleted_notify(ShapeSW *p_shape); - - Set<ConstraintSW *> constraints; - - virtual void _shapes_changed(); - void _queue_monitor_update(); - -public: - //_FORCE_INLINE_ const Transform& get_inverse_transform() const { return inverse_transform; } - //_FORCE_INLINE_ SpaceSW* get_owner() { return owner; } - - void set_monitor_callback(ObjectID p_id, const StringName &p_method); - _FORCE_INLINE_ bool has_monitor_callback() const { return monitor_callback_id.is_valid(); } - - 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(BodySW *p_body, uint32_t p_body_shape, uint32_t p_area_shape); - _FORCE_INLINE_ void remove_body_from_query(BodySW *p_body, uint32_t p_body_shape, uint32_t p_area_shape); - - _FORCE_INLINE_ void add_area_to_query(AreaSW *p_area, uint32_t p_area_shape, uint32_t p_self_shape); - _FORCE_INLINE_ void remove_area_from_query(AreaSW *p_area, uint32_t p_area_shape, uint32_t p_self_shape); - - void set_param(PhysicsServer::AreaParameter p_param, const Variant &p_value); - Variant get_param(PhysicsServer::AreaParameter p_param) const; - - void set_space_override_mode(PhysicsServer::AreaSpaceOverrideMode p_mode); - PhysicsServer::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 Vector3 &p_gravity) { gravity_vector = p_gravity; } - _FORCE_INLINE_ Vector3 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(ConstraintSW *p_constraint) { constraints.insert(p_constraint); } - _FORCE_INLINE_ void remove_constraint(ConstraintSW *p_constraint) { constraints.erase(p_constraint); } - _FORCE_INLINE_ const Set<ConstraintSW *> &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 Transform &p_transform); - - void set_space(SpaceSW *p_space); - - void call_queries(); - - AreaSW(); - ~AreaSW(); -}; - -void AreaSW::add_body_to_query(BodySW *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 AreaSW::remove_body_from_query(BodySW *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 AreaSW::add_area_to_query(AreaSW *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 AreaSW::remove_area_from_query(AreaSW *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__SW_H diff --git a/servers/physics/body_pair_sw.cpp b/servers/physics/body_pair_sw.cpp deleted file mode 100644 index 31fc1b07d9..0000000000 --- a/servers/physics/body_pair_sw.cpp +++ /dev/null @@ -1,495 +0,0 @@ -/*************************************************************************/ -/* body_pair_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 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_sw.h" - -#include "collision_solver_sw.h" -#include "core/os/os.h" -#include "space_sw.h" - -/* -#define NO_ACCUMULATE_IMPULSES -#define NO_SPLIT_IMPULSES - -#define NO_FRICTION -*/ - -#define NO_TANGENTIALS -/* BODY PAIR */ - -//#define ALLOWED_PENETRATION 0.01 -#define RELAXATION_TIMESTEPS 3 -#define MIN_VELOCITY 0.0001 -#define MAX_BIAS_ROTATION (Math_PI / 8) - -void BodyPairSW::_contact_added_callback(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata) { - - BodyPairSW *pair = (BodyPairSW *)p_userdata; - pair->contact_added_callback(p_point_A, p_point_B); -} - -void BodyPairSW::contact_added_callback(const Vector3 &p_point_A, const Vector3 &p_point_B) { - - // check if we already have the contact - - //Vector3 local_A = A->get_inv_transform().xform(p_point_A); - //Vector3 local_B = B->get_inv_transform().xform(p_point_B); - - Vector3 local_A = A->get_inv_transform().basis.xform(p_point_A); - Vector3 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_bias_impulse_center_of_mass = 0; - contact.acc_tangent_impulse = Vector3(); - contact.local_A = local_A; - contact.local_B = local_B; - 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 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) < (contact_recycle_radius * contact_recycle_radius) && - c.local_B.distance_squared_to(local_B) < (contact_recycle_radius * contact_recycle_radius)) { - - contact.acc_normal_impulse = c.acc_normal_impulse; - contact.acc_bias_impulse = c.acc_bias_impulse; - contact.acc_bias_impulse_center_of_mass = c.acc_bias_impulse_center_of_mass; - contact.acc_tangent_impulse = c.acc_tangent_impulse; - new_index = i; - break; - } - } - - // 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; - - for (int i = 0; i <= contact_count; i++) { - - Contact &c = (i == contact_count) ? contact : contacts[i]; - Vector3 global_A = A->get_transform().basis.xform(c.local_A); - Vector3 global_B = B->get_transform().basis.xform(c.local_B) + offset_B; - - Vector3 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 BodyPairSW::validate_contacts() { - - //make sure to erase contacts that are no longer valid - - real_t contact_max_separation = space->get_contact_max_separation(); - for (int i = 0; i < contact_count; i++) { - - Contact &c = contacts[i]; - - Vector3 global_A = A->get_transform().basis.xform(c.local_A); - Vector3 global_B = B->get_transform().basis.xform(c.local_B) + offset_B; - Vector3 axis = global_A - global_B; - real_t depth = axis.dot(c.normal); - - if (depth < -contact_max_separation || (global_B + c.normal * depth - global_A).length() > contact_max_separation) { - // 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 BodyPairSW::_test_ccd(real_t p_step, BodySW *p_A, int p_shape_A, const Transform &p_xform_A, BodySW *p_B, int p_shape_B, const Transform &p_xform_B) { - - Vector3 motion = p_A->get_linear_velocity() * p_step; - real_t mlen = motion.length(); - if (mlen < CMP_EPSILON) - return false; - - Vector3 mnormal = motion / mlen; - - real_t min, max; - p_A->get_shape(p_shape_A)->project_range(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 - Vector3 s = p_A->get_shape(p_shape_A)->get_support(p_xform_A.basis.xform(mnormal).normalized()); - Vector3 from = p_xform_A.xform(s); - Vector3 to = from + motion; - - Transform from_inv = p_xform_B.affine_inverse(); - - Vector3 local_from = from_inv.xform(from - mnormal * mlen * 0.1); //start from a little inside the bounding box - Vector3 local_to = from_inv.xform(to); - - Vector3 rpos, rnorm; - if (!p_B->get_shape(p_shape_B)->intersect_segment(local_from, local_to, rpos, rnorm)) { - return false; - } - - //shorten the linear velocity so it does not hit, but gets close enough, next frame will hit softly or soft enough - Vector3 hitpos = p_xform_B.xform(rpos); - - real_t newlen = hitpos.distance_to(from) - (max - min) * 0.01; - p_A->set_linear_velocity((mnormal * newlen) / p_step); - - return true; -} - -real_t combine_bounce(BodySW *A, BodySW *B) { - return CLAMP(A->get_bounce() + B->get_bounce(), 0, 1); -} - -real_t combine_friction(BodySW *A, BodySW *B) { - return ABS(MIN(A->get_friction(), B->get_friction())); -} - -bool BodyPairSW::setup(real_t p_step) { - - //cannot collide - if (!A->test_collision_mask(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self()) || (A->get_mode() <= PhysicsServer::BODY_MODE_KINEMATIC && B->get_mode() <= PhysicsServer::BODY_MODE_KINEMATIC && A->get_max_contacts_reported() == 0 && B->get_max_contacts_reported() == 0)) { - collided = false; - return false; - } - - if (A->is_shape_set_as_disabled(shape_A) || B->is_shape_set_as_disabled(shape_B)) { - collided = false; - return false; - } - - offset_B = B->get_transform().get_origin() - A->get_transform().get_origin(); - - validate_contacts(); - - Vector3 offset_A = A->get_transform().get_origin(); - Transform xform_Au = Transform(A->get_transform().basis, Vector3()); - Transform xform_A = xform_Au * A->get_shape_transform(shape_A); - - Transform xform_Bu = B->get_transform(); - xform_Bu.origin -= offset_A; - Transform xform_B = xform_Bu * B->get_shape_transform(shape_B); - - ShapeSW *shape_A_ptr = A->get_shape(shape_A); - ShapeSW *shape_B_ptr = B->get_shape(shape_B); - - bool collided = CollisionSolverSW::solve_static(shape_A_ptr, xform_A, shape_B_ptr, xform_B, _contact_added_callback, this, &sep_axis); - this->collided = collided; - - if (!collided) { - - //test ccd (currently just a raycast) - - if (A->is_continuous_collision_detection_enabled() && A->get_mode() > PhysicsServer::BODY_MODE_KINEMATIC && B->get_mode() <= PhysicsServer::BODY_MODE_KINEMATIC) { - _test_ccd(p_step, A, shape_A, xform_A, B, shape_B, xform_B); - } - - if (B->is_continuous_collision_detection_enabled() && B->get_mode() > PhysicsServer::BODY_MODE_KINEMATIC && A->get_mode() <= PhysicsServer::BODY_MODE_KINEMATIC) { - _test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A); - } - - return false; - } - - real_t max_penetration = space->get_contact_max_allowed_penetration(); - - real_t bias = (real_t)0.3; - - 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; - - for (int i = 0; i < contact_count; i++) { - - Contact &c = contacts[i]; - c.active = false; - - Vector3 global_A = xform_Au.xform(c.local_A); - Vector3 global_B = xform_Bu.xform(c.local_B); - - real_t depth = c.normal.dot(global_A - global_B); - - if (depth <= 0) { - c.active = false; - continue; - } - - c.active = true; - -#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 - A->get_center_of_mass(); - c.rB = global_B - B->get_center_of_mass() - offset_B; - - // contact query reporting... - - if (A->can_report_contacts()) { - Vector3 crA = A->get_angular_velocity().cross(c.rA) + A->get_linear_velocity(); - A->add_contact(global_A, -c.normal, depth, shape_A, global_B, shape_B, B->get_instance_id(), B->get_self(), crA); - } - - if (B->can_report_contacts()) { - Vector3 crB = B->get_angular_velocity().cross(c.rB) + B->get_linear_velocity(); - B->add_contact(global_B, c.normal, depth, shape_B, global_A, shape_A, A->get_instance_id(), A->get_self(), crB); - } - - c.active = true; - - // Precompute normal mass, tangent mass, and bias. - Vector3 inertia_A = A->get_inv_inertia_tensor().xform(c.rA.cross(c.normal)); - Vector3 inertia_B = B->get_inv_inertia_tensor().xform(c.rB.cross(c.normal)); - real_t kNormal = A->get_inv_mass() + B->get_inv_mass(); - kNormal += c.normal.dot(inertia_A.cross(c.rA)) + c.normal.dot(inertia_B.cross(c.rB)); - c.mass_normal = 1.0f / kNormal; - - c.bias = -bias * inv_dt * MIN(0.0f, -depth + max_penetration); - c.depth = depth; - - Vector3 j_vec = c.normal * c.acc_normal_impulse + c.acc_tangent_impulse; - A->apply_impulse(c.rA + A->get_center_of_mass(), -j_vec); - B->apply_impulse(c.rB + B->get_center_of_mass(), j_vec); - c.acc_bias_impulse = 0; - c.acc_bias_impulse_center_of_mass = 0; - - c.bounce = combine_bounce(A, B); - if (c.bounce) { - - Vector3 crA = A->get_angular_velocity().cross(c.rA); - Vector3 crB = B->get_angular_velocity().cross(c.rB); - Vector3 dv = B->get_linear_velocity() + crB - A->get_linear_velocity() - crA; - //normal impule - c.bounce = c.bounce * dv.dot(c.normal); - } - } - - return true; -} - -void BodyPairSW::solve(real_t p_step) { - - if (!collided) - return; - - for (int i = 0; i < contact_count; i++) { - - Contact &c = contacts[i]; - if (!c.active) - continue; - - c.active = false; //try to deactivate, will activate itself if still needed - - //bias impulse - - Vector3 crbA = A->get_biased_angular_velocity().cross(c.rA); - Vector3 crbB = B->get_biased_angular_velocity().cross(c.rB); - Vector3 dbv = B->get_biased_linear_velocity() + crbB - A->get_biased_linear_velocity() - crbA; - - real_t vbn = dbv.dot(c.normal); - - if (Math::abs(-vbn + c.bias) > MIN_VELOCITY) { - - real_t jbn = (-vbn + c.bias) * c.mass_normal; - real_t jbnOld = c.acc_bias_impulse; - c.acc_bias_impulse = MAX(jbnOld + jbn, 0.0f); - - Vector3 jb = c.normal * (c.acc_bias_impulse - jbnOld); - - A->apply_bias_impulse(c.rA + A->get_center_of_mass(), -jb, MAX_BIAS_ROTATION / p_step); - B->apply_bias_impulse(c.rB + B->get_center_of_mass(), jb, MAX_BIAS_ROTATION / p_step); - - crbA = A->get_biased_angular_velocity().cross(c.rA); - crbB = B->get_biased_angular_velocity().cross(c.rB); - dbv = B->get_biased_linear_velocity() + crbB - A->get_biased_linear_velocity() - crbA; - - vbn = dbv.dot(c.normal); - - if (Math::abs(-vbn + c.bias) > MIN_VELOCITY) { - - real_t jbn_com = (-vbn + c.bias) / (A->get_inv_mass() + B->get_inv_mass()); - real_t jbnOld_com = c.acc_bias_impulse_center_of_mass; - c.acc_bias_impulse_center_of_mass = MAX(jbnOld_com + jbn_com, 0.0f); - - Vector3 jb_com = c.normal * (c.acc_bias_impulse_center_of_mass - jbnOld_com); - - A->apply_bias_impulse(A->get_center_of_mass(), -jb_com, 0.0f); - B->apply_bias_impulse(B->get_center_of_mass(), jb_com, 0.0f); - } - - c.active = true; - } - - Vector3 crA = A->get_angular_velocity().cross(c.rA); - Vector3 crB = B->get_angular_velocity().cross(c.rB); - Vector3 dv = B->get_linear_velocity() + crB - A->get_linear_velocity() - crA; - - //normal impulse - real_t vn = dv.dot(c.normal); - - if (Math::abs(vn) > MIN_VELOCITY) { - - 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); - - Vector3 j = c.normal * (c.acc_normal_impulse - jnOld); - - A->apply_impulse(c.rA + A->get_center_of_mass(), -j); - B->apply_impulse(c.rB + B->get_center_of_mass(), j); - - c.active = true; - } - - //friction impulse - - real_t friction = combine_friction(A, B); - - Vector3 lvA = A->get_linear_velocity() + A->get_angular_velocity().cross(c.rA); - Vector3 lvB = B->get_linear_velocity() + B->get_angular_velocity().cross(c.rB); - - Vector3 dtv = lvB - lvA; - real_t tn = c.normal.dot(dtv); - - // tangential velocity - Vector3 tv = dtv - c.normal * tn; - real_t tvl = tv.length(); - - if (tvl > MIN_VELOCITY) { - - tv /= tvl; - - Vector3 temp1 = A->get_inv_inertia_tensor().xform(c.rA.cross(tv)); - Vector3 temp2 = B->get_inv_inertia_tensor().xform(c.rB.cross(tv)); - - real_t t = -tvl / - (A->get_inv_mass() + B->get_inv_mass() + tv.dot(temp1.cross(c.rA) + temp2.cross(c.rB))); - - Vector3 jt = t * tv; - - Vector3 jtOld = c.acc_tangent_impulse; - c.acc_tangent_impulse += jt; - - real_t fi_len = c.acc_tangent_impulse.length(); - real_t jtMax = c.acc_normal_impulse * friction; - - if (fi_len > CMP_EPSILON && fi_len > jtMax) { - - c.acc_tangent_impulse *= jtMax / fi_len; - } - - jt = c.acc_tangent_impulse - jtOld; - - A->apply_impulse(c.rA + A->get_center_of_mass(), -jt); - B->apply_impulse(c.rB + B->get_center_of_mass(), jt); - - c.active = true; - } - } -} - -BodyPairSW::BodyPairSW(BodySW *p_A, int p_shape_A, BodySW *p_B, int p_shape_B) : - ConstraintSW(_arr, 2) { - - A = p_A; - B = p_B; - shape_A = p_shape_A; - shape_B = p_shape_B; - space = A->get_space(); - A->add_constraint(this, 0); - B->add_constraint(this, 1); - contact_count = 0; - collided = false; -} - -BodyPairSW::~BodyPairSW() { - - A->remove_constraint(this); - B->remove_constraint(this); -} diff --git a/servers/physics/body_pair_sw.h b/servers/physics/body_pair_sw.h deleted file mode 100644 index 235aab23b5..0000000000 --- a/servers/physics/body_pair_sw.h +++ /dev/null @@ -1,97 +0,0 @@ -/*************************************************************************/ -/* body_pair_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 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_SW_H -#define BODY_PAIR_SW_H - -#include "body_sw.h" -#include "constraint_sw.h" - -class BodyPairSW : public ConstraintSW { - enum { - - MAX_CONTACTS = 4 - }; - - union { - struct { - BodySW *A; - BodySW *B; - }; - - BodySW *_arr[2]; - }; - - int shape_A; - int shape_B; - - struct Contact { - - Vector3 position; - Vector3 normal; - Vector3 local_A, local_B; - real_t acc_normal_impulse; // accumulated normal impulse (Pn) - Vector3 acc_tangent_impulse; // accumulated tangent impulse (Pt) - real_t acc_bias_impulse; // accumulated normal impulse for position bias (Pnb) - real_t acc_bias_impulse_center_of_mass; // accumulated normal impulse for position bias applied to com - real_t mass_normal; - real_t bias; - real_t bounce; - - real_t depth; - bool active; - Vector3 rA, rB; // Offset in world orientation with respect to center of mass - }; - - Vector3 offset_B; //use local A coordinates to avoid numerical issues on collision detection - - Vector3 sep_axis; - Contact contacts[MAX_CONTACTS]; - int contact_count; - bool collided; - - static void _contact_added_callback(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata); - - void contact_added_callback(const Vector3 &p_point_A, const Vector3 &p_point_B); - - void validate_contacts(); - bool _test_ccd(real_t p_step, BodySW *p_A, int p_shape_A, const Transform &p_xform_A, BodySW *p_B, int p_shape_B, const Transform &p_xform_B); - - SpaceSW *space; - -public: - bool setup(real_t p_step); - void solve(real_t p_step); - - BodyPairSW(BodySW *p_A, int p_shape_A, BodySW *p_B, int p_shape_B); - ~BodyPairSW(); -}; - -#endif // BODY_PAIR__SW_H diff --git a/servers/physics/body_sw.cpp b/servers/physics/body_sw.cpp deleted file mode 100644 index 8819941f04..0000000000 --- a/servers/physics/body_sw.cpp +++ /dev/null @@ -1,814 +0,0 @@ -/*************************************************************************/ -/* body_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 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_sw.h" -#include "area_sw.h" -#include "space_sw.h" - -void BodySW::_update_inertia() { - - if (get_space() && !inertia_update_list.in_list()) - get_space()->body_add_to_inertia_update_list(&inertia_update_list); -} - -void BodySW::_update_transform_dependant() { - - center_of_mass = get_transform().basis.xform(center_of_mass_local); - principal_inertia_axes = get_transform().basis * principal_inertia_axes_local; - - // update inertia tensor - Basis tb = principal_inertia_axes; - Basis tbt = tb.transposed(); - Basis diag; - diag.scale(_inv_inertia); - _inv_inertia_tensor = tb * diag * tbt; -} - -void BodySW::update_inertias() { - - //update shapes and motions - - switch (mode) { - - case PhysicsServer::BODY_MODE_RIGID: { - - //update tensor for all shapes, not the best way but should be somehow OK. (inspired from bullet) - real_t total_area = 0; - - for (int i = 0; i < get_shape_count(); i++) { - - total_area += get_shape_area(i); - } - - // We have to recompute the center of mass - center_of_mass_local.zero(); - - for (int i = 0; i < get_shape_count(); i++) { - real_t area = get_shape_area(i); - - real_t mass = area * this->mass / total_area; - - // NOTE: we assume that the shape origin is also its center of mass - center_of_mass_local += mass * get_shape_transform(i).origin; - } - - center_of_mass_local /= mass; - - // Recompute the inertia tensor - Basis inertia_tensor; - inertia_tensor.set_zero(); - - for (int i = 0; i < get_shape_count(); i++) { - - if (is_shape_disabled(i)) { - continue; - } - - const ShapeSW *shape = get_shape(i); - - real_t area = get_shape_area(i); - - real_t mass = area * this->mass / total_area; - - Basis shape_inertia_tensor = shape->get_moment_of_inertia(mass).to_diagonal_matrix(); - Transform shape_transform = get_shape_transform(i); - Basis shape_basis = shape_transform.basis.orthonormalized(); - - // NOTE: we don't take the scale of collision shapes into account when computing the inertia tensor! - shape_inertia_tensor = shape_basis * shape_inertia_tensor * shape_basis.transposed(); - - Vector3 shape_origin = shape_transform.origin - center_of_mass_local; - inertia_tensor += shape_inertia_tensor + (Basis() * shape_origin.dot(shape_origin) - shape_origin.outer(shape_origin)) * mass; - } - - // Compute the principal axes of inertia - principal_inertia_axes_local = inertia_tensor.diagonalize().transposed(); - _inv_inertia = inertia_tensor.get_main_diagonal().inverse(); - - if (mass) - _inv_mass = 1.0 / mass; - else - _inv_mass = 0; - - } break; - - case PhysicsServer::BODY_MODE_KINEMATIC: - case PhysicsServer::BODY_MODE_STATIC: { - - _inv_inertia_tensor.set_zero(); - _inv_mass = 0; - } break; - case PhysicsServer::BODY_MODE_CHARACTER: { - - _inv_inertia_tensor.set_zero(); - _inv_mass = 1.0 / mass; - - } break; - } - - //_update_shapes(); - - _update_transform_dependant(); -} - -void BodySW::set_active(bool p_active) { - - if (active == p_active) - return; - - active = p_active; - if (!p_active) { - if (get_space()) - get_space()->body_remove_from_active_list(&active_list); - } else { - if (mode == PhysicsServer::BODY_MODE_STATIC) - return; //static bodies can't become active - if (get_space()) - get_space()->body_add_to_active_list(&active_list); - - //still_time=0; - } - /* - if (!space) - return; - - for(int i=0;i<get_shape_count();i++) { - Shape &s=shapes[i]; - if (s.bpid>0) { - get_space()->get_broadphase()->set_active(s.bpid,active); - } - } -*/ -} - -void BodySW::set_param(PhysicsServer::BodyParameter p_param, real_t p_value) { - - switch (p_param) { - case PhysicsServer::BODY_PARAM_BOUNCE: { - - bounce = p_value; - } break; - case PhysicsServer::BODY_PARAM_FRICTION: { - - friction = p_value; - } break; - case PhysicsServer::BODY_PARAM_MASS: { - ERR_FAIL_COND(p_value <= 0); - mass = p_value; - _update_inertia(); - - } break; - case PhysicsServer::BODY_PARAM_GRAVITY_SCALE: { - gravity_scale = p_value; - } break; - case PhysicsServer::BODY_PARAM_LINEAR_DAMP: { - - linear_damp = p_value; - } break; - case PhysicsServer::BODY_PARAM_ANGULAR_DAMP: { - - angular_damp = p_value; - } break; - default: { - } - } -} - -real_t BodySW::get_param(PhysicsServer::BodyParameter p_param) const { - - switch (p_param) { - case PhysicsServer::BODY_PARAM_BOUNCE: { - - return bounce; - } break; - case PhysicsServer::BODY_PARAM_FRICTION: { - - return friction; - } break; - case PhysicsServer::BODY_PARAM_MASS: { - return mass; - } break; - case PhysicsServer::BODY_PARAM_GRAVITY_SCALE: { - return gravity_scale; - } break; - case PhysicsServer::BODY_PARAM_LINEAR_DAMP: { - - return linear_damp; - } break; - case PhysicsServer::BODY_PARAM_ANGULAR_DAMP: { - - return angular_damp; - } break; - - default: { - } - } - - return 0; -} - -void BodySW::set_mode(PhysicsServer::BodyMode p_mode) { - - PhysicsServer::BodyMode prev = mode; - mode = p_mode; - - switch (p_mode) { - //CLEAR UP EVERYTHING IN CASE IT NOT WORKS! - case PhysicsServer::BODY_MODE_STATIC: - case PhysicsServer::BODY_MODE_KINEMATIC: { - - _set_inv_transform(get_transform().affine_inverse()); - _inv_mass = 0; - _set_static(p_mode == PhysicsServer::BODY_MODE_STATIC); - //set_active(p_mode==PhysicsServer::BODY_MODE_KINEMATIC); - set_active(p_mode == PhysicsServer::BODY_MODE_KINEMATIC && contacts.size()); - linear_velocity = Vector3(); - angular_velocity = Vector3(); - if (mode == PhysicsServer::BODY_MODE_KINEMATIC && prev != mode) { - first_time_kinematic = true; - } - - } break; - case PhysicsServer::BODY_MODE_RIGID: { - - _inv_mass = mass > 0 ? (1.0 / mass) : 0; - _set_static(false); - set_active(true); - - } break; - case PhysicsServer::BODY_MODE_CHARACTER: { - - _inv_mass = mass > 0 ? (1.0 / mass) : 0; - _set_static(false); - set_active(true); - angular_velocity = Vector3(); - } break; - } - - _update_inertia(); - /* - if (get_space()) - _update_queries(); - */ -} -PhysicsServer::BodyMode BodySW::get_mode() const { - - return mode; -} - -void BodySW::_shapes_changed() { - - _update_inertia(); -} - -void BodySW::set_state(PhysicsServer::BodyState p_state, const Variant &p_variant) { - - switch (p_state) { - case PhysicsServer::BODY_STATE_TRANSFORM: { - - if (mode == PhysicsServer::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 == PhysicsServer::BODY_MODE_STATIC) { - _set_transform(p_variant); - _set_inv_transform(get_transform().affine_inverse()); - wakeup_neighbours(); - } else { - Transform t = p_variant; - t.orthonormalize(); - new_transform = get_transform(); //used as old to compute motion - if (new_transform == t) - break; - _set_transform(t); - _set_inv_transform(get_transform().inverse()); - } - wakeup(); - - } break; - case PhysicsServer::BODY_STATE_LINEAR_VELOCITY: { - - /* - if (mode==PhysicsServer::BODY_MODE_STATIC) - break; - */ - linear_velocity = p_variant; - wakeup(); - } break; - case PhysicsServer::BODY_STATE_ANGULAR_VELOCITY: { - /* - if (mode!=PhysicsServer::BODY_MODE_RIGID) - break; - */ - angular_velocity = p_variant; - wakeup(); - - } break; - case PhysicsServer::BODY_STATE_SLEEPING: { - //? - if (mode == PhysicsServer::BODY_MODE_STATIC || mode == PhysicsServer::BODY_MODE_KINEMATIC) - break; - bool do_sleep = p_variant; - if (do_sleep) { - linear_velocity = Vector3(); - //biased_linear_velocity=Vector3(); - angular_velocity = Vector3(); - //biased_angular_velocity=Vector3(); - set_active(false); - } else { - set_active(true); - } - } break; - case PhysicsServer::BODY_STATE_CAN_SLEEP: { - can_sleep = p_variant; - if (mode == PhysicsServer::BODY_MODE_RIGID && !active && !can_sleep) - set_active(true); - - } break; - } -} -Variant BodySW::get_state(PhysicsServer::BodyState p_state) const { - - switch (p_state) { - case PhysicsServer::BODY_STATE_TRANSFORM: { - return get_transform(); - } break; - case PhysicsServer::BODY_STATE_LINEAR_VELOCITY: { - return linear_velocity; - } break; - case PhysicsServer::BODY_STATE_ANGULAR_VELOCITY: { - return angular_velocity; - } break; - case PhysicsServer::BODY_STATE_SLEEPING: { - return !is_active(); - } break; - case PhysicsServer::BODY_STATE_CAN_SLEEP: { - return can_sleep; - } break; - } - - return Variant(); -} - -void BodySW::set_space(SpaceSW *p_space) { - - if (get_space()) { - - if (inertia_update_list.in_list()) - get_space()->body_remove_from_inertia_update_list(&inertia_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()) { - - _update_inertia(); - if (active) - get_space()->body_add_to_active_list(&active_list); - /* - _update_queries(); - if (is_active()) { - active=false; - set_active(true); - } - */ - } - - first_integration = true; -} - -void BodySW::_compute_area_gravity_and_dampenings(const AreaSW *p_area) { - - if (p_area->is_gravity_point()) { - if (p_area->get_gravity_distance_scale() > 0) { - Vector3 v = p_area->get_transform().xform(p_area->get_gravity_vector()) - get_transform().get_origin(); - gravity += v.normalized() * (p_area->get_gravity() / Math::pow(v.length() * p_area->get_gravity_distance_scale() + 1, 2)); - } else { - gravity += (p_area->get_transform().xform(p_area->get_gravity_vector()) - get_transform().get_origin()).normalized() * p_area->get_gravity(); - } - } else { - gravity += p_area->get_gravity_vector() * p_area->get_gravity(); - } - - area_linear_damp += p_area->get_linear_damp(); - area_angular_damp += p_area->get_angular_damp(); -} - -void BodySW::set_axis_lock(PhysicsServer::BodyAxis p_axis, bool lock) { - if (lock) { - locked_axis |= p_axis; - } else { - locked_axis &= ~p_axis; - } -} - -bool BodySW::is_axis_locked(PhysicsServer::BodyAxis p_axis) const { - return locked_axis & p_axis; -} - -void BodySW::integrate_forces(real_t p_step) { - - if (mode == PhysicsServer::BODY_MODE_STATIC) - return; - - AreaSW *def_area = get_space()->get_default_area(); - // AreaSW *damp_area = def_area; - - ERR_FAIL_COND(!def_area); - - int ac = areas.size(); - bool stopped = false; - gravity = Vector3(0, 0, 0); - area_linear_damp = 0; - area_angular_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--) { - PhysicsServer::AreaSpaceOverrideMode mode = aa[i].area->get_space_override_mode(); - switch (mode) { - case PhysicsServer::AREA_SPACE_OVERRIDE_COMBINE: - case PhysicsServer::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: { - _compute_area_gravity_and_dampenings(aa[i].area); - stopped = mode == PhysicsServer::AREA_SPACE_OVERRIDE_COMBINE_REPLACE; - } break; - case PhysicsServer::AREA_SPACE_OVERRIDE_REPLACE: - case PhysicsServer::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: { - gravity = Vector3(0, 0, 0); - area_angular_damp = 0; - area_linear_damp = 0; - _compute_area_gravity_and_dampenings(aa[i].area); - stopped = mode == PhysicsServer::AREA_SPACE_OVERRIDE_REPLACE; - } break; - default: { - } - } - } - } - - if (!stopped) { - _compute_area_gravity_and_dampenings(def_area); - } - - gravity *= gravity_scale; - - // If less than 0, override dampenings with that of the Body - 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(); - */ - - Vector3 motion; - bool do_motion = false; - - if (mode == PhysicsServer::BODY_MODE_KINEMATIC) { - - //compute motion, angular and etc. velocities from prev transform - linear_velocity = (new_transform.origin - get_transform().origin) / p_step; - - //compute a FAKE angular velocity, not so easy - Basis rot = new_transform.basis.orthonormalized().transposed() * get_transform().basis.orthonormalized(); - Vector3 axis; - real_t angle; - - rot.get_axis_angle(axis, angle); - axis.normalize(); - angular_velocity = axis.normalized() * (angle / p_step); - - motion = new_transform.origin - get_transform().origin; - do_motion = true; - - } else { - if (!omit_force_integration && !first_integration) { - //overridden by direct state query - - Vector3 force = gravity * mass; - force += applied_force; - Vector3 torque = applied_torque; - - real_t damp = 1.0 - p_step * area_linear_damp; - - if (damp < 0) // reached zero in the given time - damp = 0; - - real_t angular_damp = 1.0 - p_step * area_angular_damp; - - if (angular_damp < 0) // reached zero in the given time - angular_damp = 0; - - linear_velocity *= damp; - angular_velocity *= angular_damp; - - linear_velocity += _inv_mass * force * p_step; - angular_velocity += _inv_inertia_tensor.xform(torque) * p_step; - } - - if (continuous_cd) { - motion = linear_velocity * p_step; - do_motion = true; - } - } - - applied_force = Vector3(); - applied_torque = Vector3(); - first_integration = false; - - //motion=linear_velocity*p_step; - - biased_angular_velocity = Vector3(); - biased_linear_velocity = Vector3(); - - if (do_motion) { //shapes temporarily extend for raycast - _update_shapes_with_motion(motion); - } - - def_area = NULL; // clear the area, so it is set in the next frame - contact_count = 0; -} - -void BodySW::integrate_velocities(real_t p_step) { - - if (mode == PhysicsServer::BODY_MODE_STATIC) - return; - - if (fi_callback) - get_space()->body_add_to_state_query_list(&direct_state_query_list); - - //apply axis lock linear - for (int i = 0; i < 3; i++) { - if (is_axis_locked((PhysicsServer::BodyAxis)(1 << i))) { - linear_velocity[i] = 0; - biased_linear_velocity[i] = 0; - new_transform.origin[i] = get_transform().origin[i]; - } - } - //apply axis lock angular - for (int i = 0; i < 3; i++) { - if (is_axis_locked((PhysicsServer::BodyAxis)(1 << (i + 3)))) { - angular_velocity[i] = 0; - biased_angular_velocity[i] = 0; - } - } - - if (mode == PhysicsServer::BODY_MODE_KINEMATIC) { - - _set_transform(new_transform, false); - _set_inv_transform(new_transform.affine_inverse()); - if (contacts.size() == 0 && linear_velocity == Vector3() && angular_velocity == Vector3()) - set_active(false); //stopped moving, deactivate - - return; - } - - Vector3 total_angular_velocity = angular_velocity + biased_angular_velocity; - - real_t ang_vel = total_angular_velocity.length(); - Transform transform = get_transform(); - - if (ang_vel != 0.0) { - Vector3 ang_vel_axis = total_angular_velocity / ang_vel; - Basis rot(ang_vel_axis, ang_vel * p_step); - Basis identity3(1, 0, 0, 0, 1, 0, 0, 0, 1); - transform.origin += ((identity3 - rot) * transform.basis).xform(center_of_mass_local); - transform.basis = rot * transform.basis; - transform.orthonormalize(); - } - - Vector3 total_linear_velocity = linear_velocity + biased_linear_velocity; - /*for(int i=0;i<3;i++) { - if (axis_lock&(1<<i)) { - transform.origin[i]=0.0; - } - }*/ - - transform.origin += total_linear_velocity * p_step; - - _set_transform(transform); - _set_inv_transform(get_transform().inverse()); - - _update_transform_dependant(); - - /* - if (fi_callback) { - get_space()->body_add_to_state_query_list(&direct_state_query_list); - */ -} - -/* -void BodySW::simulate_motion(const Transform& p_xform,real_t p_step) { - - Transform inv_xform = p_xform.affine_inverse(); - if (!get_space()) { - _set_transform(p_xform); - _set_inv_transform(inv_xform); - - return; - } - - //compute a FAKE linear velocity - this is easy - - linear_velocity=(p_xform.origin - get_transform().origin)/p_step; - - //compute a FAKE angular velocity, not so easy - Basis rot=get_transform().basis.orthonormalized().transposed() * p_xform.basis.orthonormalized(); - Vector3 axis; - real_t angle; - - rot.get_axis_angle(axis,angle); - axis.normalize(); - angular_velocity=axis.normalized() * (angle/p_step); - linear_velocity = (p_xform.origin - get_transform().origin)/p_step; - - if (!direct_state_query_list.in_list())// - callalways, so lv and av are cleared && (state_query || direct_state_query)) - get_space()->body_add_to_state_query_list(&direct_state_query_list); - simulated_motion=true; - _set_transform(p_xform); - - -} -*/ - -void BodySW::wakeup_neighbours() { - - for (Map<ConstraintSW *, int>::Element *E = constraint_map.front(); E; E = E->next()) { - - const ConstraintSW *c = E->key(); - BodySW **n = c->get_body_ptr(); - int bc = c->get_body_count(); - - for (int i = 0; i < bc; i++) { - - if (i == E->get()) - continue; - BodySW *b = n[i]; - if (b->mode != PhysicsServer::BODY_MODE_RIGID) - continue; - - if (!b->is_active()) - b->set_active(true); - } - } -} - -void BodySW::call_queries() { - - if (fi_callback) { - - PhysicsDirectBodyStateSW *dbs = PhysicsDirectBodyStateSW::singleton; - dbs->body = this; - - Variant v = dbs; - - Object *obj = ObjectDB::get_instance(fi_callback->id); - if (!obj) { - - set_force_integration_callback(ObjectID(), StringName()); - } else { - const Variant *vp[2] = { &v, &fi_callback->udata }; - - Callable::CallError ce; - int argc = (fi_callback->udata.get_type() == Variant::NIL) ? 1 : 2; - obj->call(fi_callback->method, vp, argc, ce); - } - } -} - -bool BodySW::sleep_test(real_t p_step) { - - if (mode == PhysicsServer::BODY_MODE_STATIC || mode == PhysicsServer::BODY_MODE_KINEMATIC) - return true; // - else if (mode == PhysicsServer::BODY_MODE_CHARACTER) - return !active; // characters don't sleep unless asked to sleep - else if (!can_sleep) - return false; - - if (Math::abs(angular_velocity.length()) < 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 BodySW::set_force_integration_callback(ObjectID p_id, const StringName &p_method, const Variant &p_udata) { - - if (fi_callback) { - - memdelete(fi_callback); - fi_callback = NULL; - } - - if (p_id.is_valid()) { - - fi_callback = memnew(ForceIntegrationCallback); - fi_callback->id = p_id; - fi_callback->method = p_method; - fi_callback->udata = p_udata; - } -} - -void BodySW::set_kinematic_margin(real_t p_margin) { - kinematic_safe_margin = p_margin; -} - -BodySW::BodySW() : - CollisionObjectSW(TYPE_BODY), - locked_axis(0), - active_list(this), - inertia_update_list(this), - direct_state_query_list(this) { - - mode = PhysicsServer::BODY_MODE_RIGID; - active = true; - - mass = 1; - kinematic_safe_margin = 0.01; - //_inv_inertia=Transform(); - _inv_mass = 1; - bounce = 0; - friction = 1; - omit_force_integration = false; - //applied_torque=0; - island_step = 0; - island_next = NULL; - island_list_next = NULL; - first_time_kinematic = false; - first_integration = false; - _set_static(false); - - contact_count = 0; - gravity_scale = 1.0; - linear_damp = -1; - angular_damp = -1; - area_angular_damp = 0; - area_linear_damp = 0; - - still_time = 0; - continuous_cd = false; - can_sleep = true; - fi_callback = NULL; -} - -BodySW::~BodySW() { - - if (fi_callback) - memdelete(fi_callback); -} - -PhysicsDirectBodyStateSW *PhysicsDirectBodyStateSW::singleton = NULL; - -PhysicsDirectSpaceState *PhysicsDirectBodyStateSW::get_space_state() { - - return body->get_space()->get_direct_state(); -} diff --git a/servers/physics/body_sw.h b/servers/physics/body_sw.h deleted file mode 100644 index d712b09878..0000000000 --- a/servers/physics/body_sw.h +++ /dev/null @@ -1,475 +0,0 @@ -/*************************************************************************/ -/* body_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 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_SW_H -#define BODY_SW_H - -#include "area_sw.h" -#include "collision_object_sw.h" -#include "core/vset.h" - -class ConstraintSW; - -class BodySW : public CollisionObjectSW { - - PhysicsServer::BodyMode mode; - - Vector3 linear_velocity; - Vector3 angular_velocity; - - Vector3 biased_linear_velocity; - Vector3 biased_angular_velocity; - real_t mass; - real_t bounce; - real_t friction; - - real_t linear_damp; - real_t angular_damp; - real_t gravity_scale; - - uint16_t locked_axis; - - real_t kinematic_safe_margin; - real_t _inv_mass; - Vector3 _inv_inertia; // Relative to the principal axes of inertia - - // Relative to the local frame of reference - Basis principal_inertia_axes_local; - Vector3 center_of_mass_local; - - // In world orientation with local origin - Basis _inv_inertia_tensor; - Basis principal_inertia_axes; - Vector3 center_of_mass; - - Vector3 gravity; - - real_t still_time; - - Vector3 applied_force; - Vector3 applied_torque; - - real_t area_angular_damp; - real_t area_linear_damp; - - SelfList<BodySW> active_list; - SelfList<BodySW> inertia_update_list; - SelfList<BodySW> direct_state_query_list; - - VSet<RID> exceptions; - bool omit_force_integration; - bool active; - - bool first_integration; - - bool continuous_cd; - bool can_sleep; - bool first_time_kinematic; - void _update_inertia(); - virtual void _shapes_changed(); - Transform new_transform; - - Map<ConstraintSW *, int> constraint_map; - - struct AreaCMP { - - AreaSW *area; - int refCount; - _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(AreaSW *p_area) { - area = p_area; - refCount = 1; - } - }; - - Vector<AreaCMP> areas; - - struct Contact { - - Vector3 local_pos; - Vector3 local_normal; - real_t depth; - int local_shape; - Vector3 collider_pos; - int collider_shape; - ObjectID collider_instance_id; - RID collider; - Vector3 collider_velocity_at_pos; - }; - - Vector<Contact> contacts; //no contacts by default - int contact_count; - - struct ForceIntegrationCallback { - - ObjectID id; - StringName method; - Variant udata; - }; - - ForceIntegrationCallback *fi_callback; - - uint64_t island_step; - BodySW *island_next; - BodySW *island_list_next; - - _FORCE_INLINE_ void _compute_area_gravity_and_dampenings(const AreaSW *p_area); - - _FORCE_INLINE_ void _update_transform_dependant(); - - friend class PhysicsDirectBodyStateSW; // i give up, too many functions to expose - -public: - void set_force_integration_callback(ObjectID p_id, const StringName &p_method, const Variant &p_udata = Variant()); - - void set_kinematic_margin(real_t p_margin); - _FORCE_INLINE_ real_t get_kinematic_margin() { return kinematic_safe_margin; } - - _FORCE_INLINE_ void add_area(AreaSW *p_area) { - int index = areas.find(AreaCMP(p_area)); - if (index > -1) { - areas.write[index].refCount += 1; - } else { - areas.ordered_insert(AreaCMP(p_area)); - } - } - - _FORCE_INLINE_ void remove_area(AreaSW *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 == PhysicsServer::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.empty(); } - _FORCE_INLINE_ void add_contact(const Vector3 &p_local_pos, const Vector3 &p_local_normal, real_t p_depth, int p_local_shape, const Vector3 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector3 &p_collider_velocity_at_pos); - - _FORCE_INLINE_ void add_exception(const RID &p_exception) { exceptions.insert(p_exception); } - _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<RID> &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_ BodySW *get_island_next() const { return island_next; } - _FORCE_INLINE_ void set_island_next(BodySW *p_next) { island_next = p_next; } - - _FORCE_INLINE_ BodySW *get_island_list_next() const { return island_list_next; } - _FORCE_INLINE_ void set_island_list_next(BodySW *p_next) { island_list_next = p_next; } - - _FORCE_INLINE_ void add_constraint(ConstraintSW *p_constraint, int p_pos) { constraint_map[p_constraint] = p_pos; } - _FORCE_INLINE_ void remove_constraint(ConstraintSW *p_constraint) { constraint_map.erase(p_constraint); } - const Map<ConstraintSW *, int> &get_constraint_map() const { return constraint_map; } - _FORCE_INLINE_ void clear_constraint_map() { constraint_map.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_ Basis get_principal_inertia_axes() const { return principal_inertia_axes; } - _FORCE_INLINE_ Vector3 get_center_of_mass() const { return center_of_mass; } - _FORCE_INLINE_ Vector3 xform_local_to_principal(const Vector3 &p_pos) const { return principal_inertia_axes_local.xform(p_pos - center_of_mass_local); } - - _FORCE_INLINE_ void set_linear_velocity(const Vector3 &p_velocity) { linear_velocity = p_velocity; } - _FORCE_INLINE_ Vector3 get_linear_velocity() const { return linear_velocity; } - - _FORCE_INLINE_ void set_angular_velocity(const Vector3 &p_velocity) { angular_velocity = p_velocity; } - _FORCE_INLINE_ Vector3 get_angular_velocity() const { return angular_velocity; } - - _FORCE_INLINE_ const Vector3 &get_biased_linear_velocity() const { return biased_linear_velocity; } - _FORCE_INLINE_ const Vector3 &get_biased_angular_velocity() const { return biased_angular_velocity; } - - _FORCE_INLINE_ void apply_central_impulse(const Vector3 &p_j) { - linear_velocity += p_j * _inv_mass; - } - - _FORCE_INLINE_ void apply_impulse(const Vector3 &p_pos, const Vector3 &p_j) { - - linear_velocity += p_j * _inv_mass; - angular_velocity += _inv_inertia_tensor.xform((p_pos - center_of_mass).cross(p_j)); - } - - _FORCE_INLINE_ void apply_torque_impulse(const Vector3 &p_j) { - - angular_velocity += _inv_inertia_tensor.xform(p_j); - } - - _FORCE_INLINE_ void apply_bias_impulse(const Vector3 &p_pos, const Vector3 &p_j, real_t p_max_delta_av = -1.0) { - - biased_linear_velocity += p_j * _inv_mass; - if (p_max_delta_av != 0.0) { - Vector3 delta_av = _inv_inertia_tensor.xform((p_pos - center_of_mass).cross(p_j)); - if (p_max_delta_av > 0 && delta_av.length() > p_max_delta_av) { - delta_av = delta_av.normalized() * p_max_delta_av; - } - biased_angular_velocity += delta_av; - } - } - - _FORCE_INLINE_ void apply_bias_torque_impulse(const Vector3 &p_j) { - - biased_angular_velocity += _inv_inertia_tensor.xform(p_j); - } - - _FORCE_INLINE_ void add_central_force(const Vector3 &p_force) { - - applied_force += p_force; - } - - _FORCE_INLINE_ void add_force(const Vector3 &p_force, const Vector3 &p_pos) { - - applied_force += p_force; - applied_torque += p_pos.cross(p_force); - } - - _FORCE_INLINE_ void add_torque(const Vector3 &p_torque) { - applied_torque += p_torque; - } - - void set_active(bool p_active); - _FORCE_INLINE_ bool is_active() const { return active; } - - _FORCE_INLINE_ void wakeup() { - if ((!get_space()) || mode == PhysicsServer::BODY_MODE_STATIC || mode == PhysicsServer::BODY_MODE_KINEMATIC) - return; - set_active(true); - } - - void set_param(PhysicsServer::BodyParameter p_param, real_t); - real_t get_param(PhysicsServer::BodyParameter p_param) const; - - void set_mode(PhysicsServer::BodyMode p_mode); - PhysicsServer::BodyMode get_mode() const; - - void set_state(PhysicsServer::BodyState p_state, const Variant &p_variant); - Variant get_state(PhysicsServer::BodyState p_state) const; - - void set_applied_force(const Vector3 &p_force) { applied_force = p_force; } - Vector3 get_applied_force() const { return applied_force; } - - void set_applied_torque(const Vector3 &p_torque) { applied_torque = p_torque; } - Vector3 get_applied_torque() const { return applied_torque; } - - _FORCE_INLINE_ void set_continuous_collision_detection(bool p_enable) { continuous_cd = p_enable; } - _FORCE_INLINE_ bool is_continuous_collision_detection_enabled() const { return continuous_cd; } - - void set_space(SpaceSW *p_space); - - void update_inertias(); - - _FORCE_INLINE_ real_t get_inv_mass() const { return _inv_mass; } - _FORCE_INLINE_ Vector3 get_inv_inertia() const { return _inv_inertia; } - _FORCE_INLINE_ Basis get_inv_inertia_tensor() const { return _inv_inertia_tensor; } - _FORCE_INLINE_ real_t get_friction() const { return friction; } - _FORCE_INLINE_ Vector3 get_gravity() const { return gravity; } - _FORCE_INLINE_ real_t get_bounce() const { return bounce; } - - void set_axis_lock(PhysicsServer::BodyAxis p_axis, bool lock); - bool is_axis_locked(PhysicsServer::BodyAxis p_axis) const; - - void integrate_forces(real_t p_step); - void integrate_velocities(real_t p_step); - - _FORCE_INLINE_ Vector3 get_velocity_in_local_point(const Vector3 &rel_pos) const { - - return linear_velocity + angular_velocity.cross(rel_pos - center_of_mass); - } - - _FORCE_INLINE_ real_t compute_impulse_denominator(const Vector3 &p_pos, const Vector3 &p_normal) const { - - Vector3 r0 = p_pos - get_transform().origin - center_of_mass; - - Vector3 c0 = (r0).cross(p_normal); - - Vector3 vec = (_inv_inertia_tensor.xform_inv(c0)).cross(r0); - - return _inv_mass + p_normal.dot(vec); - } - - _FORCE_INLINE_ real_t compute_angular_impulse_denominator(const Vector3 &p_axis) const { - - return p_axis.dot(_inv_inertia_tensor.xform_inv(p_axis)); - } - - //void simulate_motion(const Transform& p_xform,real_t p_step); - void call_queries(); - void wakeup_neighbours(); - - bool sleep_test(real_t p_step); - - BodySW(); - ~BodySW(); -}; - -//add contact inline - -void BodySW::add_contact(const Vector3 &p_local_pos, const Vector3 &p_local_normal, real_t p_depth, int p_local_shape, const Vector3 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector3 &p_collider_velocity_at_pos) { - - 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; -} - -class PhysicsDirectBodyStateSW : public PhysicsDirectBodyState { - - GDCLASS(PhysicsDirectBodyStateSW, PhysicsDirectBodyState); - -public: - static PhysicsDirectBodyStateSW *singleton; - BodySW *body; - real_t step; - - virtual Vector3 get_total_gravity() const { return body->gravity; } // get gravity vector working on this body space/area - virtual real_t get_total_angular_damp() const { return body->area_angular_damp; } // get density of this body space/area - virtual real_t get_total_linear_damp() const { return body->area_linear_damp; } // get density of this body space/area - - virtual Vector3 get_center_of_mass() const { return body->get_center_of_mass(); } - virtual Basis get_principal_inertia_axes() const { return body->get_principal_inertia_axes(); } - - virtual real_t get_inverse_mass() const { return body->get_inv_mass(); } // get the mass - virtual Vector3 get_inverse_inertia() const { return body->get_inv_inertia(); } // get density of this body space - virtual Basis get_inverse_inertia_tensor() const { return body->get_inv_inertia_tensor(); } // get density of this body space - - virtual void set_linear_velocity(const Vector3 &p_velocity) { body->set_linear_velocity(p_velocity); } - virtual Vector3 get_linear_velocity() const { return body->get_linear_velocity(); } - - virtual void set_angular_velocity(const Vector3 &p_velocity) { body->set_angular_velocity(p_velocity); } - virtual Vector3 get_angular_velocity() const { return body->get_angular_velocity(); } - - virtual void set_transform(const Transform &p_transform) { body->set_state(PhysicsServer::BODY_STATE_TRANSFORM, p_transform); } - virtual Transform get_transform() const { return body->get_transform(); } - - virtual void add_central_force(const Vector3 &p_force) { body->add_central_force(p_force); } - virtual void add_force(const Vector3 &p_force, const Vector3 &p_pos) { body->add_force(p_force, p_pos); } - virtual void add_torque(const Vector3 &p_torque) { body->add_torque(p_torque); } - virtual void apply_central_impulse(const Vector3 &p_j) { body->apply_central_impulse(p_j); } - virtual void apply_impulse(const Vector3 &p_pos, const Vector3 &p_j) { body->apply_impulse(p_pos, p_j); } - virtual void apply_torque_impulse(const Vector3 &p_j) { body->apply_torque_impulse(p_j); } - - virtual void set_sleep_state(bool p_enable) { body->set_active(!p_enable); } - virtual bool is_sleeping() const { return !body->is_active(); } - - virtual int get_contact_count() const { return body->contact_count; } - - virtual Vector3 get_contact_local_position(int p_contact_idx) const { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3()); - return body->contacts[p_contact_idx].local_pos; - } - virtual Vector3 get_contact_local_normal(int p_contact_idx) const { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3()); - return body->contacts[p_contact_idx].local_normal; - } - virtual float get_contact_impulse(int p_contact_idx) const { - return 0.0f; // Only implemented for bullet - } - virtual int 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; - } - - virtual RID 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; - } - virtual Vector3 get_contact_collider_position(int p_contact_idx) const { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3()); - return body->contacts[p_contact_idx].collider_pos; - } - virtual ObjectID get_contact_collider_id(int p_contact_idx) const { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, ObjectID()); - return body->contacts[p_contact_idx].collider_instance_id; - } - virtual int get_contact_collider_shape(int p_contact_idx) const { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, 0); - return body->contacts[p_contact_idx].collider_shape; - } - virtual Vector3 get_contact_collider_velocity_at_position(int p_contact_idx) const { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3()); - return body->contacts[p_contact_idx].collider_velocity_at_pos; - } - - virtual PhysicsDirectSpaceState *get_space_state(); - - virtual real_t get_step() const { return step; } - PhysicsDirectBodyStateSW() { - singleton = this; - body = NULL; - } -}; - -#endif // BODY__SW_H diff --git a/servers/physics/broad_phase_basic.cpp b/servers/physics/broad_phase_basic.cpp deleted file mode 100644 index f49bf9d4cc..0000000000 --- a/servers/physics/broad_phase_basic.cpp +++ /dev/null @@ -1,225 +0,0 @@ -/*************************************************************************/ -/* broad_phase_basic.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 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_basic.h" -#include "core/list.h" -#include "core/print_string.h" - -BroadPhaseSW::ID BroadPhaseBasic::create(CollisionObjectSW *p_object, int p_subindex) { - - ERR_FAIL_COND_V(p_object == NULL, 0); - - current++; - - Element e; - e.owner = p_object; - e._static = false; - e.subindex = p_subindex; - - element_map[current] = e; - return current; -} - -void BroadPhaseBasic::move(ID p_id, const AABB &p_aabb) { - - Map<ID, Element>::Element *E = element_map.find(p_id); - ERR_FAIL_COND(!E); - E->get().aabb = p_aabb; -} -void BroadPhaseBasic::set_static(ID p_id, bool p_static) { - - Map<ID, Element>::Element *E = element_map.find(p_id); - ERR_FAIL_COND(!E); - E->get()._static = p_static; -} -void BroadPhaseBasic::remove(ID p_id) { - - Map<ID, Element>::Element *E = element_map.find(p_id); - ERR_FAIL_COND(!E); - List<PairKey> to_erase; - //unpair must be done immediately on removal to avoid potential invalid pointers - for (Map<PairKey, void *>::Element *F = pair_map.front(); F; F = F->next()) { - - if (F->key().a == p_id || F->key().b == p_id) { - - if (unpair_callback) { - Element *elem_A = &element_map[F->key().a]; - Element *elem_B = &element_map[F->key().b]; - unpair_callback(elem_A->owner, elem_A->subindex, elem_B->owner, elem_B->subindex, F->get(), unpair_userdata); - } - to_erase.push_back(F->key()); - } - } - while (to_erase.size()) { - - pair_map.erase(to_erase.front()->get()); - to_erase.pop_front(); - } - element_map.erase(E); -} - -CollisionObjectSW *BroadPhaseBasic::get_object(ID p_id) const { - - const Map<ID, Element>::Element *E = element_map.find(p_id); - ERR_FAIL_COND_V(!E, NULL); - return E->get().owner; -} -bool BroadPhaseBasic::is_static(ID p_id) const { - - const Map<ID, Element>::Element *E = element_map.find(p_id); - ERR_FAIL_COND_V(!E, false); - return E->get()._static; -} -int BroadPhaseBasic::get_subindex(ID p_id) const { - - const Map<ID, Element>::Element *E = element_map.find(p_id); - ERR_FAIL_COND_V(!E, -1); - return E->get().subindex; -} - -int BroadPhaseBasic::cull_point(const Vector3 &p_point, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices) { - - int rc = 0; - - for (Map<ID, Element>::Element *E = element_map.front(); E; E = E->next()) { - - const AABB aabb = E->get().aabb; - if (aabb.has_point(p_point)) { - - p_results[rc] = E->get().owner; - p_result_indices[rc] = E->get().subindex; - rc++; - if (rc >= p_max_results) - break; - } - } - - return rc; -} - -int BroadPhaseBasic::cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices) { - - int rc = 0; - - for (Map<ID, Element>::Element *E = element_map.front(); E; E = E->next()) { - - const AABB aabb = E->get().aabb; - if (aabb.intersects_segment(p_from, p_to)) { - - p_results[rc] = E->get().owner; - p_result_indices[rc] = E->get().subindex; - rc++; - if (rc >= p_max_results) - break; - } - } - - return rc; -} -int BroadPhaseBasic::cull_aabb(const AABB &p_aabb, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices) { - - int rc = 0; - - for (Map<ID, Element>::Element *E = element_map.front(); E; E = E->next()) { - - const AABB aabb = E->get().aabb; - if (aabb.intersects(p_aabb)) { - - p_results[rc] = E->get().owner; - p_result_indices[rc] = E->get().subindex; - rc++; - if (rc >= p_max_results) - break; - } - } - - return rc; -} - -void BroadPhaseBasic::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) { - - pair_userdata = p_userdata; - pair_callback = p_pair_callback; -} -void BroadPhaseBasic::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) { - - unpair_userdata = p_userdata; - unpair_callback = p_unpair_callback; -} - -void BroadPhaseBasic::update() { - - // recompute pairs - for (Map<ID, Element>::Element *I = element_map.front(); I; I = I->next()) { - - for (Map<ID, Element>::Element *J = I->next(); J; J = J->next()) { - - Element *elem_A = &I->get(); - Element *elem_B = &J->get(); - - if (elem_A->owner == elem_B->owner) - continue; - - bool pair_ok = elem_A->aabb.intersects(elem_B->aabb) && (!elem_A->_static || !elem_B->_static); - - PairKey key(I->key(), J->key()); - - Map<PairKey, void *>::Element *E = pair_map.find(key); - - if (!pair_ok && E) { - if (unpair_callback) - unpair_callback(elem_A->owner, elem_A->subindex, elem_B->owner, elem_B->subindex, E->get(), unpair_userdata); - pair_map.erase(key); - } - - if (pair_ok && !E) { - - void *data = NULL; - if (pair_callback) - data = pair_callback(elem_A->owner, elem_A->subindex, elem_B->owner, elem_B->subindex, unpair_userdata); - pair_map.insert(key, data); - } - } - } -} - -BroadPhaseSW *BroadPhaseBasic::_create() { - - return memnew(BroadPhaseBasic); -} - -BroadPhaseBasic::BroadPhaseBasic() { - - current = 1; - unpair_callback = NULL; - unpair_userdata = NULL; - pair_callback = NULL; - pair_userdata = NULL; -} diff --git a/servers/physics/broad_phase_basic.h b/servers/physics/broad_phase_basic.h deleted file mode 100644 index 424889d8aa..0000000000 --- a/servers/physics/broad_phase_basic.h +++ /dev/null @@ -1,108 +0,0 @@ -/*************************************************************************/ -/* broad_phase_basic.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 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_BASIC_H -#define BROAD_PHASE_BASIC_H - -#include "broad_phase_sw.h" -#include "core/map.h" - -class BroadPhaseBasic : public BroadPhaseSW { - - struct Element { - - CollisionObjectSW *owner; - bool _static; - AABB aabb; - int subindex; - }; - - Map<ID, Element> element_map; - - ID current; - - struct PairKey { - - union { - struct { - ID a; - ID b; - }; - uint64_t key; - }; - - _FORCE_INLINE_ bool operator<(const PairKey &p_key) const { - return key < p_key.key; - } - - PairKey() { key = 0; } - PairKey(ID p_a, ID p_b) { - if (p_a > p_b) { - a = p_b; - b = p_a; - } else { - a = p_a; - b = p_b; - } - } - }; - - Map<PairKey, void *> pair_map; - - PairCallback pair_callback; - void *pair_userdata; - UnpairCallback unpair_callback; - void *unpair_userdata; - -public: - // 0 is an invalid ID - virtual ID create(CollisionObjectSW *p_object, int p_subindex = 0); - virtual void move(ID p_id, const AABB &p_aabb); - virtual void set_static(ID p_id, bool p_static); - virtual void remove(ID p_id); - - virtual CollisionObjectSW *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_point(const Vector3 &p_point, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL); - virtual int cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL); - virtual int cull_aabb(const AABB &p_aabb, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL); - - virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata); - virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata); - - virtual void update(); - - static BroadPhaseSW *_create(); - BroadPhaseBasic(); -}; - -#endif // BROAD_PHASE_BASIC_H diff --git a/servers/physics/broad_phase_octree.cpp b/servers/physics/broad_phase_octree.cpp deleted file mode 100644 index a9aa662abf..0000000000 --- a/servers/physics/broad_phase_octree.cpp +++ /dev/null @@ -1,129 +0,0 @@ -/*************************************************************************/ -/* broad_phase_octree.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 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_octree.h" -#include "collision_object_sw.h" - -BroadPhaseSW::ID BroadPhaseOctree::create(CollisionObjectSW *p_object, int p_subindex) { - - ID oid = octree.create(p_object, AABB(), p_subindex, false, 1 << p_object->get_type(), 0); - return oid; -} - -void BroadPhaseOctree::move(ID p_id, const AABB &p_aabb) { - - octree.move(p_id, p_aabb); -} - -void BroadPhaseOctree::set_static(ID p_id, bool p_static) { - - CollisionObjectSW *it = octree.get(p_id); - octree.set_pairable(p_id, !p_static, 1 << it->get_type(), p_static ? 0 : 0xFFFFF); //pair everything, don't care 1? -} -void BroadPhaseOctree::remove(ID p_id) { - - octree.erase(p_id); -} - -CollisionObjectSW *BroadPhaseOctree::get_object(ID p_id) const { - - CollisionObjectSW *it = octree.get(p_id); - ERR_FAIL_COND_V(!it, NULL); - return it; -} -bool BroadPhaseOctree::is_static(ID p_id) const { - - return !octree.is_pairable(p_id); -} -int BroadPhaseOctree::get_subindex(ID p_id) const { - - return octree.get_subindex(p_id); -} - -int BroadPhaseOctree::cull_point(const Vector3 &p_point, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices) { - - return octree.cull_point(p_point, p_results, p_max_results, p_result_indices); -} - -int BroadPhaseOctree::cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices) { - - return octree.cull_segment(p_from, p_to, p_results, p_max_results, p_result_indices); -} - -int BroadPhaseOctree::cull_aabb(const AABB &p_aabb, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices) { - - return octree.cull_aabb(p_aabb, p_results, p_max_results, p_result_indices); -} - -void *BroadPhaseOctree::_pair_callback(void *self, OctreeElementID p_A, CollisionObjectSW *p_object_A, int subindex_A, OctreeElementID p_B, CollisionObjectSW *p_object_B, int subindex_B) { - - BroadPhaseOctree *bpo = (BroadPhaseOctree *)(self); - if (!bpo->pair_callback) - return NULL; - - return bpo->pair_callback(p_object_A, subindex_A, p_object_B, subindex_B, bpo->pair_userdata); -} - -void BroadPhaseOctree::_unpair_callback(void *self, OctreeElementID p_A, CollisionObjectSW *p_object_A, int subindex_A, OctreeElementID p_B, CollisionObjectSW *p_object_B, int subindex_B, void *pairdata) { - - BroadPhaseOctree *bpo = (BroadPhaseOctree *)(self); - if (!bpo->unpair_callback) - return; - - bpo->unpair_callback(p_object_A, subindex_A, p_object_B, subindex_B, pairdata, bpo->unpair_userdata); -} - -void BroadPhaseOctree::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) { - - pair_callback = p_pair_callback; - pair_userdata = p_userdata; -} -void BroadPhaseOctree::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) { - - unpair_callback = p_unpair_callback; - unpair_userdata = p_userdata; -} - -void BroadPhaseOctree::update() { - // does.. not? -} - -BroadPhaseSW *BroadPhaseOctree::_create() { - - return memnew(BroadPhaseOctree); -} - -BroadPhaseOctree::BroadPhaseOctree() { - octree.set_pair_callback(_pair_callback, this); - octree.set_unpair_callback(_unpair_callback, this); - pair_callback = NULL; - pair_userdata = NULL; - unpair_userdata = NULL; -} diff --git a/servers/physics/broad_phase_octree.h b/servers/physics/broad_phase_octree.h deleted file mode 100644 index e2a1d82b69..0000000000 --- a/servers/physics/broad_phase_octree.h +++ /dev/null @@ -1,73 +0,0 @@ -/*************************************************************************/ -/* broad_phase_octree.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 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_OCTREE_H -#define BROAD_PHASE_OCTREE_H - -#include "broad_phase_sw.h" -#include "core/math/octree.h" - -class BroadPhaseOctree : public BroadPhaseSW { - - Octree<CollisionObjectSW, true> octree; - - static void *_pair_callback(void *, OctreeElementID, CollisionObjectSW *, int, OctreeElementID, CollisionObjectSW *, int); - static void _unpair_callback(void *, OctreeElementID, CollisionObjectSW *, int, OctreeElementID, CollisionObjectSW *, int, void *); - - PairCallback pair_callback; - void *pair_userdata; - UnpairCallback unpair_callback; - void *unpair_userdata; - -public: - // 0 is an invalid ID - virtual ID create(CollisionObjectSW *p_object, int p_subindex = 0); - virtual void move(ID p_id, const AABB &p_aabb); - virtual void set_static(ID p_id, bool p_static); - virtual void remove(ID p_id); - - virtual CollisionObjectSW *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_point(const Vector3 &p_point, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL); - virtual int cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL); - virtual int cull_aabb(const AABB &p_aabb, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL); - - virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata); - virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata); - - virtual void update(); - - static BroadPhaseSW *_create(); - BroadPhaseOctree(); -}; - -#endif // BROAD_PHASE_OCTREE_H diff --git a/servers/physics/broad_phase_sw.cpp b/servers/physics/broad_phase_sw.cpp deleted file mode 100644 index a6fc253b1b..0000000000 --- a/servers/physics/broad_phase_sw.cpp +++ /dev/null @@ -1,36 +0,0 @@ -/*************************************************************************/ -/* broad_phase_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 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_sw.h" - -BroadPhaseSW::CreateFunction BroadPhaseSW::create_func = NULL; - -BroadPhaseSW::~BroadPhaseSW() { -} diff --git a/servers/physics/broad_phase_sw.h b/servers/physics/broad_phase_sw.h deleted file mode 100644 index e69a2d24ed..0000000000 --- a/servers/physics/broad_phase_sw.h +++ /dev/null @@ -1,73 +0,0 @@ -/*************************************************************************/ -/* broad_phase_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 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_SW_H -#define BROAD_PHASE_SW_H - -#include "core/math/aabb.h" -#include "core/math/math_funcs.h" - -class CollisionObjectSW; - -class BroadPhaseSW { - -public: - typedef BroadPhaseSW *(*CreateFunction)(); - - static CreateFunction create_func; - - typedef uint32_t ID; - - typedef void *(*PairCallback)(CollisionObjectSW *A, int p_subindex_A, CollisionObjectSW *B, int p_subindex_B, void *p_userdata); - typedef void (*UnpairCallback)(CollisionObjectSW *A, int p_subindex_A, CollisionObjectSW *B, int p_subindex_B, void *p_data, void *p_userdata); - - // 0 is an invalid ID - virtual ID create(CollisionObjectSW *p_object_, int p_subindex = 0) = 0; - virtual void move(ID p_id, const AABB &p_aabb) = 0; - virtual void set_static(ID p_id, bool p_static) = 0; - virtual void remove(ID p_id) = 0; - - virtual CollisionObjectSW *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_point(const Vector3 &p_point, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL) = 0; - virtual int cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL) = 0; - virtual int cull_aabb(const AABB &p_aabb, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL) = 0; - - virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata) = 0; - virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) = 0; - - virtual void update() = 0; - - virtual ~BroadPhaseSW(); -}; - -#endif // BROAD_PHASE__SW_H diff --git a/servers/physics/collision_object_sw.cpp b/servers/physics/collision_object_sw.cpp deleted file mode 100644 index 3cabf75ab6..0000000000 --- a/servers/physics/collision_object_sw.cpp +++ /dev/null @@ -1,239 +0,0 @@ -/*************************************************************************/ -/* collision_object_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 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_sw.h" -#include "servers/physics/physics_server_sw.h" -#include "space_sw.h" - -void CollisionObjectSW::add_shape(ShapeSW *p_shape, const Transform &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; - shapes.push_back(s); - p_shape->add_owner(this); - - if (!pending_shape_update_list.in_list()) { - PhysicsServerSW::singleton->pending_shape_update_list.add(&pending_shape_update_list); - } - //_update_shapes(); - //_shapes_changed(); -} - -void CollisionObjectSW::set_shape(int p_index, ShapeSW *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()) { - PhysicsServerSW::singleton->pending_shape_update_list.add(&pending_shape_update_list); - } - //_update_shapes(); - //_shapes_changed(); -} -void CollisionObjectSW::set_shape_transform(int p_index, const Transform &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()) { - PhysicsServerSW::singleton->pending_shape_update_list.add(&pending_shape_update_list); - } - //_update_shapes(); - //_shapes_changed(); -} - -void CollisionObjectSW::set_shape_as_disabled(int p_idx, bool p_enable) { - shapes.write[p_idx].disabled = p_enable; - if (!pending_shape_update_list.in_list()) { - PhysicsServerSW::singleton->pending_shape_update_list.add(&pending_shape_update_list); - } -} - -void CollisionObjectSW::remove_shape(ShapeSW *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 CollisionObjectSW::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()) { - PhysicsServerSW::singleton->pending_shape_update_list.add(&pending_shape_update_list); - } - //_update_shapes(); - //_shapes_changed(); -} - -void CollisionObjectSW::_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 CollisionObjectSW::_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 CollisionObjectSW::_update_shapes() { - - if (!space) - return; - - for (int i = 0; i < shapes.size(); i++) { - - Shape &s = shapes.write[i]; - if (s.bpid == 0) { - s.bpid = space->get_broadphase()->create(this, i); - space->get_broadphase()->set_static(s.bpid, _static); - } - - //not quite correct, should compute the next matrix.. - AABB shape_aabb = s.shape->get_aabb(); - Transform xform = transform * s.xform; - shape_aabb = xform.xform(shape_aabb); - s.aabb_cache = shape_aabb; - s.aabb_cache = s.aabb_cache.grow((s.aabb_cache.size.x + s.aabb_cache.size.y) * 0.5 * 0.05); - - Vector3 scale = xform.get_basis().get_scale(); - s.area_cache = s.shape->get_area() * scale.x * scale.y * scale.z; - - space->get_broadphase()->move(s.bpid, s.aabb_cache); - } -} - -void CollisionObjectSW::_update_shapes_with_motion(const Vector3 &p_motion) { - - if (!space) - return; - - for (int i = 0; i < shapes.size(); i++) { - - Shape &s = shapes.write[i]; - if (s.bpid == 0) { - s.bpid = space->get_broadphase()->create(this, i); - space->get_broadphase()->set_static(s.bpid, _static); - } - - //not quite correct, should compute the next matrix.. - AABB shape_aabb = s.shape->get_aabb(); - Transform xform = transform * s.xform; - shape_aabb = xform.xform(shape_aabb); - shape_aabb = shape_aabb.merge(AABB(shape_aabb.position + p_motion, shape_aabb.size)); //use motion - s.aabb_cache = shape_aabb; - - space->get_broadphase()->move(s.bpid, shape_aabb); - } -} - -void CollisionObjectSW::_set_space(SpaceSW *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 CollisionObjectSW::_shape_changed() { - - _update_shapes(); - _shapes_changed(); -} - -CollisionObjectSW::CollisionObjectSW(Type p_type) : - pending_shape_update_list(this) { - - _static = true; - type = p_type; - space = NULL; - - collision_layer = 1; - collision_mask = 1; - ray_pickable = true; -} diff --git a/servers/physics/collision_object_sw.h b/servers/physics/collision_object_sw.h deleted file mode 100644 index 7c0e66ff90..0000000000 --- a/servers/physics/collision_object_sw.h +++ /dev/null @@ -1,164 +0,0 @@ -/*************************************************************************/ -/* collision_object_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 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_SW_H -#define COLLISION_OBJECT_SW_H - -#include "broad_phase_sw.h" -#include "core/self_list.h" -#include "servers/physics_server.h" -#include "shape_sw.h" - -#ifdef DEBUG_ENABLED -#define MAX_OBJECT_DISTANCE 3.1622776601683791e+18 - -#define MAX_OBJECT_DISTANCE_X2 (MAX_OBJECT_DISTANCE * MAX_OBJECT_DISTANCE) -#endif - -class SpaceSW; - -class CollisionObjectSW : public ShapeOwnerSW { -public: - enum Type { - TYPE_AREA, - TYPE_BODY - }; - -private: - Type type; - RID self; - ObjectID instance_id; - uint32_t collision_layer; - uint32_t collision_mask; - - struct Shape { - - Transform xform; - Transform xform_inv; - BroadPhaseSW::ID bpid; - AABB aabb_cache; //for rayqueries - real_t area_cache; - ShapeSW *shape; - bool disabled; - - Shape() { disabled = false; } - }; - - Vector<Shape> shapes; - SpaceSW *space; - Transform transform; - Transform inv_transform; - bool _static; - - SelfList<CollisionObjectSW> pending_shape_update_list; - - void _update_shapes(); - -protected: - void _update_shapes_with_motion(const Vector3 &p_motion); - void _unregister_shapes(); - - _FORCE_INLINE_ void _set_transform(const Transform &p_transform, bool p_update_shapes = true) { -#ifdef DEBUG_ENABLED - - ERR_FAIL_COND_MSG(p_transform.origin.length_squared() > MAX_OBJECT_DISTANCE_X2, "Object went too far away (more than '" + itos(MAX_OBJECT_DISTANCE) + "' units from origin)."); -#endif - - transform = p_transform; - if (p_update_shapes) _update_shapes(); - } - _FORCE_INLINE_ void _set_inv_transform(const Transform &p_transform) { inv_transform = p_transform; } - void _set_static(bool p_static); - - virtual void _shapes_changed() = 0; - void _set_space(SpaceSW *p_space); - - bool ray_pickable; - - CollisionObjectSW(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; } - - void _shape_changed(); - - _FORCE_INLINE_ Type get_type() const { return type; } - void add_shape(ShapeSW *p_shape, const Transform &p_transform = Transform(), bool p_disabled = false); - void set_shape(int p_index, ShapeSW *p_shape); - void set_shape_transform(int p_index, const Transform &p_transform); - _FORCE_INLINE_ int get_shape_count() const { return shapes.size(); } - _FORCE_INLINE_ bool is_shape_disabled(int p_index) const { - CRASH_BAD_INDEX(p_index, shapes.size()); - return shapes[p_index].disabled; - } - _FORCE_INLINE_ ShapeSW *get_shape(int p_index) const { return shapes[p_index].shape; } - _FORCE_INLINE_ const Transform &get_shape_transform(int p_index) const { return shapes[p_index].xform; } - _FORCE_INLINE_ const Transform &get_shape_inv_transform(int p_index) const { return shapes[p_index].xform_inv; } - _FORCE_INLINE_ const AABB &get_shape_aabb(int p_index) const { return shapes[p_index].aabb_cache; } - _FORCE_INLINE_ real_t get_shape_area(int p_index) const { return shapes[p_index].area_cache; } - - _FORCE_INLINE_ Transform get_transform() const { return transform; } - _FORCE_INLINE_ Transform get_inv_transform() const { return inv_transform; } - _FORCE_INLINE_ SpaceSW *get_space() const { return space; } - - _FORCE_INLINE_ void set_ray_pickable(bool p_enable) { ray_pickable = p_enable; } - _FORCE_INLINE_ bool is_ray_pickable() const { return ray_pickable; } - - void set_shape_as_disabled(int p_idx, bool p_enable); - _FORCE_INLINE_ bool is_shape_set_as_disabled(int p_idx) const { - CRASH_BAD_INDEX(p_idx, shapes.size()); - return shapes[p_idx].disabled; - } - - _FORCE_INLINE_ void set_collision_layer(uint32_t p_layer) { collision_layer = p_layer; } - _FORCE_INLINE_ uint32_t get_collision_layer() const { return collision_layer; } - - _FORCE_INLINE_ void set_collision_mask(uint32_t p_mask) { collision_mask = p_mask; } - _FORCE_INLINE_ uint32_t get_collision_mask() const { return collision_mask; } - - _FORCE_INLINE_ bool test_collision_mask(CollisionObjectSW *p_other) const { - return collision_layer & p_other->collision_mask || p_other->collision_layer & collision_mask; - } - - void remove_shape(ShapeSW *p_shape); - void remove_shape(int p_index); - - virtual void set_space(SpaceSW *p_space) = 0; - - _FORCE_INLINE_ bool is_static() const { return _static; } - - virtual ~CollisionObjectSW() {} -}; - -#endif // COLLISION_OBJECT_SW_H diff --git a/servers/physics/collision_solver_sat.cpp b/servers/physics/collision_solver_sat.cpp deleted file mode 100644 index 0e0dfd3cf2..0000000000 --- a/servers/physics/collision_solver_sat.cpp +++ /dev/null @@ -1,1591 +0,0 @@ -/*************************************************************************/ -/* collision_solver_sat.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 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_sat.h" -#include "core/math/geometry.h" - -#define _EDGE_IS_VALID_SUPPORT_THRESHOLD 0.02 - -struct _CollectorCallback { - - CollisionSolverSW::CallbackResult callback; - void *userdata; - bool swap; - bool collided; - Vector3 normal; - Vector3 *prev_axis; - - _FORCE_INLINE_ void call(const Vector3 &p_point_A, const Vector3 &p_point_B) { - - if (swap) - callback(p_point_B, p_point_A, userdata); - else - callback(p_point_A, p_point_B, userdata); - } -}; - -typedef void (*GenerateContactsFunc)(const Vector3 *, int, const Vector3 *, int, _CollectorCallback *); - -static void _generate_contacts_point_point(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { - -#ifdef DEBUG_ENABLED - ERR_FAIL_COND(p_point_count_A != 1); - ERR_FAIL_COND(p_point_count_B != 1); -#endif - - p_callback->call(*p_points_A, *p_points_B); -} - -static void _generate_contacts_point_edge(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { - -#ifdef DEBUG_ENABLED - ERR_FAIL_COND(p_point_count_A != 1); - ERR_FAIL_COND(p_point_count_B != 2); -#endif - - Vector3 closest_B = Geometry::get_closest_point_to_segment_uncapped(*p_points_A, p_points_B); - p_callback->call(*p_points_A, closest_B); -} - -static void _generate_contacts_point_face(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { - -#ifdef DEBUG_ENABLED - ERR_FAIL_COND(p_point_count_A != 1); - ERR_FAIL_COND(p_point_count_B < 3); -#endif - - Vector3 closest_B = Plane(p_points_B[0], p_points_B[1], p_points_B[2]).project(*p_points_A); - - p_callback->call(*p_points_A, closest_B); -} - -static void _generate_contacts_edge_edge(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { - -#ifdef DEBUG_ENABLED - ERR_FAIL_COND(p_point_count_A != 2); - ERR_FAIL_COND(p_point_count_B != 2); // circle is actually a 4x3 matrix -#endif - - Vector3 rel_A = p_points_A[1] - p_points_A[0]; - Vector3 rel_B = p_points_B[1] - p_points_B[0]; - - Vector3 c = rel_A.cross(rel_B).cross(rel_B); - - if (Math::is_zero_approx(rel_A.dot(c))) { - - // should handle somehow.. - //ERR_PRINT("TODO FIX"); - //return; - - Vector3 axis = rel_A.normalized(); //make an axis - Vector3 base_A = p_points_A[0] - axis * axis.dot(p_points_A[0]); - Vector3 base_B = p_points_B[0] - axis * axis.dot(p_points_B[0]); - - //sort all 4 points in axis - real_t dvec[4] = { axis.dot(p_points_A[0]), axis.dot(p_points_A[1]), axis.dot(p_points_B[0]), axis.dot(p_points_B[1]) }; - - SortArray<real_t> sa; - sa.sort(dvec, 4); - - //use the middle ones as contacts - p_callback->call(base_A + axis * dvec[1], base_B + axis * dvec[1]); - p_callback->call(base_A + axis * dvec[2], base_B + axis * dvec[2]); - - return; - } - - real_t d = (c.dot(p_points_B[0]) - p_points_A[0].dot(c)) / rel_A.dot(c); - - if (d < 0.0) - d = 0.0; - else if (d > 1.0) - d = 1.0; - - Vector3 closest_A = p_points_A[0] + rel_A * d; - Vector3 closest_B = Geometry::get_closest_point_to_segment_uncapped(closest_A, p_points_B); - p_callback->call(closest_A, closest_B); -} - -static void _generate_contacts_face_face(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { - -#ifdef DEBUG_ENABLED - ERR_FAIL_COND(p_point_count_A < 2); - ERR_FAIL_COND(p_point_count_B < 3); -#endif - - static const int max_clip = 32; - - Vector3 _clipbuf1[max_clip]; - Vector3 _clipbuf2[max_clip]; - Vector3 *clipbuf_src = _clipbuf1; - Vector3 *clipbuf_dst = _clipbuf2; - int clipbuf_len = p_point_count_A; - - // copy A points to clipbuf_src - for (int i = 0; i < p_point_count_A; i++) { - - clipbuf_src[i] = p_points_A[i]; - } - - Plane plane_B(p_points_B[0], p_points_B[1], p_points_B[2]); - - // go through all of B points - for (int i = 0; i < p_point_count_B; i++) { - - int i_n = (i + 1) % p_point_count_B; - - Vector3 edge0_B = p_points_B[i]; - Vector3 edge1_B = p_points_B[i_n]; - - Vector3 clip_normal = (edge0_B - edge1_B).cross(plane_B.normal).normalized(); - // make a clip plane - - Plane clip(edge0_B, clip_normal); - // avoid double clip if A is edge - int dst_idx = 0; - bool edge = clipbuf_len == 2; - for (int j = 0; j < clipbuf_len; j++) { - - int j_n = (j + 1) % clipbuf_len; - - Vector3 edge0_A = clipbuf_src[j]; - Vector3 edge1_A = clipbuf_src[j_n]; - - real_t dist0 = clip.distance_to(edge0_A); - real_t dist1 = clip.distance_to(edge1_A); - - if (dist0 <= 0) { // behind plane - - ERR_FAIL_COND(dst_idx >= max_clip); - clipbuf_dst[dst_idx++] = clipbuf_src[j]; - } - - // check for different sides and non coplanar - //if ( (dist0*dist1) < -CMP_EPSILON && !(edge && j)) { - if ((dist0 * dist1) < 0 && !(edge && j)) { - - // calculate intersection - Vector3 rel = edge1_A - edge0_A; - real_t den = clip.normal.dot(rel); - real_t dist = -(clip.normal.dot(edge0_A) - clip.d) / den; - Vector3 inters = edge0_A + rel * dist; - - ERR_FAIL_COND(dst_idx >= max_clip); - clipbuf_dst[dst_idx] = inters; - dst_idx++; - } - } - - clipbuf_len = dst_idx; - SWAP(clipbuf_src, clipbuf_dst); - } - - // generate contacts - //Plane plane_A(p_points_A[0],p_points_A[1],p_points_A[2]); - - for (int i = 0; i < clipbuf_len; i++) { - - real_t d = plane_B.distance_to(clipbuf_src[i]); - /* - if (d>CMP_EPSILON) - continue; - */ - - Vector3 closest_B = clipbuf_src[i] - plane_B.normal * d; - - if (p_callback->normal.dot(clipbuf_src[i]) >= p_callback->normal.dot(closest_B)) - continue; - - p_callback->call(clipbuf_src[i], closest_B); - } -} - -static void _generate_contacts_from_supports(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { - -#ifdef DEBUG_ENABLED - ERR_FAIL_COND(p_point_count_A < 1); - ERR_FAIL_COND(p_point_count_B < 1); -#endif - - static const GenerateContactsFunc generate_contacts_func_table[3][3] = { - { - _generate_contacts_point_point, - _generate_contacts_point_edge, - _generate_contacts_point_face, - }, - { - 0, - _generate_contacts_edge_edge, - _generate_contacts_face_face, - }, - { - 0, - 0, - _generate_contacts_face_face, - } - }; - - int pointcount_B; - int pointcount_A; - const Vector3 *points_A; - const Vector3 *points_B; - - if (p_point_count_A > p_point_count_B) { - //swap - p_callback->swap = !p_callback->swap; - p_callback->normal = -p_callback->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 > 3 ? 3 : pointcount_A) - 1; - int version_B = (pointcount_B > 3 ? 3 : pointcount_B) - 1; - - GenerateContactsFunc contacts_func = generate_contacts_func_table[version_A][version_B]; - ERR_FAIL_COND(!contacts_func); - contacts_func(points_A, pointcount_A, points_B, pointcount_B, p_callback); -} - -template <class ShapeA, class ShapeB, bool withMargin = false> -class SeparatorAxisTest { - - const ShapeA *shape_A; - const ShapeB *shape_B; - const Transform *transform_A; - const Transform *transform_B; - real_t best_depth; - Vector3 best_axis; - _CollectorCallback *callback; - real_t margin_A; - real_t margin_B; - Vector3 separator_axis; - -public: - _FORCE_INLINE_ bool test_previous_axis() { - - if (callback && callback->prev_axis && *callback->prev_axis != Vector3()) - return test_axis(*callback->prev_axis); - else - return true; - } - - _FORCE_INLINE_ bool test_axis(const Vector3 &p_axis) { - - Vector3 axis = p_axis; - - if (Math::abs(axis.x) < CMP_EPSILON && - Math::abs(axis.y) < CMP_EPSILON && - Math::abs(axis.z) < CMP_EPSILON) { - // strange case, try an upwards separator - axis = Vector3(0.0, 1.0, 0.0); - } - - real_t min_A, max_A, min_B, max_B; - - shape_A->project_range(axis, *transform_A, min_A, max_A); - 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; - - min_B -= (min_A + max_A) * 0.5; - max_B -= (min_A + max_A) * 0.5; - - if (min_B > 0.0 || max_B < 0.0) { - separator_axis = axis; - return false; // doesn't contain 0 - } - - //use the smallest depth - - if (min_B < 0.0) { // could be +0.0, we don't want it to become -0.0 - min_B = -min_B; - } - - if (max_B < min_B) { - if (max_B < best_depth) { - best_depth = max_B; - best_axis = axis; - } - } else { - if (min_B < best_depth) { - best_depth = min_B; - best_axis = -axis; // keep it as A axis - } - } - - return true; - } - - _FORCE_INLINE_ void generate_contacts() { - - // nothing to do, don't generate - if (best_axis == Vector3(0.0, 0.0, 0.0)) - return; - - if (!callback->callback) { - //just was checking intersection? - callback->collided = true; - if (callback->prev_axis) - *callback->prev_axis = best_axis; - return; - } - - static const int max_supports = 16; - - Vector3 supports_A[max_supports]; - int support_count_A; - shape_A->get_supports(transform_A->basis.xform_inv(-best_axis).normalized(), max_supports, supports_A, support_count_A); - 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; - } - } - - Vector3 supports_B[max_supports]; - int support_count_B; - shape_B->get_supports(transform_B->basis.xform_inv(best_axis).normalized(), max_supports, supports_B, support_count_B); - 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; - } - } - - callback->normal = best_axis; - if (callback->prev_axis) - *callback->prev_axis = best_axis; - _generate_contacts_from_supports(supports_A, support_count_A, supports_B, support_count_B, callback); - - callback->collided = true; - } - - _FORCE_INLINE_ SeparatorAxisTest(const ShapeA *p_shape_A, const Transform &p_transform_A, const ShapeB *p_shape_B, const Transform &p_transform_B, _CollectorCallback *p_callback, real_t p_margin_A = 0, real_t p_margin_B = 0) { - best_depth = 1e15; - shape_A = p_shape_A; - shape_B = p_shape_B; - transform_A = &p_transform_A; - transform_B = &p_transform_B; - callback = p_callback; - margin_A = p_margin_A; - margin_B = p_margin_B; - } -}; - -/****** SAT TESTS *******/ - -typedef void (*CollisionFunc)(const ShapeSW *, const Transform &, const ShapeSW *, const Transform &, _CollectorCallback *p_callback, real_t, real_t); - -template <bool withMargin> -static void _collision_sphere_sphere(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - - const SphereShapeSW *sphere_A = static_cast<const SphereShapeSW *>(p_a); - const SphereShapeSW *sphere_B = static_cast<const SphereShapeSW *>(p_b); - - SeparatorAxisTest<SphereShapeSW, SphereShapeSW, withMargin> separator(sphere_A, p_transform_a, sphere_B, p_transform_b, p_collector, p_margin_a, p_margin_b); - - // previous axis - - if (!separator.test_previous_axis()) - return; - - if (!separator.test_axis((p_transform_a.origin - p_transform_b.origin).normalized())) - return; - - separator.generate_contacts(); -} - -template <bool withMargin> -static void _collision_sphere_box(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - - const SphereShapeSW *sphere_A = static_cast<const SphereShapeSW *>(p_a); - const BoxShapeSW *box_B = static_cast<const BoxShapeSW *>(p_b); - - SeparatorAxisTest<SphereShapeSW, BoxShapeSW, withMargin> separator(sphere_A, p_transform_a, box_B, p_transform_b, p_collector, p_margin_a, p_margin_b); - - if (!separator.test_previous_axis()) - return; - - // test faces - - for (int i = 0; i < 3; i++) { - - Vector3 axis = p_transform_b.basis.get_axis(i).normalized(); - - if (!separator.test_axis(axis)) - return; - } - - // calculate closest point to sphere - - Vector3 cnormal = p_transform_b.xform_inv(p_transform_a.origin); - - Vector3 cpoint = p_transform_b.xform(Vector3( - - (cnormal.x < 0) ? -box_B->get_half_extents().x : box_B->get_half_extents().x, - (cnormal.y < 0) ? -box_B->get_half_extents().y : box_B->get_half_extents().y, - (cnormal.z < 0) ? -box_B->get_half_extents().z : box_B->get_half_extents().z)); - - // use point to test axis - Vector3 point_axis = (p_transform_a.origin - cpoint).normalized(); - - if (!separator.test_axis(point_axis)) - return; - - // test edges - - for (int i = 0; i < 3; i++) { - - Vector3 axis = point_axis.cross(p_transform_b.basis.get_axis(i)).cross(p_transform_b.basis.get_axis(i)).normalized(); - - if (!separator.test_axis(axis)) - return; - } - - separator.generate_contacts(); -} - -template <bool withMargin> -static void _collision_sphere_capsule(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - - const SphereShapeSW *sphere_A = static_cast<const SphereShapeSW *>(p_a); - const CapsuleShapeSW *capsule_B = static_cast<const CapsuleShapeSW *>(p_b); - - SeparatorAxisTest<SphereShapeSW, CapsuleShapeSW, withMargin> separator(sphere_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_margin_a, p_margin_b); - - if (!separator.test_previous_axis()) - return; - - //capsule sphere 1, sphere - - Vector3 capsule_axis = p_transform_b.basis.get_axis(2) * (capsule_B->get_height() * 0.5); - - Vector3 capsule_ball_1 = p_transform_b.origin + capsule_axis; - - if (!separator.test_axis((capsule_ball_1 - p_transform_a.origin).normalized())) - return; - - //capsule sphere 2, sphere - - Vector3 capsule_ball_2 = p_transform_b.origin - capsule_axis; - - if (!separator.test_axis((capsule_ball_2 - p_transform_a.origin).normalized())) - return; - - //capsule edge, sphere - - Vector3 b2a = p_transform_a.origin - p_transform_b.origin; - - Vector3 axis = b2a.cross(capsule_axis).cross(capsule_axis).normalized(); - - if (!separator.test_axis(axis)) - return; - - separator.generate_contacts(); -} - -template <bool withMargin> -static void _collision_sphere_cylinder(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { -} - -template <bool withMargin> -static void _collision_sphere_convex_polygon(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - - const SphereShapeSW *sphere_A = static_cast<const SphereShapeSW *>(p_a); - const ConvexPolygonShapeSW *convex_polygon_B = static_cast<const ConvexPolygonShapeSW *>(p_b); - - SeparatorAxisTest<SphereShapeSW, ConvexPolygonShapeSW, withMargin> separator(sphere_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b); - - if (!separator.test_previous_axis()) - return; - - const Geometry::MeshData &mesh = convex_polygon_B->get_mesh(); - - const Geometry::MeshData::Face *faces = mesh.faces.ptr(); - int face_count = mesh.faces.size(); - const Geometry::MeshData::Edge *edges = mesh.edges.ptr(); - int edge_count = mesh.edges.size(); - const Vector3 *vertices = mesh.vertices.ptr(); - int vertex_count = mesh.vertices.size(); - - // faces of B - for (int i = 0; i < face_count; i++) { - - Vector3 axis = p_transform_b.xform(faces[i].plane).normal; - - if (!separator.test_axis(axis)) - return; - } - - // edges of B - for (int i = 0; i < edge_count; i++) { - - Vector3 v1 = p_transform_b.xform(vertices[edges[i].a]); - Vector3 v2 = p_transform_b.xform(vertices[edges[i].b]); - Vector3 v3 = p_transform_a.origin; - - Vector3 n1 = v2 - v1; - Vector3 n2 = v2 - v3; - - Vector3 axis = n1.cross(n2).cross(n1).normalized(); - - if (!separator.test_axis(axis)) - return; - } - - // vertices of B - for (int i = 0; i < vertex_count; i++) { - - Vector3 v1 = p_transform_b.xform(vertices[i]); - Vector3 v2 = p_transform_a.origin; - - Vector3 axis = (v2 - v1).normalized(); - - if (!separator.test_axis(axis)) - return; - } - - separator.generate_contacts(); -} - -template <bool withMargin> -static void _collision_sphere_face(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - - const SphereShapeSW *sphere_A = static_cast<const SphereShapeSW *>(p_a); - const FaceShapeSW *face_B = static_cast<const FaceShapeSW *>(p_b); - - SeparatorAxisTest<SphereShapeSW, FaceShapeSW, withMargin> separator(sphere_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b); - - Vector3 vertex[3] = { - p_transform_b.xform(face_B->vertex[0]), - p_transform_b.xform(face_B->vertex[1]), - p_transform_b.xform(face_B->vertex[2]), - }; - - if (!separator.test_axis((vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized())) - return; - - // edges and points of B - for (int i = 0; i < 3; i++) { - - Vector3 n1 = vertex[i] - p_transform_a.origin; - - if (!separator.test_axis(n1.normalized())) { - return; - } - - Vector3 n2 = vertex[(i + 1) % 3] - vertex[i]; - - Vector3 axis = n1.cross(n2).cross(n2).normalized(); - - if (!separator.test_axis(axis)) { - return; - } - } - - separator.generate_contacts(); -} - -template <bool withMargin> -static void _collision_box_box(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - - const BoxShapeSW *box_A = static_cast<const BoxShapeSW *>(p_a); - const BoxShapeSW *box_B = static_cast<const BoxShapeSW *>(p_b); - - SeparatorAxisTest<BoxShapeSW, BoxShapeSW, withMargin> separator(box_A, p_transform_a, box_B, p_transform_b, p_collector, p_margin_a, p_margin_b); - - if (!separator.test_previous_axis()) - return; - - // test faces of A - - for (int i = 0; i < 3; i++) { - - Vector3 axis = p_transform_a.basis.get_axis(i).normalized(); - - if (!separator.test_axis(axis)) - return; - } - - // test faces of B - - for (int i = 0; i < 3; i++) { - - Vector3 axis = p_transform_b.basis.get_axis(i).normalized(); - - if (!separator.test_axis(axis)) - return; - } - - // test combined edges - for (int i = 0; i < 3; i++) { - - for (int j = 0; j < 3; j++) { - - Vector3 axis = p_transform_a.basis.get_axis(i).cross(p_transform_b.basis.get_axis(j)); - - if (Math::is_zero_approx(axis.length_squared())) - continue; - axis.normalize(); - - if (!separator.test_axis(axis)) { - return; - } - } - } - - if (withMargin) { - //add endpoint test between closest vertices and edges - - // calculate closest point to sphere - - Vector3 ab_vec = p_transform_b.origin - p_transform_a.origin; - - Vector3 cnormal_a = p_transform_a.basis.xform_inv(ab_vec); - - Vector3 support_a = p_transform_a.xform(Vector3( - - (cnormal_a.x < 0) ? -box_A->get_half_extents().x : box_A->get_half_extents().x, - (cnormal_a.y < 0) ? -box_A->get_half_extents().y : box_A->get_half_extents().y, - (cnormal_a.z < 0) ? -box_A->get_half_extents().z : box_A->get_half_extents().z)); - - Vector3 cnormal_b = p_transform_b.basis.xform_inv(-ab_vec); - - Vector3 support_b = p_transform_b.xform(Vector3( - - (cnormal_b.x < 0) ? -box_B->get_half_extents().x : box_B->get_half_extents().x, - (cnormal_b.y < 0) ? -box_B->get_half_extents().y : box_B->get_half_extents().y, - (cnormal_b.z < 0) ? -box_B->get_half_extents().z : box_B->get_half_extents().z)); - - Vector3 axis_ab = (support_a - support_b); - - if (!separator.test_axis(axis_ab.normalized())) { - return; - } - - //now try edges, which become cylinders! - - for (int i = 0; i < 3; i++) { - - //a ->b - Vector3 axis_a = p_transform_a.basis.get_axis(i); - - if (!separator.test_axis(axis_ab.cross(axis_a).cross(axis_a).normalized())) - return; - - //b ->a - Vector3 axis_b = p_transform_b.basis.get_axis(i); - - if (!separator.test_axis(axis_ab.cross(axis_b).cross(axis_b).normalized())) - return; - } - } - - separator.generate_contacts(); -} - -template <bool withMargin> -static void _collision_box_capsule(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - - const BoxShapeSW *box_A = static_cast<const BoxShapeSW *>(p_a); - const CapsuleShapeSW *capsule_B = static_cast<const CapsuleShapeSW *>(p_b); - - SeparatorAxisTest<BoxShapeSW, CapsuleShapeSW, withMargin> separator(box_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_margin_a, p_margin_b); - - if (!separator.test_previous_axis()) - return; - - // faces of A - for (int i = 0; i < 3; i++) { - - Vector3 axis = p_transform_a.basis.get_axis(i); - - if (!separator.test_axis(axis)) - return; - } - - Vector3 cyl_axis = p_transform_b.basis.get_axis(2).normalized(); - - // edges of A, capsule cylinder - - for (int i = 0; i < 3; i++) { - - // cylinder - Vector3 box_axis = p_transform_a.basis.get_axis(i); - Vector3 axis = box_axis.cross(cyl_axis); - if (Math::is_zero_approx(axis.length_squared())) - continue; - - if (!separator.test_axis(axis.normalized())) - return; - } - - // points of A, capsule cylinder - // this sure could be made faster somehow.. - - for (int i = 0; i < 2; i++) { - for (int j = 0; j < 2; j++) { - for (int k = 0; k < 2; k++) { - Vector3 he = box_A->get_half_extents(); - he.x *= (i * 2 - 1); - he.y *= (j * 2 - 1); - he.z *= (k * 2 - 1); - Vector3 point = p_transform_a.origin; - for (int l = 0; l < 3; l++) - point += p_transform_a.basis.get_axis(l) * he[l]; - - //Vector3 axis = (point - cyl_axis * cyl_axis.dot(point)).normalized(); - Vector3 axis = Plane(cyl_axis, 0).project(point).normalized(); - - if (!separator.test_axis(axis)) - return; - } - } - } - - // capsule balls, edges of A - - for (int i = 0; i < 2; i++) { - - Vector3 capsule_axis = p_transform_b.basis.get_axis(2) * (capsule_B->get_height() * 0.5); - - Vector3 sphere_pos = p_transform_b.origin + ((i == 0) ? capsule_axis : -capsule_axis); - - Vector3 cnormal = p_transform_a.xform_inv(sphere_pos); - - Vector3 cpoint = p_transform_a.xform(Vector3( - - (cnormal.x < 0) ? -box_A->get_half_extents().x : box_A->get_half_extents().x, - (cnormal.y < 0) ? -box_A->get_half_extents().y : box_A->get_half_extents().y, - (cnormal.z < 0) ? -box_A->get_half_extents().z : box_A->get_half_extents().z)); - - // use point to test axis - Vector3 point_axis = (sphere_pos - cpoint).normalized(); - - if (!separator.test_axis(point_axis)) - return; - - // test edges of A - - for (int j = 0; j < 3; j++) { - - Vector3 axis = point_axis.cross(p_transform_a.basis.get_axis(j)).cross(p_transform_a.basis.get_axis(j)).normalized(); - - if (!separator.test_axis(axis)) - return; - } - } - - separator.generate_contacts(); -} - -template <bool withMargin> -static void _collision_box_cylinder(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { -} - -template <bool withMargin> -static void _collision_box_convex_polygon(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - - const BoxShapeSW *box_A = static_cast<const BoxShapeSW *>(p_a); - const ConvexPolygonShapeSW *convex_polygon_B = static_cast<const ConvexPolygonShapeSW *>(p_b); - - SeparatorAxisTest<BoxShapeSW, ConvexPolygonShapeSW, withMargin> separator(box_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b); - - if (!separator.test_previous_axis()) - return; - - const Geometry::MeshData &mesh = convex_polygon_B->get_mesh(); - - const Geometry::MeshData::Face *faces = mesh.faces.ptr(); - int face_count = mesh.faces.size(); - const Geometry::MeshData::Edge *edges = mesh.edges.ptr(); - int edge_count = mesh.edges.size(); - const Vector3 *vertices = mesh.vertices.ptr(); - int vertex_count = mesh.vertices.size(); - - // faces of A - for (int i = 0; i < 3; i++) { - - Vector3 axis = p_transform_a.basis.get_axis(i).normalized(); - - if (!separator.test_axis(axis)) - return; - } - - // faces of B - for (int i = 0; i < face_count; i++) { - - Vector3 axis = p_transform_b.xform(faces[i].plane).normal; - - if (!separator.test_axis(axis)) - return; - } - - // A<->B edges - for (int i = 0; i < 3; i++) { - - Vector3 e1 = p_transform_a.basis.get_axis(i); - - for (int j = 0; j < edge_count; j++) { - - Vector3 e2 = p_transform_b.basis.xform(vertices[edges[j].a]) - p_transform_b.basis.xform(vertices[edges[j].b]); - - Vector3 axis = e1.cross(e2).normalized(); - - if (!separator.test_axis(axis)) - return; - } - } - - if (withMargin) { - - // calculate closest points between vertices and box edges - for (int v = 0; v < vertex_count; v++) { - - Vector3 vtxb = p_transform_b.xform(vertices[v]); - Vector3 ab_vec = vtxb - p_transform_a.origin; - - Vector3 cnormal_a = p_transform_a.basis.xform_inv(ab_vec); - - Vector3 support_a = p_transform_a.xform(Vector3( - - (cnormal_a.x < 0) ? -box_A->get_half_extents().x : box_A->get_half_extents().x, - (cnormal_a.y < 0) ? -box_A->get_half_extents().y : box_A->get_half_extents().y, - (cnormal_a.z < 0) ? -box_A->get_half_extents().z : box_A->get_half_extents().z)); - - Vector3 axis_ab = support_a - vtxb; - - if (!separator.test_axis(axis_ab.normalized())) { - return; - } - - //now try edges, which become cylinders! - - for (int i = 0; i < 3; i++) { - - //a ->b - Vector3 axis_a = p_transform_a.basis.get_axis(i); - - if (!separator.test_axis(axis_ab.cross(axis_a).cross(axis_a).normalized())) - return; - } - } - - //convex edges and box points - for (int i = 0; i < 2; i++) { - for (int j = 0; j < 2; j++) { - for (int k = 0; k < 2; k++) { - Vector3 he = box_A->get_half_extents(); - he.x *= (i * 2 - 1); - he.y *= (j * 2 - 1); - he.z *= (k * 2 - 1); - Vector3 point = p_transform_a.origin; - for (int l = 0; l < 3; l++) - point += p_transform_a.basis.get_axis(l) * he[l]; - - for (int e = 0; e < edge_count; e++) { - - Vector3 p1 = p_transform_b.xform(vertices[edges[e].a]); - Vector3 p2 = p_transform_b.xform(vertices[edges[e].b]); - Vector3 n = (p2 - p1); - - if (!separator.test_axis((point - p2).cross(n).cross(n).normalized())) - return; - } - } - } - } - } - - separator.generate_contacts(); -} - -template <bool withMargin> -static void _collision_box_face(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - - const BoxShapeSW *box_A = static_cast<const BoxShapeSW *>(p_a); - const FaceShapeSW *face_B = static_cast<const FaceShapeSW *>(p_b); - - SeparatorAxisTest<BoxShapeSW, FaceShapeSW, withMargin> separator(box_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b); - - Vector3 vertex[3] = { - p_transform_b.xform(face_B->vertex[0]), - p_transform_b.xform(face_B->vertex[1]), - p_transform_b.xform(face_B->vertex[2]), - }; - - if (!separator.test_axis((vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized())) - return; - - // faces of A - for (int i = 0; i < 3; i++) { - - Vector3 axis = p_transform_a.basis.get_axis(i).normalized(); - - if (!separator.test_axis(axis)) - return; - } - - // combined edges - - for (int i = 0; i < 3; i++) { - - Vector3 e = vertex[i] - vertex[(i + 1) % 3]; - - for (int j = 0; j < 3; j++) { - - Vector3 axis = p_transform_a.basis.get_axis(j); - - if (!separator.test_axis(e.cross(axis).normalized())) - return; - } - } - - if (withMargin) { - - // calculate closest points between vertices and box edges - for (int v = 0; v < 3; v++) { - - Vector3 ab_vec = vertex[v] - p_transform_a.origin; - - Vector3 cnormal_a = p_transform_a.basis.xform_inv(ab_vec); - - Vector3 support_a = p_transform_a.xform(Vector3( - - (cnormal_a.x < 0) ? -box_A->get_half_extents().x : box_A->get_half_extents().x, - (cnormal_a.y < 0) ? -box_A->get_half_extents().y : box_A->get_half_extents().y, - (cnormal_a.z < 0) ? -box_A->get_half_extents().z : box_A->get_half_extents().z)); - - Vector3 axis_ab = support_a - vertex[v]; - - if (!separator.test_axis(axis_ab.normalized())) { - return; - } - - //now try edges, which become cylinders! - - for (int i = 0; i < 3; i++) { - - //a ->b - Vector3 axis_a = p_transform_a.basis.get_axis(i); - - if (!separator.test_axis(axis_ab.cross(axis_a).cross(axis_a).normalized())) - return; - } - } - - //convex edges and box points, there has to be a way to speed up this (get closest point?) - for (int i = 0; i < 2; i++) { - for (int j = 0; j < 2; j++) { - for (int k = 0; k < 2; k++) { - Vector3 he = box_A->get_half_extents(); - he.x *= (i * 2 - 1); - he.y *= (j * 2 - 1); - he.z *= (k * 2 - 1); - Vector3 point = p_transform_a.origin; - for (int l = 0; l < 3; l++) - point += p_transform_a.basis.get_axis(l) * he[l]; - - for (int e = 0; e < 3; e++) { - - Vector3 p1 = vertex[e]; - Vector3 p2 = vertex[(e + 1) % 3]; - - Vector3 n = (p2 - p1); - - if (!separator.test_axis((point - p2).cross(n).cross(n).normalized())) - return; - } - } - } - } - } - - separator.generate_contacts(); -} - -template <bool withMargin> -static void _collision_capsule_capsule(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - - const CapsuleShapeSW *capsule_A = static_cast<const CapsuleShapeSW *>(p_a); - const CapsuleShapeSW *capsule_B = static_cast<const CapsuleShapeSW *>(p_b); - - SeparatorAxisTest<CapsuleShapeSW, CapsuleShapeSW, withMargin> separator(capsule_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_margin_a, p_margin_b); - - if (!separator.test_previous_axis()) - return; - - // some values - - Vector3 capsule_A_axis = p_transform_a.basis.get_axis(2) * (capsule_A->get_height() * 0.5); - Vector3 capsule_B_axis = p_transform_b.basis.get_axis(2) * (capsule_B->get_height() * 0.5); - - Vector3 capsule_A_ball_1 = p_transform_a.origin + capsule_A_axis; - Vector3 capsule_A_ball_2 = p_transform_a.origin - capsule_A_axis; - Vector3 capsule_B_ball_1 = p_transform_b.origin + capsule_B_axis; - Vector3 capsule_B_ball_2 = p_transform_b.origin - capsule_B_axis; - - //balls-balls - - if (!separator.test_axis((capsule_A_ball_1 - capsule_B_ball_1).normalized())) - return; - if (!separator.test_axis((capsule_A_ball_1 - capsule_B_ball_2).normalized())) - return; - - if (!separator.test_axis((capsule_A_ball_2 - capsule_B_ball_1).normalized())) - return; - if (!separator.test_axis((capsule_A_ball_2 - capsule_B_ball_2).normalized())) - return; - - // edges-balls - - if (!separator.test_axis((capsule_A_ball_1 - capsule_B_ball_1).cross(capsule_A_axis).cross(capsule_A_axis).normalized())) - return; - - if (!separator.test_axis((capsule_A_ball_1 - capsule_B_ball_2).cross(capsule_A_axis).cross(capsule_A_axis).normalized())) - return; - - if (!separator.test_axis((capsule_B_ball_1 - capsule_A_ball_1).cross(capsule_B_axis).cross(capsule_B_axis).normalized())) - return; - - if (!separator.test_axis((capsule_B_ball_1 - capsule_A_ball_2).cross(capsule_B_axis).cross(capsule_B_axis).normalized())) - return; - - // edges - - if (!separator.test_axis(capsule_A_axis.cross(capsule_B_axis).normalized())) - return; - - separator.generate_contacts(); -} - -template <bool withMargin> -static void _collision_capsule_cylinder(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { -} - -template <bool withMargin> -static void _collision_capsule_convex_polygon(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - - const CapsuleShapeSW *capsule_A = static_cast<const CapsuleShapeSW *>(p_a); - const ConvexPolygonShapeSW *convex_polygon_B = static_cast<const ConvexPolygonShapeSW *>(p_b); - - SeparatorAxisTest<CapsuleShapeSW, ConvexPolygonShapeSW, withMargin> separator(capsule_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b); - - if (!separator.test_previous_axis()) - return; - - const Geometry::MeshData &mesh = convex_polygon_B->get_mesh(); - - const Geometry::MeshData::Face *faces = mesh.faces.ptr(); - int face_count = mesh.faces.size(); - const Geometry::MeshData::Edge *edges = mesh.edges.ptr(); - int edge_count = mesh.edges.size(); - const Vector3 *vertices = mesh.vertices.ptr(); - - // faces of B - for (int i = 0; i < face_count; i++) { - - Vector3 axis = p_transform_b.xform(faces[i].plane).normal; - - if (!separator.test_axis(axis)) - return; - } - - // edges of B, capsule cylinder - - for (int i = 0; i < edge_count; i++) { - - // cylinder - Vector3 edge_axis = p_transform_b.basis.xform(vertices[edges[i].a]) - p_transform_b.basis.xform(vertices[edges[i].b]); - Vector3 axis = edge_axis.cross(p_transform_a.basis.get_axis(2)).normalized(); - - if (!separator.test_axis(axis)) - return; - } - - // capsule balls, edges of B - - for (int i = 0; i < 2; i++) { - - // edges of B, capsule cylinder - - Vector3 capsule_axis = p_transform_a.basis.get_axis(2) * (capsule_A->get_height() * 0.5); - - Vector3 sphere_pos = p_transform_a.origin + ((i == 0) ? capsule_axis : -capsule_axis); - - for (int j = 0; j < edge_count; j++) { - - Vector3 n1 = sphere_pos - p_transform_b.xform(vertices[edges[j].a]); - Vector3 n2 = p_transform_b.basis.xform(vertices[edges[j].a]) - p_transform_b.basis.xform(vertices[edges[j].b]); - - Vector3 axis = n1.cross(n2).cross(n2).normalized(); - - if (!separator.test_axis(axis)) - return; - } - } - - separator.generate_contacts(); -} - -template <bool withMargin> -static void _collision_capsule_face(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - - const CapsuleShapeSW *capsule_A = static_cast<const CapsuleShapeSW *>(p_a); - const FaceShapeSW *face_B = static_cast<const FaceShapeSW *>(p_b); - - SeparatorAxisTest<CapsuleShapeSW, FaceShapeSW, withMargin> separator(capsule_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b); - - Vector3 vertex[3] = { - p_transform_b.xform(face_B->vertex[0]), - p_transform_b.xform(face_B->vertex[1]), - p_transform_b.xform(face_B->vertex[2]), - }; - - if (!separator.test_axis((vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized())) - return; - - // edges of B, capsule cylinder - - Vector3 capsule_axis = p_transform_a.basis.get_axis(2) * (capsule_A->get_height() * 0.5); - - for (int i = 0; i < 3; i++) { - - // edge-cylinder - Vector3 edge_axis = vertex[i] - vertex[(i + 1) % 3]; - Vector3 axis = edge_axis.cross(capsule_axis).normalized(); - - if (!separator.test_axis(axis)) - return; - - if (!separator.test_axis((p_transform_a.origin - vertex[i]).cross(capsule_axis).cross(capsule_axis).normalized())) - return; - - for (int j = 0; j < 2; j++) { - - // point-spheres - Vector3 sphere_pos = p_transform_a.origin + ((j == 0) ? capsule_axis : -capsule_axis); - - Vector3 n1 = sphere_pos - vertex[i]; - - if (!separator.test_axis(n1.normalized())) - return; - - Vector3 n2 = edge_axis; - - axis = n1.cross(n2).cross(n2); - - if (!separator.test_axis(axis.normalized())) - return; - } - } - - separator.generate_contacts(); -} - -template <bool withMargin> -static void _collision_cylinder_cylinder(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { -} - -template <bool withMargin> -static void _collision_cylinder_convex_polygon(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { -} - -template <bool withMargin> -static void _collision_cylinder_face(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { -} - -template <bool withMargin> -static void _collision_convex_polygon_convex_polygon(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - - const ConvexPolygonShapeSW *convex_polygon_A = static_cast<const ConvexPolygonShapeSW *>(p_a); - const ConvexPolygonShapeSW *convex_polygon_B = static_cast<const ConvexPolygonShapeSW *>(p_b); - - SeparatorAxisTest<ConvexPolygonShapeSW, ConvexPolygonShapeSW, withMargin> separator(convex_polygon_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b); - - if (!separator.test_previous_axis()) - return; - - const Geometry::MeshData &mesh_A = convex_polygon_A->get_mesh(); - - const Geometry::MeshData::Face *faces_A = mesh_A.faces.ptr(); - int face_count_A = mesh_A.faces.size(); - const Geometry::MeshData::Edge *edges_A = mesh_A.edges.ptr(); - int edge_count_A = mesh_A.edges.size(); - const Vector3 *vertices_A = mesh_A.vertices.ptr(); - int vertex_count_A = mesh_A.vertices.size(); - - const Geometry::MeshData &mesh_B = convex_polygon_B->get_mesh(); - - const Geometry::MeshData::Face *faces_B = mesh_B.faces.ptr(); - int face_count_B = mesh_B.faces.size(); - const Geometry::MeshData::Edge *edges_B = mesh_B.edges.ptr(); - int edge_count_B = mesh_B.edges.size(); - const Vector3 *vertices_B = mesh_B.vertices.ptr(); - int vertex_count_B = mesh_B.vertices.size(); - - // faces of A - for (int i = 0; i < face_count_A; i++) { - - Vector3 axis = p_transform_a.xform(faces_A[i].plane).normal; - //Vector3 axis = p_transform_a.basis.xform( faces_A[i].plane.normal ).normalized(); - - if (!separator.test_axis(axis)) - return; - } - - // faces of B - for (int i = 0; i < face_count_B; i++) { - - Vector3 axis = p_transform_b.xform(faces_B[i].plane).normal; - //Vector3 axis = p_transform_b.basis.xform( faces_B[i].plane.normal ).normalized(); - - if (!separator.test_axis(axis)) - return; - } - - // A<->B edges - for (int i = 0; i < edge_count_A; i++) { - - Vector3 e1 = p_transform_a.basis.xform(vertices_A[edges_A[i].a]) - p_transform_a.basis.xform(vertices_A[edges_A[i].b]); - - for (int j = 0; j < edge_count_B; j++) { - - Vector3 e2 = p_transform_b.basis.xform(vertices_B[edges_B[j].a]) - p_transform_b.basis.xform(vertices_B[edges_B[j].b]); - - Vector3 axis = e1.cross(e2).normalized(); - - if (!separator.test_axis(axis)) - return; - } - } - - if (withMargin) { - - //vertex-vertex - for (int i = 0; i < vertex_count_A; i++) { - - Vector3 va = p_transform_a.xform(vertices_A[i]); - - for (int j = 0; j < vertex_count_B; j++) { - - if (!separator.test_axis((va - p_transform_b.xform(vertices_B[j])).normalized())) - return; - } - } - //edge-vertex (shell) - - for (int i = 0; i < edge_count_A; i++) { - - Vector3 e1 = p_transform_a.basis.xform(vertices_A[edges_A[i].a]); - Vector3 e2 = p_transform_a.basis.xform(vertices_A[edges_A[i].b]); - Vector3 n = (e2 - e1); - - for (int j = 0; j < vertex_count_B; j++) { - - Vector3 e3 = p_transform_b.xform(vertices_B[j]); - - if (!separator.test_axis((e1 - e3).cross(n).cross(n).normalized())) - return; - } - } - - for (int i = 0; i < edge_count_B; i++) { - - Vector3 e1 = p_transform_b.basis.xform(vertices_B[edges_B[i].a]); - Vector3 e2 = p_transform_b.basis.xform(vertices_B[edges_B[i].b]); - Vector3 n = (e2 - e1); - - for (int j = 0; j < vertex_count_A; j++) { - - Vector3 e3 = p_transform_a.xform(vertices_A[j]); - - if (!separator.test_axis((e1 - e3).cross(n).cross(n).normalized())) - return; - } - } - } - - separator.generate_contacts(); -} - -template <bool withMargin> -static void _collision_convex_polygon_face(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - - const ConvexPolygonShapeSW *convex_polygon_A = static_cast<const ConvexPolygonShapeSW *>(p_a); - const FaceShapeSW *face_B = static_cast<const FaceShapeSW *>(p_b); - - SeparatorAxisTest<ConvexPolygonShapeSW, FaceShapeSW, withMargin> separator(convex_polygon_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b); - - const Geometry::MeshData &mesh = convex_polygon_A->get_mesh(); - - const Geometry::MeshData::Face *faces = mesh.faces.ptr(); - int face_count = mesh.faces.size(); - const Geometry::MeshData::Edge *edges = mesh.edges.ptr(); - int edge_count = mesh.edges.size(); - const Vector3 *vertices = mesh.vertices.ptr(); - int vertex_count = mesh.vertices.size(); - - Vector3 vertex[3] = { - p_transform_b.xform(face_B->vertex[0]), - p_transform_b.xform(face_B->vertex[1]), - p_transform_b.xform(face_B->vertex[2]), - }; - - if (!separator.test_axis((vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized())) - return; - - // faces of A - for (int i = 0; i < face_count; i++) { - - //Vector3 axis = p_transform_a.xform( faces[i].plane ).normal; - Vector3 axis = p_transform_a.basis.xform(faces[i].plane.normal).normalized(); - - if (!separator.test_axis(axis)) - return; - } - - // A<->B edges - for (int i = 0; i < edge_count; i++) { - - Vector3 e1 = p_transform_a.xform(vertices[edges[i].a]) - p_transform_a.xform(vertices[edges[i].b]); - - for (int j = 0; j < 3; j++) { - - Vector3 e2 = vertex[j] - vertex[(j + 1) % 3]; - - Vector3 axis = e1.cross(e2).normalized(); - - if (!separator.test_axis(axis)) - return; - } - } - - if (withMargin) { - - //vertex-vertex - for (int i = 0; i < vertex_count; i++) { - - Vector3 va = p_transform_a.xform(vertices[i]); - - for (int j = 0; j < 3; j++) { - - if (!separator.test_axis((va - vertex[j]).normalized())) - return; - } - } - //edge-vertex (shell) - - for (int i = 0; i < edge_count; i++) { - - Vector3 e1 = p_transform_a.basis.xform(vertices[edges[i].a]); - Vector3 e2 = p_transform_a.basis.xform(vertices[edges[i].b]); - Vector3 n = (e2 - e1); - - for (int j = 0; j < 3; j++) { - - Vector3 e3 = vertex[j]; - - if (!separator.test_axis((e1 - e3).cross(n).cross(n).normalized())) - return; - } - } - - for (int i = 0; i < 3; i++) { - - Vector3 e1 = vertex[i]; - Vector3 e2 = vertex[(i + 1) % 3]; - Vector3 n = (e2 - e1); - - for (int j = 0; j < vertex_count; j++) { - - Vector3 e3 = p_transform_a.xform(vertices[j]); - - if (!separator.test_axis((e1 - e3).cross(n).cross(n).normalized())) - return; - } - } - } - - separator.generate_contacts(); -} - -bool sat_calculate_penetration(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CollisionSolverSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap, Vector3 *r_prev_axis, real_t p_margin_a, real_t p_margin_b) { - - PhysicsServer::ShapeType type_A = p_shape_A->get_type(); - - ERR_FAIL_COND_V(type_A == PhysicsServer::SHAPE_PLANE, false); - ERR_FAIL_COND_V(type_A == PhysicsServer::SHAPE_RAY, false); - ERR_FAIL_COND_V(p_shape_A->is_concave(), false); - - PhysicsServer::ShapeType type_B = p_shape_B->get_type(); - - ERR_FAIL_COND_V(type_B == PhysicsServer::SHAPE_PLANE, false); - ERR_FAIL_COND_V(type_B == PhysicsServer::SHAPE_RAY, false); - ERR_FAIL_COND_V(p_shape_B->is_concave(), false); - - static const CollisionFunc collision_table[6][6] = { - { _collision_sphere_sphere<false>, - _collision_sphere_box<false>, - _collision_sphere_capsule<false>, - _collision_sphere_cylinder<false>, - _collision_sphere_convex_polygon<false>, - _collision_sphere_face<false> }, - { 0, - _collision_box_box<false>, - _collision_box_capsule<false>, - _collision_box_cylinder<false>, - _collision_box_convex_polygon<false>, - _collision_box_face<false> }, - { 0, - 0, - _collision_capsule_capsule<false>, - _collision_capsule_cylinder<false>, - _collision_capsule_convex_polygon<false>, - _collision_capsule_face<false> }, - { 0, - 0, - 0, - _collision_cylinder_cylinder<false>, - _collision_cylinder_convex_polygon<false>, - _collision_cylinder_face<false> }, - { 0, - 0, - 0, - 0, - _collision_convex_polygon_convex_polygon<false>, - _collision_convex_polygon_face<false> }, - { 0, - 0, - 0, - 0, - 0, - 0 }, - }; - - static const CollisionFunc collision_table_margin[6][6] = { - { _collision_sphere_sphere<true>, - _collision_sphere_box<true>, - _collision_sphere_capsule<true>, - _collision_sphere_cylinder<true>, - _collision_sphere_convex_polygon<true>, - _collision_sphere_face<true> }, - { 0, - _collision_box_box<true>, - _collision_box_capsule<true>, - _collision_box_cylinder<true>, - _collision_box_convex_polygon<true>, - _collision_box_face<true> }, - { 0, - 0, - _collision_capsule_capsule<true>, - _collision_capsule_cylinder<true>, - _collision_capsule_convex_polygon<true>, - _collision_capsule_face<true> }, - { 0, - 0, - 0, - _collision_cylinder_cylinder<true>, - _collision_cylinder_convex_polygon<true>, - _collision_cylinder_face<true> }, - { 0, - 0, - 0, - 0, - _collision_convex_polygon_convex_polygon<true>, - _collision_convex_polygon_face<true> }, - { 0, - 0, - 0, - 0, - 0, - 0 }, - }; - - _CollectorCallback callback; - callback.callback = p_result_callback; - callback.swap = p_swap; - callback.userdata = p_userdata; - callback.collided = false; - callback.prev_axis = r_prev_axis; - - const ShapeSW *A = p_shape_A; - const ShapeSW *B = p_shape_B; - const Transform *transform_A = &p_transform_A; - const Transform *transform_B = &p_transform_B; - real_t margin_A = p_margin_a; - real_t margin_B = p_margin_b; - - if (type_A > type_B) { - SWAP(A, B); - SWAP(transform_A, transform_B); - SWAP(type_A, type_B); - SWAP(margin_A, margin_B); - callback.swap = !callback.swap; - } - - CollisionFunc collision_func; - if (margin_A != 0.0 || margin_B != 0.0) { - collision_func = collision_table_margin[type_A - 2][type_B - 2]; - - } else { - collision_func = collision_table[type_A - 2][type_B - 2]; - } - ERR_FAIL_COND_V(!collision_func, false); - - collision_func(A, *transform_A, B, *transform_B, &callback, margin_A, margin_B); - - return callback.collided; -} diff --git a/servers/physics/collision_solver_sat.h b/servers/physics/collision_solver_sat.h deleted file mode 100644 index 31895231f0..0000000000 --- a/servers/physics/collision_solver_sat.h +++ /dev/null @@ -1,38 +0,0 @@ -/*************************************************************************/ -/* collision_solver_sat.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 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_SAT_H -#define COLLISION_SOLVER_SAT_H - -#include "collision_solver_sw.h" - -bool sat_calculate_penetration(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CollisionSolverSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap = false, Vector3 *r_prev_axis = NULL, real_t p_margin_a = 0, real_t p_margin_b = 0); - -#endif // COLLISION_SOLVER_SAT_H diff --git a/servers/physics/collision_solver_sw.cpp b/servers/physics/collision_solver_sw.cpp deleted file mode 100644 index ce24ba6bca..0000000000 --- a/servers/physics/collision_solver_sw.cpp +++ /dev/null @@ -1,372 +0,0 @@ -/*************************************************************************/ -/* collision_solver_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 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_sw.h" -#include "collision_solver_sat.h" - -#include "gjk_epa.h" - -#define collision_solver sat_calculate_penetration -//#define collision_solver gjk_epa_calculate_penetration - -bool CollisionSolverSW::solve_static_plane(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) { - - const PlaneShapeSW *plane = static_cast<const PlaneShapeSW *>(p_shape_A); - if (p_shape_B->get_type() == PhysicsServer::SHAPE_PLANE) - return false; - Plane p = p_transform_A.xform(plane->get_plane()); - - static const int max_supports = 16; - Vector3 supports[max_supports]; - int support_count; - - p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(), max_supports, supports, support_count); - - bool found = false; - - for (int i = 0; i < support_count; i++) { - - supports[i] = p_transform_B.xform(supports[i]); - if (p.distance_to(supports[i]) >= 0) - continue; - found = true; - - Vector3 support_A = p.project(supports[i]); - - 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 CollisionSolverSW::solve_ray(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) { - - const RayShapeSW *ray = static_cast<const RayShapeSW *>(p_shape_A); - - Vector3 from = p_transform_A.origin; - Vector3 to = from + p_transform_A.basis.get_axis(2) * ray->get_length(); - Vector3 support_A = to; - - Transform ai = p_transform_B.affine_inverse(); - - from = ai.xform(from); - to = ai.xform(to); - - Vector3 p, n; - if (!p_shape_B->intersect_segment(from, to, p, n)) - return false; - - Vector3 support_B = p_transform_B.xform(p); - if (ray->get_slips_on_slope()) { - Vector3 global_n = ai.basis.xform_inv(n).normalized(); - support_B = support_A + (support_B - support_A).length() * global_n; - } - - if (p_result_callback) { - if (p_swap_result) - p_result_callback(support_B, support_A, p_userdata); - else - p_result_callback(support_A, support_B, p_userdata); - } - return true; -} - -struct _ConcaveCollisionInfo { - - const Transform *transform_A; - const ShapeSW *shape_A; - const Transform *transform_B; - CollisionSolverSW::CallbackResult result_callback; - void *userdata; - bool swap_result; - bool collided; - int aabb_tests; - int collisions; - bool tested; - real_t margin_A; - real_t margin_B; - Vector3 close_A, close_B; -}; - -void CollisionSolverSW::concave_callback(void *p_userdata, ShapeSW *p_convex) { - - _ConcaveCollisionInfo &cinfo = *(_ConcaveCollisionInfo *)(p_userdata); - cinfo.aabb_tests++; - - bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, p_convex, *cinfo.transform_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result, NULL, cinfo.margin_A, cinfo.margin_B); - if (!collided) - return; - - cinfo.collided = true; - cinfo.collisions++; -} - -bool CollisionSolverSW::solve_concave(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin_A, real_t p_margin_B) { - - const ConcaveShapeSW *concave_B = static_cast<const ConcaveShapeSW *>(p_shape_B); - - _ConcaveCollisionInfo cinfo; - cinfo.transform_A = &p_transform_A; - cinfo.shape_A = p_shape_A; - cinfo.transform_B = &p_transform_B; - cinfo.result_callback = p_result_callback; - cinfo.userdata = p_userdata; - cinfo.swap_result = p_swap_result; - cinfo.collided = false; - cinfo.collisions = 0; - cinfo.margin_A = p_margin_A; - cinfo.margin_B = p_margin_B; - - cinfo.aabb_tests = 0; - - Transform rel_transform = p_transform_A; - rel_transform.origin -= p_transform_B.origin; - - //quickly compute a local AABB - - AABB local_aabb; - for (int i = 0; i < 3; i++) { - - Vector3 axis(p_transform_B.basis.get_axis(i)); - real_t axis_scale = 1.0 / axis.length(); - axis *= axis_scale; - - real_t smin, smax; - p_shape_A->project_range(axis, rel_transform, smin, smax); - smin -= p_margin_A; - smax += p_margin_A; - 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 CollisionSolverSW::solve_static(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, Vector3 *r_sep_axis, real_t p_margin_A, real_t p_margin_B) { - - PhysicsServer::ShapeType type_A = p_shape_A->get_type(); - PhysicsServer::ShapeType type_B = p_shape_B->get_type(); - bool concave_A = p_shape_A->is_concave(); - bool concave_B = p_shape_B->is_concave(); - - bool swap = false; - - if (type_A > type_B) { - SWAP(type_A, type_B); - SWAP(concave_A, concave_B); - swap = true; - } - - if (type_A == PhysicsServer::SHAPE_PLANE) { - - if (type_B == PhysicsServer::SHAPE_PLANE) - return false; - if (type_B == PhysicsServer::SHAPE_RAY) { - return false; - } - - if (swap) { - return solve_static_plane(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true); - } else { - return solve_static_plane(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false); - } - - } else if (type_A == PhysicsServer::SHAPE_RAY) { - - if (type_B == PhysicsServer::SHAPE_RAY) - return false; - - if (swap) { - return solve_ray(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true); - } else { - return solve_ray(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false); - } - - } else if (concave_B) { - - if (concave_A) - return false; - - if (!swap) - return solve_concave(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false, p_margin_A, p_margin_B); - else - return solve_concave(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true, p_margin_A, p_margin_B); - - } else { - - return collision_solver(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false, r_sep_axis, p_margin_A, p_margin_B); - } -} - -void CollisionSolverSW::concave_distance_callback(void *p_userdata, ShapeSW *p_convex) { - - _ConcaveCollisionInfo &cinfo = *(_ConcaveCollisionInfo *)(p_userdata); - cinfo.aabb_tests++; - if (cinfo.collided) - return; - - Vector3 close_A, close_B; - cinfo.collided = !gjk_epa_calculate_distance(cinfo.shape_A, *cinfo.transform_A, p_convex, *cinfo.transform_B, close_A, close_B); - - if (cinfo.collided) - return; - if (!cinfo.tested || close_A.distance_squared_to(close_B) < cinfo.close_A.distance_squared_to(cinfo.close_B)) { - - cinfo.close_A = close_A; - cinfo.close_B = close_B; - cinfo.tested = true; - } - - cinfo.collisions++; -} - -bool CollisionSolverSW::solve_distance_plane(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B) { - - const PlaneShapeSW *plane = static_cast<const PlaneShapeSW *>(p_shape_A); - if (p_shape_B->get_type() == PhysicsServer::SHAPE_PLANE) - return false; - Plane p = p_transform_A.xform(plane->get_plane()); - - static const int max_supports = 16; - Vector3 supports[max_supports]; - int support_count; - - p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(), max_supports, supports, support_count); - - bool collided = false; - Vector3 closest; - real_t closest_d = 0; - - for (int i = 0; i < support_count; i++) { - - supports[i] = p_transform_B.xform(supports[i]); - real_t d = p.distance_to(supports[i]); - if (i == 0 || d < closest_d) { - closest = supports[i]; - closest_d = d; - if (d <= 0) - collided = true; - } - } - - r_point_A = p.project(closest); - r_point_B = closest; - - return collided; -} - -bool CollisionSolverSW::solve_distance(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B, const AABB &p_concave_hint, Vector3 *r_sep_axis) { - - if (p_shape_A->is_concave()) - return false; - - if (p_shape_B->get_type() == PhysicsServer::SHAPE_PLANE) { - - Vector3 a, b; - bool col = solve_distance_plane(p_shape_B, p_transform_B, p_shape_A, p_transform_A, a, b); - r_point_A = b; - r_point_B = a; - return !col; - - } else if (p_shape_B->is_concave()) { - - if (p_shape_A->is_concave()) - return false; - - const ConcaveShapeSW *concave_B = static_cast<const ConcaveShapeSW *>(p_shape_B); - - _ConcaveCollisionInfo cinfo; - cinfo.transform_A = &p_transform_A; - cinfo.shape_A = p_shape_A; - cinfo.transform_B = &p_transform_B; - cinfo.result_callback = NULL; - cinfo.userdata = NULL; - cinfo.swap_result = false; - cinfo.collided = false; - cinfo.collisions = 0; - cinfo.aabb_tests = 0; - cinfo.tested = false; - - Transform rel_transform = p_transform_A; - rel_transform.origin -= p_transform_B.origin; - - //quickly compute a local AABB - - bool use_cc_hint = p_concave_hint != AABB(); - AABB cc_hint_aabb; - if (use_cc_hint) { - cc_hint_aabb = p_concave_hint; - cc_hint_aabb.position -= p_transform_B.origin; - } - - AABB local_aabb; - for (int i = 0; i < 3; i++) { - - Vector3 axis(p_transform_B.basis.get_axis(i)); - real_t axis_scale = ((real_t)1.0) / axis.length(); - axis *= axis_scale; - - real_t smin, smax; - - if (use_cc_hint) { - cc_hint_aabb.project_range_in_plane(Plane(axis, 0), smin, smax); - } else { - p_shape_A->project_range(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_distance_callback, &cinfo); - if (!cinfo.collided) { - r_point_A = cinfo.close_A; - r_point_B = cinfo.close_B; - } - - return !cinfo.collided; - } else { - - return gjk_epa_calculate_distance(p_shape_A, p_transform_A, p_shape_B, p_transform_B, r_point_A, r_point_B); //should pass sepaxis.. - } -} diff --git a/servers/physics/collision_solver_sw.h b/servers/physics/collision_solver_sw.h deleted file mode 100644 index d4dc1ac2a2..0000000000 --- a/servers/physics/collision_solver_sw.h +++ /dev/null @@ -1,53 +0,0 @@ -/*************************************************************************/ -/* collision_solver_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 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_SW_H -#define COLLISION_SOLVER_SW_H - -#include "shape_sw.h" - -class CollisionSolverSW { -public: - typedef void (*CallbackResult)(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata); - -private: - static void concave_callback(void *p_userdata, ShapeSW *p_convex); - static bool solve_static_plane(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); - static bool solve_ray(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); - static bool solve_concave(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin_A = 0, real_t p_margin_B = 0); - static void concave_distance_callback(void *p_userdata, ShapeSW *p_convex); - static bool solve_distance_plane(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B); - -public: - static bool solve_static(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, Vector3 *r_sep_axis = NULL, real_t p_margin_A = 0, real_t p_margin_B = 0); - static bool solve_distance(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B, const AABB &p_concave_hint, Vector3 *r_sep_axis = NULL); -}; - -#endif // COLLISION_SOLVER__SW_H diff --git a/servers/physics/constraint_sw.h b/servers/physics/constraint_sw.h deleted file mode 100644 index 22f31b411b..0000000000 --- a/servers/physics/constraint_sw.h +++ /dev/null @@ -1,85 +0,0 @@ -/*************************************************************************/ -/* constraint_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 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_SW_H -#define CONSTRAINT_SW_H - -#include "body_sw.h" - -class ConstraintSW { - - BodySW **_body_ptr; - int _body_count; - uint64_t island_step; - ConstraintSW *island_next; - ConstraintSW *island_list_next; - int priority; - bool disabled_collisions_between_bodies; - - RID self; - -protected: - ConstraintSW(BodySW **p_body_ptr = NULL, int p_body_count = 0) { - _body_ptr = p_body_ptr; - _body_count = p_body_count; - island_step = 0; - priority = 1; - disabled_collisions_between_bodies = true; - } - -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_ ConstraintSW *get_island_next() const { return island_next; } - _FORCE_INLINE_ void set_island_next(ConstraintSW *p_next) { island_next = p_next; } - - _FORCE_INLINE_ ConstraintSW *get_island_list_next() const { return island_list_next; } - _FORCE_INLINE_ void set_island_list_next(ConstraintSW *p_next) { island_list_next = p_next; } - - _FORCE_INLINE_ BodySW **get_body_ptr() const { return _body_ptr; } - _FORCE_INLINE_ int get_body_count() const { return _body_count; } - - _FORCE_INLINE_ void set_priority(int p_priority) { priority = p_priority; } - _FORCE_INLINE_ int get_priority() const { return priority; } - - _FORCE_INLINE_ void disable_collisions_between_bodies(const bool p_disabled) { disabled_collisions_between_bodies = p_disabled; } - _FORCE_INLINE_ bool is_disabled_collisions_between_bodies() const { return disabled_collisions_between_bodies; } - - virtual bool setup(real_t p_step) = 0; - virtual void solve(real_t p_step) = 0; - - virtual ~ConstraintSW() {} -}; - -#endif // CONSTRAINT__SW_H diff --git a/servers/physics/gjk_epa.cpp b/servers/physics/gjk_epa.cpp deleted file mode 100644 index abf0c0dc15..0000000000 --- a/servers/physics/gjk_epa.cpp +++ /dev/null @@ -1,946 +0,0 @@ -/*************************************************************************/ -/* gjk_epa.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 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 "gjk_epa.h" - -/* Disabling formatting for thirdparty code snippet */ -/* clang-format off */ - -/*************** Bullet's GJK-EPA2 IMPLEMENTATION *******************/ - -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the -use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it -freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not -claim that you wrote the original software. If you use this software in a -product, an acknowledgment in the product documentation would be appreciated -but is not required. -2. Altered source versions must be plainly marked as such, and must not be -misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -/* -GJK-EPA collision solver by Nathanael Presson, 2008 -*/ - - // Config - -/* GJK */ -#define GJK_MAX_ITERATIONS 128 -#define GJK_ACCURARY ((real_t)0.0001) -#define GJK_MIN_DISTANCE ((real_t)0.0001) -#define GJK_DUPLICATED_EPS ((real_t)0.0001) -#define GJK_SIMPLEX2_EPS ((real_t)0.0) -#define GJK_SIMPLEX3_EPS ((real_t)0.0) -#define GJK_SIMPLEX4_EPS ((real_t)0.0) - -/* EPA */ -#define EPA_MAX_VERTICES 64 -#define EPA_MAX_FACES (EPA_MAX_VERTICES*2) -#define EPA_MAX_ITERATIONS 255 -#define EPA_ACCURACY ((real_t)0.0001) -#define EPA_FALLBACK (10*EPA_ACCURACY) -#define EPA_PLANE_EPS ((real_t)0.00001) -#define EPA_INSIDE_EPS ((real_t)0.01) - -namespace GjkEpa2 { - - -struct sResults { - enum eStatus { - Separated, /* Shapes doesn't penetrate */ - Penetrating, /* Shapes are penetrating */ - GJK_Failed, /* GJK phase fail, no big issue, shapes are probably just 'touching' */ - EPA_Failed /* EPA phase fail, bigger problem, need to save parameters, and debug */ - } status; - - Vector3 witnesses[2]; - Vector3 normal; - real_t distance; -}; - -// Shorthands -typedef unsigned int U; -typedef unsigned char U1; - -// MinkowskiDiff -struct MinkowskiDiff { - - const ShapeSW* m_shapes[2]; - - Transform transform_A; - Transform transform_B; - - // i wonder how this could be sped up... if it can - _FORCE_INLINE_ Vector3 Support0 ( const Vector3& d ) const { - return transform_A.xform( m_shapes[0]->get_support( transform_A.basis.xform_inv(d).normalized() ) ); - } - - _FORCE_INLINE_ Vector3 Support1 ( const Vector3& d ) const { - return transform_B.xform( m_shapes[1]->get_support( transform_B.basis.xform_inv(d).normalized() ) ); - } - - _FORCE_INLINE_ Vector3 Support ( const Vector3& d ) const { - return ( Support0 ( d )-Support1 ( -d ) ); - } - - _FORCE_INLINE_ Vector3 Support ( const Vector3& d,U index ) const - { - if ( index ) - return ( Support1 ( d ) ); - else - return ( Support0 ( d ) ); - } -}; - -typedef MinkowskiDiff tShape; - - -// GJK -struct GJK -{ - /* Types */ - struct sSV - { - Vector3 d,w; - }; - struct sSimplex - { - sSV* c[4]; - real_t p[4]; - U rank; - }; - struct eStatus { enum _ { - Valid, - Inside, - Failed };}; - /* Fields */ - tShape m_shape; - Vector3 m_ray; - real_t m_distance; - sSimplex m_simplices[2]; - sSV m_store[4]; - sSV* m_free[4]; - U m_nfree; - U m_current; - sSimplex* m_simplex; - eStatus::_ m_status; - /* Methods */ - GJK() - { - Initialize(); - } - void Initialize() - { - m_ray = Vector3(0,0,0); - m_nfree = 0; - m_status = eStatus::Failed; - m_current = 0; - m_distance = 0; - } - eStatus::_ Evaluate(const tShape& shapearg,const Vector3& guess) - { - U iterations=0; - real_t sqdist=0; - real_t alpha=0; - Vector3 lastw[4]; - U clastw=0; - /* Initialize solver */ - m_free[0] = &m_store[0]; - m_free[1] = &m_store[1]; - m_free[2] = &m_store[2]; - m_free[3] = &m_store[3]; - m_nfree = 4; - m_current = 0; - m_status = eStatus::Valid; - m_shape = shapearg; - m_distance = 0; - /* Initialize simplex */ - m_simplices[0].rank = 0; - m_ray = guess; - const real_t sqrl= m_ray.length_squared(); - appendvertice(m_simplices[0],sqrl>0?-m_ray:Vector3(1,0,0)); - m_simplices[0].p[0] = 1; - m_ray = m_simplices[0].c[0]->w; - sqdist = sqrl; - lastw[0] = - lastw[1] = - lastw[2] = - lastw[3] = m_ray; - /* Loop */ - do { - const U next=1-m_current; - sSimplex& cs=m_simplices[m_current]; - sSimplex& ns=m_simplices[next]; - /* Check zero */ - const real_t rl=m_ray.length(); - if(rl<GJK_MIN_DISTANCE) - {/* Touching or inside */ - m_status=eStatus::Inside; - break; - } - /* Append new vertice in -'v' direction */ - appendvertice(cs,-m_ray); - const Vector3& w=cs.c[cs.rank-1]->w; - bool found=false; - for(U i=0;i<4;++i) - { - if((w-lastw[i]).length_squared()<GJK_DUPLICATED_EPS) - { found=true;break; } - } - if(found) - {/* Return old simplex */ - removevertice(m_simplices[m_current]); - break; - } - else - {/* Update lastw */ - lastw[clastw=(clastw+1)&3]=w; - } - /* Check for termination */ - const real_t omega=vec3_dot(m_ray,w)/rl; - alpha=MAX(omega,alpha); - if(((rl-alpha)-(GJK_ACCURARY*rl))<=0) - {/* Return old simplex */ - removevertice(m_simplices[m_current]); - break; - } - /* Reduce simplex */ - real_t weights[4]; - U mask=0; - switch(cs.rank) - { - case 2: sqdist=projectorigin( cs.c[0]->w, - cs.c[1]->w, - weights,mask);break; - case 3: sqdist=projectorigin( cs.c[0]->w, - cs.c[1]->w, - cs.c[2]->w, - weights,mask);break; - case 4: sqdist=projectorigin( cs.c[0]->w, - cs.c[1]->w, - cs.c[2]->w, - cs.c[3]->w, - weights,mask);break; - } - if(sqdist>=0) - {/* Valid */ - ns.rank = 0; - m_ray = Vector3(0,0,0); - m_current = next; - for(U i=0,ni=cs.rank;i<ni;++i) - { - if(mask&(1<<i)) - { - ns.c[ns.rank] = cs.c[i]; - ns.p[ns.rank++] = weights[i]; - m_ray += cs.c[i]->w*weights[i]; - } - else - { - m_free[m_nfree++] = cs.c[i]; - } - } - if(mask==15) m_status=eStatus::Inside; - } - else - {/* Return old simplex */ - removevertice(m_simplices[m_current]); - break; - } - m_status=((++iterations)<GJK_MAX_ITERATIONS)?m_status:eStatus::Failed; - } while(m_status==eStatus::Valid); - m_simplex=&m_simplices[m_current]; - switch(m_status) - { - case eStatus::Valid: m_distance=m_ray.length();break; - case eStatus::Inside: m_distance=0;break; - default: {} - } - return(m_status); - } - bool EncloseOrigin() - { - switch(m_simplex->rank) - { - case 1: - { - for(U i=0;i<3;++i) - { - Vector3 axis=Vector3(0,0,0); - axis[i]=1; - appendvertice(*m_simplex, axis); - if(EncloseOrigin()) return(true); - removevertice(*m_simplex); - appendvertice(*m_simplex,-axis); - if(EncloseOrigin()) return(true); - removevertice(*m_simplex); - } - } - break; - case 2: - { - const Vector3 d=m_simplex->c[1]->w-m_simplex->c[0]->w; - for(U i=0;i<3;++i) - { - Vector3 axis=Vector3(0,0,0); - axis[i]=1; - const Vector3 p=vec3_cross(d,axis); - if(p.length_squared()>0) - { - appendvertice(*m_simplex, p); - if(EncloseOrigin()) return(true); - removevertice(*m_simplex); - appendvertice(*m_simplex,-p); - if(EncloseOrigin()) return(true); - removevertice(*m_simplex); - } - } - } - break; - case 3: - { - const Vector3 n=vec3_cross(m_simplex->c[1]->w-m_simplex->c[0]->w, - m_simplex->c[2]->w-m_simplex->c[0]->w); - if(n.length_squared()>0) - { - appendvertice(*m_simplex,n); - if(EncloseOrigin()) return(true); - removevertice(*m_simplex); - appendvertice(*m_simplex,-n); - if(EncloseOrigin()) return(true); - removevertice(*m_simplex); - } - } - break; - case 4: - { - if(Math::abs(det( m_simplex->c[0]->w-m_simplex->c[3]->w, - m_simplex->c[1]->w-m_simplex->c[3]->w, - m_simplex->c[2]->w-m_simplex->c[3]->w))>0) - return(true); - } - break; - } - return(false); - } - /* Internals */ - void getsupport(const Vector3& d,sSV& sv) const - { - sv.d = d/d.length(); - sv.w = m_shape.Support(sv.d); - } - void removevertice(sSimplex& simplex) - { - m_free[m_nfree++]=simplex.c[--simplex.rank]; - } - void appendvertice(sSimplex& simplex,const Vector3& v) - { - simplex.p[simplex.rank]=0; - simplex.c[simplex.rank]=m_free[--m_nfree]; - getsupport(v,*simplex.c[simplex.rank++]); - } - static real_t det(const Vector3& a,const Vector3& b,const Vector3& c) - { - return( a.y*b.z*c.x+a.z*b.x*c.y- - a.x*b.z*c.y-a.y*b.x*c.z+ - a.x*b.y*c.z-a.z*b.y*c.x); - } - static real_t projectorigin( const Vector3& a, - const Vector3& b, - real_t* w,U& m) - { - const Vector3 d=b-a; - const real_t l=d.length_squared(); - if(l>GJK_SIMPLEX2_EPS) - { - const real_t t(l>0?-vec3_dot(a,d)/l:0); - if(t>=1) { w[0]=0;w[1]=1;m=2;return(b.length_squared()); } - else if(t<=0) { w[0]=1;w[1]=0;m=1;return(a.length_squared()); } - else { w[0]=1-(w[1]=t);m=3;return((a+d*t).length_squared()); } - } - return(-1); - } - static real_t projectorigin( const Vector3& a, - const Vector3& b, - const Vector3& c, - real_t* w,U& m) - { - static const U imd3[]={1,2,0}; - const Vector3* vt[]={&a,&b,&c}; - const Vector3 dl[]={a-b,b-c,c-a}; - const Vector3 n=vec3_cross(dl[0],dl[1]); - const real_t l=n.length_squared(); - if(l>GJK_SIMPLEX3_EPS) - { - real_t mindist=-1; - real_t subw[2] = { 0 , 0}; - U subm = 0; - for(U i=0;i<3;++i) - { - if(vec3_dot(*vt[i],vec3_cross(dl[i],n))>0) - { - const U j=imd3[i]; - const real_t subd(projectorigin(*vt[i],*vt[j],subw,subm)); - if((mindist<0)||(subd<mindist)) - { - mindist = subd; - m = static_cast<U>(((subm&1)?1<<i:0)+((subm&2)?1<<j:0)); - w[i] = subw[0]; - w[j] = subw[1]; - w[imd3[j]] = 0; - } - } - } - if(mindist<0) - { - const real_t d=vec3_dot(a,n); - const real_t s=Math::sqrt(l); - const Vector3 p=n*(d/l); - mindist = p.length_squared(); - m = 7; - w[0] = (vec3_cross(dl[1],b-p)).length()/s; - w[1] = (vec3_cross(dl[2],c-p)).length()/s; - w[2] = 1-(w[0]+w[1]); - } - return(mindist); - } - return(-1); - } - static real_t projectorigin( const Vector3& a, - const Vector3& b, - const Vector3& c, - const Vector3& d, - real_t* w,U& m) - { - static const U imd3[]={1,2,0}; - const Vector3* vt[]={&a,&b,&c,&d}; - const Vector3 dl[]={a-d,b-d,c-d}; - const real_t vl=det(dl[0],dl[1],dl[2]); - const bool ng=(vl*vec3_dot(a,vec3_cross(b-c,a-b)))<=0; - if(ng&&(Math::abs(vl)>GJK_SIMPLEX4_EPS)) - { - real_t mindist=-1; - real_t subw[3]; - U subm=0; - for(U i=0;i<3;++i) - { - const U j=imd3[i]; - const real_t s=vl*vec3_dot(d,vec3_cross(dl[i],dl[j])); - if(s>0) - { - const real_t subd=projectorigin(*vt[i],*vt[j],d,subw,subm); - if((mindist<0)||(subd<mindist)) - { - mindist = subd; - m = static_cast<U>((subm&1?1<<i:0)+ - (subm&2?1<<j:0)+ - (subm&4?8:0)); - w[i] = subw[0]; - w[j] = subw[1]; - w[imd3[j]] = 0; - w[3] = subw[2]; - } - } - } - if(mindist<0) - { - mindist = 0; - m = 15; - w[0] = det(c,b,d)/vl; - w[1] = det(a,c,d)/vl; - w[2] = det(b,a,d)/vl; - w[3] = 1-(w[0]+w[1]+w[2]); - } - return(mindist); - } - return(-1); - } -}; - - // EPA - struct EPA - { - /* Types */ - typedef GJK::sSV sSV; - struct sFace - { - Vector3 n; - real_t d; - real_t p; - sSV* c[3]; - sFace* f[3]; - sFace* l[2]; - U1 e[3]; - U1 pass; - }; - struct sList - { - sFace* root; - U count; - sList() : root(0),count(0) {} - }; - struct sHorizon - { - sFace* cf; - sFace* ff; - U nf; - sHorizon() : cf(0),ff(0),nf(0) {} - }; - struct eStatus { enum _ { - Valid, - Touching, - Degenerated, - NonConvex, - InvalidHull, - OutOfFaces, - OutOfVertices, - AccuraryReached, - FallBack, - Failed };}; - /* Fields */ - eStatus::_ m_status; - GJK::sSimplex m_result; - Vector3 m_normal; - real_t m_depth; - sSV m_sv_store[EPA_MAX_VERTICES]; - sFace m_fc_store[EPA_MAX_FACES]; - U m_nextsv; - sList m_hull; - sList m_stock; - /* Methods */ - EPA() - { - Initialize(); - } - - - static inline void bind(sFace* fa,U ea,sFace* fb,U eb) - { - fa->e[ea]=(U1)eb;fa->f[ea]=fb; - fb->e[eb]=(U1)ea;fb->f[eb]=fa; - } - static inline void append(sList& list,sFace* face) - { - face->l[0] = 0; - face->l[1] = list.root; - if(list.root) list.root->l[0]=face; - list.root = face; - ++list.count; - } - static inline void remove(sList& list,sFace* face) - { - if(face->l[1]) face->l[1]->l[0]=face->l[0]; - if(face->l[0]) face->l[0]->l[1]=face->l[1]; - if(face==list.root) list.root=face->l[1]; - --list.count; - } - - - void Initialize() - { - m_status = eStatus::Failed; - m_normal = Vector3(0,0,0); - m_depth = 0; - m_nextsv = 0; - for(U i=0;i<EPA_MAX_FACES;++i) - { - append(m_stock,&m_fc_store[EPA_MAX_FACES-i-1]); - } - } - eStatus::_ Evaluate(GJK& gjk,const Vector3& guess) - { - GJK::sSimplex& simplex=*gjk.m_simplex; - if((simplex.rank>1)&&gjk.EncloseOrigin()) - { - - /* Clean up */ - while(m_hull.root) - { - sFace* f = m_hull.root; - remove(m_hull,f); - append(m_stock,f); - } - m_status = eStatus::Valid; - m_nextsv = 0; - /* Orient simplex */ - if(gjk.det( simplex.c[0]->w-simplex.c[3]->w, - simplex.c[1]->w-simplex.c[3]->w, - simplex.c[2]->w-simplex.c[3]->w)<0) - { - SWAP(simplex.c[0],simplex.c[1]); - SWAP(simplex.p[0],simplex.p[1]); - } - /* Build initial hull */ - sFace* tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true), - newface(simplex.c[1],simplex.c[0],simplex.c[3],true), - newface(simplex.c[2],simplex.c[1],simplex.c[3],true), - newface(simplex.c[0],simplex.c[2],simplex.c[3],true)}; - if(m_hull.count==4) - { - sFace* best=findbest(); - sFace outer=*best; - U pass=0; - U iterations=0; - bind(tetra[0],0,tetra[1],0); - bind(tetra[0],1,tetra[2],0); - bind(tetra[0],2,tetra[3],0); - bind(tetra[1],1,tetra[3],2); - bind(tetra[1],2,tetra[2],1); - bind(tetra[2],2,tetra[3],1); - m_status=eStatus::Valid; - for(;iterations<EPA_MAX_ITERATIONS;++iterations) - { - if(m_nextsv<EPA_MAX_VERTICES) - { - sHorizon horizon; - sSV* w=&m_sv_store[m_nextsv++]; - bool valid=true; - best->pass = (U1)(++pass); - gjk.getsupport(best->n,*w); - const real_t wdist=vec3_dot(best->n,w->w)-best->d; - if(wdist>EPA_ACCURACY) - { - for(U j=0;(j<3)&&valid;++j) - { - valid&=expand( pass,w, - best->f[j],best->e[j], - horizon); - } - if(valid&&(horizon.nf>=3)) - { - bind(horizon.cf,1,horizon.ff,2); - remove(m_hull,best); - append(m_stock,best); - best=findbest(); - if(best->p>=outer.p) outer=*best; - } else { m_status=eStatus::InvalidHull;break; } - } else { m_status=eStatus::AccuraryReached;break; } - } else { m_status=eStatus::OutOfVertices;break; } - } - const Vector3 projection=outer.n*outer.d; - m_normal = outer.n; - m_depth = outer.d; - m_result.rank = 3; - m_result.c[0] = outer.c[0]; - m_result.c[1] = outer.c[1]; - m_result.c[2] = outer.c[2]; - m_result.p[0] = vec3_cross( outer.c[1]->w-projection, - outer.c[2]->w-projection).length(); - m_result.p[1] = vec3_cross( outer.c[2]->w-projection, - outer.c[0]->w-projection).length(); - m_result.p[2] = vec3_cross( outer.c[0]->w-projection, - outer.c[1]->w-projection).length(); - const real_t sum=m_result.p[0]+m_result.p[1]+m_result.p[2]; - m_result.p[0] /= sum; - m_result.p[1] /= sum; - m_result.p[2] /= sum; - return(m_status); - } - } - /* Fallback */ - m_status = eStatus::FallBack; - m_normal = -guess; - const real_t nl=m_normal.length(); - if(nl>0) - m_normal = m_normal/nl; - else - m_normal = Vector3(1,0,0); - m_depth = 0; - m_result.rank=1; - m_result.c[0]=simplex.c[0]; - m_result.p[0]=1; - return(m_status); - } - sFace* newface(sSV* a,sSV* b,sSV* c,bool forced) - { - if(m_stock.root) - { - sFace* face=m_stock.root; - remove(m_stock,face); - append(m_hull,face); - face->pass = 0; - face->c[0] = a; - face->c[1] = b; - face->c[2] = c; - face->n = vec3_cross(b->w-a->w,c->w-a->w); - const real_t l=face->n.length(); - const bool v=l>EPA_ACCURACY; - face->p = MIN(MIN( - vec3_dot(a->w,vec3_cross(face->n,a->w-b->w)), - vec3_dot(b->w,vec3_cross(face->n,b->w-c->w))), - vec3_dot(c->w,vec3_cross(face->n,c->w-a->w))) / - (v?l:1); - face->p = face->p>=-EPA_INSIDE_EPS?0:face->p; - if(v) - { - face->d = vec3_dot(a->w,face->n)/l; - face->n /= l; - if(forced||(face->d>=-EPA_PLANE_EPS)) - { - return(face); - } else m_status=eStatus::NonConvex; - } else m_status=eStatus::Degenerated; - remove(m_hull,face); - append(m_stock,face); - return(0); - } - // -- GODOT start -- - //m_status=m_stock.root?eStatus::OutOfVertices:eStatus::OutOfFaces; - m_status=eStatus::OutOfFaces; - // -- GODOT end -- - return(0); - } - sFace* findbest() - { - sFace* minf=m_hull.root; - real_t mind=minf->d*minf->d; - real_t maxp=minf->p; - for(sFace* f=minf->l[1];f;f=f->l[1]) - { - const real_t sqd=f->d*f->d; - if((f->p>=maxp)&&(sqd<mind)) - { - minf=f; - mind=sqd; - maxp=f->p; - } - } - return(minf); - } - bool expand(U pass,sSV* w,sFace* f,U e,sHorizon& horizon) - { - static const U i1m3[]={1,2,0}; - static const U i2m3[]={2,0,1}; - if(f->pass!=pass) - { - const U e1=i1m3[e]; - if((vec3_dot(f->n,w->w)-f->d)<-EPA_PLANE_EPS) - { - sFace* nf=newface(f->c[e1],f->c[e],w,false); - if(nf) - { - bind(nf,0,f,e); - if(horizon.cf) bind(horizon.cf,1,nf,2); else horizon.ff=nf; - horizon.cf=nf; - ++horizon.nf; - return(true); - } - } - else - { - const U e2=i2m3[e]; - f->pass = (U1)pass; - if( expand(pass,w,f->f[e1],f->e[e1],horizon)&& - expand(pass,w,f->f[e2],f->e[e2],horizon)) - { - remove(m_hull,f); - append(m_stock,f); - return(true); - } - } - } - return(false); - } - - }; - - // - static void Initialize( const ShapeSW* shape0,const Transform& wtrs0, - const ShapeSW* shape1,const Transform& wtrs1, - sResults& results, - tShape& shape, - bool withmargins) - { - /* Results */ - results.witnesses[0] = - results.witnesses[1] = Vector3(0,0,0); - results.status = sResults::Separated; - /* Shape */ - shape.m_shapes[0] = shape0; - shape.m_shapes[1] = shape1; - shape.transform_A = wtrs0; - shape.transform_B = wtrs1; - - } - - - -// -// Api -// - -// - -// -bool Distance( const ShapeSW* shape0, - const Transform& wtrs0, - const ShapeSW* shape1, - const Transform& wtrs1, - const Vector3& guess, - sResults& results) -{ - tShape shape; - Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false); - GJK gjk; - GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,guess); - if(gjk_status==GJK::eStatus::Valid) - { - Vector3 w0=Vector3(0,0,0); - Vector3 w1=Vector3(0,0,0); - for(U i=0;i<gjk.m_simplex->rank;++i) - { - const real_t p=gjk.m_simplex->p[i]; - w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p; - w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p; - } - results.witnesses[0] = w0; - results.witnesses[1] = w1; - results.normal = w0-w1; - results.distance = results.normal.length(); - results.normal /= results.distance>GJK_MIN_DISTANCE?results.distance:1; - return(true); - } - else - { - results.status = gjk_status==GJK::eStatus::Inside? - sResults::Penetrating : - sResults::GJK_Failed ; - return(false); - } -} - -// -bool Penetration( const ShapeSW* shape0, - const Transform& wtrs0, - const ShapeSW* shape1, - const Transform& wtrs1, - const Vector3& guess, - sResults& results - ) -{ - tShape shape; - Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false); - GJK gjk; - GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,-guess); - switch(gjk_status) - { - case GJK::eStatus::Inside: - { - EPA epa; - EPA::eStatus::_ epa_status=epa.Evaluate(gjk,-guess); - if(epa_status!=EPA::eStatus::Failed) - { - Vector3 w0=Vector3(0,0,0); - for(U i=0;i<epa.m_result.rank;++i) - { - w0+=shape.Support(epa.m_result.c[i]->d,0)*epa.m_result.p[i]; - } - results.status = sResults::Penetrating; - results.witnesses[0] = w0; - results.witnesses[1] = w0-epa.m_normal*epa.m_depth; - results.normal = -epa.m_normal; - results.distance = -epa.m_depth; - return(true); - } else results.status=sResults::EPA_Failed; - } - break; - case GJK::eStatus::Failed: - results.status=sResults::GJK_Failed; - break; - default: {} - } - return(false); -} - - -/* Symbols cleanup */ - -#undef GJK_MAX_ITERATIONS -#undef GJK_ACCURARY -#undef GJK_MIN_DISTANCE -#undef GJK_DUPLICATED_EPS -#undef GJK_SIMPLEX2_EPS -#undef GJK_SIMPLEX3_EPS -#undef GJK_SIMPLEX4_EPS - -#undef EPA_MAX_VERTICES -#undef EPA_MAX_FACES -#undef EPA_MAX_ITERATIONS -#undef EPA_ACCURACY -#undef EPA_FALLBACK -#undef EPA_PLANE_EPS -#undef EPA_INSIDE_EPS - - -} // end of namespace - -/* clang-format on */ - -bool gjk_epa_calculate_distance(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_result_A, Vector3 &r_result_B) { - - GjkEpa2::sResults res; - - if (GjkEpa2::Distance(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_transform_B.origin - p_transform_A.origin, res)) { - - r_result_A = res.witnesses[0]; - r_result_B = res.witnesses[1]; - return true; - } - - return false; -} - -bool gjk_epa_calculate_penetration(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CollisionSolverSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap) { - - GjkEpa2::sResults res; - - if (GjkEpa2::Penetration(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_transform_B.origin - p_transform_A.origin, res)) { - if (p_result_callback) { - if (p_swap) - p_result_callback(res.witnesses[1], res.witnesses[0], p_userdata); - else - p_result_callback(res.witnesses[0], res.witnesses[1], p_userdata); - } - return true; - } - - return false; -} diff --git a/servers/physics/gjk_epa.h b/servers/physics/gjk_epa.h deleted file mode 100644 index 38c616cde0..0000000000 --- a/servers/physics/gjk_epa.h +++ /dev/null @@ -1,40 +0,0 @@ -/*************************************************************************/ -/* gjk_epa.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 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 GJK_EPA_H -#define GJK_EPA_H - -#include "collision_solver_sw.h" -#include "shape_sw.h" - -bool gjk_epa_calculate_penetration(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CollisionSolverSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap = false); -bool gjk_epa_calculate_distance(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_result_A, Vector3 &r_result_B); - -#endif diff --git a/servers/physics/joints/SCsub b/servers/physics/joints/SCsub deleted file mode 100644 index d730144861..0000000000 --- a/servers/physics/joints/SCsub +++ /dev/null @@ -1,5 +0,0 @@ -#!/usr/bin/env python - -Import('env') - -env.add_source_files(env.servers_sources, "*.cpp") diff --git a/servers/physics/joints/cone_twist_joint_sw.cpp b/servers/physics/joints/cone_twist_joint_sw.cpp deleted file mode 100644 index 58f66f7ea2..0000000000 --- a/servers/physics/joints/cone_twist_joint_sw.cpp +++ /dev/null @@ -1,366 +0,0 @@ -/*************************************************************************/ -/* cone_twist_joint_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 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. */ -/*************************************************************************/ - -/* -Adapted to Godot from the Bullet library. -*/ - -/* -Bullet Continuous Collision Detection and Physics Library -ConeTwistJointSW is Copyright (c) 2007 Starbreeze Studios - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. - -Written by: Marcus Hennix -*/ - -#include "cone_twist_joint_sw.h" - -static void plane_space(const Vector3 &n, Vector3 &p, Vector3 &q) { - - if (Math::abs(n.z) > Math_SQRT12) { - // choose p in y-z plane - real_t a = n[1] * n[1] + n[2] * n[2]; - real_t k = 1.0 / Math::sqrt(a); - p = Vector3(0, -n[2] * k, n[1] * k); - // set q = n x p - q = Vector3(a * k, -n[0] * p[2], n[0] * p[1]); - } else { - // choose p in x-y plane - real_t a = n.x * n.x + n.y * n.y; - real_t k = 1.0 / Math::sqrt(a); - p = Vector3(-n.y * k, n.x * k, 0); - // set q = n x p - q = Vector3(-n.z * p.y, n.z * p.x, a * k); - } -} - -static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x) { - real_t coeff_1 = Math_PI / 4.0f; - real_t coeff_2 = 3.0f * coeff_1; - real_t abs_y = Math::abs(y); - real_t angle; - if (x >= 0.0f) { - real_t r = (x - abs_y) / (x + abs_y); - angle = coeff_1 - coeff_1 * r; - } else { - real_t r = (x + abs_y) / (abs_y - x); - angle = coeff_2 - coeff_1 * r; - } - return (y < 0.0f) ? -angle : angle; -} - -ConeTwistJointSW::ConeTwistJointSW(BodySW *rbA, BodySW *rbB, const Transform &rbAFrame, const Transform &rbBFrame) : - JointSW(_arr, 2) { - - A = rbA; - B = rbB; - - m_rbAFrame = rbAFrame; - m_rbBFrame = rbBFrame; - - m_swingSpan1 = Math_PI / 4.0; - m_swingSpan2 = Math_PI / 4.0; - m_twistSpan = Math_PI * 2; - m_biasFactor = 0.3f; - m_relaxationFactor = 1.0f; - - m_angularOnly = false; - m_solveTwistLimit = false; - m_solveSwingLimit = false; - - A->add_constraint(this, 0); - B->add_constraint(this, 1); - - m_appliedImpulse = 0; -} - -bool ConeTwistJointSW::setup(real_t p_timestep) { - m_appliedImpulse = real_t(0.); - - //set bias, sign, clear accumulator - m_swingCorrection = real_t(0.); - m_twistLimitSign = real_t(0.); - m_solveTwistLimit = false; - m_solveSwingLimit = false; - m_accTwistLimitImpulse = real_t(0.); - m_accSwingLimitImpulse = real_t(0.); - - if (!m_angularOnly) { - Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin); - Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin); - Vector3 relPos = pivotBInW - pivotAInW; - - Vector3 normal[3]; - if (Math::is_zero_approx(relPos.length_squared())) { - normal[0] = Vector3(real_t(1.0), 0, 0); - } else { - normal[0] = relPos.normalized(); - } - - plane_space(normal[0], normal[1], normal[2]); - - for (int i = 0; i < 3; i++) { - memnew_placement(&m_jac[i], JacobianEntrySW( - A->get_principal_inertia_axes().transposed(), - B->get_principal_inertia_axes().transposed(), - pivotAInW - A->get_transform().origin - A->get_center_of_mass(), - pivotBInW - B->get_transform().origin - B->get_center_of_mass(), - normal[i], - A->get_inv_inertia(), - A->get_inv_mass(), - B->get_inv_inertia(), - B->get_inv_mass())); - } - } - - Vector3 b1Axis1, b1Axis2, b1Axis3; - Vector3 b2Axis1, b2Axis2; - - b1Axis1 = A->get_transform().basis.xform(this->m_rbAFrame.basis.get_axis(0)); - b2Axis1 = B->get_transform().basis.xform(this->m_rbBFrame.basis.get_axis(0)); - - real_t swing1 = real_t(0.), swing2 = real_t(0.); - - real_t swx = real_t(0.), swy = real_t(0.); - real_t thresh = real_t(10.); - real_t fact; - - // Get Frame into world space - if (m_swingSpan1 >= real_t(0.05f)) { - b1Axis2 = A->get_transform().basis.xform(this->m_rbAFrame.basis.get_axis(1)); - //swing1 = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) ); - swx = b2Axis1.dot(b1Axis1); - swy = b2Axis1.dot(b1Axis2); - swing1 = atan2fast(swy, swx); - fact = (swy * swy + swx * swx) * thresh * thresh; - fact = fact / (fact + real_t(1.0)); - swing1 *= fact; - } - - if (m_swingSpan2 >= real_t(0.05f)) { - b1Axis3 = A->get_transform().basis.xform(this->m_rbAFrame.basis.get_axis(2)); - //swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) ); - swx = b2Axis1.dot(b1Axis1); - swy = b2Axis1.dot(b1Axis3); - swing2 = atan2fast(swy, swx); - fact = (swy * swy + swx * swx) * thresh * thresh; - fact = fact / (fact + real_t(1.0)); - swing2 *= fact; - } - - real_t RMaxAngle1Sq = 1.0f / (m_swingSpan1 * m_swingSpan1); - real_t RMaxAngle2Sq = 1.0f / (m_swingSpan2 * m_swingSpan2); - real_t EllipseAngle = Math::abs(swing1 * swing1) * RMaxAngle1Sq + Math::abs(swing2 * swing2) * RMaxAngle2Sq; - - if (EllipseAngle > 1.0f) { - m_swingCorrection = EllipseAngle - 1.0f; - m_solveSwingLimit = true; - - // Calculate necessary axis & factors - m_swingAxis = b2Axis1.cross(b1Axis2 * b2Axis1.dot(b1Axis2) + b1Axis3 * b2Axis1.dot(b1Axis3)); - m_swingAxis.normalize(); - - real_t swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f; - m_swingAxis *= swingAxisSign; - - m_kSwing = real_t(1.) / (A->compute_angular_impulse_denominator(m_swingAxis) + - B->compute_angular_impulse_denominator(m_swingAxis)); - } - - // Twist limits - if (m_twistSpan >= real_t(0.)) { - Vector3 b2Axis22 = B->get_transform().basis.xform(this->m_rbBFrame.basis.get_axis(1)); - Quat rotationArc = Quat(b2Axis1, b1Axis1); - Vector3 TwistRef = rotationArc.xform(b2Axis22); - real_t twist = atan2fast(TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2)); - - real_t lockedFreeFactor = (m_twistSpan > real_t(0.05f)) ? m_limitSoftness : real_t(0.); - if (twist <= -m_twistSpan * lockedFreeFactor) { - m_twistCorrection = -(twist + m_twistSpan); - m_solveTwistLimit = true; - - m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f; - m_twistAxis.normalize(); - m_twistAxis *= -1.0f; - - m_kTwist = real_t(1.) / (A->compute_angular_impulse_denominator(m_twistAxis) + - B->compute_angular_impulse_denominator(m_twistAxis)); - - } else if (twist > m_twistSpan * lockedFreeFactor) { - m_twistCorrection = (twist - m_twistSpan); - m_solveTwistLimit = true; - - m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f; - m_twistAxis.normalize(); - - m_kTwist = real_t(1.) / (A->compute_angular_impulse_denominator(m_twistAxis) + - B->compute_angular_impulse_denominator(m_twistAxis)); - } - } - - return true; -} - -void ConeTwistJointSW::solve(real_t p_timestep) { - - Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin); - Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin); - - real_t tau = real_t(0.3); - - //linear part - if (!m_angularOnly) { - Vector3 rel_pos1 = pivotAInW - A->get_transform().origin; - Vector3 rel_pos2 = pivotBInW - B->get_transform().origin; - - Vector3 vel1 = A->get_velocity_in_local_point(rel_pos1); - Vector3 vel2 = B->get_velocity_in_local_point(rel_pos2); - Vector3 vel = vel1 - vel2; - - for (int i = 0; i < 3; i++) { - const Vector3 &normal = m_jac[i].m_linearJointAxis; - real_t jacDiagABInv = real_t(1.) / m_jac[i].getDiagonal(); - - real_t rel_vel; - rel_vel = normal.dot(vel); - //positional error (zeroth order error) - real_t depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal - real_t impulse = depth * tau / p_timestep * jacDiagABInv - rel_vel * jacDiagABInv; - m_appliedImpulse += impulse; - Vector3 impulse_vector = normal * impulse; - A->apply_impulse(pivotAInW - A->get_transform().origin, impulse_vector); - B->apply_impulse(pivotBInW - B->get_transform().origin, -impulse_vector); - } - } - - { - ///solve angular part - const Vector3 &angVelA = A->get_angular_velocity(); - const Vector3 &angVelB = B->get_angular_velocity(); - - // solve swing limit - if (m_solveSwingLimit) { - real_t amplitude = ((angVelB - angVelA).dot(m_swingAxis) * m_relaxationFactor * m_relaxationFactor + m_swingCorrection * (real_t(1.) / p_timestep) * m_biasFactor); - real_t impulseMag = amplitude * m_kSwing; - - // Clamp the accumulated impulse - real_t temp = m_accSwingLimitImpulse; - m_accSwingLimitImpulse = MAX(m_accSwingLimitImpulse + impulseMag, real_t(0.0)); - impulseMag = m_accSwingLimitImpulse - temp; - - Vector3 impulse = m_swingAxis * impulseMag; - - A->apply_torque_impulse(impulse); - B->apply_torque_impulse(-impulse); - } - - // solve twist limit - if (m_solveTwistLimit) { - real_t amplitude = ((angVelB - angVelA).dot(m_twistAxis) * m_relaxationFactor * m_relaxationFactor + m_twistCorrection * (real_t(1.) / p_timestep) * m_biasFactor); - real_t impulseMag = amplitude * m_kTwist; - - // Clamp the accumulated impulse - real_t temp = m_accTwistLimitImpulse; - m_accTwistLimitImpulse = MAX(m_accTwistLimitImpulse + impulseMag, real_t(0.0)); - impulseMag = m_accTwistLimitImpulse - temp; - - Vector3 impulse = m_twistAxis * impulseMag; - - A->apply_torque_impulse(impulse); - B->apply_torque_impulse(-impulse); - } - } -} - -void ConeTwistJointSW::set_param(PhysicsServer::ConeTwistJointParam p_param, real_t p_value) { - - switch (p_param) { - case PhysicsServer::CONE_TWIST_JOINT_SWING_SPAN: { - - m_swingSpan1 = p_value; - m_swingSpan2 = p_value; - } break; - case PhysicsServer::CONE_TWIST_JOINT_TWIST_SPAN: { - - m_twistSpan = p_value; - } break; - case PhysicsServer::CONE_TWIST_JOINT_BIAS: { - - m_biasFactor = p_value; - } break; - case PhysicsServer::CONE_TWIST_JOINT_SOFTNESS: { - - m_limitSoftness = p_value; - } break; - case PhysicsServer::CONE_TWIST_JOINT_RELAXATION: { - - m_relaxationFactor = p_value; - } break; - case PhysicsServer::CONE_TWIST_MAX: break; // Can't happen, but silences warning - } -} - -real_t ConeTwistJointSW::get_param(PhysicsServer::ConeTwistJointParam p_param) const { - - switch (p_param) { - case PhysicsServer::CONE_TWIST_JOINT_SWING_SPAN: { - - return m_swingSpan1; - } break; - case PhysicsServer::CONE_TWIST_JOINT_TWIST_SPAN: { - - return m_twistSpan; - } break; - case PhysicsServer::CONE_TWIST_JOINT_BIAS: { - - return m_biasFactor; - } break; - case PhysicsServer::CONE_TWIST_JOINT_SOFTNESS: { - - return m_limitSoftness; - } break; - case PhysicsServer::CONE_TWIST_JOINT_RELAXATION: { - - return m_relaxationFactor; - } break; - case PhysicsServer::CONE_TWIST_MAX: break; // Can't happen, but silences warning - } - - return 0; -} diff --git a/servers/physics/joints/cone_twist_joint_sw.h b/servers/physics/joints/cone_twist_joint_sw.h deleted file mode 100644 index 857aaa0d86..0000000000 --- a/servers/physics/joints/cone_twist_joint_sw.h +++ /dev/null @@ -1,142 +0,0 @@ -/*************************************************************************/ -/* cone_twist_joint_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 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. */ -/*************************************************************************/ - -/* -Adapted to Godot from the Bullet library. -*/ - -/* -Bullet Continuous Collision Detection and Physics Library -ConeTwistJointSW is Copyright (c) 2007 Starbreeze Studios - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. - -Written by: Marcus Hennix -*/ - -#ifndef CONE_TWIST_JOINT_SW_H -#define CONE_TWIST_JOINT_SW_H - -#include "servers/physics/joints/jacobian_entry_sw.h" -#include "servers/physics/joints_sw.h" - -///ConeTwistJointSW can be used to simulate ragdoll joints (upper arm, leg etc) -class ConeTwistJointSW : public JointSW { -#ifdef IN_PARALLELL_SOLVER -public: -#endif - - union { - struct { - BodySW *A; - BodySW *B; - }; - - BodySW *_arr[2]; - }; - - JacobianEntrySW m_jac[3]; //3 orthogonal linear constraints - - real_t m_appliedImpulse; - Transform m_rbAFrame; - Transform m_rbBFrame; - - real_t m_limitSoftness; - real_t m_biasFactor; - real_t m_relaxationFactor; - - real_t m_swingSpan1; - real_t m_swingSpan2; - real_t m_twistSpan; - - Vector3 m_swingAxis; - Vector3 m_twistAxis; - - real_t m_kSwing; - real_t m_kTwist; - - real_t m_twistLimitSign; - real_t m_swingCorrection; - real_t m_twistCorrection; - - real_t m_accSwingLimitImpulse; - real_t m_accTwistLimitImpulse; - - bool m_angularOnly; - bool m_solveTwistLimit; - bool m_solveSwingLimit; - -public: - virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_CONE_TWIST; } - - virtual bool setup(real_t p_timestep); - virtual void solve(real_t p_timestep); - - ConeTwistJointSW(BodySW *rbA, BodySW *rbB, const Transform &rbAFrame, const Transform &rbBFrame); - - void setAngularOnly(bool angularOnly) { - m_angularOnly = angularOnly; - } - - void setLimit(real_t _swingSpan1, real_t _swingSpan2, real_t _twistSpan, real_t _softness = 0.8f, real_t _biasFactor = 0.3f, real_t _relaxationFactor = 1.0f) { - m_swingSpan1 = _swingSpan1; - m_swingSpan2 = _swingSpan2; - m_twistSpan = _twistSpan; - - m_limitSoftness = _softness; - m_biasFactor = _biasFactor; - m_relaxationFactor = _relaxationFactor; - } - - inline int getSolveTwistLimit() { - return m_solveTwistLimit; - } - - inline int getSolveSwingLimit() { - return m_solveTwistLimit; - } - - inline real_t getTwistLimitSign() { - return m_twistLimitSign; - } - - void set_param(PhysicsServer::ConeTwistJointParam p_param, real_t p_value); - real_t get_param(PhysicsServer::ConeTwistJointParam p_param) const; -}; - -#endif // CONE_TWIST_JOINT_SW_H diff --git a/servers/physics/joints/generic_6dof_joint_sw.cpp b/servers/physics/joints/generic_6dof_joint_sw.cpp deleted file mode 100644 index 8f0ccab7f7..0000000000 --- a/servers/physics/joints/generic_6dof_joint_sw.cpp +++ /dev/null @@ -1,686 +0,0 @@ -/*************************************************************************/ -/* generic_6dof_joint_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 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. */ -/*************************************************************************/ - -/* -Adapted to Godot from the Bullet library. -*/ - -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -/* -2007-09-09 -Generic6DOFJointSW Refactored by Francisco Le?n -email: projectileman@yahoo.com -http://gimpact.sf.net -*/ - -#include "generic_6dof_joint_sw.h" - -#define GENERIC_D6_DISABLE_WARMSTARTING 1 - -//////////////////////////// G6DOFRotationalLimitMotorSW //////////////////////////////////// - -int G6DOFRotationalLimitMotorSW::testLimitValue(real_t test_value) { - if (m_loLimit > m_hiLimit) { - m_currentLimit = 0; //Free from violation - return 0; - } - - if (test_value < m_loLimit) { - m_currentLimit = 1; //low limit violation - m_currentLimitError = test_value - m_loLimit; - return 1; - } else if (test_value > m_hiLimit) { - m_currentLimit = 2; //High limit violation - m_currentLimitError = test_value - m_hiLimit; - return 2; - }; - - m_currentLimit = 0; //Free from violation - return 0; -} - -real_t G6DOFRotationalLimitMotorSW::solveAngularLimits( - real_t timeStep, Vector3 &axis, real_t jacDiagABInv, - BodySW *body0, BodySW *body1) { - if (!needApplyTorques()) return 0.0f; - - real_t target_velocity = m_targetVelocity; - real_t maxMotorForce = m_maxMotorForce; - - //current error correction - if (m_currentLimit != 0) { - target_velocity = -m_ERP * m_currentLimitError / (timeStep); - maxMotorForce = m_maxLimitForce; - } - - maxMotorForce *= timeStep; - - // current velocity difference - Vector3 vel_diff = body0->get_angular_velocity(); - if (body1) { - vel_diff -= body1->get_angular_velocity(); - } - - real_t rel_vel = axis.dot(vel_diff); - - // correction velocity - real_t motor_relvel = m_limitSoftness * (target_velocity - m_damping * rel_vel); - - if (Math::is_zero_approx(motor_relvel)) { - return 0.0f; //no need for applying force - } - - // correction impulse - real_t unclippedMotorImpulse = (1 + m_bounce) * motor_relvel * jacDiagABInv; - - // clip correction impulse - real_t clippedMotorImpulse; - - ///@todo: should clip against accumulated impulse - if (unclippedMotorImpulse > 0.0f) { - clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce ? maxMotorForce : unclippedMotorImpulse; - } else { - clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce : unclippedMotorImpulse; - } - - // sort with accumulated impulses - real_t lo = real_t(-1e30); - real_t hi = real_t(1e30); - - real_t oldaccumImpulse = m_accumulatedImpulse; - real_t sum = oldaccumImpulse + clippedMotorImpulse; - m_accumulatedImpulse = sum > hi ? real_t(0.) : sum < lo ? real_t(0.) : sum; - - clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse; - - Vector3 motorImp = clippedMotorImpulse * axis; - - body0->apply_torque_impulse(motorImp); - if (body1) body1->apply_torque_impulse(-motorImp); - - return clippedMotorImpulse; -} - -//////////////////////////// End G6DOFRotationalLimitMotorSW //////////////////////////////////// - -//////////////////////////// G6DOFTranslationalLimitMotorSW //////////////////////////////////// -real_t G6DOFTranslationalLimitMotorSW::solveLinearAxis( - real_t timeStep, - real_t jacDiagABInv, - BodySW *body1, const Vector3 &pointInA, - BodySW *body2, const Vector3 &pointInB, - int limit_index, - const Vector3 &axis_normal_on_a, - const Vector3 &anchorPos) { - - ///find relative velocity - // Vector3 rel_pos1 = pointInA - body1->get_transform().origin; - // Vector3 rel_pos2 = pointInB - body2->get_transform().origin; - Vector3 rel_pos1 = anchorPos - body1->get_transform().origin; - Vector3 rel_pos2 = anchorPos - body2->get_transform().origin; - - Vector3 vel1 = body1->get_velocity_in_local_point(rel_pos1); - Vector3 vel2 = body2->get_velocity_in_local_point(rel_pos2); - Vector3 vel = vel1 - vel2; - - real_t rel_vel = axis_normal_on_a.dot(vel); - - /// apply displacement correction - - //positional error (zeroth order error) - real_t depth = -(pointInA - pointInB).dot(axis_normal_on_a); - real_t lo = real_t(-1e30); - real_t hi = real_t(1e30); - - real_t minLimit = m_lowerLimit[limit_index]; - real_t maxLimit = m_upperLimit[limit_index]; - - //handle the limits - if (minLimit < maxLimit) { - { - if (depth > maxLimit) { - depth -= maxLimit; - lo = real_t(0.); - - } else { - if (depth < minLimit) { - depth -= minLimit; - hi = real_t(0.); - } else { - return 0.0f; - } - } - } - } - - real_t normalImpulse = m_limitSoftness[limit_index] * (m_restitution[limit_index] * depth / timeStep - m_damping[limit_index] * rel_vel) * jacDiagABInv; - - real_t oldNormalImpulse = m_accumulatedImpulse[limit_index]; - real_t sum = oldNormalImpulse + normalImpulse; - m_accumulatedImpulse[limit_index] = sum > hi ? real_t(0.) : sum < lo ? real_t(0.) : sum; - normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse; - - Vector3 impulse_vector = axis_normal_on_a * normalImpulse; - body1->apply_impulse(rel_pos1, impulse_vector); - body2->apply_impulse(rel_pos2, -impulse_vector); - return normalImpulse; -} - -//////////////////////////// G6DOFTranslationalLimitMotorSW //////////////////////////////////// - -Generic6DOFJointSW::Generic6DOFJointSW(BodySW *rbA, BodySW *rbB, const Transform &frameInA, const Transform &frameInB, bool useLinearReferenceFrameA) : - JointSW(_arr, 2), - m_frameInA(frameInA), - m_frameInB(frameInB), - m_useLinearReferenceFrameA(useLinearReferenceFrameA) { - A = rbA; - B = rbB; - A->add_constraint(this, 0); - B->add_constraint(this, 1); -} - -void Generic6DOFJointSW::calculateAngleInfo() { - Basis relative_frame = m_calculatedTransformB.basis.inverse() * m_calculatedTransformA.basis; - - m_calculatedAxisAngleDiff = relative_frame.get_euler_xyz(); - - // in euler angle mode we do not actually constrain the angular velocity - // along the axes axis[0] and axis[2] (although we do use axis[1]) : - // - // to get constrain w2-w1 along ...not - // ------ --------------------- ------ - // d(angle[0])/dt = 0 ax[1] x ax[2] ax[0] - // d(angle[1])/dt = 0 ax[1] - // d(angle[2])/dt = 0 ax[0] x ax[1] ax[2] - // - // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0. - // to prove the result for angle[0], write the expression for angle[0] from - // GetInfo1 then take the derivative. to prove this for angle[2] it is - // easier to take the euler rate expression for d(angle[2])/dt with respect - // to the components of w and set that to 0. - - Vector3 axis0 = m_calculatedTransformB.basis.get_axis(0); - Vector3 axis2 = m_calculatedTransformA.basis.get_axis(2); - - m_calculatedAxis[1] = axis2.cross(axis0); - m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2); - m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]); - - /* - if(m_debugDrawer) - { - - char buff[300]; - sprintf(buff,"\n X: %.2f ; Y: %.2f ; Z: %.2f ", - m_calculatedAxisAngleDiff[0], - m_calculatedAxisAngleDiff[1], - m_calculatedAxisAngleDiff[2]); - m_debugDrawer->reportErrorWarning(buff); - } - */ -} - -void Generic6DOFJointSW::calculateTransforms() { - m_calculatedTransformA = A->get_transform() * m_frameInA; - m_calculatedTransformB = B->get_transform() * m_frameInB; - - calculateAngleInfo(); -} - -void Generic6DOFJointSW::buildLinearJacobian( - JacobianEntrySW &jacLinear, const Vector3 &normalWorld, - const Vector3 &pivotAInW, const Vector3 &pivotBInW) { - memnew_placement(&jacLinear, JacobianEntrySW( - A->get_principal_inertia_axes().transposed(), - B->get_principal_inertia_axes().transposed(), - pivotAInW - A->get_transform().origin - A->get_center_of_mass(), - pivotBInW - B->get_transform().origin - B->get_center_of_mass(), - normalWorld, - A->get_inv_inertia(), - A->get_inv_mass(), - B->get_inv_inertia(), - B->get_inv_mass())); -} - -void Generic6DOFJointSW::buildAngularJacobian( - JacobianEntrySW &jacAngular, const Vector3 &jointAxisW) { - memnew_placement(&jacAngular, JacobianEntrySW(jointAxisW, - A->get_principal_inertia_axes().transposed(), - B->get_principal_inertia_axes().transposed(), - A->get_inv_inertia(), - B->get_inv_inertia())); -} - -bool Generic6DOFJointSW::testAngularLimitMotor(int axis_index) { - real_t angle = m_calculatedAxisAngleDiff[axis_index]; - - //test limits - m_angularLimits[axis_index].testLimitValue(angle); - return m_angularLimits[axis_index].needApplyTorques(); -} - -bool Generic6DOFJointSW::setup(real_t p_timestep) { - - // Clear accumulated impulses for the next simulation step - m_linearLimits.m_accumulatedImpulse = Vector3(real_t(0.), real_t(0.), real_t(0.)); - int i; - for (i = 0; i < 3; i++) { - m_angularLimits[i].m_accumulatedImpulse = real_t(0.); - } - //calculates transform - calculateTransforms(); - - // const Vector3& pivotAInW = m_calculatedTransformA.origin; - // const Vector3& pivotBInW = m_calculatedTransformB.origin; - calcAnchorPos(); - Vector3 pivotAInW = m_AnchorPos; - Vector3 pivotBInW = m_AnchorPos; - - // not used here - // Vector3 rel_pos1 = pivotAInW - A->get_transform().origin; - // Vector3 rel_pos2 = pivotBInW - B->get_transform().origin; - - Vector3 normalWorld; - //linear part - for (i = 0; i < 3; i++) { - if (m_linearLimits.enable_limit[i] && m_linearLimits.isLimited(i)) { - if (m_useLinearReferenceFrameA) - normalWorld = m_calculatedTransformA.basis.get_axis(i); - else - normalWorld = m_calculatedTransformB.basis.get_axis(i); - - buildLinearJacobian( - m_jacLinear[i], normalWorld, - pivotAInW, pivotBInW); - } - } - - // angular part - for (i = 0; i < 3; i++) { - //calculates error angle - if (m_angularLimits[i].m_enableLimit && testAngularLimitMotor(i)) { - normalWorld = this->getAxis(i); - // Create angular atom - buildAngularJacobian(m_jacAng[i], normalWorld); - } - } - - return true; -} - -void Generic6DOFJointSW::solve(real_t p_timestep) { - m_timeStep = p_timestep; - - //calculateTransforms(); - - int i; - - // linear - - Vector3 pointInA = m_calculatedTransformA.origin; - Vector3 pointInB = m_calculatedTransformB.origin; - - real_t jacDiagABInv; - Vector3 linear_axis; - for (i = 0; i < 3; i++) { - if (m_linearLimits.enable_limit[i] && m_linearLimits.isLimited(i)) { - jacDiagABInv = real_t(1.) / m_jacLinear[i].getDiagonal(); - - if (m_useLinearReferenceFrameA) - linear_axis = m_calculatedTransformA.basis.get_axis(i); - else - linear_axis = m_calculatedTransformB.basis.get_axis(i); - - m_linearLimits.solveLinearAxis( - m_timeStep, - jacDiagABInv, - A, pointInA, - B, pointInB, - i, linear_axis, m_AnchorPos); - } - } - - // angular - Vector3 angular_axis; - real_t angularJacDiagABInv; - for (i = 0; i < 3; i++) { - if (m_angularLimits[i].m_enableLimit && m_angularLimits[i].needApplyTorques()) { - - // get axis - angular_axis = getAxis(i); - - angularJacDiagABInv = real_t(1.) / m_jacAng[i].getDiagonal(); - - m_angularLimits[i].solveAngularLimits(m_timeStep, angular_axis, angularJacDiagABInv, A, B); - } - } -} - -void Generic6DOFJointSW::updateRHS(real_t timeStep) { - (void)timeStep; -} - -Vector3 Generic6DOFJointSW::getAxis(int axis_index) const { - return m_calculatedAxis[axis_index]; -} - -real_t Generic6DOFJointSW::getAngle(int axis_index) const { - return m_calculatedAxisAngleDiff[axis_index]; -} - -void Generic6DOFJointSW::calcAnchorPos(void) { - real_t imA = A->get_inv_mass(); - real_t imB = B->get_inv_mass(); - real_t weight; - if (imB == real_t(0.0)) { - weight = real_t(1.0); - } else { - weight = imA / (imA + imB); - } - const Vector3 &pA = m_calculatedTransformA.origin; - const Vector3 &pB = m_calculatedTransformB.origin; - m_AnchorPos = pA * weight + pB * (real_t(1.0) - weight); -} // Generic6DOFJointSW::calcAnchorPos() - -void Generic6DOFJointSW::set_param(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisParam p_param, real_t p_value) { - - ERR_FAIL_INDEX(p_axis, 3); - switch (p_param) { - case PhysicsServer::G6DOF_JOINT_LINEAR_LOWER_LIMIT: { - - m_linearLimits.m_lowerLimit[p_axis] = p_value; - } break; - case PhysicsServer::G6DOF_JOINT_LINEAR_UPPER_LIMIT: { - - m_linearLimits.m_upperLimit[p_axis] = p_value; - - } break; - case PhysicsServer::G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS: { - - m_linearLimits.m_limitSoftness[p_axis] = p_value; - - } break; - case PhysicsServer::G6DOF_JOINT_LINEAR_RESTITUTION: { - - m_linearLimits.m_restitution[p_axis] = p_value; - - } break; - case PhysicsServer::G6DOF_JOINT_LINEAR_DAMPING: { - - m_linearLimits.m_damping[p_axis] = p_value; - - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_LOWER_LIMIT: { - - m_angularLimits[p_axis].m_loLimit = p_value; - - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_UPPER_LIMIT: { - - m_angularLimits[p_axis].m_hiLimit = p_value; - - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS: { - - m_angularLimits[p_axis].m_limitSoftness = p_value; - - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_DAMPING: { - - m_angularLimits[p_axis].m_damping = p_value; - - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_RESTITUTION: { - - m_angularLimits[p_axis].m_bounce = p_value; - - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_FORCE_LIMIT: { - - m_angularLimits[p_axis].m_maxLimitForce = p_value; - - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_ERP: { - - m_angularLimits[p_axis].m_ERP = p_value; - - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY: { - - m_angularLimits[p_axis].m_targetVelocity = p_value; - - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT: { - - m_angularLimits[p_axis].m_maxLimitForce = p_value; - - } break; - case PhysicsServer::G6DOF_JOINT_LINEAR_MOTOR_TARGET_VELOCITY: { - // Not implemented in GodotPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_LINEAR_MOTOR_FORCE_LIMIT: { - // Not implemented in GodotPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_LINEAR_SPRING_STIFFNESS: { - // Not implemented in GodotPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_LINEAR_SPRING_DAMPING: { - // Not implemented in GodotPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_LINEAR_SPRING_EQUILIBRIUM_POINT: { - // Not implemented in GodotPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_SPRING_STIFFNESS: { - // Not implemented in GodotPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_SPRING_DAMPING: { - // Not implemented in GodotPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_SPRING_EQUILIBRIUM_POINT: { - // Not implemented in GodotPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_MAX: break; // Can't happen, but silences warning - } -} - -real_t Generic6DOFJointSW::get_param(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisParam p_param) const { - ERR_FAIL_INDEX_V(p_axis, 3, 0); - switch (p_param) { - case PhysicsServer::G6DOF_JOINT_LINEAR_LOWER_LIMIT: { - - return m_linearLimits.m_lowerLimit[p_axis]; - } break; - case PhysicsServer::G6DOF_JOINT_LINEAR_UPPER_LIMIT: { - - return m_linearLimits.m_upperLimit[p_axis]; - - } break; - case PhysicsServer::G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS: { - - return m_linearLimits.m_limitSoftness[p_axis]; - - } break; - case PhysicsServer::G6DOF_JOINT_LINEAR_RESTITUTION: { - - return m_linearLimits.m_restitution[p_axis]; - - } break; - case PhysicsServer::G6DOF_JOINT_LINEAR_DAMPING: { - - return m_linearLimits.m_damping[p_axis]; - - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_LOWER_LIMIT: { - - return m_angularLimits[p_axis].m_loLimit; - - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_UPPER_LIMIT: { - - return m_angularLimits[p_axis].m_hiLimit; - - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS: { - - return m_angularLimits[p_axis].m_limitSoftness; - - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_DAMPING: { - - return m_angularLimits[p_axis].m_damping; - - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_RESTITUTION: { - - return m_angularLimits[p_axis].m_bounce; - - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_FORCE_LIMIT: { - - return m_angularLimits[p_axis].m_maxLimitForce; - - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_ERP: { - - return m_angularLimits[p_axis].m_ERP; - - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY: { - - return m_angularLimits[p_axis].m_targetVelocity; - - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT: { - - return m_angularLimits[p_axis].m_maxMotorForce; - - } break; - case PhysicsServer::G6DOF_JOINT_LINEAR_MOTOR_TARGET_VELOCITY: { - // Not implemented in GodotPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_LINEAR_MOTOR_FORCE_LIMIT: { - // Not implemented in GodotPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_LINEAR_SPRING_STIFFNESS: { - // Not implemented in GodotPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_LINEAR_SPRING_DAMPING: { - // Not implemented in GodotPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_LINEAR_SPRING_EQUILIBRIUM_POINT: { - // Not implemented in GodotPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_SPRING_STIFFNESS: { - // Not implemented in GodotPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_SPRING_DAMPING: { - // Not implemented in GodotPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_SPRING_EQUILIBRIUM_POINT: { - // Not implemented in GodotPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_MAX: break; // Can't happen, but silences warning - } - return 0; -} - -void Generic6DOFJointSW::set_flag(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisFlag p_flag, bool p_value) { - - ERR_FAIL_INDEX(p_axis, 3); - - switch (p_flag) { - case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT: { - - m_linearLimits.enable_limit[p_axis] = p_value; - } break; - case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT: { - - m_angularLimits[p_axis].m_enableLimit = p_value; - } break; - case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_MOTOR: { - - m_angularLimits[p_axis].m_enableMotor = p_value; - } break; - case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_MOTOR: { - // Not implemented in GodotPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_SPRING: { - // Not implemented in GodotPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_SPRING: { - // Not implemented in GodotPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_FLAG_MAX: break; // Can't happen, but silences warning - } -} -bool Generic6DOFJointSW::get_flag(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisFlag p_flag) const { - - ERR_FAIL_INDEX_V(p_axis, 3, 0); - switch (p_flag) { - case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT: { - - return m_linearLimits.enable_limit[p_axis]; - } break; - case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT: { - - return m_angularLimits[p_axis].m_enableLimit; - } break; - case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_MOTOR: { - - return m_angularLimits[p_axis].m_enableMotor; - } break; - case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_MOTOR: { - // Not implemented in GodotPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_SPRING: { - // Not implemented in GodotPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_SPRING: { - // Not implemented in GodotPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_FLAG_MAX: break; // Can't happen, but silences warning - } - - return 0; -} diff --git a/servers/physics/joints/generic_6dof_joint_sw.h b/servers/physics/joints/generic_6dof_joint_sw.h deleted file mode 100644 index 07626ffa97..0000000000 --- a/servers/physics/joints/generic_6dof_joint_sw.h +++ /dev/null @@ -1,401 +0,0 @@ -/*************************************************************************/ -/* generic_6dof_joint_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 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. */ -/*************************************************************************/ - -/* -Adapted to Godot from the Bullet library. -*/ - -#ifndef GENERIC_6DOF_JOINT_SW_H -#define GENERIC_6DOF_JOINT_SW_H - -#include "servers/physics/joints/jacobian_entry_sw.h" -#include "servers/physics/joints_sw.h" - -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -/* -2007-09-09 -Generic6DOFJointSW Refactored by Francisco Le?n -email: projectileman@yahoo.com -http://gimpact.sf.net -*/ - -//! Rotation Limit structure for generic joints -class G6DOFRotationalLimitMotorSW { -public: - //! limit_parameters - //!@{ - real_t m_loLimit; //!< joint limit - real_t m_hiLimit; //!< joint limit - real_t m_targetVelocity; //!< target motor velocity - real_t m_maxMotorForce; //!< max force on motor - real_t m_maxLimitForce; //!< max force on limit - real_t m_damping; //!< Damping. - real_t m_limitSoftness; //! Relaxation factor - real_t m_ERP; //!< Error tolerance factor when joint is at limit - real_t m_bounce; //!< restitution factor - bool m_enableMotor; - bool m_enableLimit; - - //!@} - - //! temp_variables - //!@{ - real_t m_currentLimitError; //! How much is violated this limit - int m_currentLimit; //!< 0=free, 1=at lo limit, 2=at hi limit - real_t m_accumulatedImpulse; - //!@} - - G6DOFRotationalLimitMotorSW() { - m_accumulatedImpulse = 0.f; - m_targetVelocity = 0; - m_maxMotorForce = 0.1f; - m_maxLimitForce = 300.0f; - m_loLimit = -1e30; - m_hiLimit = 1e30; - m_ERP = 0.5f; - m_bounce = 0.0f; - m_damping = 1.0f; - m_limitSoftness = 0.5f; - m_currentLimit = 0; - m_currentLimitError = 0; - m_enableMotor = false; - m_enableLimit = false; - } - - G6DOFRotationalLimitMotorSW(const G6DOFRotationalLimitMotorSW &limot) { - m_targetVelocity = limot.m_targetVelocity; - m_maxMotorForce = limot.m_maxMotorForce; - m_limitSoftness = limot.m_limitSoftness; - m_loLimit = limot.m_loLimit; - m_hiLimit = limot.m_hiLimit; - m_ERP = limot.m_ERP; - m_bounce = limot.m_bounce; - m_currentLimit = limot.m_currentLimit; - m_currentLimitError = limot.m_currentLimitError; - m_enableMotor = limot.m_enableMotor; - } - - //! Is limited - bool isLimited() { - return (m_loLimit < m_hiLimit); - } - - //! Need apply correction - bool needApplyTorques() { - return (m_enableMotor || m_currentLimit != 0); - } - - //! calculates error - /*! - calculates m_currentLimit and m_currentLimitError. - */ - int testLimitValue(real_t test_value); - - //! apply the correction impulses for two bodies - real_t solveAngularLimits(real_t timeStep, Vector3 &axis, real_t jacDiagABInv, BodySW *body0, BodySW *body1); -}; - -class G6DOFTranslationalLimitMotorSW { -public: - Vector3 m_lowerLimit; //!< the constraint lower limits - Vector3 m_upperLimit; //!< the constraint upper limits - Vector3 m_accumulatedImpulse; - //! Linear_Limit_parameters - //!@{ - Vector3 m_limitSoftness; //!< Softness for linear limit - Vector3 m_damping; //!< Damping for linear limit - Vector3 m_restitution; //! Bounce parameter for linear limit - //!@} - bool enable_limit[3]; - - G6DOFTranslationalLimitMotorSW() { - m_lowerLimit = Vector3(0.f, 0.f, 0.f); - m_upperLimit = Vector3(0.f, 0.f, 0.f); - m_accumulatedImpulse = Vector3(0.f, 0.f, 0.f); - - m_limitSoftness = Vector3(1, 1, 1) * 0.7f; - m_damping = Vector3(1, 1, 1) * real_t(1.0f); - m_restitution = Vector3(1, 1, 1) * real_t(0.5f); - - enable_limit[0] = true; - enable_limit[1] = true; - enable_limit[2] = true; - } - - G6DOFTranslationalLimitMotorSW(const G6DOFTranslationalLimitMotorSW &other) { - m_lowerLimit = other.m_lowerLimit; - m_upperLimit = other.m_upperLimit; - m_accumulatedImpulse = other.m_accumulatedImpulse; - - m_limitSoftness = other.m_limitSoftness; - m_damping = other.m_damping; - m_restitution = other.m_restitution; - } - - //! Test limit - /*! - - free means upper < lower, - - locked means upper == lower - - limited means upper > lower - - limitIndex: first 3 are linear, next 3 are angular - */ - inline bool isLimited(int limitIndex) { - return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]); - } - - real_t solveLinearAxis( - real_t timeStep, - real_t jacDiagABInv, - BodySW *body1, const Vector3 &pointInA, - BodySW *body2, const Vector3 &pointInB, - int limit_index, - const Vector3 &axis_normal_on_a, - const Vector3 &anchorPos); -}; - -class Generic6DOFJointSW : public JointSW { -protected: - union { - struct { - BodySW *A; - BodySW *B; - }; - - BodySW *_arr[2]; - }; - - //! relative_frames - //!@{ - Transform m_frameInA; //!< the constraint space w.r.t body A - Transform m_frameInB; //!< the constraint space w.r.t body B - //!@} - - //! Jacobians - //!@{ - JacobianEntrySW m_jacLinear[3]; //!< 3 orthogonal linear constraints - JacobianEntrySW m_jacAng[3]; //!< 3 orthogonal angular constraints - //!@} - - //! Linear_Limit_parameters - //!@{ - G6DOFTranslationalLimitMotorSW m_linearLimits; - //!@} - - //! hinge_parameters - //!@{ - G6DOFRotationalLimitMotorSW m_angularLimits[3]; - //!@} - -protected: - //! temporal variables - //!@{ - real_t m_timeStep; - Transform m_calculatedTransformA; - Transform m_calculatedTransformB; - Vector3 m_calculatedAxisAngleDiff; - Vector3 m_calculatedAxis[3]; - - Vector3 m_AnchorPos; // point between pivots of bodies A and B to solve linear axes - - bool m_useLinearReferenceFrameA; - - //!@} - - Generic6DOFJointSW &operator=(Generic6DOFJointSW &other) { - ERR_PRINT("pito"); - (void)other; - return *this; - } - - void buildLinearJacobian( - JacobianEntrySW &jacLinear, const Vector3 &normalWorld, - const Vector3 &pivotAInW, const Vector3 &pivotBInW); - - void buildAngularJacobian(JacobianEntrySW &jacAngular, const Vector3 &jointAxisW); - - //! calcs the euler angles between the two bodies. - void calculateAngleInfo(); - -public: - Generic6DOFJointSW(BodySW *rbA, BodySW *rbB, const Transform &frameInA, const Transform &frameInB, bool useLinearReferenceFrameA); - - virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_6DOF; } - - virtual bool setup(real_t p_timestep); - virtual void solve(real_t p_timestep); - - //! Calcs global transform of the offsets - /*! - Calcs the global transform for the joint offset for body A an B, and also calcs the agle differences between the bodies. - \sa Generic6DOFJointSW.getCalculatedTransformA , Generic6DOFJointSW.getCalculatedTransformB, Generic6DOFJointSW.calculateAngleInfo - */ - void calculateTransforms(); - - //! Gets the global transform of the offset for body A - /*! - \sa Generic6DOFJointSW.getFrameOffsetA, Generic6DOFJointSW.getFrameOffsetB, Generic6DOFJointSW.calculateAngleInfo. - */ - const Transform &getCalculatedTransformA() const { - return m_calculatedTransformA; - } - - //! Gets the global transform of the offset for body B - /*! - \sa Generic6DOFJointSW.getFrameOffsetA, Generic6DOFJointSW.getFrameOffsetB, Generic6DOFJointSW.calculateAngleInfo. - */ - const Transform &getCalculatedTransformB() const { - return m_calculatedTransformB; - } - - const Transform &getFrameOffsetA() const { - return m_frameInA; - } - - const Transform &getFrameOffsetB() const { - return m_frameInB; - } - - Transform &getFrameOffsetA() { - return m_frameInA; - } - - Transform &getFrameOffsetB() { - return m_frameInB; - } - - //! performs Jacobian calculation, and also calculates angle differences and axis - - void updateRHS(real_t timeStep); - - //! Get the rotation axis in global coordinates - /*! - \pre Generic6DOFJointSW.buildJacobian must be called previously. - */ - Vector3 getAxis(int axis_index) const; - - //! Get the relative Euler angle - /*! - \pre Generic6DOFJointSW.buildJacobian must be called previously. - */ - real_t getAngle(int axis_index) const; - - //! Test angular limit. - /*! - Calculates angular correction and returns true if limit needs to be corrected. - \pre Generic6DOFJointSW.buildJacobian must be called previously. - */ - bool testAngularLimitMotor(int axis_index); - - void setLinearLowerLimit(const Vector3 &linearLower) { - m_linearLimits.m_lowerLimit = linearLower; - } - - void setLinearUpperLimit(const Vector3 &linearUpper) { - m_linearLimits.m_upperLimit = linearUpper; - } - - void setAngularLowerLimit(const Vector3 &angularLower) { - m_angularLimits[0].m_loLimit = angularLower.x; - m_angularLimits[1].m_loLimit = angularLower.y; - m_angularLimits[2].m_loLimit = angularLower.z; - } - - void setAngularUpperLimit(const Vector3 &angularUpper) { - m_angularLimits[0].m_hiLimit = angularUpper.x; - m_angularLimits[1].m_hiLimit = angularUpper.y; - m_angularLimits[2].m_hiLimit = angularUpper.z; - } - - //! Retrieves the angular limit informacion - G6DOFRotationalLimitMotorSW *getRotationalLimitMotor(int index) { - return &m_angularLimits[index]; - } - - //! Retrieves the limit informacion - G6DOFTranslationalLimitMotorSW *getTranslationalLimitMotor() { - return &m_linearLimits; - } - - //first 3 are linear, next 3 are angular - void setLimit(int axis, real_t lo, real_t hi) { - if (axis < 3) { - m_linearLimits.m_lowerLimit[axis] = lo; - m_linearLimits.m_upperLimit[axis] = hi; - } else { - m_angularLimits[axis - 3].m_loLimit = lo; - m_angularLimits[axis - 3].m_hiLimit = hi; - } - } - - //! Test limit - /*! - - free means upper < lower, - - locked means upper == lower - - limited means upper > lower - - limitIndex: first 3 are linear, next 3 are angular - */ - bool isLimited(int limitIndex) { - if (limitIndex < 3) { - return m_linearLimits.isLimited(limitIndex); - } - return m_angularLimits[limitIndex - 3].isLimited(); - } - - const BodySW *getRigidBodyA() const { - return A; - } - const BodySW *getRigidBodyB() const { - return B; - } - - virtual void calcAnchorPos(void); // overridable - - void set_param(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisParam p_param, real_t p_value); - real_t get_param(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisParam p_param) const; - - void set_flag(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisFlag p_flag, bool p_value); - bool get_flag(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisFlag p_flag) const; -}; - -#endif // GENERIC_6DOF_JOINT_SW_H diff --git a/servers/physics/joints/hinge_joint_sw.cpp b/servers/physics/joints/hinge_joint_sw.cpp deleted file mode 100644 index 1ad3e738ba..0000000000 --- a/servers/physics/joints/hinge_joint_sw.cpp +++ /dev/null @@ -1,450 +0,0 @@ -/*************************************************************************/ -/* hinge_joint_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 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. */ -/*************************************************************************/ - -/* -Adapted to Godot from the Bullet library. -*/ - -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "hinge_joint_sw.h" - -static void plane_space(const Vector3 &n, Vector3 &p, Vector3 &q) { - - if (Math::abs(n.z) > Math_SQRT12) { - // choose p in y-z plane - real_t a = n[1] * n[1] + n[2] * n[2]; - real_t k = 1.0 / Math::sqrt(a); - p = Vector3(0, -n[2] * k, n[1] * k); - // set q = n x p - q = Vector3(a * k, -n[0] * p[2], n[0] * p[1]); - } else { - // choose p in x-y plane - real_t a = n.x * n.x + n.y * n.y; - real_t k = 1.0 / Math::sqrt(a); - p = Vector3(-n.y * k, n.x * k, 0); - // set q = n x p - q = Vector3(-n.z * p.y, n.z * p.x, a * k); - } -} - -HingeJointSW::HingeJointSW(BodySW *rbA, BodySW *rbB, const Transform &frameA, const Transform &frameB) : - JointSW(_arr, 2) { - - A = rbA; - B = rbB; - - m_rbAFrame = frameA; - m_rbBFrame = frameB; - // flip axis - m_rbBFrame.basis[0][2] *= real_t(-1.); - m_rbBFrame.basis[1][2] *= real_t(-1.); - m_rbBFrame.basis[2][2] *= real_t(-1.); - - //start with free - m_lowerLimit = Math_PI; - m_upperLimit = -Math_PI; - - m_useLimit = false; - m_biasFactor = 0.3f; - m_relaxationFactor = 1.0f; - m_limitSoftness = 0.9f; - m_solveLimit = false; - - tau = 0.3; - - m_angularOnly = false; - m_enableAngularMotor = false; - - A->add_constraint(this, 0); - B->add_constraint(this, 1); -} - -HingeJointSW::HingeJointSW(BodySW *rbA, BodySW *rbB, const Vector3 &pivotInA, const Vector3 &pivotInB, - const Vector3 &axisInA, const Vector3 &axisInB) : - JointSW(_arr, 2) { - - A = rbA; - B = rbB; - - m_rbAFrame.origin = pivotInA; - - // since no frame is given, assume this to be zero angle and just pick rb transform axis - Vector3 rbAxisA1 = rbA->get_transform().basis.get_axis(0); - - Vector3 rbAxisA2; - real_t projection = axisInA.dot(rbAxisA1); - if (projection >= 1.0f - CMP_EPSILON) { - rbAxisA1 = -rbA->get_transform().basis.get_axis(2); - rbAxisA2 = rbA->get_transform().basis.get_axis(1); - } else if (projection <= -1.0f + CMP_EPSILON) { - rbAxisA1 = rbA->get_transform().basis.get_axis(2); - rbAxisA2 = rbA->get_transform().basis.get_axis(1); - } else { - rbAxisA2 = axisInA.cross(rbAxisA1); - rbAxisA1 = rbAxisA2.cross(axisInA); - } - - m_rbAFrame.basis = Basis(rbAxisA1.x, rbAxisA2.x, axisInA.x, - rbAxisA1.y, rbAxisA2.y, axisInA.y, - rbAxisA1.z, rbAxisA2.z, axisInA.z); - - Quat rotationArc = Quat(axisInA, axisInB); - Vector3 rbAxisB1 = rotationArc.xform(rbAxisA1); - Vector3 rbAxisB2 = axisInB.cross(rbAxisB1); - - m_rbBFrame.origin = pivotInB; - m_rbBFrame.basis = Basis(rbAxisB1.x, rbAxisB2.x, -axisInB.x, - rbAxisB1.y, rbAxisB2.y, -axisInB.y, - rbAxisB1.z, rbAxisB2.z, -axisInB.z); - - //start with free - m_lowerLimit = Math_PI; - m_upperLimit = -Math_PI; - - m_useLimit = false; - m_biasFactor = 0.3f; - m_relaxationFactor = 1.0f; - m_limitSoftness = 0.9f; - m_solveLimit = false; - - tau = 0.3; - - m_angularOnly = false; - m_enableAngularMotor = false; - - A->add_constraint(this, 0); - B->add_constraint(this, 1); -} - -bool HingeJointSW::setup(real_t p_step) { - - m_appliedImpulse = real_t(0.); - - if (!m_angularOnly) { - Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin); - Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin); - Vector3 relPos = pivotBInW - pivotAInW; - - Vector3 normal[3]; - if (Math::is_zero_approx(relPos.length_squared())) { - normal[0] = Vector3(real_t(1.0), 0, 0); - } else { - normal[0] = relPos.normalized(); - } - - plane_space(normal[0], normal[1], normal[2]); - - for (int i = 0; i < 3; i++) { - memnew_placement(&m_jac[i], JacobianEntrySW( - A->get_principal_inertia_axes().transposed(), - B->get_principal_inertia_axes().transposed(), - pivotAInW - A->get_transform().origin - A->get_center_of_mass(), - pivotBInW - B->get_transform().origin - B->get_center_of_mass(), - normal[i], - A->get_inv_inertia(), - A->get_inv_mass(), - B->get_inv_inertia(), - B->get_inv_mass())); - } - } - - //calculate two perpendicular jointAxis, orthogonal to hingeAxis - //these two jointAxis require equal angular velocities for both bodies - - //this is unused for now, it's a todo - Vector3 jointAxis0local; - Vector3 jointAxis1local; - - plane_space(m_rbAFrame.basis.get_axis(2), jointAxis0local, jointAxis1local); - - Vector3 jointAxis0 = A->get_transform().basis.xform(jointAxis0local); - Vector3 jointAxis1 = A->get_transform().basis.xform(jointAxis1local); - Vector3 hingeAxisWorld = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(2)); - - memnew_placement(&m_jacAng[0], JacobianEntrySW(jointAxis0, - A->get_principal_inertia_axes().transposed(), - B->get_principal_inertia_axes().transposed(), - A->get_inv_inertia(), - B->get_inv_inertia())); - - memnew_placement(&m_jacAng[1], JacobianEntrySW(jointAxis1, - A->get_principal_inertia_axes().transposed(), - B->get_principal_inertia_axes().transposed(), - A->get_inv_inertia(), - B->get_inv_inertia())); - - memnew_placement(&m_jacAng[2], JacobianEntrySW(hingeAxisWorld, - A->get_principal_inertia_axes().transposed(), - B->get_principal_inertia_axes().transposed(), - A->get_inv_inertia(), - B->get_inv_inertia())); - - // Compute limit information - real_t hingeAngle = get_hinge_angle(); - - //set bias, sign, clear accumulator - m_correction = real_t(0.); - m_limitSign = real_t(0.); - m_solveLimit = false; - m_accLimitImpulse = real_t(0.); - - //if (m_lowerLimit < m_upperLimit) - if (m_useLimit && m_lowerLimit <= m_upperLimit) { - //if (hingeAngle <= m_lowerLimit*m_limitSoftness) - if (hingeAngle <= m_lowerLimit) { - m_correction = (m_lowerLimit - hingeAngle); - m_limitSign = 1.0f; - m_solveLimit = true; - } - //else if (hingeAngle >= m_upperLimit*m_limitSoftness) - else if (hingeAngle >= m_upperLimit) { - m_correction = m_upperLimit - hingeAngle; - m_limitSign = -1.0f; - m_solveLimit = true; - } - } - - //Compute K = J*W*J' for hinge axis - Vector3 axisA = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(2)); - m_kHinge = 1.0f / (A->compute_angular_impulse_denominator(axisA) + - B->compute_angular_impulse_denominator(axisA)); - - return true; -} - -void HingeJointSW::solve(real_t p_step) { - - Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin); - Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin); - - //real_t tau = real_t(0.3); - - //linear part - if (!m_angularOnly) { - Vector3 rel_pos1 = pivotAInW - A->get_transform().origin; - Vector3 rel_pos2 = pivotBInW - B->get_transform().origin; - - Vector3 vel1 = A->get_velocity_in_local_point(rel_pos1); - Vector3 vel2 = B->get_velocity_in_local_point(rel_pos2); - Vector3 vel = vel1 - vel2; - - for (int i = 0; i < 3; i++) { - const Vector3 &normal = m_jac[i].m_linearJointAxis; - real_t jacDiagABInv = real_t(1.) / m_jac[i].getDiagonal(); - - real_t rel_vel; - rel_vel = normal.dot(vel); - //positional error (zeroth order error) - real_t depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal - real_t impulse = depth * tau / p_step * jacDiagABInv - rel_vel * jacDiagABInv; - m_appliedImpulse += impulse; - Vector3 impulse_vector = normal * impulse; - A->apply_impulse(pivotAInW - A->get_transform().origin, impulse_vector); - B->apply_impulse(pivotBInW - B->get_transform().origin, -impulse_vector); - } - } - - { - ///solve angular part - - // get axes in world space - Vector3 axisA = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(2)); - Vector3 axisB = B->get_transform().basis.xform(m_rbBFrame.basis.get_axis(2)); - - const Vector3 &angVelA = A->get_angular_velocity(); - const Vector3 &angVelB = B->get_angular_velocity(); - - Vector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA); - Vector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB); - - Vector3 angAorthog = angVelA - angVelAroundHingeAxisA; - Vector3 angBorthog = angVelB - angVelAroundHingeAxisB; - Vector3 velrelOrthog = angAorthog - angBorthog; - { - //solve orthogonal angular velocity correction - real_t relaxation = real_t(1.); - real_t len = velrelOrthog.length(); - if (len > real_t(0.00001)) { - Vector3 normal = velrelOrthog.normalized(); - real_t denom = A->compute_angular_impulse_denominator(normal) + - B->compute_angular_impulse_denominator(normal); - // scale for mass and relaxation - velrelOrthog *= (real_t(1.) / denom) * m_relaxationFactor; - } - - //solve angular positional correction - Vector3 angularError = -axisA.cross(axisB) * (real_t(1.) / p_step); - real_t len2 = angularError.length(); - if (len2 > real_t(0.00001)) { - Vector3 normal2 = angularError.normalized(); - real_t denom2 = A->compute_angular_impulse_denominator(normal2) + - B->compute_angular_impulse_denominator(normal2); - angularError *= (real_t(1.) / denom2) * relaxation; - } - - A->apply_torque_impulse(-velrelOrthog + angularError); - B->apply_torque_impulse(velrelOrthog - angularError); - - // solve limit - if (m_solveLimit) { - real_t amplitude = ((angVelB - angVelA).dot(axisA) * m_relaxationFactor + m_correction * (real_t(1.) / p_step) * m_biasFactor) * m_limitSign; - - real_t impulseMag = amplitude * m_kHinge; - - // Clamp the accumulated impulse - real_t temp = m_accLimitImpulse; - m_accLimitImpulse = MAX(m_accLimitImpulse + impulseMag, real_t(0)); - impulseMag = m_accLimitImpulse - temp; - - Vector3 impulse = axisA * impulseMag * m_limitSign; - A->apply_torque_impulse(impulse); - B->apply_torque_impulse(-impulse); - } - } - - //apply motor - if (m_enableAngularMotor) { - //todo: add limits too - Vector3 angularLimit(0, 0, 0); - - Vector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB; - real_t projRelVel = velrel.dot(axisA); - - real_t desiredMotorVel = m_motorTargetVelocity; - real_t motor_relvel = desiredMotorVel - projRelVel; - - real_t unclippedMotorImpulse = m_kHinge * motor_relvel; - //todo: should clip against accumulated impulse - real_t clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse; - clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse; - Vector3 motorImp = clippedMotorImpulse * axisA; - - A->apply_torque_impulse(motorImp + angularLimit); - B->apply_torque_impulse(-motorImp - angularLimit); - } - } -} -/* -void HingeJointSW::updateRHS(real_t timeStep) -{ - (void)timeStep; - -} -*/ - -static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x) { - real_t coeff_1 = Math_PI / 4.0f; - real_t coeff_2 = 3.0f * coeff_1; - real_t abs_y = Math::abs(y); - real_t angle; - if (x >= 0.0f) { - real_t r = (x - abs_y) / (x + abs_y); - angle = coeff_1 - coeff_1 * r; - } else { - real_t r = (x + abs_y) / (abs_y - x); - angle = coeff_2 - coeff_1 * r; - } - return (y < 0.0f) ? -angle : angle; -} - -real_t HingeJointSW::get_hinge_angle() { - const Vector3 refAxis0 = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(0)); - const Vector3 refAxis1 = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(1)); - const Vector3 swingAxis = B->get_transform().basis.xform(m_rbBFrame.basis.get_axis(1)); - - return atan2fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)); -} - -void HingeJointSW::set_param(PhysicsServer::HingeJointParam p_param, real_t p_value) { - - switch (p_param) { - - case PhysicsServer::HINGE_JOINT_BIAS: tau = p_value; break; - case PhysicsServer::HINGE_JOINT_LIMIT_UPPER: m_upperLimit = p_value; break; - case PhysicsServer::HINGE_JOINT_LIMIT_LOWER: m_lowerLimit = p_value; break; - case PhysicsServer::HINGE_JOINT_LIMIT_BIAS: m_biasFactor = p_value; break; - case PhysicsServer::HINGE_JOINT_LIMIT_SOFTNESS: m_limitSoftness = p_value; break; - case PhysicsServer::HINGE_JOINT_LIMIT_RELAXATION: m_relaxationFactor = p_value; break; - case PhysicsServer::HINGE_JOINT_MOTOR_TARGET_VELOCITY: m_motorTargetVelocity = p_value; break; - case PhysicsServer::HINGE_JOINT_MOTOR_MAX_IMPULSE: m_maxMotorImpulse = p_value; break; - case PhysicsServer::HINGE_JOINT_MAX: break; // Can't happen, but silences warning - } -} - -real_t HingeJointSW::get_param(PhysicsServer::HingeJointParam p_param) const { - - switch (p_param) { - - case PhysicsServer::HINGE_JOINT_BIAS: return tau; - case PhysicsServer::HINGE_JOINT_LIMIT_UPPER: return m_upperLimit; - case PhysicsServer::HINGE_JOINT_LIMIT_LOWER: return m_lowerLimit; - case PhysicsServer::HINGE_JOINT_LIMIT_BIAS: return m_biasFactor; - case PhysicsServer::HINGE_JOINT_LIMIT_SOFTNESS: return m_limitSoftness; - case PhysicsServer::HINGE_JOINT_LIMIT_RELAXATION: return m_relaxationFactor; - case PhysicsServer::HINGE_JOINT_MOTOR_TARGET_VELOCITY: return m_motorTargetVelocity; - case PhysicsServer::HINGE_JOINT_MOTOR_MAX_IMPULSE: return m_maxMotorImpulse; - case PhysicsServer::HINGE_JOINT_MAX: break; // Can't happen, but silences warning - } - - return 0; -} - -void HingeJointSW::set_flag(PhysicsServer::HingeJointFlag p_flag, bool p_value) { - - switch (p_flag) { - case PhysicsServer::HINGE_JOINT_FLAG_USE_LIMIT: m_useLimit = p_value; break; - case PhysicsServer::HINGE_JOINT_FLAG_ENABLE_MOTOR: m_enableAngularMotor = p_value; break; - case PhysicsServer::HINGE_JOINT_FLAG_MAX: break; // Can't happen, but silences warning - } -} -bool HingeJointSW::get_flag(PhysicsServer::HingeJointFlag p_flag) const { - - switch (p_flag) { - case PhysicsServer::HINGE_JOINT_FLAG_USE_LIMIT: return m_useLimit; - case PhysicsServer::HINGE_JOINT_FLAG_ENABLE_MOTOR: return m_enableAngularMotor; - case PhysicsServer::HINGE_JOINT_FLAG_MAX: break; // Can't happen, but silences warning - } - - return false; -} diff --git a/servers/physics/joints/hinge_joint_sw.h b/servers/physics/joints/hinge_joint_sw.h deleted file mode 100644 index 1c160cfc09..0000000000 --- a/servers/physics/joints/hinge_joint_sw.h +++ /dev/null @@ -1,117 +0,0 @@ -/*************************************************************************/ -/* hinge_joint_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 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. */ -/*************************************************************************/ - -/* -Adapted to Godot from the Bullet library. -*/ - -#ifndef HINGE_JOINT_SW_H -#define HINGE_JOINT_SW_H - -#include "servers/physics/joints/jacobian_entry_sw.h" -#include "servers/physics/joints_sw.h" - -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -class HingeJointSW : public JointSW { - - union { - struct { - BodySW *A; - BodySW *B; - }; - - BodySW *_arr[2]; - }; - - JacobianEntrySW m_jac[3]; //3 orthogonal linear constraints - JacobianEntrySW m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor - - Transform m_rbAFrame; // constraint axii. Assumes z is hinge axis. - Transform m_rbBFrame; - - real_t m_motorTargetVelocity; - real_t m_maxMotorImpulse; - - real_t m_limitSoftness; - real_t m_biasFactor; - real_t m_relaxationFactor; - - real_t m_lowerLimit; - real_t m_upperLimit; - - real_t m_kHinge; - - real_t m_limitSign; - real_t m_correction; - - real_t m_accLimitImpulse; - - real_t tau; - - bool m_useLimit; - bool m_angularOnly; - bool m_enableAngularMotor; - bool m_solveLimit; - - real_t m_appliedImpulse; - -public: - virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_HINGE; } - - virtual bool setup(real_t p_step); - virtual void solve(real_t p_step); - - real_t get_hinge_angle(); - - void set_param(PhysicsServer::HingeJointParam p_param, real_t p_value); - real_t get_param(PhysicsServer::HingeJointParam p_param) const; - - void set_flag(PhysicsServer::HingeJointFlag p_flag, bool p_value); - bool get_flag(PhysicsServer::HingeJointFlag p_flag) const; - - HingeJointSW(BodySW *rbA, BodySW *rbB, const Transform &frameA, const Transform &frameB); - HingeJointSW(BodySW *rbA, BodySW *rbB, const Vector3 &pivotInA, const Vector3 &pivotInB, const Vector3 &axisInA, const Vector3 &axisInB); -}; - -#endif // HINGE_JOINT_SW_H diff --git a/servers/physics/joints/jacobian_entry_sw.h b/servers/physics/joints/jacobian_entry_sw.h deleted file mode 100644 index a17175e6de..0000000000 --- a/servers/physics/joints/jacobian_entry_sw.h +++ /dev/null @@ -1,169 +0,0 @@ -/*************************************************************************/ -/* jacobian_entry_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 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. */ -/*************************************************************************/ - -/* -Adapted to Godot from the Bullet library. -*/ - -#ifndef JACOBIAN_ENTRY_SW_H -#define JACOBIAN_ENTRY_SW_H - -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "core/math/transform.h" - -class JacobianEntrySW { -public: - JacobianEntrySW(){}; - //constraint between two different rigidbodies - JacobianEntrySW( - const Basis &world2A, - const Basis &world2B, - const Vector3 &rel_pos1, const Vector3 &rel_pos2, - const Vector3 &jointAxis, - const Vector3 &inertiaInvA, - const real_t massInvA, - const Vector3 &inertiaInvB, - const real_t massInvB) : - m_linearJointAxis(jointAxis) { - m_aJ = world2A.xform(rel_pos1.cross(m_linearJointAxis)); - m_bJ = world2B.xform(rel_pos2.cross(-m_linearJointAxis)); - m_0MinvJt = inertiaInvA * m_aJ; - m_1MinvJt = inertiaInvB * m_bJ; - m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ); - - ERR_FAIL_COND(m_Adiag <= real_t(0.0)); - } - - //angular constraint between two different rigidbodies - JacobianEntrySW(const Vector3 &jointAxis, - const Basis &world2A, - const Basis &world2B, - const Vector3 &inertiaInvA, - const Vector3 &inertiaInvB) : - m_linearJointAxis(Vector3(real_t(0.), real_t(0.), real_t(0.))) { - m_aJ = world2A.xform(jointAxis); - m_bJ = world2B.xform(-jointAxis); - m_0MinvJt = inertiaInvA * m_aJ; - m_1MinvJt = inertiaInvB * m_bJ; - m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ); - - ERR_FAIL_COND(m_Adiag <= real_t(0.0)); - } - - //angular constraint between two different rigidbodies - JacobianEntrySW(const Vector3 &axisInA, - const Vector3 &axisInB, - const Vector3 &inertiaInvA, - const Vector3 &inertiaInvB) : - m_linearJointAxis(Vector3(real_t(0.), real_t(0.), real_t(0.))), - m_aJ(axisInA), - m_bJ(-axisInB) { - m_0MinvJt = inertiaInvA * m_aJ; - m_1MinvJt = inertiaInvB * m_bJ; - m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ); - - ERR_FAIL_COND(m_Adiag <= real_t(0.0)); - } - - //constraint on one rigidbody - JacobianEntrySW( - const Basis &world2A, - const Vector3 &rel_pos1, const Vector3 &rel_pos2, - const Vector3 &jointAxis, - const Vector3 &inertiaInvA, - const real_t massInvA) : - m_linearJointAxis(jointAxis) { - m_aJ = world2A.xform(rel_pos1.cross(jointAxis)); - m_bJ = world2A.xform(rel_pos2.cross(-jointAxis)); - m_0MinvJt = inertiaInvA * m_aJ; - m_1MinvJt = Vector3(real_t(0.), real_t(0.), real_t(0.)); - m_Adiag = massInvA + m_0MinvJt.dot(m_aJ); - - ERR_FAIL_COND(m_Adiag <= real_t(0.0)); - } - - real_t getDiagonal() const { return m_Adiag; } - - // for two constraints on the same rigidbody (for example vehicle friction) - real_t getNonDiagonal(const JacobianEntrySW &jacB, const real_t massInvA) const { - const JacobianEntrySW &jacA = *this; - real_t lin = massInvA * jacA.m_linearJointAxis.dot(jacB.m_linearJointAxis); - real_t ang = jacA.m_0MinvJt.dot(jacB.m_aJ); - return lin + ang; - } - - // for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies) - real_t getNonDiagonal(const JacobianEntrySW &jacB, const real_t massInvA, const real_t massInvB) const { - const JacobianEntrySW &jacA = *this; - Vector3 lin = jacA.m_linearJointAxis * jacB.m_linearJointAxis; - Vector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ; - Vector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ; - Vector3 lin0 = massInvA * lin; - Vector3 lin1 = massInvB * lin; - Vector3 sum = ang0 + ang1 + lin0 + lin1; - return sum[0] + sum[1] + sum[2]; - } - - real_t getRelativeVelocity(const Vector3 &linvelA, const Vector3 &angvelA, const Vector3 &linvelB, const Vector3 &angvelB) { - Vector3 linrel = linvelA - linvelB; - Vector3 angvela = angvelA * m_aJ; - Vector3 angvelb = angvelB * m_bJ; - linrel *= m_linearJointAxis; - angvela += angvelb; - angvela += linrel; - real_t rel_vel2 = angvela[0] + angvela[1] + angvela[2]; - return rel_vel2 + CMP_EPSILON; - } - //private: - - Vector3 m_linearJointAxis; - Vector3 m_aJ; - Vector3 m_bJ; - Vector3 m_0MinvJt; - Vector3 m_1MinvJt; - //Optimization: can be stored in the w/last component of one of the vectors - real_t m_Adiag; -}; - -#endif // JACOBIAN_ENTRY_SW_H diff --git a/servers/physics/joints/pin_joint_sw.cpp b/servers/physics/joints/pin_joint_sw.cpp deleted file mode 100644 index fe994aa172..0000000000 --- a/servers/physics/joints/pin_joint_sw.cpp +++ /dev/null @@ -1,167 +0,0 @@ -/*************************************************************************/ -/* pin_joint_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 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. */ -/*************************************************************************/ - -/* -Adapted to Godot from the Bullet library. -*/ - -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "pin_joint_sw.h" - -bool PinJointSW::setup(real_t p_step) { - - m_appliedImpulse = real_t(0.); - - Vector3 normal(0, 0, 0); - - for (int i = 0; i < 3; i++) { - normal[i] = 1; - memnew_placement(&m_jac[i], JacobianEntrySW( - A->get_principal_inertia_axes().transposed(), - B->get_principal_inertia_axes().transposed(), - A->get_transform().xform(m_pivotInA) - A->get_transform().origin - A->get_center_of_mass(), - B->get_transform().xform(m_pivotInB) - B->get_transform().origin - B->get_center_of_mass(), - normal, - A->get_inv_inertia(), - A->get_inv_mass(), - B->get_inv_inertia(), - B->get_inv_mass())); - normal[i] = 0; - } - - return true; -} - -void PinJointSW::solve(real_t p_step) { - - Vector3 pivotAInW = A->get_transform().xform(m_pivotInA); - Vector3 pivotBInW = B->get_transform().xform(m_pivotInB); - - Vector3 normal(0, 0, 0); - - //Vector3 angvelA = A->get_transform().origin.getBasis().transpose() * A->getAngularVelocity(); - //Vector3 angvelB = B->get_transform().origin.getBasis().transpose() * B->getAngularVelocity(); - - for (int i = 0; i < 3; i++) { - normal[i] = 1; - real_t jacDiagABInv = real_t(1.) / m_jac[i].getDiagonal(); - - Vector3 rel_pos1 = pivotAInW - A->get_transform().origin; - Vector3 rel_pos2 = pivotBInW - B->get_transform().origin; - //this jacobian entry could be re-used for all iterations - - Vector3 vel1 = A->get_velocity_in_local_point(rel_pos1); - Vector3 vel2 = B->get_velocity_in_local_point(rel_pos2); - Vector3 vel = vel1 - vel2; - - real_t rel_vel; - rel_vel = normal.dot(vel); - - /* - //velocity error (first order error) - real_t rel_vel = m_jac[i].getRelativeVelocity(A->getLinearVelocity(),angvelA, - B->getLinearVelocity(),angvelB); - */ - - //positional error (zeroth order error) - real_t depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal - - real_t impulse = depth * m_tau / p_step * jacDiagABInv - m_damping * rel_vel * jacDiagABInv; - - real_t impulseClamp = m_impulseClamp; - if (impulseClamp > 0) { - if (impulse < -impulseClamp) - impulse = -impulseClamp; - if (impulse > impulseClamp) - impulse = impulseClamp; - } - - m_appliedImpulse += impulse; - Vector3 impulse_vector = normal * impulse; - A->apply_impulse(pivotAInW - A->get_transform().origin, impulse_vector); - B->apply_impulse(pivotBInW - B->get_transform().origin, -impulse_vector); - - normal[i] = 0; - } -} - -void PinJointSW::set_param(PhysicsServer::PinJointParam p_param, real_t p_value) { - - switch (p_param) { - case PhysicsServer::PIN_JOINT_BIAS: m_tau = p_value; break; - case PhysicsServer::PIN_JOINT_DAMPING: m_damping = p_value; break; - case PhysicsServer::PIN_JOINT_IMPULSE_CLAMP: m_impulseClamp = p_value; break; - } -} - -real_t PinJointSW::get_param(PhysicsServer::PinJointParam p_param) const { - - switch (p_param) { - case PhysicsServer::PIN_JOINT_BIAS: return m_tau; - case PhysicsServer::PIN_JOINT_DAMPING: return m_damping; - case PhysicsServer::PIN_JOINT_IMPULSE_CLAMP: return m_impulseClamp; - } - - return 0; -} - -PinJointSW::PinJointSW(BodySW *p_body_a, const Vector3 &p_pos_a, BodySW *p_body_b, const Vector3 &p_pos_b) : - JointSW(_arr, 2) { - - A = p_body_a; - B = p_body_b; - m_pivotInA = p_pos_a; - m_pivotInB = p_pos_b; - - m_tau = 0.3; - m_damping = 1; - m_impulseClamp = 0; - m_appliedImpulse = 0; - - A->add_constraint(this, 0); - B->add_constraint(this, 1); -} - -PinJointSW::~PinJointSW() { -} diff --git a/servers/physics/joints/pin_joint_sw.h b/servers/physics/joints/pin_joint_sw.h deleted file mode 100644 index 42884e4940..0000000000 --- a/servers/physics/joints/pin_joint_sw.h +++ /dev/null @@ -1,96 +0,0 @@ -/*************************************************************************/ -/* pin_joint_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 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. */ -/*************************************************************************/ - -/* -Adapted to Godot from the Bullet library. -*/ - -#ifndef PIN_JOINT_SW_H -#define PIN_JOINT_SW_H - -#include "servers/physics/joints/jacobian_entry_sw.h" -#include "servers/physics/joints_sw.h" - -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -class PinJointSW : public JointSW { - - union { - struct { - BodySW *A; - BodySW *B; - }; - - BodySW *_arr[2]; - }; - - real_t m_tau; //bias - real_t m_damping; - real_t m_impulseClamp; - real_t m_appliedImpulse; - - JacobianEntrySW m_jac[3]; //3 orthogonal linear constraints - - Vector3 m_pivotInA; - Vector3 m_pivotInB; - -public: - virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_PIN; } - - virtual bool setup(real_t p_step); - virtual void solve(real_t p_step); - - void set_param(PhysicsServer::PinJointParam p_param, real_t p_value); - real_t get_param(PhysicsServer::PinJointParam p_param) const; - - void set_pos_a(const Vector3 &p_pos) { m_pivotInA = p_pos; } - void set_pos_b(const Vector3 &p_pos) { m_pivotInB = p_pos; } - - Vector3 get_position_a() { return m_pivotInA; } - Vector3 get_position_b() { return m_pivotInB; } - - PinJointSW(BodySW *p_body_a, const Vector3 &p_pos_a, BodySW *p_body_b, const Vector3 &p_pos_b); - ~PinJointSW(); -}; - -#endif // PIN_JOINT_SW_H diff --git a/servers/physics/joints/slider_joint_sw.cpp b/servers/physics/joints/slider_joint_sw.cpp deleted file mode 100644 index 9963c7ae89..0000000000 --- a/servers/physics/joints/slider_joint_sw.cpp +++ /dev/null @@ -1,443 +0,0 @@ -/*************************************************************************/ -/* slider_joint_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 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. */ -/*************************************************************************/ - -/* -Adapted to Godot from the Bullet library. -*/ - -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -/* -Added by Roman Ponomarev (rponom@gmail.com) -April 04, 2008 - -*/ - -#include "slider_joint_sw.h" - -//----------------------------------------------------------------------------- - -static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x) { - real_t coeff_1 = Math_PI / 4.0f; - real_t coeff_2 = 3.0f * coeff_1; - real_t abs_y = Math::abs(y); - real_t angle; - if (x >= 0.0f) { - real_t r = (x - abs_y) / (x + abs_y); - angle = coeff_1 - coeff_1 * r; - } else { - real_t r = (x + abs_y) / (abs_y - x); - angle = coeff_2 - coeff_1 * r; - } - return (y < 0.0f) ? -angle : angle; -} - -void SliderJointSW::initParams() { - m_lowerLinLimit = real_t(1.0); - m_upperLinLimit = real_t(-1.0); - m_lowerAngLimit = real_t(0.); - m_upperAngLimit = real_t(0.); - m_softnessDirLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionDirLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingDirLin = real_t(0.); - m_softnessDirAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionDirAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingDirAng = real_t(0.); - m_softnessOrthoLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionOrthoLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingOrthoLin = SLIDER_CONSTRAINT_DEF_DAMPING; - m_softnessOrthoAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionOrthoAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingOrthoAng = SLIDER_CONSTRAINT_DEF_DAMPING; - m_softnessLimLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionLimLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingLimLin = SLIDER_CONSTRAINT_DEF_DAMPING; - m_softnessLimAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionLimAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingLimAng = SLIDER_CONSTRAINT_DEF_DAMPING; - - m_poweredLinMotor = false; - m_targetLinMotorVelocity = real_t(0.); - m_maxLinMotorForce = real_t(0.); - m_accumulatedLinMotorImpulse = real_t(0.0); - - m_poweredAngMotor = false; - m_targetAngMotorVelocity = real_t(0.); - m_maxAngMotorForce = real_t(0.); - m_accumulatedAngMotorImpulse = real_t(0.0); - -} // SliderJointSW::initParams() - -//----------------------------------------------------------------------------- - -//----------------------------------------------------------------------------- - -SliderJointSW::SliderJointSW(BodySW *rbA, BodySW *rbB, const Transform &frameInA, const Transform &frameInB) : - JointSW(_arr, 2), - m_frameInA(frameInA), - m_frameInB(frameInB) { - - A = rbA; - B = rbB; - - A->add_constraint(this, 0); - B->add_constraint(this, 1); - - initParams(); -} // SliderJointSW::SliderJointSW() - -//----------------------------------------------------------------------------- - -bool SliderJointSW::setup(real_t p_step) { - - //calculate transforms - m_calculatedTransformA = A->get_transform() * m_frameInA; - m_calculatedTransformB = B->get_transform() * m_frameInB; - m_realPivotAInW = m_calculatedTransformA.origin; - m_realPivotBInW = m_calculatedTransformB.origin; - m_sliderAxis = m_calculatedTransformA.basis.get_axis(0); // along X - m_delta = m_realPivotBInW - m_realPivotAInW; - m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis; - m_relPosA = m_projPivotInW - A->get_transform().origin; - m_relPosB = m_realPivotBInW - B->get_transform().origin; - Vector3 normalWorld; - int i; - //linear part - for (i = 0; i < 3; i++) { - normalWorld = m_calculatedTransformA.basis.get_axis(i); - memnew_placement(&m_jacLin[i], JacobianEntrySW( - A->get_principal_inertia_axes().transposed(), - B->get_principal_inertia_axes().transposed(), - m_relPosA - A->get_center_of_mass(), - m_relPosB - B->get_center_of_mass(), - normalWorld, - A->get_inv_inertia(), - A->get_inv_mass(), - B->get_inv_inertia(), - B->get_inv_mass())); - m_jacLinDiagABInv[i] = real_t(1.) / m_jacLin[i].getDiagonal(); - m_depth[i] = m_delta.dot(normalWorld); - } - testLinLimits(); - // angular part - for (i = 0; i < 3; i++) { - normalWorld = m_calculatedTransformA.basis.get_axis(i); - memnew_placement(&m_jacAng[i], JacobianEntrySW( - normalWorld, - A->get_principal_inertia_axes().transposed(), - B->get_principal_inertia_axes().transposed(), - A->get_inv_inertia(), - B->get_inv_inertia())); - } - testAngLimits(); - Vector3 axisA = m_calculatedTransformA.basis.get_axis(0); - m_kAngle = real_t(1.0) / (A->compute_angular_impulse_denominator(axisA) + B->compute_angular_impulse_denominator(axisA)); - // clear accumulator for motors - m_accumulatedLinMotorImpulse = real_t(0.0); - m_accumulatedAngMotorImpulse = real_t(0.0); - - return true; -} // SliderJointSW::buildJacobianInt() - -//----------------------------------------------------------------------------- - -void SliderJointSW::solve(real_t p_step) { - - int i; - // linear - Vector3 velA = A->get_velocity_in_local_point(m_relPosA); - Vector3 velB = B->get_velocity_in_local_point(m_relPosB); - Vector3 vel = velA - velB; - for (i = 0; i < 3; i++) { - const Vector3 &normal = m_jacLin[i].m_linearJointAxis; - real_t rel_vel = normal.dot(vel); - // calculate positional error - real_t depth = m_depth[i]; - // get parameters - real_t softness = (i) ? m_softnessOrthoLin : (m_solveLinLim ? m_softnessLimLin : m_softnessDirLin); - real_t restitution = (i) ? m_restitutionOrthoLin : (m_solveLinLim ? m_restitutionLimLin : m_restitutionDirLin); - real_t damping = (i) ? m_dampingOrthoLin : (m_solveLinLim ? m_dampingLimLin : m_dampingDirLin); - // calcutate and apply impulse - real_t normalImpulse = softness * (restitution * depth / p_step - damping * rel_vel) * m_jacLinDiagABInv[i]; - Vector3 impulse_vector = normal * normalImpulse; - A->apply_impulse(m_relPosA, impulse_vector); - B->apply_impulse(m_relPosB, -impulse_vector); - if (m_poweredLinMotor && (!i)) { // apply linear motor - if (m_accumulatedLinMotorImpulse < m_maxLinMotorForce) { - real_t desiredMotorVel = m_targetLinMotorVelocity; - real_t motor_relvel = desiredMotorVel + rel_vel; - normalImpulse = -motor_relvel * m_jacLinDiagABInv[i]; - // clamp accumulated impulse - real_t new_acc = m_accumulatedLinMotorImpulse + Math::abs(normalImpulse); - if (new_acc > m_maxLinMotorForce) { - new_acc = m_maxLinMotorForce; - } - real_t del = new_acc - m_accumulatedLinMotorImpulse; - if (normalImpulse < real_t(0.0)) { - normalImpulse = -del; - } else { - normalImpulse = del; - } - m_accumulatedLinMotorImpulse = new_acc; - // apply clamped impulse - impulse_vector = normal * normalImpulse; - A->apply_impulse(m_relPosA, impulse_vector); - B->apply_impulse(m_relPosB, -impulse_vector); - } - } - } - // angular - // get axes in world space - Vector3 axisA = m_calculatedTransformA.basis.get_axis(0); - Vector3 axisB = m_calculatedTransformB.basis.get_axis(0); - - const Vector3 &angVelA = A->get_angular_velocity(); - const Vector3 &angVelB = B->get_angular_velocity(); - - Vector3 angVelAroundAxisA = axisA * axisA.dot(angVelA); - Vector3 angVelAroundAxisB = axisB * axisB.dot(angVelB); - - Vector3 angAorthog = angVelA - angVelAroundAxisA; - Vector3 angBorthog = angVelB - angVelAroundAxisB; - Vector3 velrelOrthog = angAorthog - angBorthog; - //solve orthogonal angular velocity correction - real_t len = velrelOrthog.length(); - if (len > real_t(0.00001)) { - Vector3 normal = velrelOrthog.normalized(); - real_t denom = A->compute_angular_impulse_denominator(normal) + B->compute_angular_impulse_denominator(normal); - velrelOrthog *= (real_t(1.) / denom) * m_dampingOrthoAng * m_softnessOrthoAng; - } - //solve angular positional correction - Vector3 angularError = axisA.cross(axisB) * (real_t(1.) / p_step); - real_t len2 = angularError.length(); - if (len2 > real_t(0.00001)) { - Vector3 normal2 = angularError.normalized(); - real_t denom2 = A->compute_angular_impulse_denominator(normal2) + B->compute_angular_impulse_denominator(normal2); - angularError *= (real_t(1.) / denom2) * m_restitutionOrthoAng * m_softnessOrthoAng; - } - // apply impulse - A->apply_torque_impulse(-velrelOrthog + angularError); - B->apply_torque_impulse(velrelOrthog - angularError); - real_t impulseMag; - //solve angular limits - if (m_solveAngLim) { - impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingLimAng + m_angDepth * m_restitutionLimAng / p_step; - impulseMag *= m_kAngle * m_softnessLimAng; - } else { - impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingDirAng + m_angDepth * m_restitutionDirAng / p_step; - impulseMag *= m_kAngle * m_softnessDirAng; - } - Vector3 impulse = axisA * impulseMag; - A->apply_torque_impulse(impulse); - B->apply_torque_impulse(-impulse); - //apply angular motor - if (m_poweredAngMotor) { - if (m_accumulatedAngMotorImpulse < m_maxAngMotorForce) { - Vector3 velrel = angVelAroundAxisA - angVelAroundAxisB; - real_t projRelVel = velrel.dot(axisA); - - real_t desiredMotorVel = m_targetAngMotorVelocity; - real_t motor_relvel = desiredMotorVel - projRelVel; - - real_t angImpulse = m_kAngle * motor_relvel; - // clamp accumulated impulse - real_t new_acc = m_accumulatedAngMotorImpulse + Math::abs(angImpulse); - if (new_acc > m_maxAngMotorForce) { - new_acc = m_maxAngMotorForce; - } - real_t del = new_acc - m_accumulatedAngMotorImpulse; - if (angImpulse < real_t(0.0)) { - angImpulse = -del; - } else { - angImpulse = del; - } - m_accumulatedAngMotorImpulse = new_acc; - // apply clamped impulse - Vector3 motorImp = angImpulse * axisA; - A->apply_torque_impulse(motorImp); - B->apply_torque_impulse(-motorImp); - } - } -} // SliderJointSW::solveConstraint() - -//----------------------------------------------------------------------------- - -//----------------------------------------------------------------------------- - -void SliderJointSW::calculateTransforms(void) { - m_calculatedTransformA = A->get_transform() * m_frameInA; - m_calculatedTransformB = B->get_transform() * m_frameInB; - m_realPivotAInW = m_calculatedTransformA.origin; - m_realPivotBInW = m_calculatedTransformB.origin; - m_sliderAxis = m_calculatedTransformA.basis.get_axis(0); // along X - m_delta = m_realPivotBInW - m_realPivotAInW; - m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis; - Vector3 normalWorld; - int i; - //linear part - for (i = 0; i < 3; i++) { - normalWorld = m_calculatedTransformA.basis.get_axis(i); - m_depth[i] = m_delta.dot(normalWorld); - } -} // SliderJointSW::calculateTransforms() - -//----------------------------------------------------------------------------- - -void SliderJointSW::testLinLimits(void) { - m_solveLinLim = false; - m_linPos = m_depth[0]; - if (m_lowerLinLimit <= m_upperLinLimit) { - if (m_depth[0] > m_upperLinLimit) { - m_depth[0] -= m_upperLinLimit; - m_solveLinLim = true; - } else if (m_depth[0] < m_lowerLinLimit) { - m_depth[0] -= m_lowerLinLimit; - m_solveLinLim = true; - } else { - m_depth[0] = real_t(0.); - } - } else { - m_depth[0] = real_t(0.); - } -} // SliderJointSW::testLinLimits() - -//----------------------------------------------------------------------------- - -void SliderJointSW::testAngLimits(void) { - m_angDepth = real_t(0.); - m_solveAngLim = false; - if (m_lowerAngLimit <= m_upperAngLimit) { - const Vector3 axisA0 = m_calculatedTransformA.basis.get_axis(1); - const Vector3 axisA1 = m_calculatedTransformA.basis.get_axis(2); - const Vector3 axisB0 = m_calculatedTransformB.basis.get_axis(1); - real_t rot = atan2fast(axisB0.dot(axisA1), axisB0.dot(axisA0)); - if (rot < m_lowerAngLimit) { - m_angDepth = rot - m_lowerAngLimit; - m_solveAngLim = true; - } else if (rot > m_upperAngLimit) { - m_angDepth = rot - m_upperAngLimit; - m_solveAngLim = true; - } - } -} // SliderJointSW::testAngLimits() - -//----------------------------------------------------------------------------- - -Vector3 SliderJointSW::getAncorInA(void) { - Vector3 ancorInA; - ancorInA = m_realPivotAInW + (m_lowerLinLimit + m_upperLinLimit) * real_t(0.5) * m_sliderAxis; - ancorInA = A->get_transform().inverse().xform(ancorInA); - return ancorInA; -} // SliderJointSW::getAncorInA() - -//----------------------------------------------------------------------------- - -Vector3 SliderJointSW::getAncorInB(void) { - Vector3 ancorInB; - ancorInB = m_frameInB.origin; - return ancorInB; -} // SliderJointSW::getAncorInB(); - -void SliderJointSW::set_param(PhysicsServer::SliderJointParam p_param, real_t p_value) { - - switch (p_param) { - case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_UPPER: m_upperLinLimit = p_value; break; - case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_LOWER: m_lowerLinLimit = p_value; break; - case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS: m_softnessLimLin = p_value; break; - case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION: m_restitutionLimLin = p_value; break; - case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_DAMPING: m_dampingLimLin = p_value; break; - case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_SOFTNESS: m_softnessDirLin = p_value; break; - case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_RESTITUTION: m_restitutionDirLin = p_value; break; - case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_DAMPING: m_dampingDirLin = p_value; break; - case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS: m_softnessOrthoLin = p_value; break; - case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION: m_restitutionOrthoLin = p_value; break; - case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING: m_dampingOrthoLin = p_value; break; - - case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_UPPER: m_upperAngLimit = p_value; break; - case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_LOWER: m_lowerAngLimit = p_value; break; - case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS: m_softnessLimAng = p_value; break; - case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION: m_restitutionLimAng = p_value; break; - case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_DAMPING: m_dampingLimAng = p_value; break; - case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS: m_softnessDirAng = p_value; break; - case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION: m_restitutionDirAng = p_value; break; - case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_DAMPING: m_dampingDirAng = p_value; break; - case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS: m_softnessOrthoAng = p_value; break; - case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION: m_restitutionOrthoAng = p_value; break; - case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING: m_dampingOrthoAng = p_value; break; - - case PhysicsServer::SLIDER_JOINT_MAX: break; // Can't happen, but silences warning - } -} - -real_t SliderJointSW::get_param(PhysicsServer::SliderJointParam p_param) const { - - switch (p_param) { - case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_UPPER: return m_upperLinLimit; - case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_LOWER: return m_lowerLinLimit; - case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS: return m_softnessLimLin; - case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION: return m_restitutionLimLin; - case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_DAMPING: return m_dampingLimLin; - case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_SOFTNESS: return m_softnessDirLin; - case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_RESTITUTION: return m_restitutionDirLin; - case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_DAMPING: return m_dampingDirLin; - case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS: return m_softnessOrthoLin; - case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION: return m_restitutionOrthoLin; - case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING: return m_dampingOrthoLin; - - case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_UPPER: return m_upperAngLimit; - case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_LOWER: return m_lowerAngLimit; - case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS: return m_softnessLimAng; - case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION: return m_restitutionLimAng; - case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_DAMPING: return m_dampingLimAng; - case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS: return m_softnessDirAng; - case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION: return m_restitutionDirAng; - case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_DAMPING: return m_dampingDirAng; - case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS: return m_softnessOrthoAng; - case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION: return m_restitutionOrthoAng; - case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING: return m_dampingOrthoAng; - - case PhysicsServer::SLIDER_JOINT_MAX: break; // Can't happen, but silences warning - } - - return 0; -} diff --git a/servers/physics/joints/slider_joint_sw.h b/servers/physics/joints/slider_joint_sw.h deleted file mode 100644 index 8b416eafc9..0000000000 --- a/servers/physics/joints/slider_joint_sw.h +++ /dev/null @@ -1,249 +0,0 @@ -/*************************************************************************/ -/* slider_joint_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 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. */ -/*************************************************************************/ - -/* -Adapted to Godot from the Bullet library. -*/ - -#ifndef SLIDER_JOINT_SW_H -#define SLIDER_JOINT_SW_H - -#include "servers/physics/joints/jacobian_entry_sw.h" -#include "servers/physics/joints_sw.h" - -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -/* -Added by Roman Ponomarev (rponom@gmail.com) -April 04, 2008 - -*/ - -#define SLIDER_CONSTRAINT_DEF_SOFTNESS (real_t(1.0)) -#define SLIDER_CONSTRAINT_DEF_DAMPING (real_t(1.0)) -#define SLIDER_CONSTRAINT_DEF_RESTITUTION (real_t(0.7)) - -//----------------------------------------------------------------------------- - -class SliderJointSW : public JointSW { -protected: - union { - struct { - BodySW *A; - BodySW *B; - }; - - BodySW *_arr[2]; - }; - - Transform m_frameInA; - Transform m_frameInB; - - // linear limits - real_t m_lowerLinLimit; - real_t m_upperLinLimit; - // angular limits - real_t m_lowerAngLimit; - real_t m_upperAngLimit; - // softness, restitution and damping for different cases - // DirLin - moving inside linear limits - // LimLin - hitting linear limit - // DirAng - moving inside angular limits - // LimAng - hitting angular limit - // OrthoLin, OrthoAng - against constraint axis - real_t m_softnessDirLin; - real_t m_restitutionDirLin; - real_t m_dampingDirLin; - real_t m_softnessDirAng; - real_t m_restitutionDirAng; - real_t m_dampingDirAng; - real_t m_softnessLimLin; - real_t m_restitutionLimLin; - real_t m_dampingLimLin; - real_t m_softnessLimAng; - real_t m_restitutionLimAng; - real_t m_dampingLimAng; - real_t m_softnessOrthoLin; - real_t m_restitutionOrthoLin; - real_t m_dampingOrthoLin; - real_t m_softnessOrthoAng; - real_t m_restitutionOrthoAng; - real_t m_dampingOrthoAng; - - // for interlal use - bool m_solveLinLim; - bool m_solveAngLim; - - JacobianEntrySW m_jacLin[3]; - real_t m_jacLinDiagABInv[3]; - - JacobianEntrySW m_jacAng[3]; - - real_t m_timeStep; - Transform m_calculatedTransformA; - Transform m_calculatedTransformB; - - Vector3 m_sliderAxis; - Vector3 m_realPivotAInW; - Vector3 m_realPivotBInW; - Vector3 m_projPivotInW; - Vector3 m_delta; - Vector3 m_depth; - Vector3 m_relPosA; - Vector3 m_relPosB; - - real_t m_linPos; - - real_t m_angDepth; - real_t m_kAngle; - - bool m_poweredLinMotor; - real_t m_targetLinMotorVelocity; - real_t m_maxLinMotorForce; - real_t m_accumulatedLinMotorImpulse; - - bool m_poweredAngMotor; - real_t m_targetAngMotorVelocity; - real_t m_maxAngMotorForce; - real_t m_accumulatedAngMotorImpulse; - - //------------------------ - void initParams(); - -public: - // constructors - SliderJointSW(BodySW *rbA, BodySW *rbB, const Transform &frameInA, const Transform &frameInB); - //SliderJointSW(); - // overrides - - // access - const BodySW *getRigidBodyA() const { return A; } - const BodySW *getRigidBodyB() const { return B; } - const Transform &getCalculatedTransformA() const { return m_calculatedTransformA; } - const Transform &getCalculatedTransformB() const { return m_calculatedTransformB; } - const Transform &getFrameOffsetA() const { return m_frameInA; } - const Transform &getFrameOffsetB() const { return m_frameInB; } - Transform &getFrameOffsetA() { return m_frameInA; } - Transform &getFrameOffsetB() { return m_frameInB; } - real_t getLowerLinLimit() { return m_lowerLinLimit; } - void setLowerLinLimit(real_t lowerLimit) { m_lowerLinLimit = lowerLimit; } - real_t getUpperLinLimit() { return m_upperLinLimit; } - void setUpperLinLimit(real_t upperLimit) { m_upperLinLimit = upperLimit; } - real_t getLowerAngLimit() { return m_lowerAngLimit; } - void setLowerAngLimit(real_t lowerLimit) { m_lowerAngLimit = lowerLimit; } - real_t getUpperAngLimit() { return m_upperAngLimit; } - void setUpperAngLimit(real_t upperLimit) { m_upperAngLimit = upperLimit; } - - real_t getSoftnessDirLin() { return m_softnessDirLin; } - real_t getRestitutionDirLin() { return m_restitutionDirLin; } - real_t getDampingDirLin() { return m_dampingDirLin; } - real_t getSoftnessDirAng() { return m_softnessDirAng; } - real_t getRestitutionDirAng() { return m_restitutionDirAng; } - real_t getDampingDirAng() { return m_dampingDirAng; } - real_t getSoftnessLimLin() { return m_softnessLimLin; } - real_t getRestitutionLimLin() { return m_restitutionLimLin; } - real_t getDampingLimLin() { return m_dampingLimLin; } - real_t getSoftnessLimAng() { return m_softnessLimAng; } - real_t getRestitutionLimAng() { return m_restitutionLimAng; } - real_t getDampingLimAng() { return m_dampingLimAng; } - real_t getSoftnessOrthoLin() { return m_softnessOrthoLin; } - real_t getRestitutionOrthoLin() { return m_restitutionOrthoLin; } - real_t getDampingOrthoLin() { return m_dampingOrthoLin; } - real_t getSoftnessOrthoAng() { return m_softnessOrthoAng; } - real_t getRestitutionOrthoAng() { return m_restitutionOrthoAng; } - real_t getDampingOrthoAng() { return m_dampingOrthoAng; } - void setSoftnessDirLin(real_t softnessDirLin) { m_softnessDirLin = softnessDirLin; } - void setRestitutionDirLin(real_t restitutionDirLin) { m_restitutionDirLin = restitutionDirLin; } - void setDampingDirLin(real_t dampingDirLin) { m_dampingDirLin = dampingDirLin; } - void setSoftnessDirAng(real_t softnessDirAng) { m_softnessDirAng = softnessDirAng; } - void setRestitutionDirAng(real_t restitutionDirAng) { m_restitutionDirAng = restitutionDirAng; } - void setDampingDirAng(real_t dampingDirAng) { m_dampingDirAng = dampingDirAng; } - void setSoftnessLimLin(real_t softnessLimLin) { m_softnessLimLin = softnessLimLin; } - void setRestitutionLimLin(real_t restitutionLimLin) { m_restitutionLimLin = restitutionLimLin; } - void setDampingLimLin(real_t dampingLimLin) { m_dampingLimLin = dampingLimLin; } - void setSoftnessLimAng(real_t softnessLimAng) { m_softnessLimAng = softnessLimAng; } - void setRestitutionLimAng(real_t restitutionLimAng) { m_restitutionLimAng = restitutionLimAng; } - void setDampingLimAng(real_t dampingLimAng) { m_dampingLimAng = dampingLimAng; } - void setSoftnessOrthoLin(real_t softnessOrthoLin) { m_softnessOrthoLin = softnessOrthoLin; } - void setRestitutionOrthoLin(real_t restitutionOrthoLin) { m_restitutionOrthoLin = restitutionOrthoLin; } - void setDampingOrthoLin(real_t dampingOrthoLin) { m_dampingOrthoLin = dampingOrthoLin; } - void setSoftnessOrthoAng(real_t softnessOrthoAng) { m_softnessOrthoAng = softnessOrthoAng; } - void setRestitutionOrthoAng(real_t restitutionOrthoAng) { m_restitutionOrthoAng = restitutionOrthoAng; } - void setDampingOrthoAng(real_t dampingOrthoAng) { m_dampingOrthoAng = dampingOrthoAng; } - void setPoweredLinMotor(bool onOff) { m_poweredLinMotor = onOff; } - bool getPoweredLinMotor() { return m_poweredLinMotor; } - void setTargetLinMotorVelocity(real_t targetLinMotorVelocity) { m_targetLinMotorVelocity = targetLinMotorVelocity; } - real_t getTargetLinMotorVelocity() { return m_targetLinMotorVelocity; } - void setMaxLinMotorForce(real_t maxLinMotorForce) { m_maxLinMotorForce = maxLinMotorForce; } - real_t getMaxLinMotorForce() { return m_maxLinMotorForce; } - void setPoweredAngMotor(bool onOff) { m_poweredAngMotor = onOff; } - bool getPoweredAngMotor() { return m_poweredAngMotor; } - void setTargetAngMotorVelocity(real_t targetAngMotorVelocity) { m_targetAngMotorVelocity = targetAngMotorVelocity; } - real_t getTargetAngMotorVelocity() { return m_targetAngMotorVelocity; } - void setMaxAngMotorForce(real_t maxAngMotorForce) { m_maxAngMotorForce = maxAngMotorForce; } - real_t getMaxAngMotorForce() { return m_maxAngMotorForce; } - real_t getLinearPos() { return m_linPos; } - - // access for ODE solver - bool getSolveLinLimit() { return m_solveLinLim; } - real_t getLinDepth() { return m_depth[0]; } - bool getSolveAngLimit() { return m_solveAngLim; } - real_t getAngDepth() { return m_angDepth; } - // shared code used by ODE solver - void calculateTransforms(void); - void testLinLimits(void); - void testAngLimits(void); - // access for PE Solver - Vector3 getAncorInA(void); - Vector3 getAncorInB(void); - - void set_param(PhysicsServer::SliderJointParam p_param, real_t p_value); - real_t get_param(PhysicsServer::SliderJointParam p_param) const; - - bool setup(real_t p_step); - void solve(real_t p_step); - - virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_SLIDER; } -}; - -#endif // SLIDER_JOINT_SW_H diff --git a/servers/physics/joints_sw.h b/servers/physics/joints_sw.h deleted file mode 100644 index c284d541e3..0000000000 --- a/servers/physics/joints_sw.h +++ /dev/null @@ -1,46 +0,0 @@ -/*************************************************************************/ -/* joints_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 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_SW_H -#define JOINTS_SW_H - -#include "body_sw.h" -#include "constraint_sw.h" - -class JointSW : public ConstraintSW { - -public: - virtual PhysicsServer::JointType get_type() const = 0; - _FORCE_INLINE_ JointSW(BodySW **p_body_ptr = NULL, int p_body_count = 0) : - ConstraintSW(p_body_ptr, p_body_count) { - } -}; - -#endif // JOINTS_SW_H diff --git a/servers/physics/physics_server_sw.cpp b/servers/physics/physics_server_sw.cpp deleted file mode 100644 index 25b21a5394..0000000000 --- a/servers/physics/physics_server_sw.cpp +++ /dev/null @@ -1,1589 +0,0 @@ -/*************************************************************************/ -/* physics_server_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 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_sw.h" - -#include "broad_phase_basic.h" -#include "broad_phase_octree.h" -#include "core/debugger/engine_debugger.h" -#include "core/os/os.h" -#include "joints/cone_twist_joint_sw.h" -#include "joints/generic_6dof_joint_sw.h" -#include "joints/hinge_joint_sw.h" -#include "joints/pin_joint_sw.h" -#include "joints/slider_joint_sw.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 PhysicsServerSW::shape_create(ShapeType p_shape) { - - ShapeSW *shape = NULL; - switch (p_shape) { - - case SHAPE_PLANE: { - - shape = memnew(PlaneShapeSW); - } break; - case SHAPE_RAY: { - - shape = memnew(RayShapeSW); - } break; - case SHAPE_SPHERE: { - - shape = memnew(SphereShapeSW); - } break; - case SHAPE_BOX: { - - shape = memnew(BoxShapeSW); - } break; - case SHAPE_CAPSULE: { - - shape = memnew(CapsuleShapeSW); - } break; - case SHAPE_CYLINDER: { - - ERR_FAIL_V_MSG(RID(), "CylinderShape is not supported in GodotPhysics. Please switch to Bullet in the Project Settings."); - } break; - case SHAPE_CONVEX_POLYGON: { - - shape = memnew(ConvexPolygonShapeSW); - } break; - case SHAPE_CONCAVE_POLYGON: { - - shape = memnew(ConcavePolygonShapeSW); - } break; - case SHAPE_HEIGHTMAP: { - - shape = memnew(HeightMapShapeSW); - } break; - case SHAPE_CUSTOM: { - - ERR_FAIL_V(RID()); - - } break; - } - - RID id = shape_owner.make_rid(shape); - shape->set_self(id); - - return id; -}; - -void PhysicsServerSW::shape_set_data(RID p_shape, const Variant &p_data) { - - ShapeSW *shape = shape_owner.getornull(p_shape); - ERR_FAIL_COND(!shape); - shape->set_data(p_data); -}; - -void PhysicsServerSW::shape_set_custom_solver_bias(RID p_shape, real_t p_bias) { - - ShapeSW *shape = shape_owner.getornull(p_shape); - ERR_FAIL_COND(!shape); - shape->set_custom_bias(p_bias); -} - -PhysicsServer::ShapeType PhysicsServerSW::shape_get_type(RID p_shape) const { - - const ShapeSW *shape = shape_owner.getornull(p_shape); - ERR_FAIL_COND_V(!shape, SHAPE_CUSTOM); - return shape->get_type(); -}; - -Variant PhysicsServerSW::shape_get_data(RID p_shape) const { - - const ShapeSW *shape = shape_owner.getornull(p_shape); - ERR_FAIL_COND_V(!shape, Variant()); - ERR_FAIL_COND_V(!shape->is_configured(), Variant()); - return shape->get_data(); -}; - -void PhysicsServerSW::shape_set_margin(RID p_shape, real_t p_margin) { -} - -real_t PhysicsServerSW::shape_get_margin(RID p_shape) const { - return 0.0; -} - -real_t PhysicsServerSW::shape_get_custom_solver_bias(RID p_shape) const { - - const ShapeSW *shape = shape_owner.getornull(p_shape); - ERR_FAIL_COND_V(!shape, 0); - return shape->get_custom_bias(); -} - -RID PhysicsServerSW::space_create() { - - SpaceSW *space = memnew(SpaceSW); - RID id = space_owner.make_rid(space); - space->set_self(id); - RID area_id = area_create(); - AreaSW *area = area_owner.getornull(area_id); - ERR_FAIL_COND_V(!area, RID()); - space->set_default_area(area); - area->set_space(space); - area->set_priority(-1); - RID sgb = body_create(); - body_set_space(sgb, id); - body_set_mode(sgb, BODY_MODE_STATIC); - space->set_static_global_body(sgb); - - return id; -}; - -void PhysicsServerSW::space_set_active(RID p_space, bool p_active) { - - SpaceSW *space = space_owner.getornull(p_space); - ERR_FAIL_COND(!space); - if (p_active) - active_spaces.insert(space); - else - active_spaces.erase(space); -} - -bool PhysicsServerSW::space_is_active(RID p_space) const { - - const SpaceSW *space = space_owner.getornull(p_space); - ERR_FAIL_COND_V(!space, false); - - return active_spaces.has(space); -} - -void PhysicsServerSW::space_set_param(RID p_space, SpaceParameter p_param, real_t p_value) { - - SpaceSW *space = space_owner.getornull(p_space); - ERR_FAIL_COND(!space); - - space->set_param(p_param, p_value); -} - -real_t PhysicsServerSW::space_get_param(RID p_space, SpaceParameter p_param) const { - - const SpaceSW *space = space_owner.getornull(p_space); - ERR_FAIL_COND_V(!space, 0); - return space->get_param(p_param); -} - -PhysicsDirectSpaceState *PhysicsServerSW::space_get_direct_state(RID p_space) { - - SpaceSW *space = space_owner.getornull(p_space); - ERR_FAIL_COND_V(!space, NULL); - ERR_FAIL_COND_V_MSG(!doing_sync || space->is_locked(), NULL, "Space state is inaccessible right now, wait for iteration or physics process notification."); - - return space->get_direct_state(); -} - -void PhysicsServerSW::space_set_debug_contacts(RID p_space, int p_max_contacts) { - - SpaceSW *space = space_owner.getornull(p_space); - ERR_FAIL_COND(!space); - space->set_debug_contacts(p_max_contacts); -} - -Vector<Vector3> PhysicsServerSW::space_get_contacts(RID p_space) const { - - SpaceSW *space = space_owner.getornull(p_space); - ERR_FAIL_COND_V(!space, Vector<Vector3>()); - return space->get_debug_contacts(); -} - -int PhysicsServerSW::space_get_contact_count(RID p_space) const { - - SpaceSW *space = space_owner.getornull(p_space); - ERR_FAIL_COND_V(!space, 0); - return space->get_debug_contact_count(); -} - -RID PhysicsServerSW::area_create() { - - AreaSW *area = memnew(AreaSW); - RID rid = area_owner.make_rid(area); - area->set_self(rid); - return rid; -}; - -void PhysicsServerSW::area_set_space(RID p_area, RID p_space) { - - AreaSW *area = area_owner.getornull(p_area); - ERR_FAIL_COND(!area); - - SpaceSW *space = NULL; - if (p_space.is_valid()) { - space = space_owner.getornull(p_space); - ERR_FAIL_COND(!space); - } - - if (area->get_space() == space) - return; //pointless - - area->clear_constraints(); - area->set_space(space); -}; - -RID PhysicsServerSW::area_get_space(RID p_area) const { - - AreaSW *area = area_owner.getornull(p_area); - ERR_FAIL_COND_V(!area, RID()); - - SpaceSW *space = area->get_space(); - if (!space) - return RID(); - return space->get_self(); -}; - -void PhysicsServerSW::area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode) { - - AreaSW *area = area_owner.getornull(p_area); - ERR_FAIL_COND(!area); - - area->set_space_override_mode(p_mode); -} - -PhysicsServer::AreaSpaceOverrideMode PhysicsServerSW::area_get_space_override_mode(RID p_area) const { - - const AreaSW *area = area_owner.getornull(p_area); - ERR_FAIL_COND_V(!area, AREA_SPACE_OVERRIDE_DISABLED); - - return area->get_space_override_mode(); -} - -void PhysicsServerSW::area_add_shape(RID p_area, RID p_shape, const Transform &p_transform, bool p_disabled) { - - AreaSW *area = area_owner.getornull(p_area); - ERR_FAIL_COND(!area); - - ShapeSW *shape = shape_owner.getornull(p_shape); - ERR_FAIL_COND(!shape); - - area->add_shape(shape, p_transform, p_disabled); -} - -void PhysicsServerSW::area_set_shape(RID p_area, int p_shape_idx, RID p_shape) { - - AreaSW *area = area_owner.getornull(p_area); - ERR_FAIL_COND(!area); - - ShapeSW *shape = shape_owner.getornull(p_shape); - ERR_FAIL_COND(!shape); - ERR_FAIL_COND(!shape->is_configured()); - - area->set_shape(p_shape_idx, shape); -} - -void PhysicsServerSW::area_set_shape_transform(RID p_area, int p_shape_idx, const Transform &p_transform) { - - AreaSW *area = area_owner.getornull(p_area); - ERR_FAIL_COND(!area); - - area->set_shape_transform(p_shape_idx, p_transform); -} - -int PhysicsServerSW::area_get_shape_count(RID p_area) const { - - AreaSW *area = area_owner.getornull(p_area); - ERR_FAIL_COND_V(!area, -1); - - return area->get_shape_count(); -} -RID PhysicsServerSW::area_get_shape(RID p_area, int p_shape_idx) const { - - AreaSW *area = area_owner.getornull(p_area); - ERR_FAIL_COND_V(!area, RID()); - - ShapeSW *shape = area->get_shape(p_shape_idx); - ERR_FAIL_COND_V(!shape, RID()); - - return shape->get_self(); -} -Transform PhysicsServerSW::area_get_shape_transform(RID p_area, int p_shape_idx) const { - - AreaSW *area = area_owner.getornull(p_area); - ERR_FAIL_COND_V(!area, Transform()); - - return area->get_shape_transform(p_shape_idx); -} - -void PhysicsServerSW::area_remove_shape(RID p_area, int p_shape_idx) { - - AreaSW *area = area_owner.getornull(p_area); - ERR_FAIL_COND(!area); - - area->remove_shape(p_shape_idx); -} - -void PhysicsServerSW::area_clear_shapes(RID p_area) { - - AreaSW *area = area_owner.getornull(p_area); - ERR_FAIL_COND(!area); - - while (area->get_shape_count()) - area->remove_shape(0); -} - -void PhysicsServerSW::area_set_shape_disabled(RID p_area, int p_shape_idx, bool p_disabled) { - - AreaSW *area = area_owner.getornull(p_area); - ERR_FAIL_COND(!area); - ERR_FAIL_INDEX(p_shape_idx, area->get_shape_count()); - FLUSH_QUERY_CHECK(area); - area->set_shape_as_disabled(p_shape_idx, p_disabled); -} - -void PhysicsServerSW::area_attach_object_instance_id(RID p_area, ObjectID p_id) { - - if (space_owner.owns(p_area)) { - SpaceSW *space = space_owner.getornull(p_area); - p_area = space->get_default_area()->get_self(); - } - AreaSW *area = area_owner.getornull(p_area); - ERR_FAIL_COND(!area); - area->set_instance_id(p_id); -} -ObjectID PhysicsServerSW::area_get_object_instance_id(RID p_area) const { - - if (space_owner.owns(p_area)) { - SpaceSW *space = space_owner.getornull(p_area); - p_area = space->get_default_area()->get_self(); - } - AreaSW *area = area_owner.getornull(p_area); - ERR_FAIL_COND_V(!area, ObjectID()); - return area->get_instance_id(); -} - -void PhysicsServerSW::area_set_param(RID p_area, AreaParameter p_param, const Variant &p_value) { - - if (space_owner.owns(p_area)) { - SpaceSW *space = space_owner.getornull(p_area); - p_area = space->get_default_area()->get_self(); - } - AreaSW *area = area_owner.getornull(p_area); - ERR_FAIL_COND(!area); - area->set_param(p_param, p_value); -}; - -void PhysicsServerSW::area_set_transform(RID p_area, const Transform &p_transform) { - - AreaSW *area = area_owner.getornull(p_area); - ERR_FAIL_COND(!area); - area->set_transform(p_transform); -}; - -Variant PhysicsServerSW::area_get_param(RID p_area, AreaParameter p_param) const { - - if (space_owner.owns(p_area)) { - SpaceSW *space = space_owner.getornull(p_area); - p_area = space->get_default_area()->get_self(); - } - AreaSW *area = area_owner.getornull(p_area); - ERR_FAIL_COND_V(!area, Variant()); - - return area->get_param(p_param); -}; - -Transform PhysicsServerSW::area_get_transform(RID p_area) const { - - AreaSW *area = area_owner.getornull(p_area); - ERR_FAIL_COND_V(!area, Transform()); - - return area->get_transform(); -}; - -void PhysicsServerSW::area_set_collision_layer(RID p_area, uint32_t p_layer) { - - AreaSW *area = area_owner.getornull(p_area); - ERR_FAIL_COND(!area); - - area->set_collision_layer(p_layer); -} - -void PhysicsServerSW::area_set_collision_mask(RID p_area, uint32_t p_mask) { - - AreaSW *area = area_owner.getornull(p_area); - ERR_FAIL_COND(!area); - - area->set_collision_mask(p_mask); -} - -void PhysicsServerSW::area_set_monitorable(RID p_area, bool p_monitorable) { - - AreaSW *area = area_owner.getornull(p_area); - ERR_FAIL_COND(!area); - FLUSH_QUERY_CHECK(area); - - area->set_monitorable(p_monitorable); -} - -void PhysicsServerSW::area_set_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) { - - AreaSW *area = area_owner.getornull(p_area); - ERR_FAIL_COND(!area); - - area->set_monitor_callback(p_receiver ? p_receiver->get_instance_id() : ObjectID(), p_method); -} - -void PhysicsServerSW::area_set_ray_pickable(RID p_area, bool p_enable) { - - AreaSW *area = area_owner.getornull(p_area); - ERR_FAIL_COND(!area); - - area->set_ray_pickable(p_enable); -} - -bool PhysicsServerSW::area_is_ray_pickable(RID p_area) const { - - AreaSW *area = area_owner.getornull(p_area); - ERR_FAIL_COND_V(!area, false); - - return area->is_ray_pickable(); -} - -void PhysicsServerSW::area_set_area_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) { - - AreaSW *area = area_owner.getornull(p_area); - ERR_FAIL_COND(!area); - - area->set_area_monitor_callback(p_receiver ? p_receiver->get_instance_id() : ObjectID(), p_method); -} - -/* BODY API */ - -RID PhysicsServerSW::body_create(BodyMode p_mode, bool p_init_sleeping) { - - BodySW *body = memnew(BodySW); - if (p_mode != BODY_MODE_RIGID) - body->set_mode(p_mode); - if (p_init_sleeping) - body->set_state(BODY_STATE_SLEEPING, p_init_sleeping); - RID rid = body_owner.make_rid(body); - body->set_self(rid); - return rid; -}; - -void PhysicsServerSW::body_set_space(RID p_body, RID p_space) { - - BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND(!body); - - SpaceSW *space = NULL; - if (p_space.is_valid()) { - space = space_owner.getornull(p_space); - ERR_FAIL_COND(!space); - } - - if (body->get_space() == space) - return; //pointless - - body->clear_constraint_map(); - body->set_space(space); -}; - -RID PhysicsServerSW::body_get_space(RID p_body) const { - - BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND_V(!body, RID()); - - SpaceSW *space = body->get_space(); - if (!space) - return RID(); - return space->get_self(); -}; - -void PhysicsServerSW::body_set_mode(RID p_body, BodyMode p_mode) { - - BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND(!body); - - body->set_mode(p_mode); -}; - -PhysicsServer::BodyMode PhysicsServerSW::body_get_mode(RID p_body) const { - - BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND_V(!body, BODY_MODE_STATIC); - - return body->get_mode(); -}; - -void PhysicsServerSW::body_add_shape(RID p_body, RID p_shape, const Transform &p_transform, bool p_disabled) { - - BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND(!body); - - ShapeSW *shape = shape_owner.getornull(p_shape); - ERR_FAIL_COND(!shape); - - body->add_shape(shape, p_transform, p_disabled); -} - -void PhysicsServerSW::body_set_shape(RID p_body, int p_shape_idx, RID p_shape) { - - BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND(!body); - - ShapeSW *shape = shape_owner.getornull(p_shape); - ERR_FAIL_COND(!shape); - ERR_FAIL_COND(!shape->is_configured()); - - body->set_shape(p_shape_idx, shape); -} -void PhysicsServerSW::body_set_shape_transform(RID p_body, int p_shape_idx, const Transform &p_transform) { - - BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND(!body); - - body->set_shape_transform(p_shape_idx, p_transform); -} - -int PhysicsServerSW::body_get_shape_count(RID p_body) const { - - BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND_V(!body, -1); - - return body->get_shape_count(); -} -RID PhysicsServerSW::body_get_shape(RID p_body, int p_shape_idx) const { - - BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND_V(!body, RID()); - - ShapeSW *shape = body->get_shape(p_shape_idx); - ERR_FAIL_COND_V(!shape, RID()); - - return shape->get_self(); -} - -void PhysicsServerSW::body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled) { - - BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND(!body); - ERR_FAIL_INDEX(p_shape_idx, body->get_shape_count()); - FLUSH_QUERY_CHECK(body); - - body->set_shape_as_disabled(p_shape_idx, p_disabled); -} - -Transform PhysicsServerSW::body_get_shape_transform(RID p_body, int p_shape_idx) const { - - BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND_V(!body, Transform()); - - return body->get_shape_transform(p_shape_idx); -} - -void PhysicsServerSW::body_remove_shape(RID p_body, int p_shape_idx) { - - BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND(!body); - - body->remove_shape(p_shape_idx); -} - -void PhysicsServerSW::body_clear_shapes(RID p_body) { - - BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND(!body); - - while (body->get_shape_count()) - body->remove_shape(0); -} - -void PhysicsServerSW::body_set_enable_continuous_collision_detection(RID p_body, bool p_enable) { - - BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND(!body); - - body->set_continuous_collision_detection(p_enable); -} - -bool PhysicsServerSW::body_is_continuous_collision_detection_enabled(RID p_body) const { - - BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND_V(!body, false); - - return body->is_continuous_collision_detection_enabled(); -} - -void PhysicsServerSW::body_set_collision_layer(RID p_body, uint32_t p_layer) { - - BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND(!body); - - body->set_collision_layer(p_layer); - body->wakeup(); -} - -uint32_t PhysicsServerSW::body_get_collision_layer(RID p_body) const { - - const BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND_V(!body, 0); - - return body->get_collision_layer(); -} - -void PhysicsServerSW::body_set_collision_mask(RID p_body, uint32_t p_mask) { - - BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND(!body); - - body->set_collision_mask(p_mask); - body->wakeup(); -} - -uint32_t PhysicsServerSW::body_get_collision_mask(RID p_body) const { - - const BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND_V(!body, 0); - - return body->get_collision_mask(); -} - -void PhysicsServerSW::body_attach_object_instance_id(RID p_body, ObjectID p_id) { - - BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND(!body); - - body->set_instance_id(p_id); -}; - -ObjectID PhysicsServerSW::body_get_object_instance_id(RID p_body) const { - - BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND_V(!body, ObjectID()); - - return body->get_instance_id(); -}; - -void PhysicsServerSW::body_set_user_flags(RID p_body, uint32_t p_flags) { - - BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND(!body); -}; - -uint32_t PhysicsServerSW::body_get_user_flags(RID p_body) const { - - BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND_V(!body, 0); - - return 0; -}; - -void PhysicsServerSW::body_set_param(RID p_body, BodyParameter p_param, real_t p_value) { - - BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND(!body); - - body->set_param(p_param, p_value); -}; - -real_t PhysicsServerSW::body_get_param(RID p_body, BodyParameter p_param) const { - - BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND_V(!body, 0); - - return body->get_param(p_param); -}; - -void PhysicsServerSW::body_set_kinematic_safe_margin(RID p_body, real_t p_margin) { - BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND(!body); - body->set_kinematic_margin(p_margin); -} - -real_t PhysicsServerSW::body_get_kinematic_safe_margin(RID p_body) const { - BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND_V(!body, 0); - - return body->get_kinematic_margin(); -} - -void PhysicsServerSW::body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) { - - BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND(!body); - - body->set_state(p_state, p_variant); -}; - -Variant PhysicsServerSW::body_get_state(RID p_body, BodyState p_state) const { - - BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND_V(!body, Variant()); - - return body->get_state(p_state); -}; - -void PhysicsServerSW::body_set_applied_force(RID p_body, const Vector3 &p_force) { - - BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND(!body); - - body->set_applied_force(p_force); - body->wakeup(); -}; - -Vector3 PhysicsServerSW::body_get_applied_force(RID p_body) const { - - BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND_V(!body, Vector3()); - return body->get_applied_force(); -}; - -void PhysicsServerSW::body_set_applied_torque(RID p_body, const Vector3 &p_torque) { - - BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND(!body); - - body->set_applied_torque(p_torque); - body->wakeup(); -}; - -Vector3 PhysicsServerSW::body_get_applied_torque(RID p_body) const { - - BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND_V(!body, Vector3()); - - return body->get_applied_torque(); -}; - -void PhysicsServerSW::body_add_central_force(RID p_body, const Vector3 &p_force) { - BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND(!body); - - body->add_central_force(p_force); - body->wakeup(); -} - -void PhysicsServerSW::body_add_force(RID p_body, const Vector3 &p_force, const Vector3 &p_pos) { - BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND(!body); - - body->add_force(p_force, p_pos); - body->wakeup(); -}; - -void PhysicsServerSW::body_add_torque(RID p_body, const Vector3 &p_torque) { - BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND(!body); - - body->add_torque(p_torque); - body->wakeup(); -}; - -void PhysicsServerSW::body_apply_central_impulse(RID p_body, const Vector3 &p_impulse) { - BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND(!body); - - _update_shapes(); - - body->apply_central_impulse(p_impulse); - body->wakeup(); -} - -void PhysicsServerSW::body_apply_impulse(RID p_body, const Vector3 &p_pos, const Vector3 &p_impulse) { - - BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND(!body); - - _update_shapes(); - - body->apply_impulse(p_pos, p_impulse); - body->wakeup(); -}; - -void PhysicsServerSW::body_apply_torque_impulse(RID p_body, const Vector3 &p_impulse) { - - BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND(!body); - - _update_shapes(); - - body->apply_torque_impulse(p_impulse); - body->wakeup(); -}; - -void PhysicsServerSW::body_set_axis_velocity(RID p_body, const Vector3 &p_axis_velocity) { - - BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND(!body); - - _update_shapes(); - - Vector3 v = body->get_linear_velocity(); - Vector3 axis = p_axis_velocity.normalized(); - v -= axis * axis.dot(v); - v += p_axis_velocity; - body->set_linear_velocity(v); - body->wakeup(); -}; - -void PhysicsServerSW::body_set_axis_lock(RID p_body, BodyAxis p_axis, bool p_lock) { - - BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND(!body); - - body->set_axis_lock(p_axis, p_lock); - body->wakeup(); -} - -bool PhysicsServerSW::body_is_axis_locked(RID p_body, BodyAxis p_axis) const { - - const BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND_V(!body, 0); - return body->is_axis_locked(p_axis); -} - -void PhysicsServerSW::body_add_collision_exception(RID p_body, RID p_body_b) { - - BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND(!body); - - body->add_exception(p_body_b); - body->wakeup(); -}; - -void PhysicsServerSW::body_remove_collision_exception(RID p_body, RID p_body_b) { - - BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND(!body); - - body->remove_exception(p_body_b); - body->wakeup(); -}; - -void PhysicsServerSW::body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) { - - BodySW *body = body_owner.getornull(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 PhysicsServerSW::body_set_contacts_reported_depth_threshold(RID p_body, real_t p_threshold) { - - BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND(!body); -}; - -real_t PhysicsServerSW::body_get_contacts_reported_depth_threshold(RID p_body) const { - - BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND_V(!body, 0); - return 0; -}; - -void PhysicsServerSW::body_set_omit_force_integration(RID p_body, bool p_omit) { - - BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND(!body); - - body->set_omit_force_integration(p_omit); -}; - -bool PhysicsServerSW::body_is_omitting_force_integration(RID p_body) const { - - BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND_V(!body, false); - return body->get_omit_force_integration(); -}; - -void PhysicsServerSW::body_set_max_contacts_reported(RID p_body, int p_contacts) { - - BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND(!body); - body->set_max_contacts_reported(p_contacts); -} - -int PhysicsServerSW::body_get_max_contacts_reported(RID p_body) const { - - BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND_V(!body, -1); - return body->get_max_contacts_reported(); -} - -void PhysicsServerSW::body_set_force_integration_callback(RID p_body, Object *p_receiver, const StringName &p_method, const Variant &p_udata) { - - BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND(!body); - body->set_force_integration_callback(p_receiver ? p_receiver->get_instance_id() : ObjectID(), p_method, p_udata); -} - -void PhysicsServerSW::body_set_ray_pickable(RID p_body, bool p_enable) { - - BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND(!body); - body->set_ray_pickable(p_enable); -} - -bool PhysicsServerSW::body_is_ray_pickable(RID p_body) const { - - BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND_V(!body, false); - return body->is_ray_pickable(); -} - -bool PhysicsServerSW::body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, MotionResult *r_result, bool p_exclude_raycast_shapes) { - - BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND_V(!body, false); - ERR_FAIL_COND_V(!body->get_space(), false); - ERR_FAIL_COND_V(body->get_space()->is_locked(), false); - - _update_shapes(); - - return body->get_space()->test_body_motion(body, p_from, p_motion, p_infinite_inertia, body->get_kinematic_margin(), r_result, p_exclude_raycast_shapes); -} - -int PhysicsServerSW::body_test_ray_separation(RID p_body, const Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, SeparationResult *r_results, int p_result_max, float p_margin) { - - BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND_V(!body, false); - ERR_FAIL_COND_V(!body->get_space(), false); - ERR_FAIL_COND_V(body->get_space()->is_locked(), false); - - _update_shapes(); - - return body->get_space()->test_body_ray_separation(body, p_transform, p_infinite_inertia, r_recover_motion, r_results, p_result_max, p_margin); -} - -PhysicsDirectBodyState *PhysicsServerSW::body_get_direct_state(RID p_body) { - - BodySW *body = body_owner.getornull(p_body); - ERR_FAIL_COND_V(!body, NULL); - ERR_FAIL_COND_V_MSG(!doing_sync || body->get_space()->is_locked(), NULL, "Body state is inaccessible right now, wait for iteration or physics process notification."); - - direct_state->body = body; - return direct_state; -} - -/* JOINT API */ - -RID PhysicsServerSW::joint_create_pin(RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B) { - - BodySW *body_A = body_owner.getornull(p_body_A); - ERR_FAIL_COND_V(!body_A, RID()); - - if (!p_body_B.is_valid()) { - ERR_FAIL_COND_V(!body_A->get_space(), RID()); - p_body_B = body_A->get_space()->get_static_global_body(); - } - - BodySW *body_B = body_owner.getornull(p_body_B); - ERR_FAIL_COND_V(!body_B, RID()); - - ERR_FAIL_COND_V(body_A == body_B, RID()); - - JointSW *joint = memnew(PinJointSW(body_A, p_local_A, body_B, p_local_B)); - RID rid = joint_owner.make_rid(joint); - joint->set_self(rid); - return rid; -} - -void PhysicsServerSW::pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value) { - - JointSW *joint = joint_owner.getornull(p_joint); - ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_PIN); - PinJointSW *pin_joint = static_cast<PinJointSW *>(joint); - pin_joint->set_param(p_param, p_value); -} -real_t PhysicsServerSW::pin_joint_get_param(RID p_joint, PinJointParam p_param) const { - - JointSW *joint = joint_owner.getornull(p_joint); - ERR_FAIL_COND_V(!joint, 0); - ERR_FAIL_COND_V(joint->get_type() != JOINT_PIN, 0); - PinJointSW *pin_joint = static_cast<PinJointSW *>(joint); - return pin_joint->get_param(p_param); -} - -void PhysicsServerSW::pin_joint_set_local_a(RID p_joint, const Vector3 &p_A) { - - JointSW *joint = joint_owner.getornull(p_joint); - ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_PIN); - PinJointSW *pin_joint = static_cast<PinJointSW *>(joint); - pin_joint->set_pos_a(p_A); -} -Vector3 PhysicsServerSW::pin_joint_get_local_a(RID p_joint) const { - - JointSW *joint = joint_owner.getornull(p_joint); - ERR_FAIL_COND_V(!joint, Vector3()); - ERR_FAIL_COND_V(joint->get_type() != JOINT_PIN, Vector3()); - PinJointSW *pin_joint = static_cast<PinJointSW *>(joint); - return pin_joint->get_position_a(); -} - -void PhysicsServerSW::pin_joint_set_local_b(RID p_joint, const Vector3 &p_B) { - - JointSW *joint = joint_owner.getornull(p_joint); - ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_PIN); - PinJointSW *pin_joint = static_cast<PinJointSW *>(joint); - pin_joint->set_pos_b(p_B); -} -Vector3 PhysicsServerSW::pin_joint_get_local_b(RID p_joint) const { - - JointSW *joint = joint_owner.getornull(p_joint); - ERR_FAIL_COND_V(!joint, Vector3()); - ERR_FAIL_COND_V(joint->get_type() != JOINT_PIN, Vector3()); - PinJointSW *pin_joint = static_cast<PinJointSW *>(joint); - return pin_joint->get_position_b(); -} - -RID PhysicsServerSW::joint_create_hinge(RID p_body_A, const Transform &p_frame_A, RID p_body_B, const Transform &p_frame_B) { - - BodySW *body_A = body_owner.getornull(p_body_A); - ERR_FAIL_COND_V(!body_A, RID()); - - if (!p_body_B.is_valid()) { - ERR_FAIL_COND_V(!body_A->get_space(), RID()); - p_body_B = body_A->get_space()->get_static_global_body(); - } - - BodySW *body_B = body_owner.getornull(p_body_B); - ERR_FAIL_COND_V(!body_B, RID()); - - ERR_FAIL_COND_V(body_A == body_B, RID()); - - JointSW *joint = memnew(HingeJointSW(body_A, body_B, p_frame_A, p_frame_B)); - RID rid = joint_owner.make_rid(joint); - joint->set_self(rid); - return rid; -} - -RID PhysicsServerSW::joint_create_hinge_simple(RID p_body_A, const Vector3 &p_pivot_A, const Vector3 &p_axis_A, RID p_body_B, const Vector3 &p_pivot_B, const Vector3 &p_axis_B) { - - BodySW *body_A = body_owner.getornull(p_body_A); - ERR_FAIL_COND_V(!body_A, RID()); - - if (!p_body_B.is_valid()) { - ERR_FAIL_COND_V(!body_A->get_space(), RID()); - p_body_B = body_A->get_space()->get_static_global_body(); - } - - BodySW *body_B = body_owner.getornull(p_body_B); - ERR_FAIL_COND_V(!body_B, RID()); - - ERR_FAIL_COND_V(body_A == body_B, RID()); - - JointSW *joint = memnew(HingeJointSW(body_A, body_B, p_pivot_A, p_pivot_B, p_axis_A, p_axis_B)); - RID rid = joint_owner.make_rid(joint); - joint->set_self(rid); - return rid; -} - -void PhysicsServerSW::hinge_joint_set_param(RID p_joint, HingeJointParam p_param, real_t p_value) { - - JointSW *joint = joint_owner.getornull(p_joint); - ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_HINGE); - HingeJointSW *hinge_joint = static_cast<HingeJointSW *>(joint); - hinge_joint->set_param(p_param, p_value); -} -real_t PhysicsServerSW::hinge_joint_get_param(RID p_joint, HingeJointParam p_param) const { - - JointSW *joint = joint_owner.getornull(p_joint); - ERR_FAIL_COND_V(!joint, 0); - ERR_FAIL_COND_V(joint->get_type() != JOINT_HINGE, 0); - HingeJointSW *hinge_joint = static_cast<HingeJointSW *>(joint); - return hinge_joint->get_param(p_param); -} - -void PhysicsServerSW::hinge_joint_set_flag(RID p_joint, HingeJointFlag p_flag, bool p_value) { - - JointSW *joint = joint_owner.getornull(p_joint); - ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_HINGE); - HingeJointSW *hinge_joint = static_cast<HingeJointSW *>(joint); - hinge_joint->set_flag(p_flag, p_value); -} -bool PhysicsServerSW::hinge_joint_get_flag(RID p_joint, HingeJointFlag p_flag) const { - - JointSW *joint = joint_owner.getornull(p_joint); - ERR_FAIL_COND_V(!joint, false); - ERR_FAIL_COND_V(joint->get_type() != JOINT_HINGE, false); - HingeJointSW *hinge_joint = static_cast<HingeJointSW *>(joint); - return hinge_joint->get_flag(p_flag); -} - -void PhysicsServerSW::joint_set_solver_priority(RID p_joint, int p_priority) { - - JointSW *joint = joint_owner.getornull(p_joint); - ERR_FAIL_COND(!joint); - joint->set_priority(p_priority); -} - -int PhysicsServerSW::joint_get_solver_priority(RID p_joint) const { - - JointSW *joint = joint_owner.getornull(p_joint); - ERR_FAIL_COND_V(!joint, 0); - return joint->get_priority(); -} - -void PhysicsServerSW::joint_disable_collisions_between_bodies(RID p_joint, const bool p_disable) { - JointSW *joint = joint_owner.getornull(p_joint); - ERR_FAIL_COND(!joint); - - joint->disable_collisions_between_bodies(p_disable); - - if (2 == joint->get_body_count()) { - BodySW *body_a = *joint->get_body_ptr(); - BodySW *body_b = *(joint->get_body_ptr() + 1); - - if (p_disable) { - body_add_collision_exception(body_a->get_self(), body_b->get_self()); - body_add_collision_exception(body_b->get_self(), body_a->get_self()); - } else { - body_remove_collision_exception(body_a->get_self(), body_b->get_self()); - body_remove_collision_exception(body_b->get_self(), body_a->get_self()); - } - } -} - -bool PhysicsServerSW::joint_is_disabled_collisions_between_bodies(RID p_joint) const { - JointSW *joint = joint_owner.getornull(p_joint); - ERR_FAIL_COND_V(!joint, true); - - return joint->is_disabled_collisions_between_bodies(); -} - -PhysicsServerSW::JointType PhysicsServerSW::joint_get_type(RID p_joint) const { - - JointSW *joint = joint_owner.getornull(p_joint); - ERR_FAIL_COND_V(!joint, JOINT_PIN); - return joint->get_type(); -} - -RID PhysicsServerSW::joint_create_slider(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) { - - BodySW *body_A = body_owner.getornull(p_body_A); - ERR_FAIL_COND_V(!body_A, RID()); - - if (!p_body_B.is_valid()) { - ERR_FAIL_COND_V(!body_A->get_space(), RID()); - p_body_B = body_A->get_space()->get_static_global_body(); - } - - BodySW *body_B = body_owner.getornull(p_body_B); - ERR_FAIL_COND_V(!body_B, RID()); - - ERR_FAIL_COND_V(body_A == body_B, RID()); - - JointSW *joint = memnew(SliderJointSW(body_A, body_B, p_local_frame_A, p_local_frame_B)); - RID rid = joint_owner.make_rid(joint); - joint->set_self(rid); - return rid; -} - -void PhysicsServerSW::slider_joint_set_param(RID p_joint, SliderJointParam p_param, real_t p_value) { - - JointSW *joint = joint_owner.getornull(p_joint); - ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_SLIDER); - SliderJointSW *slider_joint = static_cast<SliderJointSW *>(joint); - slider_joint->set_param(p_param, p_value); -} -real_t PhysicsServerSW::slider_joint_get_param(RID p_joint, SliderJointParam p_param) const { - - JointSW *joint = joint_owner.getornull(p_joint); - ERR_FAIL_COND_V(!joint, 0); - ERR_FAIL_COND_V(joint->get_type() != JOINT_CONE_TWIST, 0); - SliderJointSW *slider_joint = static_cast<SliderJointSW *>(joint); - return slider_joint->get_param(p_param); -} - -RID PhysicsServerSW::joint_create_cone_twist(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) { - - BodySW *body_A = body_owner.getornull(p_body_A); - ERR_FAIL_COND_V(!body_A, RID()); - - if (!p_body_B.is_valid()) { - ERR_FAIL_COND_V(!body_A->get_space(), RID()); - p_body_B = body_A->get_space()->get_static_global_body(); - } - - BodySW *body_B = body_owner.getornull(p_body_B); - ERR_FAIL_COND_V(!body_B, RID()); - - ERR_FAIL_COND_V(body_A == body_B, RID()); - - JointSW *joint = memnew(ConeTwistJointSW(body_A, body_B, p_local_frame_A, p_local_frame_B)); - RID rid = joint_owner.make_rid(joint); - joint->set_self(rid); - return rid; -} - -void PhysicsServerSW::cone_twist_joint_set_param(RID p_joint, ConeTwistJointParam p_param, real_t p_value) { - - JointSW *joint = joint_owner.getornull(p_joint); - ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_CONE_TWIST); - ConeTwistJointSW *cone_twist_joint = static_cast<ConeTwistJointSW *>(joint); - cone_twist_joint->set_param(p_param, p_value); -} -real_t PhysicsServerSW::cone_twist_joint_get_param(RID p_joint, ConeTwistJointParam p_param) const { - - JointSW *joint = joint_owner.getornull(p_joint); - ERR_FAIL_COND_V(!joint, 0); - ERR_FAIL_COND_V(joint->get_type() != JOINT_CONE_TWIST, 0); - ConeTwistJointSW *cone_twist_joint = static_cast<ConeTwistJointSW *>(joint); - return cone_twist_joint->get_param(p_param); -} - -RID PhysicsServerSW::joint_create_generic_6dof(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) { - - BodySW *body_A = body_owner.getornull(p_body_A); - ERR_FAIL_COND_V(!body_A, RID()); - - if (!p_body_B.is_valid()) { - ERR_FAIL_COND_V(!body_A->get_space(), RID()); - p_body_B = body_A->get_space()->get_static_global_body(); - } - - BodySW *body_B = body_owner.getornull(p_body_B); - ERR_FAIL_COND_V(!body_B, RID()); - - ERR_FAIL_COND_V(body_A == body_B, RID()); - - JointSW *joint = memnew(Generic6DOFJointSW(body_A, body_B, p_local_frame_A, p_local_frame_B, true)); - RID rid = joint_owner.make_rid(joint); - joint->set_self(rid); - return rid; -} - -void PhysicsServerSW::generic_6dof_joint_set_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParam p_param, real_t p_value) { - - JointSW *joint = joint_owner.getornull(p_joint); - ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_6DOF); - Generic6DOFJointSW *generic_6dof_joint = static_cast<Generic6DOFJointSW *>(joint); - generic_6dof_joint->set_param(p_axis, p_param, p_value); -} -real_t PhysicsServerSW::generic_6dof_joint_get_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParam p_param) { - - JointSW *joint = joint_owner.getornull(p_joint); - ERR_FAIL_COND_V(!joint, 0); - ERR_FAIL_COND_V(joint->get_type() != JOINT_6DOF, 0); - Generic6DOFJointSW *generic_6dof_joint = static_cast<Generic6DOFJointSW *>(joint); - return generic_6dof_joint->get_param(p_axis, p_param); -} - -void PhysicsServerSW::generic_6dof_joint_set_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlag p_flag, bool p_enable) { - - JointSW *joint = joint_owner.getornull(p_joint); - ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_6DOF); - Generic6DOFJointSW *generic_6dof_joint = static_cast<Generic6DOFJointSW *>(joint); - generic_6dof_joint->set_flag(p_axis, p_flag, p_enable); -} -bool PhysicsServerSW::generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlag p_flag) { - - JointSW *joint = joint_owner.getornull(p_joint); - ERR_FAIL_COND_V(!joint, false); - ERR_FAIL_COND_V(joint->get_type() != JOINT_6DOF, false); - Generic6DOFJointSW *generic_6dof_joint = static_cast<Generic6DOFJointSW *>(joint); - return generic_6dof_joint->get_flag(p_axis, p_flag); -} - -void PhysicsServerSW::free(RID p_rid) { - - _update_shapes(); //just in case - - if (shape_owner.owns(p_rid)) { - - ShapeSW *shape = shape_owner.getornull(p_rid); - - while (shape->get_owners().size()) { - ShapeOwnerSW *so = shape->get_owners().front()->key(); - so->remove_shape(shape); - } - - shape_owner.free(p_rid); - memdelete(shape); - } else if (body_owner.owns(p_rid)) { - - BodySW *body = body_owner.getornull(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(NULL); - - while (body->get_shape_count()) { - - body->remove_shape(0); - } - - body_owner.free(p_rid); - memdelete(body); - - } else if (area_owner.owns(p_rid)) { - - AreaSW *area = area_owner.getornull(p_rid); - - /* - if (area->get_monitor_query()) - _clear_query(area->get_monitor_query()); - */ - - area->set_space(NULL); - - while (area->get_shape_count()) { - - area->remove_shape(0); - } - - area_owner.free(p_rid); - memdelete(area); - } else if (space_owner.owns(p_rid)) { - - SpaceSW *space = space_owner.getornull(p_rid); - - while (space->get_objects().size()) { - CollisionObjectSW *co = (CollisionObjectSW *)space->get_objects().front()->get(); - co->set_space(NULL); - } - - active_spaces.erase(space); - free(space->get_default_area()->get_self()); - free(space->get_static_global_body()); - - space_owner.free(p_rid); - memdelete(space); - } else if (joint_owner.owns(p_rid)) { - - JointSW *joint = joint_owner.getornull(p_rid); - - for (int i = 0; i < joint->get_body_count(); i++) { - - joint->get_body_ptr()[i]->remove_constraint(joint); - } - joint_owner.free(p_rid); - memdelete(joint); - - } else { - - ERR_FAIL_MSG("Invalid ID."); - } -}; - -void PhysicsServerSW::set_active(bool p_active) { - - active = p_active; -}; - -void PhysicsServerSW::init() { - - doing_sync = true; - last_step = 0.001; - iterations = 8; // 8? - stepper = memnew(StepSW); - direct_state = memnew(PhysicsDirectBodyStateSW); -}; - -void PhysicsServerSW::step(real_t p_step) { - -#ifndef _3D_DISABLED - - if (!active) - return; - - _update_shapes(); - - doing_sync = false; - - last_step = p_step; - PhysicsDirectBodyStateSW::singleton->step = p_step; - - island_count = 0; - active_objects = 0; - collision_pairs = 0; - for (Set<const SpaceSW *>::Element *E = active_spaces.front(); E; E = E->next()) { - - stepper->step((SpaceSW *)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(); - } -#endif -} - -void PhysicsServerSW::sync(){ - -}; - -void PhysicsServerSW::flush_queries() { - -#ifndef _3D_DISABLED - - if (!active) - return; - - doing_sync = true; - - flushing_queries = true; - - uint64_t time_beg = OS::get_singleton()->get_ticks_usec(); - - for (Set<const SpaceSW *>::Element *E = active_spaces.front(); E; E = E->next()) { - - SpaceSW *space = (SpaceSW *)E->get(); - space->call_queries(); - } - - flushing_queries = false; - - if (EngineDebugger::is_profiling("servers")) { - - uint64_t total_time[SpaceSW::ELAPSED_TIME_MAX]; - static const char *time_name[SpaceSW::ELAPSED_TIME_MAX] = { - "integrate_forces", - "generate_islands", - "setup_constraints", - "solve_constraints", - "integrate_velocities" - }; - - for (int i = 0; i < SpaceSW::ELAPSED_TIME_MAX; i++) { - total_time[i] = 0; - } - - for (Set<const SpaceSW *>::Element *E = active_spaces.front(); E; E = E->next()) { - - for (int i = 0; i < SpaceSW::ELAPSED_TIME_MAX; i++) { - total_time[i] += E->get()->get_elapsed_time(SpaceSW::ElapsedTime(i)); - } - } - - Array values; - values.resize(SpaceSW::ELAPSED_TIME_MAX * 2); - for (int i = 0; i < SpaceSW::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"); - EngineDebugger::profiler_add_frame_data("server", values); - } -#endif -}; - -void PhysicsServerSW::finish() { - - memdelete(stepper); - memdelete(direct_state); -}; - -int PhysicsServerSW::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; -} - -void PhysicsServerSW::_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()); - } -} - -void PhysicsServerSW::_shape_col_cbk(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata) { - - CollCbkData *cbk = (CollCbkData *)p_userdata; - - if (cbk->max == 0) - 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; - } - } - - real_t d = p_point_A.distance_squared_to(p_point_B); - if (d < min_depth) - return; - cbk->ptr[min_depth_idx * 2 + 0] = p_point_A; - cbk->ptr[min_depth_idx * 2 + 1] = p_point_B; - - } else { - - cbk->ptr[cbk->amount * 2 + 0] = p_point_A; - cbk->ptr[cbk->amount * 2 + 1] = p_point_B; - cbk->amount++; - } -} - -PhysicsServerSW *PhysicsServerSW::singleton = NULL; -PhysicsServerSW::PhysicsServerSW() { - singleton = this; - BroadPhaseSW::create_func = BroadPhaseOctree::_create; - island_count = 0; - active_objects = 0; - collision_pairs = 0; - - active = true; - flushing_queries = false; -}; - -PhysicsServerSW::~PhysicsServerSW(){ - -}; diff --git a/servers/physics/physics_server_sw.h b/servers/physics/physics_server_sw.h deleted file mode 100644 index 459c7688f0..0000000000 --- a/servers/physics/physics_server_sw.h +++ /dev/null @@ -1,382 +0,0 @@ -/*************************************************************************/ -/* physics_server_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 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_SERVER_SW -#define PHYSICS_SERVER_SW - -#include "core/rid_owner.h" -#include "joints_sw.h" -#include "servers/physics_server.h" -#include "shape_sw.h" -#include "space_sw.h" -#include "step_sw.h" - -class PhysicsServerSW : public PhysicsServer { - - GDCLASS(PhysicsServerSW, PhysicsServer); - - friend class PhysicsDirectSpaceStateSW; - bool active; - int iterations; - bool doing_sync; - real_t last_step; - - int island_count; - int active_objects; - int collision_pairs; - - bool flushing_queries; - - StepSW *stepper; - Set<const SpaceSW *> active_spaces; - - PhysicsDirectBodyStateSW *direct_state; - - mutable RID_PtrOwner<ShapeSW> shape_owner; - mutable RID_PtrOwner<SpaceSW> space_owner; - mutable RID_PtrOwner<AreaSW> area_owner; - mutable RID_PtrOwner<BodySW> body_owner; - mutable RID_PtrOwner<JointSW> joint_owner; - - //void _clear_query(QuerySW *p_query); - friend class CollisionObjectSW; - SelfList<CollisionObjectSW>::List pending_shape_update_list; - void _update_shapes(); - -public: - static PhysicsServerSW *singleton; - - struct CollCbkData { - - int max; - int amount; - Vector3 *ptr; - }; - - static void _shape_col_cbk(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata); - - virtual RID shape_create(ShapeType p_shape); - virtual void shape_set_data(RID p_shape, const Variant &p_data); - virtual void shape_set_custom_solver_bias(RID p_shape, real_t p_bias); - - virtual ShapeType shape_get_type(RID p_shape) const; - virtual Variant shape_get_data(RID p_shape) const; - - virtual void shape_set_margin(RID p_shape, real_t p_margin); - virtual real_t shape_get_margin(RID p_shape) const; - - virtual real_t shape_get_custom_solver_bias(RID p_shape) const; - - /* SPACE API */ - - virtual RID space_create(); - virtual void space_set_active(RID p_space, bool p_active); - virtual bool space_is_active(RID p_space) const; - - virtual void space_set_param(RID p_space, SpaceParameter p_param, real_t p_value); - virtual real_t space_get_param(RID p_space, SpaceParameter p_param) const; - - // this function only works on physics process, errors and returns null otherwise - virtual PhysicsDirectSpaceState *space_get_direct_state(RID p_space); - - virtual void space_set_debug_contacts(RID p_space, int p_max_contacts); - virtual Vector<Vector3> space_get_contacts(RID p_space) const; - virtual int space_get_contact_count(RID p_space) const; - - /* AREA API */ - - virtual RID area_create(); - - virtual void area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode); - virtual AreaSpaceOverrideMode area_get_space_override_mode(RID p_area) const; - - virtual void area_set_space(RID p_area, RID p_space); - virtual RID area_get_space(RID p_area) const; - - virtual void area_add_shape(RID p_area, RID p_shape, const Transform &p_transform = Transform(), bool p_disabled = false); - virtual void area_set_shape(RID p_area, int p_shape_idx, RID p_shape); - virtual void area_set_shape_transform(RID p_area, int p_shape_idx, const Transform &p_transform); - - virtual int area_get_shape_count(RID p_area) const; - virtual RID area_get_shape(RID p_area, int p_shape_idx) const; - virtual Transform area_get_shape_transform(RID p_area, int p_shape_idx) const; - - virtual void area_remove_shape(RID p_area, int p_shape_idx); - virtual void area_clear_shapes(RID p_area); - - virtual void area_set_shape_disabled(RID p_area, int p_shape_idx, bool p_disabled); - - virtual void area_attach_object_instance_id(RID p_area, ObjectID p_id); - virtual ObjectID area_get_object_instance_id(RID p_area) const; - - virtual void area_set_param(RID p_area, AreaParameter p_param, const Variant &p_value); - virtual void area_set_transform(RID p_area, const Transform &p_transform); - - virtual Variant area_get_param(RID p_area, AreaParameter p_param) const; - virtual Transform area_get_transform(RID p_area) const; - - virtual void area_set_ray_pickable(RID p_area, bool p_enable); - virtual bool area_is_ray_pickable(RID p_area) const; - - virtual void area_set_collision_mask(RID p_area, uint32_t p_mask); - virtual void area_set_collision_layer(RID p_area, uint32_t p_layer); - - virtual void area_set_monitorable(RID p_area, bool p_monitorable); - - virtual void area_set_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method); - virtual void area_set_area_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method); - - /* BODY API */ - - // create a body of a given type - virtual RID body_create(BodyMode p_mode = BODY_MODE_RIGID, bool p_init_sleeping = false); - - virtual void body_set_space(RID p_body, RID p_space); - virtual RID body_get_space(RID p_body) const; - - virtual void body_set_mode(RID p_body, BodyMode p_mode); - virtual BodyMode body_get_mode(RID p_body) const; - - virtual void body_add_shape(RID p_body, RID p_shape, const Transform &p_transform = Transform(), bool p_disabled = false); - virtual void body_set_shape(RID p_body, int p_shape_idx, RID p_shape); - virtual void body_set_shape_transform(RID p_body, int p_shape_idx, const Transform &p_transform); - - virtual int body_get_shape_count(RID p_body) const; - virtual RID body_get_shape(RID p_body, int p_shape_idx) const; - virtual Transform body_get_shape_transform(RID p_body, int p_shape_idx) const; - - virtual void body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled); - - virtual void body_remove_shape(RID p_body, int p_shape_idx); - virtual void body_clear_shapes(RID p_body); - - virtual void body_attach_object_instance_id(RID p_body, ObjectID p_id); - virtual ObjectID body_get_object_instance_id(RID p_body) const; - - virtual void body_set_enable_continuous_collision_detection(RID p_body, bool p_enable); - virtual bool body_is_continuous_collision_detection_enabled(RID p_body) const; - - virtual void body_set_collision_layer(RID p_body, uint32_t p_layer); - virtual uint32_t body_get_collision_layer(RID p_body) const; - - virtual void body_set_collision_mask(RID p_body, uint32_t p_mask); - virtual uint32_t body_get_collision_mask(RID p_body) const; - - virtual void body_set_user_flags(RID p_body, uint32_t p_flags); - virtual uint32_t body_get_user_flags(RID p_body) const; - - virtual void body_set_param(RID p_body, BodyParameter p_param, real_t p_value); - virtual real_t body_get_param(RID p_body, BodyParameter p_param) const; - - virtual void body_set_kinematic_safe_margin(RID p_body, real_t p_margin); - virtual real_t body_get_kinematic_safe_margin(RID p_body) const; - - virtual void body_set_state(RID p_body, BodyState p_state, const Variant &p_variant); - virtual Variant body_get_state(RID p_body, BodyState p_state) const; - - virtual void body_set_applied_force(RID p_body, const Vector3 &p_force); - virtual Vector3 body_get_applied_force(RID p_body) const; - - virtual void body_set_applied_torque(RID p_body, const Vector3 &p_torque); - virtual Vector3 body_get_applied_torque(RID p_body) const; - - virtual void body_add_central_force(RID p_body, const Vector3 &p_force); - virtual void body_add_force(RID p_body, const Vector3 &p_force, const Vector3 &p_pos); - virtual void body_add_torque(RID p_body, const Vector3 &p_torque); - - virtual void body_apply_central_impulse(RID p_body, const Vector3 &p_impulse); - virtual void body_apply_impulse(RID p_body, const Vector3 &p_pos, const Vector3 &p_impulse); - virtual void body_apply_torque_impulse(RID p_body, const Vector3 &p_impulse); - virtual void body_set_axis_velocity(RID p_body, const Vector3 &p_axis_velocity); - - virtual void body_set_axis_lock(RID p_body, BodyAxis p_axis, bool p_lock); - virtual bool body_is_axis_locked(RID p_body, BodyAxis p_axis) const; - - virtual void body_add_collision_exception(RID p_body, RID p_body_b); - virtual void body_remove_collision_exception(RID p_body, RID p_body_b); - virtual void body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions); - - virtual void body_set_contacts_reported_depth_threshold(RID p_body, real_t p_threshold); - virtual real_t body_get_contacts_reported_depth_threshold(RID p_body) const; - - virtual void body_set_omit_force_integration(RID p_body, bool p_omit); - virtual bool body_is_omitting_force_integration(RID p_body) const; - - virtual void body_set_max_contacts_reported(RID p_body, int p_contacts); - virtual int body_get_max_contacts_reported(RID p_body) const; - - virtual void body_set_force_integration_callback(RID p_body, Object *p_receiver, const StringName &p_method, const Variant &p_udata = Variant()); - - virtual void body_set_ray_pickable(RID p_body, bool p_enable); - virtual bool body_is_ray_pickable(RID p_body) const; - - virtual bool body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, MotionResult *r_result = NULL, bool p_exclude_raycast_shapes = true); - virtual int body_test_ray_separation(RID p_body, const Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, SeparationResult *r_results, int p_result_max, float p_margin = 0.001); - - // this function only works on physics process, errors and returns null otherwise - virtual PhysicsDirectBodyState *body_get_direct_state(RID p_body); - - /* SOFT BODY */ - - virtual RID soft_body_create(bool p_init_sleeping = false) { return RID(); } - - virtual void soft_body_update_visual_server(RID p_body, class SoftBodyVisualServerHandler *p_visual_server_handler) {} - - virtual void soft_body_set_space(RID p_body, RID p_space) {} - virtual RID soft_body_get_space(RID p_body) const { return RID(); } - - virtual void soft_body_set_collision_layer(RID p_body, uint32_t p_layer) {} - virtual uint32_t soft_body_get_collision_layer(RID p_body) const { return 0; } - - virtual void soft_body_set_collision_mask(RID p_body, uint32_t p_mask) {} - virtual uint32_t soft_body_get_collision_mask(RID p_body) const { return 0; } - - virtual void soft_body_add_collision_exception(RID p_body, RID p_body_b) {} - virtual void soft_body_remove_collision_exception(RID p_body, RID p_body_b) {} - virtual void soft_body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) {} - - virtual void soft_body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) {} - virtual Variant soft_body_get_state(RID p_body, BodyState p_state) const { return Variant(); } - - virtual void soft_body_set_transform(RID p_body, const Transform &p_transform) {} - virtual Vector3 soft_body_get_vertex_position(RID p_body, int vertex_index) const { return Vector3(); } - - virtual void soft_body_set_ray_pickable(RID p_body, bool p_enable) {} - virtual bool soft_body_is_ray_pickable(RID p_body) const { return false; } - - virtual void soft_body_set_simulation_precision(RID p_body, int p_simulation_precision) {} - virtual int soft_body_get_simulation_precision(RID p_body) { return 0; } - - virtual void soft_body_set_total_mass(RID p_body, real_t p_total_mass) {} - virtual real_t soft_body_get_total_mass(RID p_body) { return 0.; } - - virtual void soft_body_set_linear_stiffness(RID p_body, real_t p_stiffness) {} - virtual real_t soft_body_get_linear_stiffness(RID p_body) { return 0.; } - - virtual void soft_body_set_areaAngular_stiffness(RID p_body, real_t p_stiffness) {} - virtual real_t soft_body_get_areaAngular_stiffness(RID p_body) { return 0.; } - - virtual void soft_body_set_volume_stiffness(RID p_body, real_t p_stiffness) {} - virtual real_t soft_body_get_volume_stiffness(RID p_body) { return 0.; } - - virtual void soft_body_set_pressure_coefficient(RID p_body, real_t p_pressure_coefficient) {} - virtual real_t soft_body_get_pressure_coefficient(RID p_body) { return 0.; } - - virtual void soft_body_set_pose_matching_coefficient(RID p_body, real_t p_pose_matching_coefficient) {} - virtual real_t soft_body_get_pose_matching_coefficient(RID p_body) { return 0.; } - - virtual void soft_body_set_damping_coefficient(RID p_body, real_t p_damping_coefficient) {} - virtual real_t soft_body_get_damping_coefficient(RID p_body) { return 0.; } - - virtual void soft_body_set_drag_coefficient(RID p_body, real_t p_drag_coefficient) {} - virtual real_t soft_body_get_drag_coefficient(RID p_body) { return 0.; } - - virtual void soft_body_set_mesh(RID p_body, const REF &p_mesh) {} - - virtual void soft_body_move_point(RID p_body, int p_point_index, const Vector3 &p_global_position) {} - virtual Vector3 soft_body_get_point_global_position(RID p_body, int p_point_index) { return Vector3(); } - - virtual Vector3 soft_body_get_point_offset(RID p_body, int p_point_index) const { return Vector3(); } - - virtual void soft_body_remove_all_pinned_points(RID p_body) {} - virtual void soft_body_pin_point(RID p_body, int p_point_index, bool p_pin) {} - virtual bool soft_body_is_point_pinned(RID p_body, int p_point_index) { return 0; } - - /* JOINT API */ - - virtual RID joint_create_pin(RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B); - - virtual void pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value); - virtual real_t pin_joint_get_param(RID p_joint, PinJointParam p_param) const; - - virtual void pin_joint_set_local_a(RID p_joint, const Vector3 &p_A); - virtual Vector3 pin_joint_get_local_a(RID p_joint) const; - - virtual void pin_joint_set_local_b(RID p_joint, const Vector3 &p_B); - virtual Vector3 pin_joint_get_local_b(RID p_joint) const; - - virtual RID joint_create_hinge(RID p_body_A, const Transform &p_frame_A, RID p_body_B, const Transform &p_frame_B); - virtual RID joint_create_hinge_simple(RID p_body_A, const Vector3 &p_pivot_A, const Vector3 &p_axis_A, RID p_body_B, const Vector3 &p_pivot_B, const Vector3 &p_axis_B); - - virtual void hinge_joint_set_param(RID p_joint, HingeJointParam p_param, real_t p_value); - virtual real_t hinge_joint_get_param(RID p_joint, HingeJointParam p_param) const; - - virtual void hinge_joint_set_flag(RID p_joint, HingeJointFlag p_flag, bool p_value); - virtual bool hinge_joint_get_flag(RID p_joint, HingeJointFlag p_flag) const; - - virtual RID joint_create_slider(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B); //reference frame is A - - virtual void slider_joint_set_param(RID p_joint, SliderJointParam p_param, real_t p_value); - virtual real_t slider_joint_get_param(RID p_joint, SliderJointParam p_param) const; - - virtual RID joint_create_cone_twist(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B); //reference frame is A - - virtual void cone_twist_joint_set_param(RID p_joint, ConeTwistJointParam p_param, real_t p_value); - virtual real_t cone_twist_joint_get_param(RID p_joint, ConeTwistJointParam p_param) const; - - virtual RID joint_create_generic_6dof(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B); //reference frame is A - - virtual void generic_6dof_joint_set_param(RID p_joint, Vector3::Axis, G6DOFJointAxisParam p_param, real_t p_value); - virtual real_t generic_6dof_joint_get_param(RID p_joint, Vector3::Axis, G6DOFJointAxisParam p_param); - - virtual void generic_6dof_joint_set_flag(RID p_joint, Vector3::Axis, G6DOFJointAxisFlag p_flag, bool p_enable); - virtual bool generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis, G6DOFJointAxisFlag p_flag); - - virtual void generic_6dof_joint_set_precision(RID p_joint, int precision) {} - virtual int generic_6dof_joint_get_precision(RID p_joint) { return 0; } - - virtual JointType joint_get_type(RID p_joint) const; - - virtual void joint_set_solver_priority(RID p_joint, int p_priority); - virtual int joint_get_solver_priority(RID p_joint) const; - - virtual void joint_disable_collisions_between_bodies(RID p_joint, const bool p_disable); - virtual bool joint_is_disabled_collisions_between_bodies(RID p_joint) const; - - /* MISC */ - - virtual void free(RID p_rid); - - virtual void set_active(bool p_active); - virtual void init(); - virtual void step(real_t p_step); - virtual void sync(); - virtual void flush_queries(); - virtual void finish(); - - virtual bool is_flushing_queries() const { return flushing_queries; } - - int get_process_info(ProcessInfo p_info); - - PhysicsServerSW(); - ~PhysicsServerSW(); -}; - -#endif diff --git a/servers/physics/shape_sw.cpp b/servers/physics/shape_sw.cpp deleted file mode 100644 index 4a6ed6be58..0000000000 --- a/servers/physics/shape_sw.cpp +++ /dev/null @@ -1,1655 +0,0 @@ -/*************************************************************************/ -/* shape_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 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_sw.h" - -#include "core/math/geometry.h" -#include "core/math/quick_hull.h" -#include "core/sort_array.h" - -#define _POINT_SNAP 0.001953125 -#define _EDGE_IS_VALID_SUPPORT_THRESHOLD 0.0002 -#define _FACE_IS_VALID_SUPPORT_THRESHOLD 0.9998 - -void ShapeSW::configure(const AABB &p_aabb) { - aabb = p_aabb; - configured = true; - for (Map<ShapeOwnerSW *, int>::Element *E = owners.front(); E; E = E->next()) { - ShapeOwnerSW *co = (ShapeOwnerSW *)E->key(); - co->_shape_changed(); - } -} - -Vector3 ShapeSW::get_support(const Vector3 &p_normal) const { - - Vector3 res; - int amnt; - get_supports(p_normal, 1, &res, amnt); - return res; -} - -void ShapeSW::add_owner(ShapeOwnerSW *p_owner) { - - Map<ShapeOwnerSW *, int>::Element *E = owners.find(p_owner); - if (E) { - E->get()++; - } else { - owners[p_owner] = 1; - } -} - -void ShapeSW::remove_owner(ShapeOwnerSW *p_owner) { - - Map<ShapeOwnerSW *, int>::Element *E = owners.find(p_owner); - ERR_FAIL_COND(!E); - E->get()--; - if (E->get() == 0) { - owners.erase(E); - } -} - -bool ShapeSW::is_owner(ShapeOwnerSW *p_owner) const { - - return owners.has(p_owner); -} - -const Map<ShapeOwnerSW *, int> &ShapeSW::get_owners() const { - return owners; -} - -ShapeSW::ShapeSW() { - - custom_bias = 0; - configured = false; -} - -ShapeSW::~ShapeSW() { - - ERR_FAIL_COND(owners.size()); -} - -Plane PlaneShapeSW::get_plane() const { - - return plane; -} - -void PlaneShapeSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { - - // gibberish, a plane is infinity - r_min = -1e7; - r_max = 1e7; -} - -Vector3 PlaneShapeSW::get_support(const Vector3 &p_normal) const { - - return p_normal * 1e15; -} - -bool PlaneShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { - - bool inters = plane.intersects_segment(p_begin, p_end, &r_result); - if (inters) - r_normal = plane.normal; - return inters; -} - -bool PlaneShapeSW::intersect_point(const Vector3 &p_point) const { - - return plane.distance_to(p_point) < 0; -} - -Vector3 PlaneShapeSW::get_closest_point_to(const Vector3 &p_point) const { - - if (plane.is_point_over(p_point)) { - return plane.project(p_point); - } else { - return p_point; - } -} - -Vector3 PlaneShapeSW::get_moment_of_inertia(real_t p_mass) const { - - return Vector3(); //wtf -} - -void PlaneShapeSW::_setup(const Plane &p_plane) { - - plane = p_plane; - configure(AABB(Vector3(-1e4, -1e4, -1e4), Vector3(1e4 * 2, 1e4 * 2, 1e4 * 2))); -} - -void PlaneShapeSW::set_data(const Variant &p_data) { - - _setup(p_data); -} - -Variant PlaneShapeSW::get_data() const { - - return plane; -} - -PlaneShapeSW::PlaneShapeSW() { -} - -// - -real_t RayShapeSW::get_length() const { - - return length; -} - -bool RayShapeSW::get_slips_on_slope() const { - return slips_on_slope; -} - -void RayShapeSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { - - // don't think this will be even used - r_min = 0; - r_max = 1; -} - -Vector3 RayShapeSW::get_support(const Vector3 &p_normal) const { - - if (p_normal.z > 0) - return Vector3(0, 0, length); - else - return Vector3(0, 0, 0); -} - -void RayShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { - - if (Math::abs(p_normal.z) < _EDGE_IS_VALID_SUPPORT_THRESHOLD) { - - r_amount = 2; - r_supports[0] = Vector3(0, 0, 0); - r_supports[1] = Vector3(0, 0, length); - } else if (p_normal.z > 0) { - r_amount = 1; - *r_supports = Vector3(0, 0, length); - } else { - r_amount = 1; - *r_supports = Vector3(0, 0, 0); - } -} - -bool RayShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { - - return false; //simply not possible -} - -bool RayShapeSW::intersect_point(const Vector3 &p_point) const { - - return false; //simply not possible -} - -Vector3 RayShapeSW::get_closest_point_to(const Vector3 &p_point) const { - - Vector3 s[2] = { - Vector3(0, 0, 0), - Vector3(0, 0, length) - }; - - return Geometry::get_closest_point_to_segment(p_point, s); -} - -Vector3 RayShapeSW::get_moment_of_inertia(real_t p_mass) const { - - return Vector3(); -} - -void RayShapeSW::_setup(real_t p_length, bool p_slips_on_slope) { - - length = p_length; - slips_on_slope = p_slips_on_slope; - configure(AABB(Vector3(0, 0, 0), Vector3(0.1, 0.1, length))); -} - -void RayShapeSW::set_data(const Variant &p_data) { - - Dictionary d = p_data; - _setup(d["length"], d["slips_on_slope"]); -} - -Variant RayShapeSW::get_data() const { - - Dictionary d; - d["length"] = length; - d["slips_on_slope"] = slips_on_slope; - return d; -} - -RayShapeSW::RayShapeSW() { - - length = 1; - slips_on_slope = false; -} - -/********** SPHERE *************/ - -real_t SphereShapeSW::get_radius() const { - - return radius; -} - -void SphereShapeSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { - - real_t d = p_normal.dot(p_transform.origin); - - // figure out scale at point - Vector3 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; -} - -Vector3 SphereShapeSW::get_support(const Vector3 &p_normal) const { - - return p_normal * radius; -} - -void SphereShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { - - *r_supports = p_normal * radius; - r_amount = 1; -} - -bool SphereShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { - - return Geometry::segment_intersects_sphere(p_begin, p_end, Vector3(), radius, &r_result, &r_normal); -} - -bool SphereShapeSW::intersect_point(const Vector3 &p_point) const { - - return p_point.length() < radius; -} - -Vector3 SphereShapeSW::get_closest_point_to(const Vector3 &p_point) const { - - Vector3 p = p_point; - float l = p.length(); - if (l < radius) - return p_point; - return (p / l) * radius; -} - -Vector3 SphereShapeSW::get_moment_of_inertia(real_t p_mass) const { - - real_t s = 0.4 * p_mass * radius * radius; - return Vector3(s, s, s); -} - -void SphereShapeSW::_setup(real_t p_radius) { - - radius = p_radius; - configure(AABB(Vector3(-radius, -radius, -radius), Vector3(radius * 2.0, radius * 2.0, radius * 2.0))); -} - -void SphereShapeSW::set_data(const Variant &p_data) { - - _setup(p_data); -} - -Variant SphereShapeSW::get_data() const { - - return radius; -} - -SphereShapeSW::SphereShapeSW() { - - radius = 0; -} - -/********** BOX *************/ - -void BoxShapeSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { - - // no matter the angle, the box is mirrored anyway - Vector3 local_normal = p_transform.basis.xform_inv(p_normal); - - real_t length = local_normal.abs().dot(half_extents); - real_t distance = p_normal.dot(p_transform.origin); - - r_min = distance - length; - r_max = distance + length; -} - -Vector3 BoxShapeSW::get_support(const Vector3 &p_normal) const { - - Vector3 point( - (p_normal.x < 0) ? -half_extents.x : half_extents.x, - (p_normal.y < 0) ? -half_extents.y : half_extents.y, - (p_normal.z < 0) ? -half_extents.z : half_extents.z); - - return point; -} - -void BoxShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { - - static const int next[3] = { 1, 2, 0 }; - static const int next2[3] = { 2, 0, 1 }; - - for (int i = 0; i < 3; i++) { - - Vector3 axis; - axis[i] = 1.0; - real_t dot = p_normal.dot(axis); - if (Math::abs(dot) > _FACE_IS_VALID_SUPPORT_THRESHOLD) { - - //Vector3 axis_b; - - bool neg = dot < 0; - r_amount = 4; - - Vector3 point; - point[i] = half_extents[i]; - - int i_n = next[i]; - int i_n2 = next2[i]; - - static const real_t sign[4][2] = { - - { -1.0, 1.0 }, - { 1.0, 1.0 }, - { 1.0, -1.0 }, - { -1.0, -1.0 }, - }; - - for (int j = 0; j < 4; j++) { - - point[i_n] = sign[j][0] * half_extents[i_n]; - point[i_n2] = sign[j][1] * half_extents[i_n2]; - r_supports[j] = neg ? -point : point; - } - - if (neg) { - SWAP(r_supports[1], r_supports[2]); - SWAP(r_supports[0], r_supports[3]); - } - - return; - } - - r_amount = 0; - } - - for (int i = 0; i < 3; i++) { - - Vector3 axis; - axis[i] = 1.0; - - if (Math::abs(p_normal.dot(axis)) < _EDGE_IS_VALID_SUPPORT_THRESHOLD) { - - r_amount = 2; - - int i_n = next[i]; - int i_n2 = next2[i]; - - Vector3 point = half_extents; - - if (p_normal[i_n] < 0) { - point[i_n] = -point[i_n]; - } - if (p_normal[i_n2] < 0) { - point[i_n2] = -point[i_n2]; - } - - r_supports[0] = point; - point[i] = -point[i]; - r_supports[1] = point; - return; - } - } - /* USE POINT */ - - Vector3 point( - (p_normal.x < 0) ? -half_extents.x : half_extents.x, - (p_normal.y < 0) ? -half_extents.y : half_extents.y, - (p_normal.z < 0) ? -half_extents.z : half_extents.z); - - r_amount = 1; - r_supports[0] = point; -} - -bool BoxShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { - - AABB aabb(-half_extents, half_extents * 2.0); - - return aabb.intersects_segment(p_begin, p_end, &r_result, &r_normal); -} - -bool BoxShapeSW::intersect_point(const Vector3 &p_point) const { - - return (Math::abs(p_point.x) < half_extents.x && Math::abs(p_point.y) < half_extents.y && Math::abs(p_point.z) < half_extents.z); -} - -Vector3 BoxShapeSW::get_closest_point_to(const Vector3 &p_point) const { - - int outside = 0; - Vector3 min_point; - - for (int i = 0; i < 3; i++) { - - if (Math::abs(p_point[i]) > half_extents[i]) { - outside++; - if (outside == 1) { - //use plane if only one side matches - Vector3 n; - n[i] = SGN(p_point[i]); - - Plane p(n, half_extents[i]); - min_point = p.project(p_point); - } - } - } - - if (!outside) - return p_point; //it's inside, don't do anything else - - if (outside == 1) //if only above one plane, this plane clearly wins - return min_point; - - //check segments - float min_distance = 1e20; - Vector3 closest_vertex = half_extents * p_point.sign(); - Vector3 s[2] = { - closest_vertex, - closest_vertex - }; - - for (int i = 0; i < 3; i++) { - - s[1] = closest_vertex; - s[1][i] = -s[1][i]; //edge - - Vector3 closest_edge = Geometry::get_closest_point_to_segment(p_point, s); - - float d = p_point.distance_to(closest_edge); - if (d < min_distance) { - min_point = closest_edge; - min_distance = d; - } - } - - return min_point; -} - -Vector3 BoxShapeSW::get_moment_of_inertia(real_t p_mass) const { - - real_t lx = half_extents.x; - real_t ly = half_extents.y; - real_t lz = half_extents.z; - - return Vector3((p_mass / 3.0) * (ly * ly + lz * lz), (p_mass / 3.0) * (lx * lx + lz * lz), (p_mass / 3.0) * (lx * lx + ly * ly)); -} - -void BoxShapeSW::_setup(const Vector3 &p_half_extents) { - - half_extents = p_half_extents.abs(); - - configure(AABB(-half_extents, half_extents * 2)); -} - -void BoxShapeSW::set_data(const Variant &p_data) { - - _setup(p_data); -} - -Variant BoxShapeSW::get_data() const { - - return half_extents; -} - -BoxShapeSW::BoxShapeSW() { -} - -/********** CAPSULE *************/ - -void CapsuleShapeSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { - - Vector3 n = p_transform.basis.xform_inv(p_normal).normalized(); - real_t h = (n.z > 0) ? height : -height; - - n *= radius; - n.z += h * 0.5; - - r_max = p_normal.dot(p_transform.xform(n)); - r_min = p_normal.dot(p_transform.xform(-n)); -} - -Vector3 CapsuleShapeSW::get_support(const Vector3 &p_normal) const { - - Vector3 n = p_normal; - - real_t h = (n.z > 0) ? height : -height; - - n *= radius; - n.z += h * 0.5; - return n; -} - -void CapsuleShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { - - Vector3 n = p_normal; - - real_t d = n.z; - - if (Math::abs(d) < _EDGE_IS_VALID_SUPPORT_THRESHOLD) { - - // make it flat - n.z = 0.0; - n.normalize(); - n *= radius; - - r_amount = 2; - r_supports[0] = n; - r_supports[0].z += height * 0.5; - r_supports[1] = n; - r_supports[1].z -= height * 0.5; - - } else { - - real_t h = (d > 0) ? height : -height; - - n *= radius; - n.z += h * 0.5; - r_amount = 1; - *r_supports = n; - } -} - -bool CapsuleShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { - - Vector3 norm = (p_end - p_begin).normalized(); - real_t min_d = 1e20; - - Vector3 res, n; - bool collision = false; - - Vector3 auxres, auxn; - bool collided; - - // test against cylinder and spheres :-| - - collided = Geometry::segment_intersects_cylinder(p_begin, p_end, height, radius, &auxres, &auxn); - - if (collided) { - real_t d = norm.dot(auxres); - if (d < min_d) { - min_d = d; - res = auxres; - n = auxn; - collision = true; - } - } - - collided = Geometry::segment_intersects_sphere(p_begin, p_end, Vector3(0, 0, height * 0.5), radius, &auxres, &auxn); - - if (collided) { - real_t d = norm.dot(auxres); - if (d < min_d) { - min_d = d; - res = auxres; - n = auxn; - collision = true; - } - } - - collided = Geometry::segment_intersects_sphere(p_begin, p_end, Vector3(0, 0, height * -0.5), radius, &auxres, &auxn); - - if (collided) { - real_t d = norm.dot(auxres); - - if (d < min_d) { - min_d = d; - res = auxres; - n = auxn; - collision = true; - } - } - - if (collision) { - - r_result = res; - r_normal = n; - } - return collision; -} - -bool CapsuleShapeSW::intersect_point(const Vector3 &p_point) const { - - if (Math::abs(p_point.z) < height * 0.5) { - return Vector3(p_point.x, p_point.y, 0).length() < radius; - } else { - Vector3 p = p_point; - p.z = Math::abs(p.z) - height * 0.5; - return p.length() < radius; - } -} - -Vector3 CapsuleShapeSW::get_closest_point_to(const Vector3 &p_point) const { - - Vector3 s[2] = { - Vector3(0, 0, -height * 0.5), - Vector3(0, 0, height * 0.5), - }; - - Vector3 p = Geometry::get_closest_point_to_segment(p_point, s); - - if (p.distance_to(p_point) < radius) - return p_point; - - return p + (p_point - p).normalized() * radius; -} - -Vector3 CapsuleShapeSW::get_moment_of_inertia(real_t p_mass) const { - - // use bad AABB approximation - Vector3 extents = get_aabb().size * 0.5; - - return Vector3( - (p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z), - (p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z), - (p_mass / 3.0) * (extents.y * extents.y + extents.y * extents.y)); -} - -void CapsuleShapeSW::_setup(real_t p_height, real_t p_radius) { - - height = p_height; - radius = p_radius; - configure(AABB(Vector3(-radius, -radius, -height * 0.5 - radius), Vector3(radius * 2, radius * 2, height + radius * 2.0))); -} - -void CapsuleShapeSW::set_data(const Variant &p_data) { - - Dictionary d = p_data; - ERR_FAIL_COND(!d.has("radius")); - ERR_FAIL_COND(!d.has("height")); - _setup(d["height"], d["radius"]); -} - -Variant CapsuleShapeSW::get_data() const { - - Dictionary d; - d["radius"] = radius; - d["height"] = height; - return d; -} - -CapsuleShapeSW::CapsuleShapeSW() { - - height = radius = 0; -} - -/********** CONVEX POLYGON *************/ - -void ConvexPolygonShapeSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { - - int vertex_count = mesh.vertices.size(); - if (vertex_count == 0) - return; - - const Vector3 *vrts = &mesh.vertices[0]; - - for (int i = 0; i < vertex_count; i++) { - - real_t d = p_normal.dot(p_transform.xform(vrts[i])); - - if (i == 0 || d > r_max) - r_max = d; - if (i == 0 || d < r_min) - r_min = d; - } -} - -Vector3 ConvexPolygonShapeSW::get_support(const Vector3 &p_normal) const { - - Vector3 n = p_normal; - - int vert_support_idx = -1; - real_t support_max = 0; - - int vertex_count = mesh.vertices.size(); - if (vertex_count == 0) - return Vector3(); - - const Vector3 *vrts = &mesh.vertices[0]; - - for (int i = 0; i < vertex_count; i++) { - - real_t d = n.dot(vrts[i]); - - if (i == 0 || d > support_max) { - support_max = d; - vert_support_idx = i; - } - } - - return vrts[vert_support_idx]; -} - -void ConvexPolygonShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { - - const Geometry::MeshData::Face *faces = mesh.faces.ptr(); - int fc = mesh.faces.size(); - - const Geometry::MeshData::Edge *edges = mesh.edges.ptr(); - int ec = mesh.edges.size(); - - const Vector3 *vertices = mesh.vertices.ptr(); - int vc = mesh.vertices.size(); - - //find vertex first - real_t max = 0; - int vtx = 0; - - for (int i = 0; i < vc; i++) { - - real_t d = p_normal.dot(vertices[i]); - - if (i == 0 || d > max) { - max = d; - vtx = i; - } - } - - for (int i = 0; i < fc; i++) { - - if (faces[i].plane.normal.dot(p_normal) > _FACE_IS_VALID_SUPPORT_THRESHOLD) { - - int ic = faces[i].indices.size(); - const int *ind = faces[i].indices.ptr(); - - bool valid = false; - for (int j = 0; j < ic; j++) { - if (ind[j] == vtx) { - valid = true; - break; - } - } - - if (!valid) - continue; - - int m = MIN(p_max, ic); - for (int j = 0; j < m; j++) { - - r_supports[j] = vertices[ind[j]]; - } - r_amount = m; - return; - } - } - - for (int i = 0; i < ec; i++) { - - real_t dot = (vertices[edges[i].a] - vertices[edges[i].b]).normalized().dot(p_normal); - dot = ABS(dot); - if (dot < _EDGE_IS_VALID_SUPPORT_THRESHOLD && (edges[i].a == vtx || edges[i].b == vtx)) { - - r_amount = 2; - r_supports[0] = vertices[edges[i].a]; - r_supports[1] = vertices[edges[i].b]; - return; - } - } - - r_supports[0] = vertices[vtx]; - r_amount = 1; -} - -bool ConvexPolygonShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { - - const Geometry::MeshData::Face *faces = mesh.faces.ptr(); - int fc = mesh.faces.size(); - - const Vector3 *vertices = mesh.vertices.ptr(); - - Vector3 n = p_end - p_begin; - real_t min = 1e20; - bool col = false; - - for (int i = 0; i < fc; i++) { - - if (faces[i].plane.normal.dot(n) > 0) - continue; //opposing face - - int ic = faces[i].indices.size(); - const int *ind = faces[i].indices.ptr(); - - for (int j = 1; j < ic - 1; j++) { - - Face3 f(vertices[ind[0]], vertices[ind[j]], vertices[ind[j + 1]]); - Vector3 result; - if (f.intersects_segment(p_begin, p_end, &result)) { - real_t d = n.dot(result); - if (d < min) { - min = d; - r_result = result; - r_normal = faces[i].plane.normal; - col = true; - } - - break; - } - } - } - - return col; -} - -bool ConvexPolygonShapeSW::intersect_point(const Vector3 &p_point) const { - - const Geometry::MeshData::Face *faces = mesh.faces.ptr(); - int fc = mesh.faces.size(); - - for (int i = 0; i < fc; i++) { - - if (faces[i].plane.distance_to(p_point) >= 0) - return false; - } - - return true; -} - -Vector3 ConvexPolygonShapeSW::get_closest_point_to(const Vector3 &p_point) const { - - const Geometry::MeshData::Face *faces = mesh.faces.ptr(); - int fc = mesh.faces.size(); - const Vector3 *vertices = mesh.vertices.ptr(); - - bool all_inside = true; - for (int i = 0; i < fc; i++) { - - if (!faces[i].plane.is_point_over(p_point)) - continue; - - all_inside = false; - bool is_inside = true; - int ic = faces[i].indices.size(); - const int *indices = faces[i].indices.ptr(); - - for (int j = 0; j < ic; j++) { - - Vector3 a = vertices[indices[j]]; - Vector3 b = vertices[indices[(j + 1) % ic]]; - Vector3 n = (a - b).cross(faces[i].plane.normal).normalized(); - if (Plane(a, n).is_point_over(p_point)) { - is_inside = false; - break; - } - } - - if (is_inside) { - return faces[i].plane.project(p_point); - } - } - - if (all_inside) { - return p_point; - } - - float min_distance = 1e20; - Vector3 min_point; - - //check edges - const Geometry::MeshData::Edge *edges = mesh.edges.ptr(); - int ec = mesh.edges.size(); - for (int i = 0; i < ec; i++) { - - Vector3 s[2] = { - vertices[edges[i].a], - vertices[edges[i].b] - }; - - Vector3 closest = Geometry::get_closest_point_to_segment(p_point, s); - float d = closest.distance_to(p_point); - if (d < min_distance) { - min_distance = d; - min_point = closest; - } - } - - return min_point; -} - -Vector3 ConvexPolygonShapeSW::get_moment_of_inertia(real_t p_mass) const { - - // use bad AABB approximation - Vector3 extents = get_aabb().size * 0.5; - - return Vector3( - (p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z), - (p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z), - (p_mass / 3.0) * (extents.y * extents.y + extents.y * extents.y)); -} - -void ConvexPolygonShapeSW::_setup(const Vector<Vector3> &p_vertices) { - - Error err = QuickHull::build(p_vertices, mesh); - if (err != OK) - ERR_PRINT("Failed to build QuickHull"); - - AABB _aabb; - - for (int i = 0; i < mesh.vertices.size(); i++) { - - if (i == 0) - _aabb.position = mesh.vertices[i]; - else - _aabb.expand_to(mesh.vertices[i]); - } - - configure(_aabb); -} - -void ConvexPolygonShapeSW::set_data(const Variant &p_data) { - - _setup(p_data); -} - -Variant ConvexPolygonShapeSW::get_data() const { - - return mesh.vertices; -} - -ConvexPolygonShapeSW::ConvexPolygonShapeSW() { -} - -/********** FACE POLYGON *************/ - -void FaceShapeSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { - - for (int i = 0; i < 3; i++) { - - Vector3 v = p_transform.xform(vertex[i]); - real_t d = p_normal.dot(v); - - if (i == 0 || d > r_max) - r_max = d; - - if (i == 0 || d < r_min) - r_min = d; - } -} - -Vector3 FaceShapeSW::get_support(const Vector3 &p_normal) const { - - int vert_support_idx = -1; - real_t support_max = 0; - - for (int i = 0; i < 3; i++) { - - real_t d = p_normal.dot(vertex[i]); - - if (i == 0 || d > support_max) { - support_max = d; - vert_support_idx = i; - } - } - - return vertex[vert_support_idx]; -} - -void FaceShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { - - Vector3 n = p_normal; - - /** TEST FACE AS SUPPORT **/ - if (normal.dot(n) > _FACE_IS_VALID_SUPPORT_THRESHOLD) { - - r_amount = 3; - for (int i = 0; i < 3; i++) { - - r_supports[i] = vertex[i]; - } - return; - } - - /** FIND SUPPORT VERTEX **/ - - int vert_support_idx = -1; - real_t support_max = 0; - - for (int i = 0; i < 3; i++) { - - real_t d = n.dot(vertex[i]); - - if (i == 0 || d > support_max) { - support_max = d; - vert_support_idx = i; - } - } - - /** TEST EDGES AS SUPPORT **/ - - for (int i = 0; i < 3; i++) { - - int nx = (i + 1) % 3; - if (i != vert_support_idx && nx != vert_support_idx) - continue; - - // check if edge is valid as a support - real_t dot = (vertex[i] - vertex[nx]).normalized().dot(n); - dot = ABS(dot); - if (dot < _EDGE_IS_VALID_SUPPORT_THRESHOLD) { - - r_amount = 2; - r_supports[0] = vertex[i]; - r_supports[1] = vertex[nx]; - return; - } - } - - r_amount = 1; - r_supports[0] = vertex[vert_support_idx]; -} - -bool FaceShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { - - bool c = Geometry::segment_intersects_triangle(p_begin, p_end, vertex[0], vertex[1], vertex[2], &r_result); - if (c) { - r_normal = Plane(vertex[0], vertex[1], vertex[2]).normal; - if (r_normal.dot(p_end - p_begin) > 0) { - r_normal = -r_normal; - } - } - - return c; -} - -bool FaceShapeSW::intersect_point(const Vector3 &p_point) const { - - return false; //face is flat -} - -Vector3 FaceShapeSW::get_closest_point_to(const Vector3 &p_point) const { - - return Face3(vertex[0], vertex[1], vertex[2]).get_closest_point_to(p_point); -} - -Vector3 FaceShapeSW::get_moment_of_inertia(real_t p_mass) const { - - return Vector3(); // Sorry, but i don't think anyone cares, FaceShape! -} - -FaceShapeSW::FaceShapeSW() { - - configure(AABB()); -} - -Vector<Vector3> ConcavePolygonShapeSW::get_faces() const { - - Vector<Vector3> rfaces; - rfaces.resize(faces.size() * 3); - - for (int i = 0; i < faces.size(); i++) { - - Face f = faces.get(i); - - for (int j = 0; j < 3; j++) { - - rfaces.set(i * 3 + j, vertices.get(f.indices[j])); - } - } - - return rfaces; -} - -void ConcavePolygonShapeSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { - - int count = vertices.size(); - if (count == 0) { - r_min = 0; - r_max = 0; - return; - } - const Vector3 *vptr = vertices.ptr(); - - for (int i = 0; i < count; i++) { - - real_t d = p_normal.dot(p_transform.xform(vptr[i])); - - if (i == 0 || d > r_max) - r_max = d; - if (i == 0 || d < r_min) - r_min = d; - } -} - -Vector3 ConcavePolygonShapeSW::get_support(const Vector3 &p_normal) const { - - int count = vertices.size(); - if (count == 0) - return Vector3(); - - const Vector3 *vptr = vertices.ptr(); - - Vector3 n = p_normal; - - int vert_support_idx = -1; - real_t support_max = 0; - - for (int i = 0; i < count; i++) { - - real_t d = n.dot(vptr[i]); - - if (i == 0 || d > support_max) { - support_max = d; - vert_support_idx = i; - } - } - - return vptr[vert_support_idx]; -} - -void ConcavePolygonShapeSW::_cull_segment(int p_idx, _SegmentCullParams *p_params) const { - - const BVH *bvh = &p_params->bvh[p_idx]; - - /* - if (p_params->dir.dot(bvh->aabb.get_support(-p_params->dir))>p_params->min_d) - return; //test against whole AABB, which isn't very costly - */ - - //printf("addr: %p\n",bvh); - if (!bvh->aabb.intersects_segment(p_params->from, p_params->to)) { - - return; - } - - if (bvh->face_index >= 0) { - - Vector3 res; - Vector3 vertices[3] = { - p_params->vertices[p_params->faces[bvh->face_index].indices[0]], - p_params->vertices[p_params->faces[bvh->face_index].indices[1]], - p_params->vertices[p_params->faces[bvh->face_index].indices[2]] - }; - - if (Geometry::segment_intersects_triangle( - p_params->from, - p_params->to, - vertices[0], - vertices[1], - vertices[2], - &res)) { - - real_t d = p_params->dir.dot(res) - p_params->dir.dot(p_params->from); - //TODO, seems segmen/triangle intersection is broken :( - if (d > 0 && d < p_params->min_d) { - - p_params->min_d = d; - p_params->result = res; - p_params->normal = Plane(vertices[0], vertices[1], vertices[2]).normal; - p_params->collisions++; - } - } - - } else { - - if (bvh->left >= 0) - _cull_segment(bvh->left, p_params); - if (bvh->right >= 0) - _cull_segment(bvh->right, p_params); - } -} - -bool ConcavePolygonShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { - - if (faces.size() == 0) - return false; - - // unlock data - const Face *fr = faces.ptr(); - const Vector3 *vr = vertices.ptr(); - const BVH *br = bvh.ptr(); - - _SegmentCullParams params; - params.from = p_begin; - params.to = p_end; - params.collisions = 0; - params.dir = (p_end - p_begin).normalized(); - - params.faces = fr; - params.vertices = vr; - params.bvh = br; - - params.min_d = 1e20; - // cull - _cull_segment(0, ¶ms); - - if (params.collisions > 0) { - - r_result = params.result; - r_normal = params.normal; - return true; - } else { - - return false; - } -} - -bool ConcavePolygonShapeSW::intersect_point(const Vector3 &p_point) const { - - return false; //face is flat -} - -Vector3 ConcavePolygonShapeSW::get_closest_point_to(const Vector3 &p_point) const { - - return Vector3(); -} - -void ConcavePolygonShapeSW::_cull(int p_idx, _CullParams *p_params) const { - - const BVH *bvh = &p_params->bvh[p_idx]; - - if (!p_params->aabb.intersects(bvh->aabb)) - return; - - if (bvh->face_index >= 0) { - - const Face *f = &p_params->faces[bvh->face_index]; - FaceShapeSW *face = p_params->face; - face->normal = f->normal; - face->vertex[0] = p_params->vertices[f->indices[0]]; - face->vertex[1] = p_params->vertices[f->indices[1]]; - face->vertex[2] = p_params->vertices[f->indices[2]]; - p_params->callback(p_params->userdata, face); - - } else { - - if (bvh->left >= 0) { - - _cull(bvh->left, p_params); - } - - if (bvh->right >= 0) { - - _cull(bvh->right, p_params); - } - } -} - -void ConcavePolygonShapeSW::cull(const AABB &p_local_aabb, Callback p_callback, void *p_userdata) const { - - // make matrix local to concave - if (faces.size() == 0) - return; - - AABB local_aabb = p_local_aabb; - - // unlock data - const Face *fr = faces.ptr(); - const Vector3 *vr = vertices.ptr(); - const BVH *br = bvh.ptr(); - - FaceShapeSW face; // use this to send in the callback - - _CullParams params; - params.aabb = local_aabb; - params.face = &face; - params.faces = fr; - params.vertices = vr; - params.bvh = br; - params.callback = p_callback; - params.userdata = p_userdata; - - // cull - _cull(0, ¶ms); -} - -Vector3 ConcavePolygonShapeSW::get_moment_of_inertia(real_t p_mass) const { - - // use bad AABB approximation - Vector3 extents = get_aabb().size * 0.5; - - return Vector3( - (p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z), - (p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z), - (p_mass / 3.0) * (extents.y * extents.y + extents.y * extents.y)); -} - -struct _VolumeSW_BVH_Element { - - AABB aabb; - Vector3 center; - int face_index; -}; - -struct _VolumeSW_BVH_CompareX { - - _FORCE_INLINE_ bool operator()(const _VolumeSW_BVH_Element &a, const _VolumeSW_BVH_Element &b) const { - - return a.center.x < b.center.x; - } -}; - -struct _VolumeSW_BVH_CompareY { - - _FORCE_INLINE_ bool operator()(const _VolumeSW_BVH_Element &a, const _VolumeSW_BVH_Element &b) const { - - return a.center.y < b.center.y; - } -}; - -struct _VolumeSW_BVH_CompareZ { - - _FORCE_INLINE_ bool operator()(const _VolumeSW_BVH_Element &a, const _VolumeSW_BVH_Element &b) const { - - return a.center.z < b.center.z; - } -}; - -struct _VolumeSW_BVH { - - AABB aabb; - _VolumeSW_BVH *left; - _VolumeSW_BVH *right; - - int face_index; -}; - -_VolumeSW_BVH *_volume_sw_build_bvh(_VolumeSW_BVH_Element *p_elements, int p_size, int &count) { - - _VolumeSW_BVH *bvh = memnew(_VolumeSW_BVH); - - if (p_size == 1) { - //leaf - bvh->aabb = p_elements[0].aabb; - bvh->left = NULL; - bvh->right = NULL; - bvh->face_index = p_elements->face_index; - count++; - return bvh; - } else { - - bvh->face_index = -1; - } - - AABB aabb; - for (int i = 0; i < p_size; i++) { - - if (i == 0) - aabb = p_elements[i].aabb; - else - aabb.merge_with(p_elements[i].aabb); - } - bvh->aabb = aabb; - switch (aabb.get_longest_axis_index()) { - - case 0: { - - SortArray<_VolumeSW_BVH_Element, _VolumeSW_BVH_CompareX> sort_x; - sort_x.sort(p_elements, p_size); - - } break; - case 1: { - - SortArray<_VolumeSW_BVH_Element, _VolumeSW_BVH_CompareY> sort_y; - sort_y.sort(p_elements, p_size); - } break; - case 2: { - - SortArray<_VolumeSW_BVH_Element, _VolumeSW_BVH_CompareZ> sort_z; - sort_z.sort(p_elements, p_size); - } break; - } - - int split = p_size / 2; - bvh->left = _volume_sw_build_bvh(p_elements, split, count); - bvh->right = _volume_sw_build_bvh(&p_elements[split], p_size - split, count); - - //printf("branch at %p - %i: %i\n",bvh,count,bvh->face_index); - count++; - return bvh; -} - -void ConcavePolygonShapeSW::_fill_bvh(_VolumeSW_BVH *p_bvh_tree, BVH *p_bvh_array, int &p_idx) { - - int idx = p_idx; - - p_bvh_array[idx].aabb = p_bvh_tree->aabb; - p_bvh_array[idx].face_index = p_bvh_tree->face_index; - //printf("%p - %i: %i(%p) -- %p:%p\n",%p_bvh_array[idx],p_idx,p_bvh_array[i]->face_index,&p_bvh_tree->face_index,p_bvh_tree->left,p_bvh_tree->right); - - if (p_bvh_tree->left) { - p_bvh_array[idx].left = ++p_idx; - _fill_bvh(p_bvh_tree->left, p_bvh_array, p_idx); - - } else { - - p_bvh_array[p_idx].left = -1; - } - - if (p_bvh_tree->right) { - p_bvh_array[idx].right = ++p_idx; - _fill_bvh(p_bvh_tree->right, p_bvh_array, p_idx); - - } else { - - p_bvh_array[p_idx].right = -1; - } - - memdelete(p_bvh_tree); -} - -void ConcavePolygonShapeSW::_setup(Vector<Vector3> p_faces) { - - int src_face_count = p_faces.size(); - if (src_face_count == 0) { - configure(AABB()); - return; - } - ERR_FAIL_COND(src_face_count % 3); - src_face_count /= 3; - - const Vector3 *facesr = p_faces.ptr(); - - Vector<_VolumeSW_BVH_Element> bvh_array; - bvh_array.resize(src_face_count); - - _VolumeSW_BVH_Element *bvh_arrayw = bvh_array.ptrw(); - - faces.resize(src_face_count); - Face *facesw = faces.ptrw(); - - vertices.resize(src_face_count * 3); - - Vector3 *verticesw = vertices.ptrw(); - - AABB _aabb; - - for (int i = 0; i < src_face_count; i++) { - - Face3 face(facesr[i * 3 + 0], facesr[i * 3 + 1], facesr[i * 3 + 2]); - - bvh_arrayw[i].aabb = face.get_aabb(); - bvh_arrayw[i].center = bvh_arrayw[i].aabb.position + bvh_arrayw[i].aabb.size * 0.5; - bvh_arrayw[i].face_index = i; - facesw[i].indices[0] = i * 3 + 0; - facesw[i].indices[1] = i * 3 + 1; - facesw[i].indices[2] = i * 3 + 2; - facesw[i].normal = face.get_plane().normal; - verticesw[i * 3 + 0] = face.vertex[0]; - verticesw[i * 3 + 1] = face.vertex[1]; - verticesw[i * 3 + 2] = face.vertex[2]; - if (i == 0) - _aabb = bvh_arrayw[i].aabb; - else - _aabb.merge_with(bvh_arrayw[i].aabb); - } - - int count = 0; - _VolumeSW_BVH *bvh_tree = _volume_sw_build_bvh(bvh_arrayw, src_face_count, count); - - bvh.resize(count + 1); - - BVH *bvh_arrayw2 = bvh.ptrw(); - - int idx = 0; - _fill_bvh(bvh_tree, bvh_arrayw2, idx); - - configure(_aabb); // this type of shape has no margin -} - -void ConcavePolygonShapeSW::set_data(const Variant &p_data) { - - _setup(p_data); -} - -Variant ConcavePolygonShapeSW::get_data() const { - - return get_faces(); -} - -ConcavePolygonShapeSW::ConcavePolygonShapeSW() { -} - -/* HEIGHT MAP SHAPE */ - -Vector<real_t> HeightMapShapeSW::get_heights() const { - - return heights; -} -int HeightMapShapeSW::get_width() const { - - return width; -} -int HeightMapShapeSW::get_depth() const { - - return depth; -} -real_t HeightMapShapeSW::get_cell_size() const { - - return cell_size; -} - -void HeightMapShapeSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { - - //not very useful, but not very used either - p_transform.xform(get_aabb()).project_range_in_plane(Plane(p_normal, 0), r_min, r_max); -} - -Vector3 HeightMapShapeSW::get_support(const Vector3 &p_normal) const { - - //not very useful, but not very used either - return get_aabb().get_support(p_normal); -} - -bool HeightMapShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal) const { - - return false; -} - -bool HeightMapShapeSW::intersect_point(const Vector3 &p_point) const { - return false; -} - -Vector3 HeightMapShapeSW::get_closest_point_to(const Vector3 &p_point) const { - - return Vector3(); -} - -void HeightMapShapeSW::cull(const AABB &p_local_aabb, Callback p_callback, void *p_userdata) const { -} - -Vector3 HeightMapShapeSW::get_moment_of_inertia(real_t p_mass) const { - - // use bad AABB approximation - Vector3 extents = get_aabb().size * 0.5; - - return Vector3( - (p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z), - (p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z), - (p_mass / 3.0) * (extents.y * extents.y + extents.y * extents.y)); -} - -void HeightMapShapeSW::_setup(Vector<real_t> p_heights, int p_width, int p_depth, real_t p_cell_size) { - - heights = p_heights; - width = p_width; - depth = p_depth; - cell_size = p_cell_size; - - const real_t *r = heights.ptr(); - - AABB aabb; - - for (int i = 0; i < depth; i++) { - - for (int j = 0; j < width; j++) { - - real_t h = r[i * width + j]; - - Vector3 pos(j * cell_size, h, i * cell_size); - if (i == 0 || j == 0) - aabb.position = pos; - else - aabb.expand_to(pos); - } - } - - configure(aabb); -} - -void HeightMapShapeSW::set_data(const Variant &p_data) { - - ERR_FAIL_COND(p_data.get_type() != Variant::DICTIONARY); - Dictionary d = p_data; - ERR_FAIL_COND(!d.has("width")); - ERR_FAIL_COND(!d.has("depth")); - ERR_FAIL_COND(!d.has("cell_size")); - ERR_FAIL_COND(!d.has("heights")); - - int width = d["width"]; - int depth = d["depth"]; - real_t cell_size = d["cell_size"]; - Vector<real_t> heights = d["heights"]; - - ERR_FAIL_COND(width <= 0); - ERR_FAIL_COND(depth <= 0); - ERR_FAIL_COND(cell_size <= CMP_EPSILON); - ERR_FAIL_COND(heights.size() != (width * depth)); - _setup(heights, width, depth, cell_size); -} - -Variant HeightMapShapeSW::get_data() const { - - ERR_FAIL_V(Variant()); -} - -HeightMapShapeSW::HeightMapShapeSW() { - - width = 0; - depth = 0; - cell_size = 0; -} diff --git a/servers/physics/shape_sw.h b/servers/physics/shape_sw.h deleted file mode 100644 index eaae64be66..0000000000 --- a/servers/physics/shape_sw.h +++ /dev/null @@ -1,470 +0,0 @@ -/*************************************************************************/ -/* shape_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 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_SW_H -#define SHAPE_SW_H - -#include "core/math/geometry.h" -#include "servers/physics_server.h" -/* - -SHAPE_LINE, ///< plane:"plane" -SHAPE_SEGMENT, ///< real_t:"length" -SHAPE_CIRCLE, ///< real_t:"radius" -SHAPE_RECTANGLE, ///< vec3:"extents" -SHAPE_CONVEX_POLYGON, ///< array of planes:"planes" -SHAPE_CONCAVE_POLYGON, ///< Vector3 array:"triangles" , or Dictionary with "indices" (int array) and "triangles" (Vector3 array) -SHAPE_CUSTOM, ///< Server-Implementation based custom shape, calling shape_create() with this value will result in an error - -*/ - -class ShapeSW; - -class ShapeOwnerSW { -public: - virtual void _shape_changed() = 0; - virtual void remove_shape(ShapeSW *p_shape) = 0; - - virtual ~ShapeOwnerSW() {} -}; - -class ShapeSW { - - RID self; - AABB aabb; - bool configured; - real_t custom_bias; - - Map<ShapeOwnerSW *, int> owners; - -protected: - void configure(const AABB &p_aabb); - -public: - enum { - MAX_SUPPORTS = 8 - }; - - virtual real_t get_area() const { return aabb.get_area(); } - - _FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; } - _FORCE_INLINE_ RID get_self() const { return self; } - - virtual PhysicsServer::ShapeType get_type() const = 0; - - _FORCE_INLINE_ AABB get_aabb() const { return aabb; } - _FORCE_INLINE_ bool is_configured() const { return configured; } - - virtual bool is_concave() const { return false; } - - virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const = 0; - virtual Vector3 get_support(const Vector3 &p_normal) const; - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const = 0; - virtual Vector3 get_closest_point_to(const Vector3 &p_point) const = 0; - virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal) const = 0; - virtual bool intersect_point(const Vector3 &p_point) const = 0; - virtual Vector3 get_moment_of_inertia(real_t p_mass) 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(ShapeOwnerSW *p_owner); - void remove_owner(ShapeOwnerSW *p_owner); - bool is_owner(ShapeOwnerSW *p_owner) const; - const Map<ShapeOwnerSW *, int> &get_owners() const; - - ShapeSW(); - virtual ~ShapeSW(); -}; - -class ConcaveShapeSW : public ShapeSW { - -public: - virtual bool is_concave() const { return true; } - typedef void (*Callback)(void *p_userdata, ShapeSW *p_convex); - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { r_amount = 0; } - - virtual void cull(const AABB &p_local_aabb, Callback p_callback, void *p_userdata) const = 0; - - ConcaveShapeSW() {} -}; - -class PlaneShapeSW : public ShapeSW { - - Plane plane; - - void _setup(const Plane &p_plane); - -public: - Plane get_plane() const; - - virtual real_t get_area() const { return Math_INF; } - virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_PLANE; } - virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; - virtual Vector3 get_support(const Vector3 &p_normal) const; - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { r_amount = 0; } - - virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; - virtual bool intersect_point(const Vector3 &p_point) const; - virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; - virtual Vector3 get_moment_of_inertia(real_t p_mass) const; - - virtual void set_data(const Variant &p_data); - virtual Variant get_data() const; - - PlaneShapeSW(); -}; - -class RayShapeSW : public ShapeSW { - - real_t length; - bool slips_on_slope; - - void _setup(real_t p_length, bool p_slips_on_slope); - -public: - real_t get_length() const; - bool get_slips_on_slope() const; - - virtual real_t get_area() const { return 0.0; } - virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_RAY; } - virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; - virtual Vector3 get_support(const Vector3 &p_normal) const; - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const; - - virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; - virtual bool intersect_point(const Vector3 &p_point) const; - virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; - - virtual Vector3 get_moment_of_inertia(real_t p_mass) const; - - virtual void set_data(const Variant &p_data); - virtual Variant get_data() const; - - RayShapeSW(); -}; - -class SphereShapeSW : public ShapeSW { - - real_t radius; - - void _setup(real_t p_radius); - -public: - real_t get_radius() const; - - virtual real_t get_area() const { return 4.0 / 3.0 * Math_PI * radius * radius * radius; } - - virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_SPHERE; } - - virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; - virtual Vector3 get_support(const Vector3 &p_normal) const; - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const; - virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; - virtual bool intersect_point(const Vector3 &p_point) const; - virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; - - virtual Vector3 get_moment_of_inertia(real_t p_mass) const; - - virtual void set_data(const Variant &p_data); - virtual Variant get_data() const; - - SphereShapeSW(); -}; - -class BoxShapeSW : public ShapeSW { - - Vector3 half_extents; - void _setup(const Vector3 &p_half_extents); - -public: - _FORCE_INLINE_ Vector3 get_half_extents() const { return half_extents; } - virtual real_t get_area() const { return 8 * half_extents.x * half_extents.y * half_extents.z; } - - virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_BOX; } - - virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; - virtual Vector3 get_support(const Vector3 &p_normal) const; - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const; - virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; - virtual bool intersect_point(const Vector3 &p_point) const; - virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; - - virtual Vector3 get_moment_of_inertia(real_t p_mass) const; - - virtual void set_data(const Variant &p_data); - virtual Variant get_data() const; - - BoxShapeSW(); -}; - -class CapsuleShapeSW : public ShapeSW { - - real_t height; - real_t radius; - - void _setup(real_t p_height, real_t p_radius); - -public: - _FORCE_INLINE_ real_t get_height() const { return height; } - _FORCE_INLINE_ real_t get_radius() const { return radius; } - - virtual real_t get_area() const { return 4.0 / 3.0 * Math_PI * radius * radius * radius + height * Math_PI * radius * radius; } - - virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CAPSULE; } - - virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; - virtual Vector3 get_support(const Vector3 &p_normal) const; - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const; - virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; - virtual bool intersect_point(const Vector3 &p_point) const; - virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; - - virtual Vector3 get_moment_of_inertia(real_t p_mass) const; - - virtual void set_data(const Variant &p_data); - virtual Variant get_data() const; - - CapsuleShapeSW(); -}; - -struct ConvexPolygonShapeSW : public ShapeSW { - - Geometry::MeshData mesh; - - void _setup(const Vector<Vector3> &p_vertices); - -public: - const Geometry::MeshData &get_mesh() const { return mesh; } - - virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CONVEX_POLYGON; } - - virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; - virtual Vector3 get_support(const Vector3 &p_normal) const; - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const; - virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; - virtual bool intersect_point(const Vector3 &p_point) const; - virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; - - virtual Vector3 get_moment_of_inertia(real_t p_mass) const; - - virtual void set_data(const Variant &p_data); - virtual Variant get_data() const; - - ConvexPolygonShapeSW(); -}; - -struct _VolumeSW_BVH; -struct FaceShapeSW; - -struct ConcavePolygonShapeSW : public ConcaveShapeSW { - // always a trimesh - - struct Face { - - Vector3 normal; - int indices[3]; - }; - - Vector<Face> faces; - Vector<Vector3> vertices; - - struct BVH { - - AABB aabb; - int left; - int right; - - int face_index; - }; - - Vector<BVH> bvh; - - struct _CullParams { - - AABB aabb; - Callback callback; - void *userdata; - const Face *faces; - const Vector3 *vertices; - const BVH *bvh; - FaceShapeSW *face; - }; - - struct _SegmentCullParams { - - Vector3 from; - Vector3 to; - const Face *faces; - const Vector3 *vertices; - const BVH *bvh; - Vector3 dir; - - Vector3 result; - Vector3 normal; - real_t min_d; - int collisions; - }; - - void _cull_segment(int p_idx, _SegmentCullParams *p_params) const; - void _cull(int p_idx, _CullParams *p_params) const; - - void _fill_bvh(_VolumeSW_BVH *p_bvh_tree, BVH *p_bvh_array, int &p_idx); - - void _setup(Vector<Vector3> p_faces); - -public: - Vector<Vector3> get_faces() const; - - virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CONCAVE_POLYGON; } - - virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; - virtual Vector3 get_support(const Vector3 &p_normal) const; - - virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; - virtual bool intersect_point(const Vector3 &p_point) const; - virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; - - virtual void cull(const AABB &p_local_aabb, Callback p_callback, void *p_userdata) const; - - virtual Vector3 get_moment_of_inertia(real_t p_mass) const; - - virtual void set_data(const Variant &p_data); - virtual Variant get_data() const; - - ConcavePolygonShapeSW(); -}; - -struct HeightMapShapeSW : public ConcaveShapeSW { - - Vector<real_t> heights; - int width; - int depth; - real_t cell_size; - - //void _cull_segment(int p_idx,_SegmentCullParams *p_params) const; - //void _cull(int p_idx,_CullParams *p_params) const; - - void _setup(Vector<real_t> p_heights, int p_width, int p_depth, real_t p_cell_size); - -public: - Vector<real_t> get_heights() const; - int get_width() const; - int get_depth() const; - real_t get_cell_size() const; - - virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_HEIGHTMAP; } - - virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; - virtual Vector3 get_support(const Vector3 &p_normal) const; - virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal) const; - virtual bool intersect_point(const Vector3 &p_point) const; - - virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; - virtual void cull(const AABB &p_local_aabb, Callback p_callback, void *p_userdata) const; - - virtual Vector3 get_moment_of_inertia(real_t p_mass) const; - - virtual void set_data(const Variant &p_data); - virtual Variant get_data() const; - - HeightMapShapeSW(); -}; - -//used internally -struct FaceShapeSW : public ShapeSW { - - Vector3 normal; //cache - Vector3 vertex[3]; - - virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CONCAVE_POLYGON; } - - const Vector3 &get_vertex(int p_idx) const { return vertex[p_idx]; } - - void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; - Vector3 get_support(const Vector3 &p_normal) const; - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const; - bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; - virtual bool intersect_point(const Vector3 &p_point) const; - virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; - - Vector3 get_moment_of_inertia(real_t p_mass) const; - - virtual void set_data(const Variant &p_data) {} - virtual Variant get_data() const { return Variant(); } - - FaceShapeSW(); -}; - -struct MotionShapeSW : public ShapeSW { - - ShapeSW *shape; - Vector3 motion; - - virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CONVEX_POLYGON; } - - void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { - - Vector3 cast = p_transform.basis.xform(motion); - real_t mina, maxa; - real_t minb, maxb; - Transform ofsb = p_transform; - ofsb.origin += cast; - shape->project_range(p_normal, p_transform, mina, maxa); - shape->project_range(p_normal, ofsb, minb, maxb); - r_min = MIN(mina, minb); - r_max = MAX(maxa, maxb); - } - - Vector3 get_support(const Vector3 &p_normal) const { - - Vector3 support = shape->get_support(p_normal); - if (p_normal.dot(motion) > 0) { - support += motion; - } - return support; - } - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { r_amount = 0; } - bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { return false; } - virtual bool intersect_point(const Vector3 &p_point) const { return false; } - virtual Vector3 get_closest_point_to(const Vector3 &p_point) const { return p_point; } - - Vector3 get_moment_of_inertia(real_t p_mass) const { return Vector3(); } - - virtual void set_data(const Variant &p_data) {} - virtual Variant get_data() const { return Variant(); } - - MotionShapeSW() { configure(AABB()); } -}; - -#endif // SHAPE_SW_H diff --git a/servers/physics/space_sw.cpp b/servers/physics/space_sw.cpp deleted file mode 100644 index 110520db5a..0000000000 --- a/servers/physics/space_sw.cpp +++ /dev/null @@ -1,1242 +0,0 @@ -/*************************************************************************/ -/* space_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 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_sw.h" - -#include "collision_solver_sw.h" -#include "core/project_settings.h" -#include "physics_server_sw.h" - -_FORCE_INLINE_ static bool _can_collide_with(CollisionObjectSW *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() == CollisionObjectSW::TYPE_AREA && !p_collide_with_areas) - return false; - - if (p_object->get_type() == CollisionObjectSW::TYPE_BODY && !p_collide_with_bodies) - return false; - - return true; -} - -int PhysicsDirectSpaceStateSW::intersect_point(const Vector3 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { - - ERR_FAIL_COND_V(space->locked, false); - int amount = space->broadphase->cull_point(p_point, space->intersection_query_results, SpaceSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); - int cc = 0; - - //Transform ai = p_xform.affine_inverse(); - - 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; - - //area can't be picked by ray (default) - - if (p_exclude.has(space->intersection_query_results[i]->get_self())) - continue; - - const CollisionObjectSW *col_obj = space->intersection_query_results[i]; - int shape_idx = space->intersection_query_subindex_results[i]; - - Transform inv_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); - inv_xform.affine_invert(); - - if (!col_obj->get_shape(shape_idx)->intersect_point(inv_xform.xform(p_point))) - 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); - else - r_results[cc].collider = NULL; - r_results[cc].rid = col_obj->get_self(); - r_results[cc].shape = shape_idx; - - cc++; - } - - return cc; -} - -bool PhysicsDirectSpaceStateSW::intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, bool p_pick_ray) { - - ERR_FAIL_COND_V(space->locked, false); - - Vector3 begin, end; - Vector3 normal; - begin = p_from; - end = p_to; - normal = (end - begin).normalized(); - - int amount = space->broadphase->cull_segment(begin, end, space->intersection_query_results, SpaceSW::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; - Vector3 res_point, res_normal; - int res_shape; - const CollisionObjectSW *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_pick_ray && !(space->intersection_query_results[i]->is_ray_pickable())) - continue; - - if (p_exclude.has(space->intersection_query_results[i]->get_self())) - continue; - - const CollisionObjectSW *col_obj = space->intersection_query_results[i]; - - int shape_idx = space->intersection_query_subindex_results[i]; - Transform inv_xform = col_obj->get_shape_inv_transform(shape_idx) * col_obj->get_inv_transform(); - - Vector3 local_from = inv_xform.xform(begin); - Vector3 local_to = inv_xform.xform(end); - - const ShapeSW *shape = col_obj->get_shape(shape_idx); - - Vector3 shape_point, shape_normal; - - if (shape->intersect_segment(local_from, local_to, shape_point, shape_normal)) { - - Transform 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); - else - r_result.collider = NULL; - 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 PhysicsDirectSpaceStateSW::intersect_shape(const RID &p_shape, const Transform &p_xform, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { - - if (p_result_max <= 0) - return 0; - - ShapeSW *shape = static_cast<PhysicsServerSW *>(PhysicsServer::get_singleton())->shape_owner.getornull(p_shape); - ERR_FAIL_COND_V(!shape, 0); - - AABB aabb = p_xform.xform(shape->get_aabb()); - - int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, SpaceSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); - - int cc = 0; - - //Transform ai = p_xform.affine_inverse(); - - 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; - - //area can't be picked by ray (default) - - if (p_exclude.has(space->intersection_query_results[i]->get_self())) - continue; - - const CollisionObjectSW *col_obj = space->intersection_query_results[i]; - int shape_idx = space->intersection_query_subindex_results[i]; - - if (!CollisionSolverSW::solve_static(shape, p_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), NULL, NULL, NULL, p_margin, 0)) - continue; - - if (r_results) { - 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); - else - r_results[cc].collider = NULL; - r_results[cc].rid = col_obj->get_self(); - r_results[cc].shape = shape_idx; - } - - cc++; - } - - return cc; -} - -bool PhysicsDirectSpaceStateSW::cast_motion(const RID &p_shape, const Transform &p_xform, const Vector3 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, ShapeRestInfo *r_info) { - - ShapeSW *shape = static_cast<PhysicsServerSW *>(PhysicsServer::get_singleton())->shape_owner.getornull(p_shape); - ERR_FAIL_COND_V(!shape, false); - - AABB aabb = p_xform.xform(shape->get_aabb()); - aabb = aabb.merge(AABB(aabb.position + p_motion, aabb.size)); //motion - aabb = aabb.grow(p_margin); - - int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, SpaceSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); - - real_t best_safe = 1; - real_t best_unsafe = 1; - - Transform xform_inv = p_xform.affine_inverse(); - MotionShapeSW mshape; - mshape.shape = shape; - mshape.motion = xform_inv.basis.xform(p_motion); - - bool best_first = true; - - Vector3 closest_A, closest_B; - - 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 CollisionObjectSW *col_obj = space->intersection_query_results[i]; - int shape_idx = space->intersection_query_subindex_results[i]; - - Vector3 point_A, point_B; - Vector3 sep_axis = p_motion.normalized(); - - Transform col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); - //test initial overlap, does it collide if going all the way? - if (CollisionSolverSW::solve_distance(&mshape, p_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, aabb, &sep_axis)) { - continue; - } - - //test initial overlap - sep_axis = p_motion.normalized(); - - if (!CollisionSolverSW::solve_distance(shape, p_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, aabb, &sep_axis)) { - return false; - } - - //just do kinematic solving - real_t low = 0; - real_t hi = 1; - Vector3 mnormal = p_motion.normalized(); - - for (int j = 0; j < 8; j++) { //steps should be customizable.. - - real_t ofs = (low + hi) * 0.5; - - Vector3 sep = mnormal; //important optimization for this to work fast enough - - mshape.motion = xform_inv.basis.xform(p_motion * ofs); - - Vector3 lA, lB; - - bool collided = !CollisionSolverSW::solve_distance(&mshape, p_xform, col_obj->get_shape(shape_idx), col_obj_xform, lA, lB, aabb, &sep); - - if (collided) { - - hi = ofs; - } else { - - point_A = lA; - point_B = lB; - low = ofs; - } - } - - if (low < best_safe) { - best_first = true; //force reset - best_safe = low; - best_unsafe = hi; - } - - if (r_info && (best_first || (point_A.distance_squared_to(point_B) < closest_A.distance_squared_to(closest_B) && low <= best_safe))) { - closest_A = point_A; - closest_B = point_B; - r_info->collider_id = col_obj->get_instance_id(); - r_info->rid = col_obj->get_self(); - r_info->shape = shape_idx; - r_info->point = closest_B; - r_info->normal = (closest_A - closest_B).normalized(); - best_first = false; - if (col_obj->get_type() == CollisionObjectSW::TYPE_BODY) { - const BodySW *body = static_cast<const BodySW *>(col_obj); - r_info->linear_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(body->get_transform().origin - closest_B); - } - } - } - - p_closest_safe = best_safe; - p_closest_unsafe = best_unsafe; - - return true; -} - -bool PhysicsDirectSpaceStateSW::collide_shape(RID p_shape, const Transform &p_shape_xform, real_t p_margin, Vector3 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { - - if (p_result_max <= 0) - return 0; - - ShapeSW *shape = static_cast<PhysicsServerSW *>(PhysicsServer::get_singleton())->shape_owner.getornull(p_shape); - ERR_FAIL_COND_V(!shape, 0); - - AABB aabb = p_shape_xform.xform(shape->get_aabb()); - aabb = aabb.grow(p_margin); - - int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, SpaceSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); - - bool collided = false; - r_result_count = 0; - - PhysicsServerSW::CollCbkData cbk; - cbk.max = p_result_max; - cbk.amount = 0; - cbk.ptr = r_results; - CollisionSolverSW::CallbackResult cbkres = PhysicsServerSW::_shape_col_cbk; - - PhysicsServerSW::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 CollisionObjectSW *col_obj = space->intersection_query_results[i]; - int shape_idx = space->intersection_query_subindex_results[i]; - - if (p_exclude.has(col_obj->get_self())) { - continue; - } - - if (CollisionSolverSW::solve_static(shape, p_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, NULL, p_margin)) { - collided = true; - } - } - - r_result_count = cbk.amount; - - return collided; -} - -struct _RestCallbackData { - - const CollisionObjectSW *object; - const CollisionObjectSW *best_object; - int shape; - int best_shape; - Vector3 best_contact; - Vector3 best_normal; - real_t best_len; - real_t min_allowed_depth; -}; - -static void _rest_cbk_result(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata) { - - _RestCallbackData *rd = (_RestCallbackData *)p_userdata; - - Vector3 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; - - rd->best_len = len; - rd->best_contact = p_point_B; - rd->best_normal = contact_rel / len; - rd->best_object = rd->object; - rd->best_shape = rd->shape; -} -bool PhysicsDirectSpaceStateSW::rest_info(RID p_shape, const Transform &p_shape_xform, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { - - ShapeSW *shape = static_cast<PhysicsServerSW *>(PhysicsServer::get_singleton())->shape_owner.getornull(p_shape); - ERR_FAIL_COND_V(!shape, 0); - - AABB aabb = p_shape_xform.xform(shape->get_aabb()); - aabb = aabb.grow(p_margin); - - int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, SpaceSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); - - _RestCallbackData rcd; - rcd.best_len = 0; - rcd.best_object = NULL; - rcd.best_shape = 0; - rcd.min_allowed_depth = space->test_motion_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 CollisionObjectSW *col_obj = space->intersection_query_results[i]; - int shape_idx = space->intersection_query_subindex_results[i]; - - if (p_exclude.has(col_obj->get_self())) - continue; - - rcd.object = col_obj; - rcd.shape = shape_idx; - bool sc = CollisionSolverSW::solve_static(shape, p_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), _rest_cbk_result, &rcd, NULL, 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() == CollisionObjectSW::TYPE_BODY) { - - const BodySW *body = static_cast<const BodySW *>(rcd.best_object); - r_info->linear_velocity = body->get_linear_velocity() + - (body->get_angular_velocity()).cross(body->get_transform().origin - rcd.best_contact); // * mPos); - - } else { - r_info->linear_velocity = Vector3(); - } - - return true; -} - -Vector3 PhysicsDirectSpaceStateSW::get_closest_point_to_object_volume(RID p_object, const Vector3 p_point) const { - - CollisionObjectSW *obj = PhysicsServerSW::singleton->area_owner.getornull(p_object); - if (!obj) { - obj = PhysicsServerSW::singleton->body_owner.getornull(p_object); - } - ERR_FAIL_COND_V(!obj, Vector3()); - - ERR_FAIL_COND_V(obj->get_space() != space, Vector3()); - - float min_distance = 1e20; - Vector3 min_point; - - bool shapes_found = false; - - for (int i = 0; i < obj->get_shape_count(); i++) { - - if (obj->is_shape_set_as_disabled(i)) - continue; - - Transform shape_xform = obj->get_transform() * obj->get_shape_transform(i); - ShapeSW *shape = obj->get_shape(i); - - Vector3 point = shape->get_closest_point_to(shape_xform.affine_inverse().xform(p_point)); - point = shape_xform.xform(point); - - float dist = point.distance_to(p_point); - if (dist < min_distance) { - min_distance = dist; - min_point = point; - } - shapes_found = true; - } - - if (!shapes_found) { - return obj->get_transform().origin; //no shapes found, use distance to origin. - } else { - return min_point; - } -} - -PhysicsDirectSpaceStateSW::PhysicsDirectSpaceStateSW() { - - space = NULL; -} - -//////////////////////////////////////////////////////////////////////////////////////////////////////////// - -int SpaceSW::_cull_aabb_for_body(BodySW *p_body, const AABB &p_aabb) { - - int amount = broadphase->cull_aabb(p_aabb, intersection_query_results, INTERSECTION_QUERY_MAX, intersection_query_subindex_results); - - 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() == CollisionObjectSW::TYPE_AREA) - keep = false; - else if ((static_cast<BodySW *>(intersection_query_results[i])->test_collision_mask(p_body)) == 0) - keep = false; - else if (static_cast<BodySW *>(intersection_query_results[i])->has_exception(p_body->get_self()) || p_body->has_exception(intersection_query_results[i]->get_self())) - keep = false; - else if (static_cast<BodySW *>(intersection_query_results[i])->is_shape_set_as_disabled(intersection_query_subindex_results[i])) - 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; -} - -int SpaceSW::test_body_ray_separation(BodySW *p_body, const Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, PhysicsServer::SeparationResult *r_results, int p_result_max, real_t p_margin) { - - AABB body_aabb; - - bool shapes_found = false; - - for (int i = 0; i < p_body->get_shape_count(); i++) { - - if (p_body->is_shape_set_as_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) { - return 0; - } - // Undo the currently transform the physics server is aware of and apply the provided one - body_aabb = p_transform.xform(p_body->get_inv_transform().xform(body_aabb)); - body_aabb = body_aabb.grow(p_margin); - - Transform body_transform = p_transform; - - for (int i = 0; i < p_result_max; i++) { - //reset results - r_results[i].collision_depth = 0; - } - - int rays_found = 0; - - { - // raycast AND separate - - const int max_results = 32; - int recover_attempts = 4; - Vector3 sr[max_results * 2]; - PhysicsServerSW::CollCbkData cbk; - cbk.max = max_results; - PhysicsServerSW::CollCbkData *cbkptr = &cbk; - CollisionSolverSW::CallbackResult cbkres = PhysicsServerSW::_shape_col_cbk; - - do { - - Vector3 recover_motion; - - bool collided = false; - - int amount = _cull_aabb_for_body(p_body, body_aabb); - - for (int j = 0; j < p_body->get_shape_count(); j++) { - if (p_body->is_shape_set_as_disabled(j)) - continue; - - ShapeSW *body_shape = p_body->get_shape(j); - - if (body_shape->get_type() != PhysicsServer::SHAPE_RAY) - continue; - - Transform body_shape_xform = body_transform * p_body->get_shape_transform(j); - - for (int i = 0; i < amount; i++) { - - const CollisionObjectSW *col_obj = intersection_query_results[i]; - int shape_idx = intersection_query_subindex_results[i]; - - cbk.amount = 0; - cbk.ptr = sr; - - if (CollisionObjectSW::TYPE_BODY == col_obj->get_type()) { - const BodySW *b = static_cast<const BodySW *>(col_obj); - if (p_infinite_inertia && PhysicsServer::BODY_MODE_STATIC != b->get_mode() && PhysicsServer::BODY_MODE_KINEMATIC != b->get_mode()) { - continue; - } - } - - ShapeSW *against_shape = col_obj->get_shape(shape_idx); - if (CollisionSolverSW::solve_static(body_shape, body_shape_xform, against_shape, col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, NULL, p_margin)) { - if (cbk.amount > 0) { - collided = true; - } - - int ray_index = -1; //reuse shape - for (int k = 0; k < rays_found; k++) { - if (r_results[k].collision_local_shape == j) { - ray_index = k; - } - } - - if (ray_index == -1 && rays_found < p_result_max) { - ray_index = rays_found; - rays_found++; - } - - if (ray_index != -1) { - PhysicsServer::SeparationResult &result = r_results[ray_index]; - - for (int k = 0; k < cbk.amount; k++) { - Vector3 a = sr[k * 2 + 0]; - Vector3 b = sr[k * 2 + 1]; - - recover_motion += (b - a) * 0.4; - - float depth = a.distance_to(b); - if (depth > result.collision_depth) { - - result.collision_depth = depth; - result.collision_point = b; - result.collision_normal = (b - a).normalized(); - result.collision_local_shape = j; - result.collider = col_obj->get_self(); - result.collider_id = col_obj->get_instance_id(); - result.collider_shape = shape_idx; - //result.collider_metadata = col_obj->get_shape_metadata(shape_idx); - if (col_obj->get_type() == CollisionObjectSW::TYPE_BODY) { - BodySW *body = (BodySW *)col_obj; - - Vector3 rel_vec = b - body->get_transform().get_origin(); - //result.collider_velocity = Vector3(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity(); - result.collider_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(body->get_transform().origin - rel_vec); // * mPos); - } - } - } - } - } - } - } - - if (!collided || recover_motion == Vector3()) { - break; - } - - body_transform.origin += recover_motion; - body_aabb.position += recover_motion; - - recover_attempts--; - } while (recover_attempts); - } - - //optimize results (remove non colliding) - for (int i = 0; i < rays_found; i++) { - if (r_results[i].collision_depth == 0) { - rays_found--; - SWAP(r_results[i], r_results[rays_found]); - } - } - - r_recover_motion = body_transform.origin - p_transform.origin; - return rays_found; -} - -bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, real_t p_margin, PhysicsServer::MotionResult *r_result, bool p_exclude_raycast_shapes) { - - //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; - } - AABB body_aabb; - bool shapes_found = false; - - for (int i = 0; i < p_body->get_shape_count(); i++) { - - if (p_body->is_shape_set_as_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 = PhysicsServer::MotionResult(); - r_result->motion = p_motion; - } - - return false; - } - - // Undo the currently transform the physics server is aware of and apply the provided one - body_aabb = p_from.xform(p_body->get_inv_transform().xform(body_aabb)); - body_aabb = body_aabb.grow(p_margin); - - Transform body_transform = p_from; - - { - //STEP 1, FREE BODY IF STUCK - - const int max_results = 32; - int recover_attempts = 4; - Vector3 sr[max_results * 2]; - - do { - - PhysicsServerSW::CollCbkData cbk; - cbk.max = max_results; - cbk.amount = 0; - cbk.ptr = sr; - - PhysicsServerSW::CollCbkData *cbkptr = &cbk; - CollisionSolverSW::CallbackResult cbkres = PhysicsServerSW::_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_set_as_disabled(j)) - continue; - - Transform body_shape_xform = body_transform * p_body->get_shape_transform(j); - ShapeSW *body_shape = p_body->get_shape(j); - if (p_exclude_raycast_shapes && body_shape->get_type() == PhysicsServer::SHAPE_RAY) { - continue; - } - - for (int i = 0; i < amount; i++) { - - const CollisionObjectSW *col_obj = intersection_query_results[i]; - int shape_idx = intersection_query_subindex_results[i]; - - if (CollisionSolverSW::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, NULL, p_margin)) { - collided = cbk.amount > 0; - } - } - } - - if (!collided) { - break; - } - - Vector3 recover_motion; - - for (int i = 0; i < cbk.amount; i++) { - - Vector3 a = sr[i * 2 + 0]; - Vector3 b = sr[i * 2 + 1]; - recover_motion += (b - a) * 0.4; - } - - if (recover_motion == Vector3()) { - collided = false; - break; - } - - body_transform.origin += 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 - - AABB motion_aabb = body_aabb; - motion_aabb.position += p_motion; - motion_aabb = motion_aabb.merge(body_aabb); - - int amount = _cull_aabb_for_body(p_body, motion_aabb); - - for (int j = 0; j < p_body->get_shape_count(); j++) { - - if (p_body->is_shape_set_as_disabled(j)) - continue; - - Transform body_shape_xform = body_transform * p_body->get_shape_transform(j); - ShapeSW *body_shape = p_body->get_shape(j); - - if (p_exclude_raycast_shapes && body_shape->get_type() == PhysicsServer::SHAPE_RAY) { - continue; - } - - Transform body_shape_xform_inv = body_shape_xform.affine_inverse(); - MotionShapeSW mshape; - mshape.shape = body_shape; - mshape.motion = body_shape_xform_inv.basis.xform(p_motion); - - bool stuck = false; - - real_t best_safe = 1; - real_t best_unsafe = 1; - - for (int i = 0; i < amount; i++) { - - const CollisionObjectSW *col_obj = intersection_query_results[i]; - int shape_idx = intersection_query_subindex_results[i]; - - //test initial overlap, does it collide if going all the way? - Vector3 point_A, point_B; - Vector3 sep_axis = p_motion.normalized(); - - Transform col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); - //test initial overlap, does it collide if going all the way? - if (CollisionSolverSW::solve_distance(&mshape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, motion_aabb, &sep_axis)) { - continue; - } - sep_axis = p_motion.normalized(); - - if (!CollisionSolverSW::solve_distance(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, motion_aabb, &sep_axis)) { - stuck = true; - break; - } - - //just do kinematic solving - real_t low = 0; - real_t hi = 1; - Vector3 mnormal = p_motion.normalized(); - - for (int k = 0; k < 8; k++) { //steps should be customizable.. - - real_t ofs = (low + hi) * 0.5; - - Vector3 sep = mnormal; //important optimization for this to work fast enough - - mshape.motion = body_shape_xform_inv.basis.xform(p_motion * ofs); - - Vector3 lA, lB; - - bool collided = !CollisionSolverSW::solve_distance(&mshape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, lA, lB, motion_aabb, &sep); - - if (collided) { - - hi = ofs; - } else { - - point_A = lA; - point_B = lB; - low = ofs; - } - } - - if (low < best_safe) { - best_safe = low; - best_unsafe = hi; - } - } - - if (stuck) { - - safe = 0; - unsafe = 0; - best_shape = j; //sadly it's the best - break; - } - if (best_safe == 1.0) { - continue; - } - if (best_safe < safe) { - - safe = best_safe; - unsafe = best_unsafe; - best_shape = j; - } - } - } - - bool collided = false; - if (safe >= 1) { - //not collided - collided = false; - if (r_result) { - - r_result->motion = p_motion; - r_result->remainder = Vector3(); - r_result->motion += (body_transform.get_origin() - p_from.get_origin()); - } - - } else { - - //it collided, let's get the rest info in unsafe advance - Transform ugt = body_transform; - ugt.origin += p_motion * unsafe; - - _RestCallbackData rcd; - rcd.best_len = 0; - rcd.best_object = NULL; - rcd.best_shape = 0; - rcd.min_allowed_depth = test_motion_min_contact_depth; - - Transform body_shape_xform = ugt * p_body->get_shape_transform(best_shape); - ShapeSW *body_shape = p_body->get_shape(best_shape); - - body_aabb.position += p_motion * unsafe; - - int amount = _cull_aabb_for_body(p_body, body_aabb); - - for (int i = 0; i < amount; i++) { - - const CollisionObjectSW *col_obj = intersection_query_results[i]; - int shape_idx = intersection_query_subindex_results[i]; - - rcd.object = col_obj; - rcd.shape = shape_idx; - bool sc = CollisionSolverSW::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), _rest_cbk_result, &rcd, NULL, p_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 = best_shape; - r_result->collision_normal = rcd.best_normal; - r_result->collision_point = rcd.best_contact; - //r_result->collider_metadata = rcd.best_object->get_shape_metadata(rcd.best_shape); - - const BodySW *body = static_cast<const BodySW *>(rcd.best_object); - //Vector3 rel_vec = r_result->collision_point - body->get_transform().get_origin(); - // r_result->collider_velocity = Vector3(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity(); - r_result->collider_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(body->get_transform().origin - rcd.best_contact); // * mPos); - - r_result->motion = safe * p_motion; - r_result->remainder = p_motion - safe * p_motion; - r_result->motion += (body_transform.get_origin() - p_from.get_origin()); - } - - collided = true; - } else { - if (r_result) { - - r_result->motion = p_motion; - r_result->remainder = Vector3(); - r_result->motion += (body_transform.get_origin() - p_from.get_origin()); - } - - collided = false; - } - } - - return collided; -} - -void *SpaceSW::_broadphase_pair(CollisionObjectSW *A, int p_subindex_A, CollisionObjectSW *B, int p_subindex_B, void *p_self) { - - CollisionObjectSW::Type type_A = A->get_type(); - CollisionObjectSW::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); - } - - SpaceSW *self = (SpaceSW *)p_self; - - self->collision_pairs++; - - if (type_A == CollisionObjectSW::TYPE_AREA) { - - AreaSW *area = static_cast<AreaSW *>(A); - if (type_B == CollisionObjectSW::TYPE_AREA) { - - AreaSW *area_b = static_cast<AreaSW *>(B); - Area2PairSW *area2_pair = memnew(Area2PairSW(area_b, p_subindex_B, area, p_subindex_A)); - return area2_pair; - } else { - - BodySW *body = static_cast<BodySW *>(B); - AreaPairSW *area_pair = memnew(AreaPairSW(body, p_subindex_B, area, p_subindex_A)); - return area_pair; - } - } else { - - BodyPairSW *b = memnew(BodyPairSW((BodySW *)A, p_subindex_A, (BodySW *)B, p_subindex_B)); - return b; - } - - return NULL; -} - -void SpaceSW::_broadphase_unpair(CollisionObjectSW *A, int p_subindex_A, CollisionObjectSW *B, int p_subindex_B, void *p_data, void *p_self) { - - SpaceSW *self = (SpaceSW *)p_self; - self->collision_pairs--; - ConstraintSW *c = (ConstraintSW *)p_data; - memdelete(c); -} - -const SelfList<BodySW>::List &SpaceSW::get_active_body_list() const { - - return active_list; -} -void SpaceSW::body_add_to_active_list(SelfList<BodySW> *p_body) { - - active_list.add(p_body); -} -void SpaceSW::body_remove_from_active_list(SelfList<BodySW> *p_body) { - - active_list.remove(p_body); -} - -void SpaceSW::body_add_to_inertia_update_list(SelfList<BodySW> *p_body) { - - inertia_update_list.add(p_body); -} - -void SpaceSW::body_remove_from_inertia_update_list(SelfList<BodySW> *p_body) { - - inertia_update_list.remove(p_body); -} - -BroadPhaseSW *SpaceSW::get_broadphase() { - - return broadphase; -} - -void SpaceSW::add_object(CollisionObjectSW *p_object) { - - ERR_FAIL_COND(objects.has(p_object)); - objects.insert(p_object); -} - -void SpaceSW::remove_object(CollisionObjectSW *p_object) { - - ERR_FAIL_COND(!objects.has(p_object)); - objects.erase(p_object); -} - -const Set<CollisionObjectSW *> &SpaceSW::get_objects() const { - - return objects; -} - -void SpaceSW::body_add_to_state_query_list(SelfList<BodySW> *p_body) { - - state_query_list.add(p_body); -} -void SpaceSW::body_remove_from_state_query_list(SelfList<BodySW> *p_body) { - - state_query_list.remove(p_body); -} - -void SpaceSW::area_add_to_monitor_query_list(SelfList<AreaSW> *p_area) { - - monitor_query_list.add(p_area); -} -void SpaceSW::area_remove_from_monitor_query_list(SelfList<AreaSW> *p_area) { - - monitor_query_list.remove(p_area); -} - -void SpaceSW::area_add_to_moved_list(SelfList<AreaSW> *p_area) { - - area_moved_list.add(p_area); -} - -void SpaceSW::area_remove_from_moved_list(SelfList<AreaSW> *p_area) { - - area_moved_list.remove(p_area); -} - -const SelfList<AreaSW>::List &SpaceSW::get_moved_area_list() const { - - return area_moved_list; -} - -void SpaceSW::call_queries() { - - while (state_query_list.first()) { - - BodySW *b = state_query_list.first()->self(); - state_query_list.remove(state_query_list.first()); - b->call_queries(); - } - - while (monitor_query_list.first()) { - - AreaSW *a = monitor_query_list.first()->self(); - monitor_query_list.remove(monitor_query_list.first()); - a->call_queries(); - } -} - -void SpaceSW::setup() { - - contact_debug_count = 0; - while (inertia_update_list.first()) { - inertia_update_list.first()->self()->update_inertias(); - inertia_update_list.remove(inertia_update_list.first()); - } -} - -void SpaceSW::update() { - - broadphase->update(); -} - -void SpaceSW::set_param(PhysicsServer::SpaceParameter p_param, real_t p_value) { - - switch (p_param) { - - case PhysicsServer::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: contact_recycle_radius = p_value; break; - case PhysicsServer::SPACE_PARAM_CONTACT_MAX_SEPARATION: contact_max_separation = p_value; break; - case PhysicsServer::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: contact_max_allowed_penetration = p_value; break; - case PhysicsServer::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD: body_linear_velocity_sleep_threshold = p_value; break; - case PhysicsServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD: body_angular_velocity_sleep_threshold = p_value; break; - case PhysicsServer::SPACE_PARAM_BODY_TIME_TO_SLEEP: body_time_to_sleep = p_value; break; - case PhysicsServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO: body_angular_velocity_damp_ratio = p_value; break; - case PhysicsServer::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: constraint_bias = p_value; break; - case PhysicsServer::SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH: test_motion_min_contact_depth = p_value; break; - } -} - -real_t SpaceSW::get_param(PhysicsServer::SpaceParameter p_param) const { - - switch (p_param) { - - case PhysicsServer::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: return contact_recycle_radius; - case PhysicsServer::SPACE_PARAM_CONTACT_MAX_SEPARATION: return contact_max_separation; - case PhysicsServer::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: return contact_max_allowed_penetration; - case PhysicsServer::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD: return body_linear_velocity_sleep_threshold; - case PhysicsServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD: return body_angular_velocity_sleep_threshold; - case PhysicsServer::SPACE_PARAM_BODY_TIME_TO_SLEEP: return body_time_to_sleep; - case PhysicsServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO: return body_angular_velocity_damp_ratio; - case PhysicsServer::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: return constraint_bias; - case PhysicsServer::SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH: return test_motion_min_contact_depth; - } - return 0; -} - -void SpaceSW::lock() { - - locked = true; -} - -void SpaceSW::unlock() { - - locked = false; -} - -bool SpaceSW::is_locked() const { - - return locked; -} - -PhysicsDirectSpaceStateSW *SpaceSW::get_direct_state() { - - return direct_access; -} - -SpaceSW::SpaceSW() { - - collision_pairs = 0; - active_objects = 0; - island_count = 0; - contact_debug_count = 0; - - locked = false; - contact_recycle_radius = 0.01; - contact_max_separation = 0.05; - contact_max_allowed_penetration = 0.01; - test_motion_min_contact_depth = 0.00001; - - constraint_bias = 0.01; - body_linear_velocity_sleep_threshold = GLOBAL_DEF("physics/3d/sleep_threshold_linear", 0.1); - body_angular_velocity_sleep_threshold = GLOBAL_DEF("physics/3d/sleep_threshold_angular", (8.0 / 180.0 * Math_PI)); - body_time_to_sleep = GLOBAL_DEF("physics/3d/time_before_sleep", 0.5); - ProjectSettings::get_singleton()->set_custom_property_info("physics/3d/time_before_sleep", PropertyInfo(Variant::FLOAT, "physics/3d/time_before_sleep", PROPERTY_HINT_RANGE, "0,5,0.01,or_greater")); - body_angular_velocity_damp_ratio = 10; - - broadphase = BroadPhaseSW::create_func(); - broadphase->set_pair_callback(_broadphase_pair, this); - broadphase->set_unpair_callback(_broadphase_unpair, this); - area = NULL; - - direct_access = memnew(PhysicsDirectSpaceStateSW); - direct_access->space = this; - - for (int i = 0; i < ELAPSED_TIME_MAX; i++) - elapsed_time[i] = 0; -} - -SpaceSW::~SpaceSW() { - - memdelete(broadphase); - memdelete(direct_access); -} diff --git a/servers/physics/space_sw.h b/servers/physics/space_sw.h deleted file mode 100644 index 9e82720a75..0000000000 --- a/servers/physics/space_sw.h +++ /dev/null @@ -1,208 +0,0 @@ -/*************************************************************************/ -/* space_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 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_SW_H -#define SPACE_SW_H - -#include "area_pair_sw.h" -#include "area_sw.h" -#include "body_pair_sw.h" -#include "body_sw.h" -#include "broad_phase_sw.h" -#include "collision_object_sw.h" -#include "core/hash_map.h" -#include "core/project_settings.h" -#include "core/typedefs.h" - -class PhysicsDirectSpaceStateSW : public PhysicsDirectSpaceState { - - GDCLASS(PhysicsDirectSpaceStateSW, PhysicsDirectSpaceState); - -public: - SpaceSW *space; - - virtual int intersect_point(const Vector3 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false); - virtual bool intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, bool p_pick_ray = false); - virtual int intersect_shape(const RID &p_shape, const Transform &p_xform, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false); - virtual bool cast_motion(const RID &p_shape, const Transform &p_xform, const Vector3 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, ShapeRestInfo *r_info = NULL); - virtual bool collide_shape(RID p_shape, const Transform &p_shape_xform, real_t p_margin, Vector3 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false); - virtual bool rest_info(RID p_shape, const Transform &p_shape_xform, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false); - virtual Vector3 get_closest_point_to_object_volume(RID p_object, const Vector3 p_point) const; - - PhysicsDirectSpaceStateSW(); -}; - -class SpaceSW { - -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: - uint64_t elapsed_time[ELAPSED_TIME_MAX]; - - PhysicsDirectSpaceStateSW *direct_access; - RID self; - - BroadPhaseSW *broadphase; - SelfList<BodySW>::List active_list; - SelfList<BodySW>::List inertia_update_list; - SelfList<BodySW>::List state_query_list; - SelfList<AreaSW>::List monitor_query_list; - SelfList<AreaSW>::List area_moved_list; - - static void *_broadphase_pair(CollisionObjectSW *A, int p_subindex_A, CollisionObjectSW *B, int p_subindex_B, void *p_self); - static void _broadphase_unpair(CollisionObjectSW *A, int p_subindex_A, CollisionObjectSW *B, int p_subindex_B, void *p_data, void *p_self); - - Set<CollisionObjectSW *> objects; - - AreaSW *area; - - real_t contact_recycle_radius; - real_t contact_max_separation; - real_t contact_max_allowed_penetration; - real_t constraint_bias; - real_t test_motion_min_contact_depth; - - enum { - - INTERSECTION_QUERY_MAX = 2048 - }; - - CollisionObjectSW *intersection_query_results[INTERSECTION_QUERY_MAX]; - int intersection_query_subindex_results[INTERSECTION_QUERY_MAX]; - - real_t body_linear_velocity_sleep_threshold; - real_t body_angular_velocity_sleep_threshold; - real_t body_time_to_sleep; - real_t body_angular_velocity_damp_ratio; - - bool locked; - - int island_count; - int active_objects; - int collision_pairs; - - RID static_global_body; - - Vector<Vector3> contact_debug; - int contact_debug_count; - - friend class PhysicsDirectSpaceStateSW; - - int _cull_aabb_for_body(BodySW *p_body, const AABB &p_aabb); - -public: - _FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; } - _FORCE_INLINE_ RID get_self() const { return self; } - - void set_default_area(AreaSW *p_area) { area = p_area; } - AreaSW *get_default_area() const { return area; } - - const SelfList<BodySW>::List &get_active_body_list() const; - void body_add_to_active_list(SelfList<BodySW> *p_body); - void body_remove_from_active_list(SelfList<BodySW> *p_body); - void body_add_to_inertia_update_list(SelfList<BodySW> *p_body); - void body_remove_from_inertia_update_list(SelfList<BodySW> *p_body); - - void body_add_to_state_query_list(SelfList<BodySW> *p_body); - void body_remove_from_state_query_list(SelfList<BodySW> *p_body); - - void area_add_to_monitor_query_list(SelfList<AreaSW> *p_area); - void area_remove_from_monitor_query_list(SelfList<AreaSW> *p_area); - void area_add_to_moved_list(SelfList<AreaSW> *p_area); - void area_remove_from_moved_list(SelfList<AreaSW> *p_area); - const SelfList<AreaSW>::List &get_moved_area_list() const; - - BroadPhaseSW *get_broadphase(); - - void add_object(CollisionObjectSW *p_object); - void remove_object(CollisionObjectSW *p_object); - const Set<CollisionObjectSW *> &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; } - _FORCE_INLINE_ real_t get_body_angular_velocity_damp_ratio() const { return body_angular_velocity_damp_ratio; } - - void update(); - void setup(); - void call_queries(); - - bool is_locked() const; - void lock(); - void unlock(); - - void set_param(PhysicsServer::SpaceParameter p_param, real_t p_value); - real_t get_param(PhysicsServer::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; } - - PhysicsDirectSpaceStateSW *get_direct_state(); - - void set_debug_contacts(int p_amount) { contact_debug.resize(p_amount); } - _FORCE_INLINE_ bool is_debugging_contacts() const { return !contact_debug.empty(); } - _FORCE_INLINE_ void add_debug_contact(const Vector3 &p_contact) { - if (contact_debug_count < contact_debug.size()) contact_debug.write[contact_debug_count++] = p_contact; - } - _FORCE_INLINE_ Vector<Vector3> get_debug_contacts() { return contact_debug; } - _FORCE_INLINE_ int get_debug_contact_count() { return contact_debug_count; } - - void set_static_global_body(RID p_body) { static_global_body = p_body; } - RID get_static_global_body() { return static_global_body; } - - 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]; } - - int test_body_ray_separation(BodySW *p_body, const Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, PhysicsServer::SeparationResult *r_results, int p_result_max, real_t p_margin); - bool test_body_motion(BodySW *p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, real_t p_margin, PhysicsServer::MotionResult *r_result, bool p_exclude_raycast_shapes); - - SpaceSW(); - ~SpaceSW(); -}; - -#endif // SPACE__SW_H diff --git a/servers/physics/step_sw.cpp b/servers/physics/step_sw.cpp deleted file mode 100644 index f4055ecff7..0000000000 --- a/servers/physics/step_sw.cpp +++ /dev/null @@ -1,299 +0,0 @@ -/*************************************************************************/ -/* step_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 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_sw.h" -#include "joints_sw.h" - -#include "core/os/os.h" - -void StepSW::_populate_island(BodySW *p_body, BodySW **p_island, ConstraintSW **p_constraint_island) { - - p_body->set_island_step(_step); - p_body->set_island_next(*p_island); - *p_island = p_body; - - for (Map<ConstraintSW *, int>::Element *E = p_body->get_constraint_map().front(); E; E = E->next()) { - - ConstraintSW *c = (ConstraintSW *)E->key(); - if (c->get_island_step() == _step) - continue; //already processed - c->set_island_step(_step); - c->set_island_next(*p_constraint_island); - *p_constraint_island = c; - - for (int i = 0; i < c->get_body_count(); i++) { - if (i == E->get()) - continue; - BodySW *b = c->get_body_ptr()[i]; - if (b->get_island_step() == _step || b->get_mode() == PhysicsServer::BODY_MODE_STATIC || b->get_mode() == PhysicsServer::BODY_MODE_KINEMATIC) - continue; //no go - _populate_island(c->get_body_ptr()[i], p_island, p_constraint_island); - } - } -} - -void StepSW::_setup_island(ConstraintSW *p_island, real_t p_delta) { - - ConstraintSW *ci = p_island; - while (ci) { - ci->setup(p_delta); - //todo remove from island if process fails - ci = ci->get_island_next(); - } -} - -void StepSW::_solve_island(ConstraintSW *p_island, int p_iterations, real_t p_delta) { - - int at_priority = 1; - - while (p_island) { - - for (int i = 0; i < p_iterations; i++) { - - ConstraintSW *ci = p_island; - while (ci) { - ci->solve(p_delta); - ci = ci->get_island_next(); - } - } - - at_priority++; - - { - ConstraintSW *ci = p_island; - ConstraintSW *prev = NULL; - while (ci) { - if (ci->get_priority() < at_priority) { - if (prev) { - prev->set_island_next(ci->get_island_next()); //remove - } else { - p_island = ci->get_island_next(); - } - } else { - - prev = ci; - } - - ci = ci->get_island_next(); - } - } - } -} - -void StepSW::_check_suspend(BodySW *p_island, real_t p_delta) { - - bool can_sleep = true; - - BodySW *b = p_island; - while (b) { - - if (b->get_mode() == PhysicsServer::BODY_MODE_STATIC || b->get_mode() == PhysicsServer::BODY_MODE_KINEMATIC) { - b = b->get_island_next(); - continue; //ignore for static - } - - if (!b->sleep_test(p_delta)) - can_sleep = false; - - b = b->get_island_next(); - } - - //put all to sleep or wake up everyoen - - b = p_island; - while (b) { - - if (b->get_mode() == PhysicsServer::BODY_MODE_STATIC || b->get_mode() == PhysicsServer::BODY_MODE_KINEMATIC) { - b = b->get_island_next(); - continue; //ignore for static - } - - bool active = b->is_active(); - - if (active == can_sleep) - b->set_active(!can_sleep); - - b = b->get_island_next(); - } -} - -void StepSW::step(SpaceSW *p_space, real_t p_delta, int p_iterations) { - - p_space->lock(); // can't access space during this - - p_space->setup(); //update inertias, etc - - const SelfList<BodySW>::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<BodySW> *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(SpaceSW::ELAPSED_TIME_INTEGRATE_FORCES, profile_endtime - profile_begtime); - profile_begtime = profile_endtime; - } - - /* GENERATE CONSTRAINT ISLANDS */ - - BodySW *island_list = NULL; - ConstraintSW *constraint_island_list = NULL; - b = body_list->first(); - - int island_count = 0; - - while (b) { - BodySW *body = b->self(); - - if (body->get_island_step() != _step) { - - BodySW *island = NULL; - ConstraintSW *constraint_island = NULL; - _populate_island(body, &island, &constraint_island); - - island->set_island_list_next(island_list); - island_list = island; - - if (constraint_island) { - constraint_island->set_island_list_next(constraint_island_list); - constraint_island_list = constraint_island; - island_count++; - } - } - b = b->next(); - } - - p_space->set_island_count(island_count); - - const SelfList<AreaSW>::List &aml = p_space->get_moved_area_list(); - - while (aml.first()) { - for (const Set<ConstraintSW *>::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) { - - ConstraintSW *c = E->get(); - if (c->get_island_step() == _step) - continue; - c->set_island_step(_step); - c->set_island_next(NULL); - c->set_island_list_next(constraint_island_list); - constraint_island_list = c; - } - p_space->area_remove_from_moved_list((SelfList<AreaSW> *)aml.first()); //faster to remove here - } - - { //profile - profile_endtime = OS::get_singleton()->get_ticks_usec(); - p_space->set_elapsed_time(SpaceSW::ELAPSED_TIME_GENERATE_ISLANDS, profile_endtime - profile_begtime); - profile_begtime = profile_endtime; - } - - /* SETUP CONSTRAINT ISLANDS */ - - { - ConstraintSW *ci = constraint_island_list; - while (ci) { - - _setup_island(ci, p_delta); - ci = ci->get_island_list_next(); - } - } - - { //profile - profile_endtime = OS::get_singleton()->get_ticks_usec(); - p_space->set_elapsed_time(SpaceSW::ELAPSED_TIME_SETUP_CONSTRAINTS, profile_endtime - profile_begtime); - profile_begtime = profile_endtime; - } - - /* SOLVE CONSTRAINT ISLANDS */ - - { - ConstraintSW *ci = constraint_island_list; - while (ci) { - //iterating each island separatedly improves cache efficiency - _solve_island(ci, p_iterations, p_delta); - ci = ci->get_island_list_next(); - } - } - - { //profile - profile_endtime = OS::get_singleton()->get_ticks_usec(); - p_space->set_elapsed_time(SpaceSW::ELAPSED_TIME_SOLVE_CONSTRAINTS, profile_endtime - profile_begtime); - profile_begtime = profile_endtime; - } - - /* INTEGRATE VELOCITIES */ - - b = body_list->first(); - while (b) { - const SelfList<BodySW> *n = b->next(); - b->self()->integrate_velocities(p_delta); - b = n; - } - - /* SLEEP / WAKE UP ISLANDS */ - - { - BodySW *bi = island_list; - while (bi) { - - _check_suspend(bi, p_delta); - bi = bi->get_island_list_next(); - } - } - - { //profile - profile_endtime = OS::get_singleton()->get_ticks_usec(); - p_space->set_elapsed_time(SpaceSW::ELAPSED_TIME_INTEGRATE_VELOCITIES, profile_endtime - profile_begtime); - profile_begtime = profile_endtime; - } - - p_space->update(); - p_space->unlock(); - _step++; -} - -StepSW::StepSW() { - - _step = 1; -} diff --git a/servers/physics/step_sw.h b/servers/physics/step_sw.h deleted file mode 100644 index 40022ef8b9..0000000000 --- a/servers/physics/step_sw.h +++ /dev/null @@ -1,50 +0,0 @@ -/*************************************************************************/ -/* step_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 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_SW_H -#define STEP_SW_H - -#include "space_sw.h" - -class StepSW { - - uint64_t _step; - - void _populate_island(BodySW *p_body, BodySW **p_island, ConstraintSW **p_constraint_island); - void _setup_island(ConstraintSW *p_island, real_t p_delta); - void _solve_island(ConstraintSW *p_island, int p_iterations, real_t p_delta); - void _check_suspend(BodySW *p_island, real_t p_delta); - -public: - void step(SpaceSW *p_space, real_t p_delta, int p_iterations); - StepSW(); -}; - -#endif // STEP__SW_H |