diff options
Diffstat (limited to 'servers/physics_3d')
45 files changed, 15988 insertions, 0 deletions
diff --git a/servers/physics_3d/SCsub b/servers/physics_3d/SCsub new file mode 100644 index 0000000000..df7b521693 --- /dev/null +++ b/servers/physics_3d/SCsub @@ -0,0 +1,7 @@ +#!/usr/bin/env python + +Import("env") + +env.add_source_files(env.servers_sources, "*.cpp") + +SConscript("joints/SCsub") diff --git a/servers/physics_3d/area_3d_sw.cpp b/servers/physics_3d/area_3d_sw.cpp new file mode 100644 index 0000000000..911a664a10 --- /dev/null +++ b/servers/physics_3d/area_3d_sw.cpp @@ -0,0 +1,264 @@ +/*************************************************************************/ +/* area_3d_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_3d_sw.h" +#include "body_3d_sw.h" +#include "space_3d_sw.h" + +Area3DSW::BodyKey::BodyKey(Body3DSW *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; +} +Area3DSW::BodyKey::BodyKey(Area3DSW *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 Area3DSW::_shapes_changed() { + + if (!moved_list.in_list() && get_space()) + get_space()->area_add_to_moved_list(&moved_list); +} + +void Area3DSW::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 Area3DSW::set_space(Space3DSW *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 Area3DSW::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 Area3DSW::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 Area3DSW::set_space_override_mode(PhysicsServer3D::AreaSpaceOverrideMode p_mode) { + bool do_override = p_mode != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED; + if (do_override == (space_override_mode != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED)) + return; + _unregister_shapes(); + space_override_mode = p_mode; + _shape_changed(); +} + +void Area3DSW::set_param(PhysicsServer3D::AreaParameter p_param, const Variant &p_value) { + + switch (p_param) { + case PhysicsServer3D::AREA_PARAM_GRAVITY: gravity = p_value; break; + case PhysicsServer3D::AREA_PARAM_GRAVITY_VECTOR: gravity_vector = p_value; break; + case PhysicsServer3D::AREA_PARAM_GRAVITY_IS_POINT: gravity_is_point = p_value; break; + case PhysicsServer3D::AREA_PARAM_GRAVITY_DISTANCE_SCALE: gravity_distance_scale = p_value; break; + case PhysicsServer3D::AREA_PARAM_GRAVITY_POINT_ATTENUATION: point_attenuation = p_value; break; + case PhysicsServer3D::AREA_PARAM_LINEAR_DAMP: linear_damp = p_value; break; + case PhysicsServer3D::AREA_PARAM_ANGULAR_DAMP: angular_damp = p_value; break; + case PhysicsServer3D::AREA_PARAM_PRIORITY: priority = p_value; break; + } +} + +Variant Area3DSW::get_param(PhysicsServer3D::AreaParameter p_param) const { + + switch (p_param) { + case PhysicsServer3D::AREA_PARAM_GRAVITY: return gravity; + case PhysicsServer3D::AREA_PARAM_GRAVITY_VECTOR: return gravity_vector; + case PhysicsServer3D::AREA_PARAM_GRAVITY_IS_POINT: return gravity_is_point; + case PhysicsServer3D::AREA_PARAM_GRAVITY_DISTANCE_SCALE: return gravity_distance_scale; + case PhysicsServer3D::AREA_PARAM_GRAVITY_POINT_ATTENUATION: return point_attenuation; + case PhysicsServer3D::AREA_PARAM_LINEAR_DAMP: return linear_damp; + case PhysicsServer3D::AREA_PARAM_ANGULAR_DAMP: return angular_damp; + case PhysicsServer3D::AREA_PARAM_PRIORITY: return priority; + } + + return Variant(); +} + +void Area3DSW::_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 Area3DSW::set_monitorable(bool p_monitorable) { + + if (monitorable == p_monitorable) + return; + + monitorable = p_monitorable; + _set_static(!monitorable); +} + +void Area3DSW::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 ? PhysicsServer3D::AREA_BODY_ADDED : PhysicsServer3D::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 ? PhysicsServer3D::AREA_BODY_ADDED : PhysicsServer3D::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); +} + +Area3DSW::Area3DSW() : + CollisionObject3DSW(TYPE_AREA), + monitor_query_list(this), + moved_list(this) { + + _set_static(true); //areas are never active + space_override_mode = PhysicsServer3D::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; +} + +Area3DSW::~Area3DSW() { +} diff --git a/servers/physics_3d/area_3d_sw.h b/servers/physics_3d/area_3d_sw.h new file mode 100644 index 0000000000..05e74e63dc --- /dev/null +++ b/servers/physics_3d/area_3d_sw.h @@ -0,0 +1,203 @@ +/*************************************************************************/ +/* area_3d_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_3d_sw.h" +#include "core/self_list.h" +#include "servers/physics_server_3d.h" +//#include "servers/physics_3d/query_sw.h" + +class Space3DSW; +class Body3DSW; +class Constraint3DSW; + +class Area3DSW : public CollisionObject3DSW { + + PhysicsServer3D::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<Area3DSW> monitor_query_list; + SelfList<Area3DSW> 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(Body3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape); + BodyKey(Area3DSW *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<Constraint3DSW *> 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(Body3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape); + _FORCE_INLINE_ void remove_body_from_query(Body3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape); + + _FORCE_INLINE_ void add_area_to_query(Area3DSW *p_area, uint32_t p_area_shape, uint32_t p_self_shape); + _FORCE_INLINE_ void remove_area_from_query(Area3DSW *p_area, uint32_t p_area_shape, uint32_t p_self_shape); + + void set_param(PhysicsServer3D::AreaParameter p_param, const Variant &p_value); + Variant get_param(PhysicsServer3D::AreaParameter p_param) const; + + void set_space_override_mode(PhysicsServer3D::AreaSpaceOverrideMode p_mode); + PhysicsServer3D::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(Constraint3DSW *p_constraint) { constraints.insert(p_constraint); } + _FORCE_INLINE_ void remove_constraint(Constraint3DSW *p_constraint) { constraints.erase(p_constraint); } + _FORCE_INLINE_ const Set<Constraint3DSW *> &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(Space3DSW *p_space); + + void call_queries(); + + Area3DSW(); + ~Area3DSW(); +}; + +void Area3DSW::add_body_to_query(Body3DSW *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 Area3DSW::remove_body_from_query(Body3DSW *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 Area3DSW::add_area_to_query(Area3DSW *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 Area3DSW::remove_area_from_query(Area3DSW *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_3d/area_pair_3d_sw.cpp b/servers/physics_3d/area_pair_3d_sw.cpp new file mode 100644 index 0000000000..fa2fb2dabb --- /dev/null +++ b/servers/physics_3d/area_pair_3d_sw.cpp @@ -0,0 +1,159 @@ +/*************************************************************************/ +/* area_pair_3d_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_3d_sw.h" +#include "collision_solver_3d_sw.h" + +bool AreaPair3DSW::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) && CollisionSolver3DSW::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), nullptr, this)) { + result = true; + } + + if (result != colliding) { + + if (result) { + + if (area->get_space_override_mode() != PhysicsServer3D::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() != PhysicsServer3D::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 AreaPair3DSW::solve(real_t p_step) { +} + +AreaPair3DSW::AreaPair3DSW(Body3DSW *p_body, int p_body_shape, Area3DSW *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() == PhysicsServer3D::BODY_MODE_KINEMATIC) + p_body->set_active(true); +} + +AreaPair3DSW::~AreaPair3DSW() { + + if (colliding) { + + if (area->get_space_override_mode() != PhysicsServer3D::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 Area2Pair3DSW::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) && CollisionSolver3DSW::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), nullptr, 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 Area2Pair3DSW::solve(real_t p_step) { +} + +Area2Pair3DSW::Area2Pair3DSW(Area3DSW *p_area_a, int p_shape_a, Area3DSW *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); +} + +Area2Pair3DSW::~Area2Pair3DSW() { + + 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_3d/area_pair_3d_sw.h b/servers/physics_3d/area_pair_3d_sw.h new file mode 100644 index 0000000000..3490f41c26 --- /dev/null +++ b/servers/physics_3d/area_pair_3d_sw.h @@ -0,0 +1,70 @@ +/*************************************************************************/ +/* area_pair_3d_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_3d_sw.h" +#include "body_3d_sw.h" +#include "constraint_3d_sw.h" + +class AreaPair3DSW : public Constraint3DSW { + + Body3DSW *body; + Area3DSW *area; + int body_shape; + int area_shape; + bool colliding; + +public: + bool setup(real_t p_step); + void solve(real_t p_step); + + AreaPair3DSW(Body3DSW *p_body, int p_body_shape, Area3DSW *p_area, int p_area_shape); + ~AreaPair3DSW(); +}; + +class Area2Pair3DSW : public Constraint3DSW { + + Area3DSW *area_a; + Area3DSW *area_b; + int shape_a; + int shape_b; + bool colliding; + +public: + bool setup(real_t p_step); + void solve(real_t p_step); + + Area2Pair3DSW(Area3DSW *p_area_a, int p_shape_a, Area3DSW *p_area_b, int p_shape_b); + ~Area2Pair3DSW(); +}; + +#endif // AREA_PAIR__SW_H diff --git a/servers/physics_3d/body_3d_sw.cpp b/servers/physics_3d/body_3d_sw.cpp new file mode 100644 index 0000000000..fea5aed6ad --- /dev/null +++ b/servers/physics_3d/body_3d_sw.cpp @@ -0,0 +1,814 @@ +/*************************************************************************/ +/* body_3d_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_3d_sw.h" +#include "area_3d_sw.h" +#include "space_3d_sw.h" + +void Body3DSW::_update_inertia() { + + if (get_space() && !inertia_update_list.in_list()) + get_space()->body_add_to_inertia_update_list(&inertia_update_list); +} + +void Body3DSW::_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 Body3DSW::update_inertias() { + + //update shapes and motions + + switch (mode) { + + case PhysicsServer3D::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 Shape3DSW *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 PhysicsServer3D::BODY_MODE_KINEMATIC: + case PhysicsServer3D::BODY_MODE_STATIC: { + + _inv_inertia_tensor.set_zero(); + _inv_mass = 0; + } break; + case PhysicsServer3D::BODY_MODE_CHARACTER: { + + _inv_inertia_tensor.set_zero(); + _inv_mass = 1.0 / mass; + + } break; + } + + //_update_shapes(); + + _update_transform_dependant(); +} + +void Body3DSW::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 == PhysicsServer3D::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 Body3DSW::set_param(PhysicsServer3D::BodyParameter p_param, real_t p_value) { + + switch (p_param) { + case PhysicsServer3D::BODY_PARAM_BOUNCE: { + + bounce = p_value; + } break; + case PhysicsServer3D::BODY_PARAM_FRICTION: { + + friction = p_value; + } break; + case PhysicsServer3D::BODY_PARAM_MASS: { + ERR_FAIL_COND(p_value <= 0); + mass = p_value; + _update_inertia(); + + } break; + case PhysicsServer3D::BODY_PARAM_GRAVITY_SCALE: { + gravity_scale = p_value; + } break; + case PhysicsServer3D::BODY_PARAM_LINEAR_DAMP: { + + linear_damp = p_value; + } break; + case PhysicsServer3D::BODY_PARAM_ANGULAR_DAMP: { + + angular_damp = p_value; + } break; + default: { + } + } +} + +real_t Body3DSW::get_param(PhysicsServer3D::BodyParameter p_param) const { + + switch (p_param) { + case PhysicsServer3D::BODY_PARAM_BOUNCE: { + + return bounce; + } break; + case PhysicsServer3D::BODY_PARAM_FRICTION: { + + return friction; + } break; + case PhysicsServer3D::BODY_PARAM_MASS: { + return mass; + } break; + case PhysicsServer3D::BODY_PARAM_GRAVITY_SCALE: { + return gravity_scale; + } break; + case PhysicsServer3D::BODY_PARAM_LINEAR_DAMP: { + + return linear_damp; + } break; + case PhysicsServer3D::BODY_PARAM_ANGULAR_DAMP: { + + return angular_damp; + } break; + + default: { + } + } + + return 0; +} + +void Body3DSW::set_mode(PhysicsServer3D::BodyMode p_mode) { + + PhysicsServer3D::BodyMode prev = mode; + mode = p_mode; + + switch (p_mode) { + //CLEAR UP EVERYTHING IN CASE IT NOT WORKS! + case PhysicsServer3D::BODY_MODE_STATIC: + case PhysicsServer3D::BODY_MODE_KINEMATIC: { + + _set_inv_transform(get_transform().affine_inverse()); + _inv_mass = 0; + _set_static(p_mode == PhysicsServer3D::BODY_MODE_STATIC); + //set_active(p_mode==PhysicsServer3D::BODY_MODE_KINEMATIC); + set_active(p_mode == PhysicsServer3D::BODY_MODE_KINEMATIC && contacts.size()); + linear_velocity = Vector3(); + angular_velocity = Vector3(); + if (mode == PhysicsServer3D::BODY_MODE_KINEMATIC && prev != mode) { + first_time_kinematic = true; + } + + } break; + case PhysicsServer3D::BODY_MODE_RIGID: { + + _inv_mass = mass > 0 ? (1.0 / mass) : 0; + _set_static(false); + set_active(true); + + } break; + case PhysicsServer3D::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(); + */ +} +PhysicsServer3D::BodyMode Body3DSW::get_mode() const { + + return mode; +} + +void Body3DSW::_shapes_changed() { + + _update_inertia(); +} + +void Body3DSW::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_variant) { + + switch (p_state) { + case PhysicsServer3D::BODY_STATE_TRANSFORM: { + + if (mode == PhysicsServer3D::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 == PhysicsServer3D::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 PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY: { + + /* + if (mode==PhysicsServer3D::BODY_MODE_STATIC) + break; + */ + linear_velocity = p_variant; + wakeup(); + } break; + case PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY: { + /* + if (mode!=PhysicsServer3D::BODY_MODE_RIGID) + break; + */ + angular_velocity = p_variant; + wakeup(); + + } break; + case PhysicsServer3D::BODY_STATE_SLEEPING: { + //? + if (mode == PhysicsServer3D::BODY_MODE_STATIC || mode == PhysicsServer3D::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 PhysicsServer3D::BODY_STATE_CAN_SLEEP: { + can_sleep = p_variant; + if (mode == PhysicsServer3D::BODY_MODE_RIGID && !active && !can_sleep) + set_active(true); + + } break; + } +} +Variant Body3DSW::get_state(PhysicsServer3D::BodyState p_state) const { + + switch (p_state) { + case PhysicsServer3D::BODY_STATE_TRANSFORM: { + return get_transform(); + } break; + case PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY: { + return linear_velocity; + } break; + case PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY: { + return angular_velocity; + } break; + case PhysicsServer3D::BODY_STATE_SLEEPING: { + return !is_active(); + } break; + case PhysicsServer3D::BODY_STATE_CAN_SLEEP: { + return can_sleep; + } break; + } + + return Variant(); +} + +void Body3DSW::set_space(Space3DSW *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 Body3DSW::_compute_area_gravity_and_dampenings(const Area3DSW *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 Body3DSW::set_axis_lock(PhysicsServer3D::BodyAxis p_axis, bool lock) { + if (lock) { + locked_axis |= p_axis; + } else { + locked_axis &= ~p_axis; + } +} + +bool Body3DSW::is_axis_locked(PhysicsServer3D::BodyAxis p_axis) const { + return locked_axis & p_axis; +} + +void Body3DSW::integrate_forces(real_t p_step) { + + if (mode == PhysicsServer3D::BODY_MODE_STATIC) + return; + + Area3DSW *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--) { + PhysicsServer3D::AreaSpaceOverrideMode mode = aa[i].area->get_space_override_mode(); + switch (mode) { + case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE: + case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: { + _compute_area_gravity_and_dampenings(aa[i].area); + stopped = mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE; + } break; + case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE: + case PhysicsServer3D::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 == PhysicsServer3D::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 == PhysicsServer3D::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 = nullptr; // clear the area, so it is set in the next frame + contact_count = 0; +} + +void Body3DSW::integrate_velocities(real_t p_step) { + + if (mode == PhysicsServer3D::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((PhysicsServer3D::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((PhysicsServer3D::BodyAxis)(1 << (i + 3)))) { + angular_velocity[i] = 0; + biased_angular_velocity[i] = 0; + } + } + + if (mode == PhysicsServer3D::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 Body3DSW::wakeup_neighbours() { + + for (Map<Constraint3DSW *, int>::Element *E = constraint_map.front(); E; E = E->next()) { + + const Constraint3DSW *c = E->key(); + Body3DSW **n = c->get_body_ptr(); + int bc = c->get_body_count(); + + for (int i = 0; i < bc; i++) { + + if (i == E->get()) + continue; + Body3DSW *b = n[i]; + if (b->mode != PhysicsServer3D::BODY_MODE_RIGID) + continue; + + if (!b->is_active()) + b->set_active(true); + } + } +} + +void Body3DSW::call_queries() { + + if (fi_callback) { + + PhysicsDirectBodyState3DSW *dbs = PhysicsDirectBodyState3DSW::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 Body3DSW::sleep_test(real_t p_step) { + + if (mode == PhysicsServer3D::BODY_MODE_STATIC || mode == PhysicsServer3D::BODY_MODE_KINEMATIC) + return true; // + else if (mode == PhysicsServer3D::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 Body3DSW::set_force_integration_callback(ObjectID p_id, const StringName &p_method, const Variant &p_udata) { + + if (fi_callback) { + + memdelete(fi_callback); + fi_callback = nullptr; + } + + 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 Body3DSW::set_kinematic_margin(real_t p_margin) { + kinematic_safe_margin = p_margin; +} + +Body3DSW::Body3DSW() : + CollisionObject3DSW(TYPE_BODY), + locked_axis(0), + active_list(this), + inertia_update_list(this), + direct_state_query_list(this) { + + mode = PhysicsServer3D::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 = nullptr; + island_list_next = nullptr; + 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 = nullptr; +} + +Body3DSW::~Body3DSW() { + + if (fi_callback) + memdelete(fi_callback); +} + +PhysicsDirectBodyState3DSW *PhysicsDirectBodyState3DSW::singleton = nullptr; + +PhysicsDirectSpaceState3D *PhysicsDirectBodyState3DSW::get_space_state() { + + return body->get_space()->get_direct_state(); +} diff --git a/servers/physics_3d/body_3d_sw.h b/servers/physics_3d/body_3d_sw.h new file mode 100644 index 0000000000..2bd335e6c0 --- /dev/null +++ b/servers/physics_3d/body_3d_sw.h @@ -0,0 +1,475 @@ +/*************************************************************************/ +/* body_3d_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_3d_sw.h" +#include "collision_object_3d_sw.h" +#include "core/vset.h" + +class Constraint3DSW; + +class Body3DSW : public CollisionObject3DSW { + + PhysicsServer3D::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<Body3DSW> active_list; + SelfList<Body3DSW> inertia_update_list; + SelfList<Body3DSW> 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<Constraint3DSW *, int> constraint_map; + + struct AreaCMP { + + Area3DSW *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(Area3DSW *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; + Body3DSW *island_next; + Body3DSW *island_list_next; + + _FORCE_INLINE_ void _compute_area_gravity_and_dampenings(const Area3DSW *p_area); + + _FORCE_INLINE_ void _update_transform_dependant(); + + friend class PhysicsDirectBodyState3DSW; // 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(Area3DSW *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(Area3DSW *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 == PhysicsServer3D::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_ Body3DSW *get_island_next() const { return island_next; } + _FORCE_INLINE_ void set_island_next(Body3DSW *p_next) { island_next = p_next; } + + _FORCE_INLINE_ Body3DSW *get_island_list_next() const { return island_list_next; } + _FORCE_INLINE_ void set_island_list_next(Body3DSW *p_next) { island_list_next = p_next; } + + _FORCE_INLINE_ void add_constraint(Constraint3DSW *p_constraint, int p_pos) { constraint_map[p_constraint] = p_pos; } + _FORCE_INLINE_ void remove_constraint(Constraint3DSW *p_constraint) { constraint_map.erase(p_constraint); } + const Map<Constraint3DSW *, 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 - center_of_mass).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 == PhysicsServer3D::BODY_MODE_STATIC || mode == PhysicsServer3D::BODY_MODE_KINEMATIC) + return; + set_active(true); + } + + void set_param(PhysicsServer3D::BodyParameter p_param, real_t); + real_t get_param(PhysicsServer3D::BodyParameter p_param) const; + + void set_mode(PhysicsServer3D::BodyMode p_mode); + PhysicsServer3D::BodyMode get_mode() const; + + void set_state(PhysicsServer3D::BodyState p_state, const Variant &p_variant); + Variant get_state(PhysicsServer3D::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(Space3DSW *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(PhysicsServer3D::BodyAxis p_axis, bool lock); + bool is_axis_locked(PhysicsServer3D::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); + + Body3DSW(); + ~Body3DSW(); +}; + +//add contact inline + +void Body3DSW::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 PhysicsDirectBodyState3DSW : public PhysicsDirectBodyState3D { + + GDCLASS(PhysicsDirectBodyState3DSW, PhysicsDirectBodyState3D); + +public: + static PhysicsDirectBodyState3DSW *singleton; + Body3DSW *body; + real_t step; + + virtual Vector3 get_total_gravity() const { return body->gravity; } // get gravity vector working on this body space/area + virtual real_t get_total_angular_damp() const { return body->area_angular_damp; } // get density of this body space/area + virtual real_t get_total_linear_damp() const { return body->area_linear_damp; } // get density of this body space/area + + virtual Vector3 get_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(PhysicsServer3D::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_sleep) { body->set_active(!p_sleep); } + 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 PhysicsDirectSpaceState3D *get_space_state(); + + virtual real_t get_step() const { return step; } + PhysicsDirectBodyState3DSW() { + singleton = this; + body = nullptr; + } +}; + +#endif // BODY__SW_H diff --git a/servers/physics_3d/body_pair_3d_sw.cpp b/servers/physics_3d/body_pair_3d_sw.cpp new file mode 100644 index 0000000000..245fb3449c --- /dev/null +++ b/servers/physics_3d/body_pair_3d_sw.cpp @@ -0,0 +1,495 @@ +/*************************************************************************/ +/* body_pair_3d_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_3d_sw.h" + +#include "collision_solver_3d_sw.h" +#include "core/os/os.h" +#include "space_3d_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 BodyPair3DSW::_contact_added_callback(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata) { + + BodyPair3DSW *pair = (BodyPair3DSW *)p_userdata; + pair->contact_added_callback(p_point_A, p_point_B); +} + +void BodyPair3DSW::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 BodyPair3DSW::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 BodyPair3DSW::_test_ccd(real_t p_step, Body3DSW *p_A, int p_shape_A, const Transform &p_xform_A, Body3DSW *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(Body3DSW *A, Body3DSW *B) { + return CLAMP(A->get_bounce() + B->get_bounce(), 0, 1); +} + +real_t combine_friction(Body3DSW *A, Body3DSW *B) { + return ABS(MIN(A->get_friction(), B->get_friction())); +} + +bool BodyPair3DSW::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() <= PhysicsServer3D::BODY_MODE_KINEMATIC && B->get_mode() <= PhysicsServer3D::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); + + Shape3DSW *shape_A_ptr = A->get_shape(shape_A); + Shape3DSW *shape_B_ptr = B->get_shape(shape_B); + + bool collided = CollisionSolver3DSW::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() > PhysicsServer3D::BODY_MODE_KINEMATIC && B->get_mode() <= PhysicsServer3D::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() > PhysicsServer3D::BODY_MODE_KINEMATIC && A->get_mode() <= PhysicsServer3D::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 BodyPair3DSW::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; + } + } +} + +BodyPair3DSW::BodyPair3DSW(Body3DSW *p_A, int p_shape_A, Body3DSW *p_B, int p_shape_B) : + Constraint3DSW(_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; +} + +BodyPair3DSW::~BodyPair3DSW() { + + A->remove_constraint(this); + B->remove_constraint(this); +} diff --git a/servers/physics_3d/body_pair_3d_sw.h b/servers/physics_3d/body_pair_3d_sw.h new file mode 100644 index 0000000000..7f4afb9dca --- /dev/null +++ b/servers/physics_3d/body_pair_3d_sw.h @@ -0,0 +1,97 @@ +/*************************************************************************/ +/* body_pair_3d_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_3d_sw.h" +#include "constraint_3d_sw.h" + +class BodyPair3DSW : public Constraint3DSW { + enum { + + MAX_CONTACTS = 4 + }; + + union { + struct { + Body3DSW *A; + Body3DSW *B; + }; + + Body3DSW *_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, Body3DSW *p_A, int p_shape_A, const Transform &p_xform_A, Body3DSW *p_B, int p_shape_B, const Transform &p_xform_B); + + Space3DSW *space; + +public: + bool setup(real_t p_step); + void solve(real_t p_step); + + BodyPair3DSW(Body3DSW *p_A, int p_shape_A, Body3DSW *p_B, int p_shape_B); + ~BodyPair3DSW(); +}; + +#endif // BODY_PAIR__SW_H diff --git a/servers/physics_3d/broad_phase_3d_basic.cpp b/servers/physics_3d/broad_phase_3d_basic.cpp new file mode 100644 index 0000000000..08ea219869 --- /dev/null +++ b/servers/physics_3d/broad_phase_3d_basic.cpp @@ -0,0 +1,225 @@ +/*************************************************************************/ +/* broad_phase_3d_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_3d_basic.h" +#include "core/list.h" +#include "core/print_string.h" + +BroadPhase3DSW::ID BroadPhase3DBasic::create(CollisionObject3DSW *p_object, int p_subindex) { + + ERR_FAIL_COND_V(p_object == nullptr, 0); + + current++; + + Element e; + e.owner = p_object; + e._static = false; + e.subindex = p_subindex; + + element_map[current] = e; + return current; +} + +void BroadPhase3DBasic::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 BroadPhase3DBasic::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 BroadPhase3DBasic::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); +} + +CollisionObject3DSW *BroadPhase3DBasic::get_object(ID p_id) const { + + const Map<ID, Element>::Element *E = element_map.find(p_id); + ERR_FAIL_COND_V(!E, nullptr); + return E->get().owner; +} +bool BroadPhase3DBasic::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 BroadPhase3DBasic::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 BroadPhase3DBasic::cull_point(const Vector3 &p_point, CollisionObject3DSW **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 BroadPhase3DBasic::cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObject3DSW **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 BroadPhase3DBasic::cull_aabb(const AABB &p_aabb, CollisionObject3DSW **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 BroadPhase3DBasic::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) { + + pair_userdata = p_userdata; + pair_callback = p_pair_callback; +} +void BroadPhase3DBasic::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) { + + unpair_userdata = p_userdata; + unpair_callback = p_unpair_callback; +} + +void BroadPhase3DBasic::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 = nullptr; + 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); + } + } + } +} + +BroadPhase3DSW *BroadPhase3DBasic::_create() { + + return memnew(BroadPhase3DBasic); +} + +BroadPhase3DBasic::BroadPhase3DBasic() { + + current = 1; + unpair_callback = nullptr; + unpair_userdata = nullptr; + pair_callback = nullptr; + pair_userdata = nullptr; +} diff --git a/servers/physics_3d/broad_phase_3d_basic.h b/servers/physics_3d/broad_phase_3d_basic.h new file mode 100644 index 0000000000..563dda6931 --- /dev/null +++ b/servers/physics_3d/broad_phase_3d_basic.h @@ -0,0 +1,108 @@ +/*************************************************************************/ +/* broad_phase_3d_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_3d_sw.h" +#include "core/map.h" + +class BroadPhase3DBasic : public BroadPhase3DSW { + + struct Element { + + CollisionObject3DSW *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(CollisionObject3DSW *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 CollisionObject3DSW *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, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices = nullptr); + virtual int cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices = nullptr); + virtual int cull_aabb(const AABB &p_aabb, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices = nullptr); + + virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata); + virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata); + + virtual void update(); + + static BroadPhase3DSW *_create(); + BroadPhase3DBasic(); +}; + +#endif // BROAD_PHASE_BASIC_H diff --git a/servers/physics_3d/broad_phase_3d_sw.cpp b/servers/physics_3d/broad_phase_3d_sw.cpp new file mode 100644 index 0000000000..1a20fdd0cb --- /dev/null +++ b/servers/physics_3d/broad_phase_3d_sw.cpp @@ -0,0 +1,36 @@ +/*************************************************************************/ +/* broad_phase_3d_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_3d_sw.h" + +BroadPhase3DSW::CreateFunction BroadPhase3DSW::create_func = nullptr; + +BroadPhase3DSW::~BroadPhase3DSW() { +} diff --git a/servers/physics_3d/broad_phase_3d_sw.h b/servers/physics_3d/broad_phase_3d_sw.h new file mode 100644 index 0000000000..5950489619 --- /dev/null +++ b/servers/physics_3d/broad_phase_3d_sw.h @@ -0,0 +1,73 @@ +/*************************************************************************/ +/* broad_phase_3d_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 CollisionObject3DSW; + +class BroadPhase3DSW { + +public: + typedef BroadPhase3DSW *(*CreateFunction)(); + + static CreateFunction create_func; + + typedef uint32_t ID; + + typedef void *(*PairCallback)(CollisionObject3DSW *A, int p_subindex_A, CollisionObject3DSW *B, int p_subindex_B, void *p_userdata); + typedef void (*UnpairCallback)(CollisionObject3DSW *A, int p_subindex_A, CollisionObject3DSW *B, int p_subindex_B, void *p_data, void *p_userdata); + + // 0 is an invalid ID + virtual ID create(CollisionObject3DSW *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 CollisionObject3DSW *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, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices = nullptr) = 0; + virtual int cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices = nullptr) = 0; + virtual int cull_aabb(const AABB &p_aabb, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices = nullptr) = 0; + + virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata) = 0; + virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) = 0; + + virtual void update() = 0; + + virtual ~BroadPhase3DSW(); +}; + +#endif // BROAD_PHASE__SW_H diff --git a/servers/physics_3d/broad_phase_octree.cpp b/servers/physics_3d/broad_phase_octree.cpp new file mode 100644 index 0000000000..264ab21e1e --- /dev/null +++ b/servers/physics_3d/broad_phase_octree.cpp @@ -0,0 +1,129 @@ +/*************************************************************************/ +/* 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_3d_sw.h" + +BroadPhase3DSW::ID BroadPhaseOctree::create(CollisionObject3DSW *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) { + + CollisionObject3DSW *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); +} + +CollisionObject3DSW *BroadPhaseOctree::get_object(ID p_id) const { + + CollisionObject3DSW *it = octree.get(p_id); + ERR_FAIL_COND_V(!it, nullptr); + 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, CollisionObject3DSW **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, CollisionObject3DSW **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, CollisionObject3DSW **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, CollisionObject3DSW *p_object_A, int subindex_A, OctreeElementID p_B, CollisionObject3DSW *p_object_B, int subindex_B) { + + BroadPhaseOctree *bpo = (BroadPhaseOctree *)(self); + if (!bpo->pair_callback) + return nullptr; + + return bpo->pair_callback(p_object_A, subindex_A, p_object_B, subindex_B, bpo->pair_userdata); +} + +void BroadPhaseOctree::_unpair_callback(void *self, OctreeElementID p_A, CollisionObject3DSW *p_object_A, int subindex_A, OctreeElementID p_B, CollisionObject3DSW *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? +} + +BroadPhase3DSW *BroadPhaseOctree::_create() { + + return memnew(BroadPhaseOctree); +} + +BroadPhaseOctree::BroadPhaseOctree() { + octree.set_pair_callback(_pair_callback, this); + octree.set_unpair_callback(_unpair_callback, this); + pair_callback = nullptr; + pair_userdata = nullptr; + unpair_userdata = nullptr; +} diff --git a/servers/physics_3d/broad_phase_octree.h b/servers/physics_3d/broad_phase_octree.h new file mode 100644 index 0000000000..0ad59d8b0c --- /dev/null +++ b/servers/physics_3d/broad_phase_octree.h @@ -0,0 +1,73 @@ +/*************************************************************************/ +/* 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_3d_sw.h" +#include "core/math/octree.h" + +class BroadPhaseOctree : public BroadPhase3DSW { + + Octree<CollisionObject3DSW, true> octree; + + static void *_pair_callback(void *, OctreeElementID, CollisionObject3DSW *, int, OctreeElementID, CollisionObject3DSW *, int); + static void _unpair_callback(void *, OctreeElementID, CollisionObject3DSW *, int, OctreeElementID, CollisionObject3DSW *, int, void *); + + PairCallback pair_callback; + void *pair_userdata; + UnpairCallback unpair_callback; + void *unpair_userdata; + +public: + // 0 is an invalid ID + virtual ID create(CollisionObject3DSW *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 CollisionObject3DSW *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, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices = nullptr); + virtual int cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices = nullptr); + virtual int cull_aabb(const AABB &p_aabb, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices = nullptr); + + virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata); + virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata); + + virtual void update(); + + static BroadPhase3DSW *_create(); + BroadPhaseOctree(); +}; + +#endif // BROAD_PHASE_OCTREE_H diff --git a/servers/physics_3d/collision_object_3d_sw.cpp b/servers/physics_3d/collision_object_3d_sw.cpp new file mode 100644 index 0000000000..24715d211d --- /dev/null +++ b/servers/physics_3d/collision_object_3d_sw.cpp @@ -0,0 +1,239 @@ +/*************************************************************************/ +/* collision_object_3d_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_3d_sw.h" +#include "servers/physics_3d/physics_server_3d_sw.h" +#include "space_3d_sw.h" + +void CollisionObject3DSW::add_shape(Shape3DSW *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()) { + PhysicsServer3DSW::singleton->pending_shape_update_list.add(&pending_shape_update_list); + } + //_update_shapes(); + //_shapes_changed(); +} + +void CollisionObject3DSW::set_shape(int p_index, Shape3DSW *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()) { + PhysicsServer3DSW::singleton->pending_shape_update_list.add(&pending_shape_update_list); + } + //_update_shapes(); + //_shapes_changed(); +} +void CollisionObject3DSW::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()) { + PhysicsServer3DSW::singleton->pending_shape_update_list.add(&pending_shape_update_list); + } + //_update_shapes(); + //_shapes_changed(); +} + +void CollisionObject3DSW::set_shape_as_disabled(int p_idx, bool p_enable) { + shapes.write[p_idx].disabled = p_enable; + if (!pending_shape_update_list.in_list()) { + PhysicsServer3DSW::singleton->pending_shape_update_list.add(&pending_shape_update_list); + } +} + +void CollisionObject3DSW::remove_shape(Shape3DSW *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 CollisionObject3DSW::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()) { + PhysicsServer3DSW::singleton->pending_shape_update_list.add(&pending_shape_update_list); + } + //_update_shapes(); + //_shapes_changed(); +} + +void CollisionObject3DSW::_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 CollisionObject3DSW::_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 CollisionObject3DSW::_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 CollisionObject3DSW::_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 CollisionObject3DSW::_set_space(Space3DSW *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 CollisionObject3DSW::_shape_changed() { + + _update_shapes(); + _shapes_changed(); +} + +CollisionObject3DSW::CollisionObject3DSW(Type p_type) : + pending_shape_update_list(this) { + + _static = true; + type = p_type; + space = nullptr; + + collision_layer = 1; + collision_mask = 1; + ray_pickable = true; +} diff --git a/servers/physics_3d/collision_object_3d_sw.h b/servers/physics_3d/collision_object_3d_sw.h new file mode 100644 index 0000000000..c5773d0c61 --- /dev/null +++ b/servers/physics_3d/collision_object_3d_sw.h @@ -0,0 +1,164 @@ +/*************************************************************************/ +/* collision_object_3d_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_3d_sw.h" +#include "core/self_list.h" +#include "servers/physics_server_3d.h" +#include "shape_3d_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 Space3DSW; + +class CollisionObject3DSW : public ShapeOwner3DSW { +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; + BroadPhase3DSW::ID bpid; + AABB aabb_cache; //for rayqueries + real_t area_cache; + Shape3DSW *shape; + bool disabled; + + Shape() { disabled = false; } + }; + + Vector<Shape> shapes; + Space3DSW *space; + Transform transform; + Transform inv_transform; + bool _static; + + SelfList<CollisionObject3DSW> 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(Space3DSW *p_space); + + bool ray_pickable; + + CollisionObject3DSW(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(Shape3DSW *p_shape, const Transform &p_transform = Transform(), bool p_disabled = false); + void set_shape(int p_index, Shape3DSW *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_ Shape3DSW *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_ Space3DSW *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(CollisionObject3DSW *p_other) const { + return collision_layer & p_other->collision_mask || p_other->collision_layer & collision_mask; + } + + void remove_shape(Shape3DSW *p_shape); + void remove_shape(int p_index); + + virtual void set_space(Space3DSW *p_space) = 0; + + _FORCE_INLINE_ bool is_static() const { return _static; } + + virtual ~CollisionObject3DSW() {} +}; + +#endif // COLLISION_OBJECT_SW_H diff --git a/servers/physics_3d/collision_solver_3d_sat.cpp b/servers/physics_3d/collision_solver_3d_sat.cpp new file mode 100644 index 0000000000..5096b080ab --- /dev/null +++ b/servers/physics_3d/collision_solver_3d_sat.cpp @@ -0,0 +1,1591 @@ +/*************************************************************************/ +/* collision_solver_3d_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_3d_sat.h" +#include "core/math/geometry.h" + +#define _EDGE_IS_VALID_SUPPORT_THRESHOLD 0.02 + +struct _CollectorCallback { + + CollisionSolver3DSW::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, + }, + { + nullptr, + _generate_contacts_edge_edge, + _generate_contacts_face_face, + }, + { + nullptr, + nullptr, + _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 Shape3DSW *, const Transform &, const Shape3DSW *, const Transform &, _CollectorCallback *p_callback, real_t, real_t); + +template <bool withMargin> +static void _collision_sphere_sphere(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + + const SphereShape3DSW *sphere_A = static_cast<const SphereShape3DSW *>(p_a); + const SphereShape3DSW *sphere_B = static_cast<const SphereShape3DSW *>(p_b); + + SeparatorAxisTest<SphereShape3DSW, SphereShape3DSW, 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 Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + + const SphereShape3DSW *sphere_A = static_cast<const SphereShape3DSW *>(p_a); + const BoxShape3DSW *box_B = static_cast<const BoxShape3DSW *>(p_b); + + SeparatorAxisTest<SphereShape3DSW, BoxShape3DSW, 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 Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + + const SphereShape3DSW *sphere_A = static_cast<const SphereShape3DSW *>(p_a); + const CapsuleShape3DSW *capsule_B = static_cast<const CapsuleShape3DSW *>(p_b); + + SeparatorAxisTest<SphereShape3DSW, CapsuleShape3DSW, 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 Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { +} + +template <bool withMargin> +static void _collision_sphere_convex_polygon(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + + const SphereShape3DSW *sphere_A = static_cast<const SphereShape3DSW *>(p_a); + const ConvexPolygonShape3DSW *convex_polygon_B = static_cast<const ConvexPolygonShape3DSW *>(p_b); + + SeparatorAxisTest<SphereShape3DSW, ConvexPolygonShape3DSW, 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 Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + + const SphereShape3DSW *sphere_A = static_cast<const SphereShape3DSW *>(p_a); + const FaceShape3DSW *face_B = static_cast<const FaceShape3DSW *>(p_b); + + SeparatorAxisTest<SphereShape3DSW, FaceShape3DSW, 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 Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + + const BoxShape3DSW *box_A = static_cast<const BoxShape3DSW *>(p_a); + const BoxShape3DSW *box_B = static_cast<const BoxShape3DSW *>(p_b); + + SeparatorAxisTest<BoxShape3DSW, BoxShape3DSW, 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 Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + + const BoxShape3DSW *box_A = static_cast<const BoxShape3DSW *>(p_a); + const CapsuleShape3DSW *capsule_B = static_cast<const CapsuleShape3DSW *>(p_b); + + SeparatorAxisTest<BoxShape3DSW, CapsuleShape3DSW, 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 Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { +} + +template <bool withMargin> +static void _collision_box_convex_polygon(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + + const BoxShape3DSW *box_A = static_cast<const BoxShape3DSW *>(p_a); + const ConvexPolygonShape3DSW *convex_polygon_B = static_cast<const ConvexPolygonShape3DSW *>(p_b); + + SeparatorAxisTest<BoxShape3DSW, ConvexPolygonShape3DSW, 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 Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + + const BoxShape3DSW *box_A = static_cast<const BoxShape3DSW *>(p_a); + const FaceShape3DSW *face_B = static_cast<const FaceShape3DSW *>(p_b); + + SeparatorAxisTest<BoxShape3DSW, FaceShape3DSW, 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 Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + + const CapsuleShape3DSW *capsule_A = static_cast<const CapsuleShape3DSW *>(p_a); + const CapsuleShape3DSW *capsule_B = static_cast<const CapsuleShape3DSW *>(p_b); + + SeparatorAxisTest<CapsuleShape3DSW, CapsuleShape3DSW, 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 Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { +} + +template <bool withMargin> +static void _collision_capsule_convex_polygon(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + + const CapsuleShape3DSW *capsule_A = static_cast<const CapsuleShape3DSW *>(p_a); + const ConvexPolygonShape3DSW *convex_polygon_B = static_cast<const ConvexPolygonShape3DSW *>(p_b); + + SeparatorAxisTest<CapsuleShape3DSW, ConvexPolygonShape3DSW, 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 Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + + const CapsuleShape3DSW *capsule_A = static_cast<const CapsuleShape3DSW *>(p_a); + const FaceShape3DSW *face_B = static_cast<const FaceShape3DSW *>(p_b); + + SeparatorAxisTest<CapsuleShape3DSW, FaceShape3DSW, 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 Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { +} + +template <bool withMargin> +static void _collision_cylinder_convex_polygon(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { +} + +template <bool withMargin> +static void _collision_cylinder_face(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { +} + +template <bool withMargin> +static void _collision_convex_polygon_convex_polygon(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + + const ConvexPolygonShape3DSW *convex_polygon_A = static_cast<const ConvexPolygonShape3DSW *>(p_a); + const ConvexPolygonShape3DSW *convex_polygon_B = static_cast<const ConvexPolygonShape3DSW *>(p_b); + + SeparatorAxisTest<ConvexPolygonShape3DSW, ConvexPolygonShape3DSW, 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 Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + + const ConvexPolygonShape3DSW *convex_polygon_A = static_cast<const ConvexPolygonShape3DSW *>(p_a); + const FaceShape3DSW *face_B = static_cast<const FaceShape3DSW *>(p_b); + + SeparatorAxisTest<ConvexPolygonShape3DSW, FaceShape3DSW, withMargin> separator(convex_polygon_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b); + + const Geometry::MeshData &mesh = convex_polygon_A->get_mesh(); + + const 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 Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CollisionSolver3DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap, Vector3 *r_prev_axis, real_t p_margin_a, real_t p_margin_b) { + + PhysicsServer3D::ShapeType type_A = p_shape_A->get_type(); + + ERR_FAIL_COND_V(type_A == PhysicsServer3D::SHAPE_PLANE, false); + ERR_FAIL_COND_V(type_A == PhysicsServer3D::SHAPE_RAY, false); + ERR_FAIL_COND_V(p_shape_A->is_concave(), false); + + PhysicsServer3D::ShapeType type_B = p_shape_B->get_type(); + + ERR_FAIL_COND_V(type_B == PhysicsServer3D::SHAPE_PLANE, false); + ERR_FAIL_COND_V(type_B == PhysicsServer3D::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> }, + { nullptr, + _collision_box_box<false>, + _collision_box_capsule<false>, + _collision_box_cylinder<false>, + _collision_box_convex_polygon<false>, + _collision_box_face<false> }, + { nullptr, + nullptr, + _collision_capsule_capsule<false>, + _collision_capsule_cylinder<false>, + _collision_capsule_convex_polygon<false>, + _collision_capsule_face<false> }, + { nullptr, + nullptr, + nullptr, + _collision_cylinder_cylinder<false>, + _collision_cylinder_convex_polygon<false>, + _collision_cylinder_face<false> }, + { nullptr, + nullptr, + nullptr, + nullptr, + _collision_convex_polygon_convex_polygon<false>, + _collision_convex_polygon_face<false> }, + { nullptr, + nullptr, + nullptr, + nullptr, + nullptr, + nullptr }, + }; + + 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> }, + { nullptr, + _collision_box_box<true>, + _collision_box_capsule<true>, + _collision_box_cylinder<true>, + _collision_box_convex_polygon<true>, + _collision_box_face<true> }, + { nullptr, + nullptr, + _collision_capsule_capsule<true>, + _collision_capsule_cylinder<true>, + _collision_capsule_convex_polygon<true>, + _collision_capsule_face<true> }, + { nullptr, + nullptr, + nullptr, + _collision_cylinder_cylinder<true>, + _collision_cylinder_convex_polygon<true>, + _collision_cylinder_face<true> }, + { nullptr, + nullptr, + nullptr, + nullptr, + _collision_convex_polygon_convex_polygon<true>, + _collision_convex_polygon_face<true> }, + { nullptr, + nullptr, + nullptr, + nullptr, + nullptr, + nullptr }, + }; + + _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 Shape3DSW *A = p_shape_A; + const Shape3DSW *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_3d/collision_solver_3d_sat.h b/servers/physics_3d/collision_solver_3d_sat.h new file mode 100644 index 0000000000..5eccfda9ac --- /dev/null +++ b/servers/physics_3d/collision_solver_3d_sat.h @@ -0,0 +1,38 @@ +/*************************************************************************/ +/* collision_solver_3d_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_3d_sw.h" + +bool sat_calculate_penetration(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CollisionSolver3DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap = false, Vector3 *r_prev_axis = nullptr, real_t p_margin_a = 0, real_t p_margin_b = 0); + +#endif // COLLISION_SOLVER_SAT_H diff --git a/servers/physics_3d/collision_solver_3d_sw.cpp b/servers/physics_3d/collision_solver_3d_sw.cpp new file mode 100644 index 0000000000..5d31e1f546 --- /dev/null +++ b/servers/physics_3d/collision_solver_3d_sw.cpp @@ -0,0 +1,372 @@ +/*************************************************************************/ +/* collision_solver_3d_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_3d_sw.h" +#include "collision_solver_3d_sat.h" + +#include "gjk_epa.h" + +#define collision_solver sat_calculate_penetration +//#define collision_solver gjk_epa_calculate_penetration + +bool CollisionSolver3DSW::solve_static_plane(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) { + + const PlaneShape3DSW *plane = static_cast<const PlaneShape3DSW *>(p_shape_A); + if (p_shape_B->get_type() == PhysicsServer3D::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 CollisionSolver3DSW::solve_ray(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) { + + const RayShape3DSW *ray = static_cast<const RayShape3DSW *>(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 Shape3DSW *shape_A; + const Transform *transform_B; + CollisionSolver3DSW::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 CollisionSolver3DSW::concave_callback(void *p_userdata, Shape3DSW *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, nullptr, cinfo.margin_A, cinfo.margin_B); + if (!collided) + return; + + cinfo.collided = true; + cinfo.collisions++; +} + +bool CollisionSolver3DSW::solve_concave(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *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 ConcaveShape3DSW *concave_B = static_cast<const ConcaveShape3DSW *>(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 CollisionSolver3DSW::solve_static(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *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) { + + PhysicsServer3D::ShapeType type_A = p_shape_A->get_type(); + PhysicsServer3D::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 == PhysicsServer3D::SHAPE_PLANE) { + + if (type_B == PhysicsServer3D::SHAPE_PLANE) + return false; + if (type_B == PhysicsServer3D::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 == PhysicsServer3D::SHAPE_RAY) { + + if (type_B == PhysicsServer3D::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 CollisionSolver3DSW::concave_distance_callback(void *p_userdata, Shape3DSW *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 CollisionSolver3DSW::solve_distance_plane(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B) { + + const PlaneShape3DSW *plane = static_cast<const PlaneShape3DSW *>(p_shape_A); + if (p_shape_B->get_type() == PhysicsServer3D::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 CollisionSolver3DSW::solve_distance(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *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() == PhysicsServer3D::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 ConcaveShape3DSW *concave_B = static_cast<const ConcaveShape3DSW *>(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 = nullptr; + cinfo.userdata = nullptr; + 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_3d/collision_solver_3d_sw.h b/servers/physics_3d/collision_solver_3d_sw.h new file mode 100644 index 0000000000..13f54ca8fb --- /dev/null +++ b/servers/physics_3d/collision_solver_3d_sw.h @@ -0,0 +1,53 @@ +/*************************************************************************/ +/* collision_solver_3d_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_3d_sw.h" + +class CollisionSolver3DSW { +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, Shape3DSW *p_convex); + static bool solve_static_plane(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); + static bool solve_ray(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); + static bool solve_concave(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *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, Shape3DSW *p_convex); + static bool solve_distance_plane(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B); + +public: + static bool solve_static(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, Vector3 *r_sep_axis = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0); + static bool solve_distance(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *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 = nullptr); +}; + +#endif // COLLISION_SOLVER__SW_H diff --git a/servers/physics_3d/constraint_3d_sw.h b/servers/physics_3d/constraint_3d_sw.h new file mode 100644 index 0000000000..5e2b00404b --- /dev/null +++ b/servers/physics_3d/constraint_3d_sw.h @@ -0,0 +1,85 @@ +/*************************************************************************/ +/* constraint_3d_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_3d_sw.h" + +class Constraint3DSW { + + Body3DSW **_body_ptr; + int _body_count; + uint64_t island_step; + Constraint3DSW *island_next; + Constraint3DSW *island_list_next; + int priority; + bool disabled_collisions_between_bodies; + + RID self; + +protected: + Constraint3DSW(Body3DSW **p_body_ptr = nullptr, 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_ Constraint3DSW *get_island_next() const { return island_next; } + _FORCE_INLINE_ void set_island_next(Constraint3DSW *p_next) { island_next = p_next; } + + _FORCE_INLINE_ Constraint3DSW *get_island_list_next() const { return island_list_next; } + _FORCE_INLINE_ void set_island_list_next(Constraint3DSW *p_next) { island_list_next = p_next; } + + _FORCE_INLINE_ Body3DSW **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 ~Constraint3DSW() {} +}; + +#endif // CONSTRAINT__SW_H diff --git a/servers/physics_3d/gjk_epa.cpp b/servers/physics_3d/gjk_epa.cpp new file mode 100644 index 0000000000..db37f261ce --- /dev/null +++ b/servers/physics_3d/gjk_epa.cpp @@ -0,0 +1,946 @@ +/*************************************************************************/ +/* 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 Shape3DSW* 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(nullptr),count(0) {} + }; + struct sHorizon + { + sFace* cf; + sFace* ff; + U nf; + sHorizon() : cf(nullptr),ff(nullptr),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] = nullptr; + 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(nullptr); + } + // -- GODOT start -- + //m_status=m_stock.root?eStatus::OutOfVertices:eStatus::OutOfFaces; + m_status=eStatus::OutOfFaces; + // -- GODOT end -- + return(nullptr); + } + 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 Shape3DSW* shape0,const Transform& wtrs0, + const Shape3DSW* 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 Shape3DSW* shape0, + const Transform& wtrs0, + const Shape3DSW* 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 Shape3DSW* shape0, + const Transform& wtrs0, + const Shape3DSW* 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 Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *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 Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CollisionSolver3DSW::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_3d/gjk_epa.h b/servers/physics_3d/gjk_epa.h new file mode 100644 index 0000000000..dec0f269e1 --- /dev/null +++ b/servers/physics_3d/gjk_epa.h @@ -0,0 +1,40 @@ +/*************************************************************************/ +/* 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_3d_sw.h" +#include "shape_3d_sw.h" + +bool gjk_epa_calculate_penetration(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CollisionSolver3DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap = false); +bool gjk_epa_calculate_distance(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_result_A, Vector3 &r_result_B); + +#endif diff --git a/servers/physics_3d/joints/SCsub b/servers/physics_3d/joints/SCsub new file mode 100644 index 0000000000..86681f9c74 --- /dev/null +++ b/servers/physics_3d/joints/SCsub @@ -0,0 +1,5 @@ +#!/usr/bin/env python + +Import("env") + +env.add_source_files(env.servers_sources, "*.cpp") diff --git a/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp b/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp new file mode 100644 index 0000000000..a072c1f3a0 --- /dev/null +++ b/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp @@ -0,0 +1,366 @@ +/*************************************************************************/ +/* cone_twist_joint_3d_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_3d_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; +} + +ConeTwistJoint3DSW::ConeTwistJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform &rbAFrame, const Transform &rbBFrame) : + Joint3DSW(_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 ConeTwistJoint3DSW::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], JacobianEntry3DSW( + 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 ConeTwistJoint3DSW::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 ConeTwistJoint3DSW::set_param(PhysicsServer3D::ConeTwistJointParam p_param, real_t p_value) { + + switch (p_param) { + case PhysicsServer3D::CONE_TWIST_JOINT_SWING_SPAN: { + + m_swingSpan1 = p_value; + m_swingSpan2 = p_value; + } break; + case PhysicsServer3D::CONE_TWIST_JOINT_TWIST_SPAN: { + + m_twistSpan = p_value; + } break; + case PhysicsServer3D::CONE_TWIST_JOINT_BIAS: { + + m_biasFactor = p_value; + } break; + case PhysicsServer3D::CONE_TWIST_JOINT_SOFTNESS: { + + m_limitSoftness = p_value; + } break; + case PhysicsServer3D::CONE_TWIST_JOINT_RELAXATION: { + + m_relaxationFactor = p_value; + } break; + case PhysicsServer3D::CONE_TWIST_MAX: break; // Can't happen, but silences warning + } +} + +real_t ConeTwistJoint3DSW::get_param(PhysicsServer3D::ConeTwistJointParam p_param) const { + + switch (p_param) { + case PhysicsServer3D::CONE_TWIST_JOINT_SWING_SPAN: { + + return m_swingSpan1; + } break; + case PhysicsServer3D::CONE_TWIST_JOINT_TWIST_SPAN: { + + return m_twistSpan; + } break; + case PhysicsServer3D::CONE_TWIST_JOINT_BIAS: { + + return m_biasFactor; + } break; + case PhysicsServer3D::CONE_TWIST_JOINT_SOFTNESS: { + + return m_limitSoftness; + } break; + case PhysicsServer3D::CONE_TWIST_JOINT_RELAXATION: { + + return m_relaxationFactor; + } break; + case PhysicsServer3D::CONE_TWIST_MAX: break; // Can't happen, but silences warning + } + + return 0; +} diff --git a/servers/physics_3d/joints/cone_twist_joint_3d_sw.h b/servers/physics_3d/joints/cone_twist_joint_3d_sw.h new file mode 100644 index 0000000000..c713d8cf17 --- /dev/null +++ b/servers/physics_3d/joints/cone_twist_joint_3d_sw.h @@ -0,0 +1,142 @@ +/*************************************************************************/ +/* cone_twist_joint_3d_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_3d/joints/jacobian_entry_3d_sw.h" +#include "servers/physics_3d/joints_3d_sw.h" + +///ConeTwistJointSW can be used to simulate ragdoll joints (upper arm, leg etc) +class ConeTwistJoint3DSW : public Joint3DSW { +#ifdef IN_PARALLELL_SOLVER +public: +#endif + + union { + struct { + Body3DSW *A; + Body3DSW *B; + }; + + Body3DSW *_arr[2]; + }; + + JacobianEntry3DSW 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 PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_CONE_TWIST; } + + virtual bool setup(real_t p_timestep); + virtual void solve(real_t p_timestep); + + ConeTwistJoint3DSW(Body3DSW *rbA, Body3DSW *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(PhysicsServer3D::ConeTwistJointParam p_param, real_t p_value); + real_t get_param(PhysicsServer3D::ConeTwistJointParam p_param) const; +}; + +#endif // CONE_TWIST_JOINT_SW_H diff --git a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp new file mode 100644 index 0000000000..e15aeca842 --- /dev/null +++ b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp @@ -0,0 +1,686 @@ +/*************************************************************************/ +/* generic_6dof_joint_3d_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_3d_sw.h" + +#define GENERIC_D6_DISABLE_WARMSTARTING 1 + +//////////////////////////// G6DOFRotationalLimitMotorSW //////////////////////////////////// + +int G6DOFRotationalLimitMotor3DSW::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 G6DOFRotationalLimitMotor3DSW::solveAngularLimits( + real_t timeStep, Vector3 &axis, real_t jacDiagABInv, + Body3DSW *body0, Body3DSW *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 G6DOFTranslationalLimitMotor3DSW::solveLinearAxis( + real_t timeStep, + real_t jacDiagABInv, + Body3DSW *body1, const Vector3 &pointInA, + Body3DSW *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 //////////////////////////////////// + +Generic6DOFJoint3DSW::Generic6DOFJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform &frameInA, const Transform &frameInB, bool useLinearReferenceFrameA) : + Joint3DSW(_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 Generic6DOFJoint3DSW::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 Generic6DOFJoint3DSW::calculateTransforms() { + m_calculatedTransformA = A->get_transform() * m_frameInA; + m_calculatedTransformB = B->get_transform() * m_frameInB; + + calculateAngleInfo(); +} + +void Generic6DOFJoint3DSW::buildLinearJacobian( + JacobianEntry3DSW &jacLinear, const Vector3 &normalWorld, + const Vector3 &pivotAInW, const Vector3 &pivotBInW) { + memnew_placement(&jacLinear, JacobianEntry3DSW( + 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 Generic6DOFJoint3DSW::buildAngularJacobian( + JacobianEntry3DSW &jacAngular, const Vector3 &jointAxisW) { + memnew_placement(&jacAngular, JacobianEntry3DSW(jointAxisW, + A->get_principal_inertia_axes().transposed(), + B->get_principal_inertia_axes().transposed(), + A->get_inv_inertia(), + B->get_inv_inertia())); +} + +bool Generic6DOFJoint3DSW::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 Generic6DOFJoint3DSW::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 Generic6DOFJoint3DSW::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 Generic6DOFJoint3DSW::updateRHS(real_t timeStep) { + (void)timeStep; +} + +Vector3 Generic6DOFJoint3DSW::getAxis(int axis_index) const { + return m_calculatedAxis[axis_index]; +} + +real_t Generic6DOFJoint3DSW::getAngle(int axis_index) const { + return m_calculatedAxisAngleDiff[axis_index]; +} + +void Generic6DOFJoint3DSW::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 Generic6DOFJoint3DSW::set_param(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param, real_t p_value) { + + ERR_FAIL_INDEX(p_axis, 3); + switch (p_param) { + case PhysicsServer3D::G6DOF_JOINT_LINEAR_LOWER_LIMIT: { + + m_linearLimits.m_lowerLimit[p_axis] = p_value; + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_UPPER_LIMIT: { + + m_linearLimits.m_upperLimit[p_axis] = p_value; + + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS: { + + m_linearLimits.m_limitSoftness[p_axis] = p_value; + + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_RESTITUTION: { + + m_linearLimits.m_restitution[p_axis] = p_value; + + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_DAMPING: { + + m_linearLimits.m_damping[p_axis] = p_value; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_LOWER_LIMIT: { + + m_angularLimits[p_axis].m_loLimit = p_value; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_UPPER_LIMIT: { + + m_angularLimits[p_axis].m_hiLimit = p_value; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS: { + + m_angularLimits[p_axis].m_limitSoftness = p_value; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_DAMPING: { + + m_angularLimits[p_axis].m_damping = p_value; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_RESTITUTION: { + + m_angularLimits[p_axis].m_bounce = p_value; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_FORCE_LIMIT: { + + m_angularLimits[p_axis].m_maxLimitForce = p_value; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_ERP: { + + m_angularLimits[p_axis].m_ERP = p_value; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY: { + + m_angularLimits[p_axis].m_targetVelocity = p_value; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT: { + + m_angularLimits[p_axis].m_maxLimitForce = p_value; + + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_MOTOR_TARGET_VELOCITY: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_MOTOR_FORCE_LIMIT: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_STIFFNESS: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_DAMPING: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_EQUILIBRIUM_POINT: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_STIFFNESS: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_DAMPING: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_EQUILIBRIUM_POINT: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_MAX: break; // Can't happen, but silences warning + } +} + +real_t Generic6DOFJoint3DSW::get_param(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param) const { + ERR_FAIL_INDEX_V(p_axis, 3, 0); + switch (p_param) { + case PhysicsServer3D::G6DOF_JOINT_LINEAR_LOWER_LIMIT: { + + return m_linearLimits.m_lowerLimit[p_axis]; + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_UPPER_LIMIT: { + + return m_linearLimits.m_upperLimit[p_axis]; + + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS: { + + return m_linearLimits.m_limitSoftness[p_axis]; + + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_RESTITUTION: { + + return m_linearLimits.m_restitution[p_axis]; + + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_DAMPING: { + + return m_linearLimits.m_damping[p_axis]; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_LOWER_LIMIT: { + + return m_angularLimits[p_axis].m_loLimit; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_UPPER_LIMIT: { + + return m_angularLimits[p_axis].m_hiLimit; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS: { + + return m_angularLimits[p_axis].m_limitSoftness; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_DAMPING: { + + return m_angularLimits[p_axis].m_damping; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_RESTITUTION: { + + return m_angularLimits[p_axis].m_bounce; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_FORCE_LIMIT: { + + return m_angularLimits[p_axis].m_maxLimitForce; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_ERP: { + + return m_angularLimits[p_axis].m_ERP; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY: { + + return m_angularLimits[p_axis].m_targetVelocity; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT: { + + return m_angularLimits[p_axis].m_maxMotorForce; + + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_MOTOR_TARGET_VELOCITY: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_MOTOR_FORCE_LIMIT: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_STIFFNESS: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_DAMPING: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_EQUILIBRIUM_POINT: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_STIFFNESS: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_DAMPING: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_EQUILIBRIUM_POINT: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_MAX: break; // Can't happen, but silences warning + } + return 0; +} + +void Generic6DOFJoint3DSW::set_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag, bool p_value) { + + ERR_FAIL_INDEX(p_axis, 3); + + switch (p_flag) { + case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT: { + + m_linearLimits.enable_limit[p_axis] = p_value; + } break; + case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT: { + + m_angularLimits[p_axis].m_enableLimit = p_value; + } break; + case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_MOTOR: { + + m_angularLimits[p_axis].m_enableMotor = p_value; + } break; + case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_MOTOR: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_SPRING: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_SPRING: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_FLAG_MAX: break; // Can't happen, but silences warning + } +} +bool Generic6DOFJoint3DSW::get_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag) const { + + ERR_FAIL_INDEX_V(p_axis, 3, 0); + switch (p_flag) { + case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT: { + + return m_linearLimits.enable_limit[p_axis]; + } break; + case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT: { + + return m_angularLimits[p_axis].m_enableLimit; + } break; + case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_MOTOR: { + + return m_angularLimits[p_axis].m_enableMotor; + } break; + case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_MOTOR: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_SPRING: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_SPRING: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_FLAG_MAX: break; // Can't happen, but silences warning + } + + return 0; +} diff --git a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h new file mode 100644 index 0000000000..f7aa607901 --- /dev/null +++ b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h @@ -0,0 +1,401 @@ +/*************************************************************************/ +/* generic_6dof_joint_3d_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_3d/joints/jacobian_entry_3d_sw.h" +#include "servers/physics_3d/joints_3d_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 G6DOFRotationalLimitMotor3DSW { +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; + //!@} + + G6DOFRotationalLimitMotor3DSW() { + 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; + } + + G6DOFRotationalLimitMotor3DSW(const G6DOFRotationalLimitMotor3DSW &limot) { + m_targetVelocity = limot.m_targetVelocity; + m_maxMotorForce = limot.m_maxMotorForce; + m_limitSoftness = limot.m_limitSoftness; + m_loLimit = limot.m_loLimit; + m_hiLimit = limot.m_hiLimit; + m_ERP = limot.m_ERP; + m_bounce = limot.m_bounce; + m_currentLimit = limot.m_currentLimit; + m_currentLimitError = limot.m_currentLimitError; + m_enableMotor = limot.m_enableMotor; + } + + //! Is limited + bool isLimited() { + return (m_loLimit < m_hiLimit); + } + + //! 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, Body3DSW *body0, Body3DSW *body1); +}; + +class G6DOFTranslationalLimitMotor3DSW { +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]; + + G6DOFTranslationalLimitMotor3DSW() { + 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; + } + + G6DOFTranslationalLimitMotor3DSW(const G6DOFTranslationalLimitMotor3DSW &other) { + m_lowerLimit = other.m_lowerLimit; + m_upperLimit = other.m_upperLimit; + m_accumulatedImpulse = other.m_accumulatedImpulse; + + m_limitSoftness = other.m_limitSoftness; + m_damping = other.m_damping; + m_restitution = other.m_restitution; + } + + //! Test limit + /*! + - free means upper < lower, + - 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, + Body3DSW *body1, const Vector3 &pointInA, + Body3DSW *body2, const Vector3 &pointInB, + int limit_index, + const Vector3 &axis_normal_on_a, + const Vector3 &anchorPos); +}; + +class Generic6DOFJoint3DSW : public Joint3DSW { +protected: + union { + struct { + Body3DSW *A; + Body3DSW *B; + }; + + Body3DSW *_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 + //!@{ + JacobianEntry3DSW m_jacLinear[3]; //!< 3 orthogonal linear constraints + JacobianEntry3DSW m_jacAng[3]; //!< 3 orthogonal angular constraints + //!@} + + //! Linear_Limit_parameters + //!@{ + G6DOFTranslationalLimitMotor3DSW m_linearLimits; + //!@} + + //! hinge_parameters + //!@{ + G6DOFRotationalLimitMotor3DSW 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; + + //!@} + + Generic6DOFJoint3DSW &operator=(Generic6DOFJoint3DSW &other) { + ERR_PRINT("pito"); + (void)other; + return *this; + } + + void buildLinearJacobian( + JacobianEntry3DSW &jacLinear, const Vector3 &normalWorld, + const Vector3 &pivotAInW, const Vector3 &pivotBInW); + + void buildAngularJacobian(JacobianEntry3DSW &jacAngular, const Vector3 &jointAxisW); + + //! calcs the euler angles between the two bodies. + void calculateAngleInfo(); + +public: + Generic6DOFJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform &frameInA, const Transform &frameInB, bool useLinearReferenceFrameA); + + virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_6DOF; } + + virtual 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 + G6DOFRotationalLimitMotor3DSW *getRotationalLimitMotor(int index) { + return &m_angularLimits[index]; + } + + //! Retrieves the limit informacion + G6DOFTranslationalLimitMotor3DSW *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 Body3DSW *getRigidBodyA() const { + return A; + } + const Body3DSW *getRigidBodyB() const { + return B; + } + + virtual void calcAnchorPos(void); // overridable + + void set_param(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param, real_t p_value); + real_t get_param(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param) const; + + void set_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag, bool p_value); + bool get_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag) const; +}; + +#endif // GENERIC_6DOF_JOINT_SW_H diff --git a/servers/physics_3d/joints/hinge_joint_3d_sw.cpp b/servers/physics_3d/joints/hinge_joint_3d_sw.cpp new file mode 100644 index 0000000000..e76d366422 --- /dev/null +++ b/servers/physics_3d/joints/hinge_joint_3d_sw.cpp @@ -0,0 +1,450 @@ +/*************************************************************************/ +/* hinge_joint_3d_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_3d_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); + } +} + +HingeJoint3DSW::HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform &frameA, const Transform &frameB) : + Joint3DSW(_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); +} + +HingeJoint3DSW::HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Vector3 &pivotInA, const Vector3 &pivotInB, + const Vector3 &axisInA, const Vector3 &axisInB) : + Joint3DSW(_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 HingeJoint3DSW::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], JacobianEntry3DSW( + 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], JacobianEntry3DSW(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], JacobianEntry3DSW(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], JacobianEntry3DSW(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 HingeJoint3DSW::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 HingeJoint3DSW::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 HingeJoint3DSW::set_param(PhysicsServer3D::HingeJointParam p_param, real_t p_value) { + + switch (p_param) { + + case PhysicsServer3D::HINGE_JOINT_BIAS: tau = p_value; break; + case PhysicsServer3D::HINGE_JOINT_LIMIT_UPPER: m_upperLimit = p_value; break; + case PhysicsServer3D::HINGE_JOINT_LIMIT_LOWER: m_lowerLimit = p_value; break; + case PhysicsServer3D::HINGE_JOINT_LIMIT_BIAS: m_biasFactor = p_value; break; + case PhysicsServer3D::HINGE_JOINT_LIMIT_SOFTNESS: m_limitSoftness = p_value; break; + case PhysicsServer3D::HINGE_JOINT_LIMIT_RELAXATION: m_relaxationFactor = p_value; break; + case PhysicsServer3D::HINGE_JOINT_MOTOR_TARGET_VELOCITY: m_motorTargetVelocity = p_value; break; + case PhysicsServer3D::HINGE_JOINT_MOTOR_MAX_IMPULSE: m_maxMotorImpulse = p_value; break; + case PhysicsServer3D::HINGE_JOINT_MAX: break; // Can't happen, but silences warning + } +} + +real_t HingeJoint3DSW::get_param(PhysicsServer3D::HingeJointParam p_param) const { + + switch (p_param) { + + case PhysicsServer3D::HINGE_JOINT_BIAS: return tau; + case PhysicsServer3D::HINGE_JOINT_LIMIT_UPPER: return m_upperLimit; + case PhysicsServer3D::HINGE_JOINT_LIMIT_LOWER: return m_lowerLimit; + case PhysicsServer3D::HINGE_JOINT_LIMIT_BIAS: return m_biasFactor; + case PhysicsServer3D::HINGE_JOINT_LIMIT_SOFTNESS: return m_limitSoftness; + case PhysicsServer3D::HINGE_JOINT_LIMIT_RELAXATION: return m_relaxationFactor; + case PhysicsServer3D::HINGE_JOINT_MOTOR_TARGET_VELOCITY: return m_motorTargetVelocity; + case PhysicsServer3D::HINGE_JOINT_MOTOR_MAX_IMPULSE: return m_maxMotorImpulse; + case PhysicsServer3D::HINGE_JOINT_MAX: break; // Can't happen, but silences warning + } + + return 0; +} + +void HingeJoint3DSW::set_flag(PhysicsServer3D::HingeJointFlag p_flag, bool p_value) { + + switch (p_flag) { + case PhysicsServer3D::HINGE_JOINT_FLAG_USE_LIMIT: m_useLimit = p_value; break; + case PhysicsServer3D::HINGE_JOINT_FLAG_ENABLE_MOTOR: m_enableAngularMotor = p_value; break; + case PhysicsServer3D::HINGE_JOINT_FLAG_MAX: break; // Can't happen, but silences warning + } +} +bool HingeJoint3DSW::get_flag(PhysicsServer3D::HingeJointFlag p_flag) const { + + switch (p_flag) { + case PhysicsServer3D::HINGE_JOINT_FLAG_USE_LIMIT: return m_useLimit; + case PhysicsServer3D::HINGE_JOINT_FLAG_ENABLE_MOTOR: return m_enableAngularMotor; + case PhysicsServer3D::HINGE_JOINT_FLAG_MAX: break; // Can't happen, but silences warning + } + + return false; +} diff --git a/servers/physics_3d/joints/hinge_joint_3d_sw.h b/servers/physics_3d/joints/hinge_joint_3d_sw.h new file mode 100644 index 0000000000..eebead20b8 --- /dev/null +++ b/servers/physics_3d/joints/hinge_joint_3d_sw.h @@ -0,0 +1,117 @@ +/*************************************************************************/ +/* hinge_joint_3d_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_3d/joints/jacobian_entry_3d_sw.h" +#include "servers/physics_3d/joints_3d_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 HingeJoint3DSW : public Joint3DSW { + + union { + struct { + Body3DSW *A; + Body3DSW *B; + }; + + Body3DSW *_arr[2]; + }; + + JacobianEntry3DSW m_jac[3]; //3 orthogonal linear constraints + JacobianEntry3DSW 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 PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_HINGE; } + + virtual bool setup(real_t p_step); + virtual void solve(real_t p_step); + + real_t get_hinge_angle(); + + void set_param(PhysicsServer3D::HingeJointParam p_param, real_t p_value); + real_t get_param(PhysicsServer3D::HingeJointParam p_param) const; + + void set_flag(PhysicsServer3D::HingeJointFlag p_flag, bool p_value); + bool get_flag(PhysicsServer3D::HingeJointFlag p_flag) const; + + HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform &frameA, const Transform &frameB); + HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Vector3 &pivotInA, const Vector3 &pivotInB, const Vector3 &axisInA, const Vector3 &axisInB); +}; + +#endif // HINGE_JOINT_SW_H diff --git a/servers/physics_3d/joints/jacobian_entry_3d_sw.h b/servers/physics_3d/joints/jacobian_entry_3d_sw.h new file mode 100644 index 0000000000..7e605ab173 --- /dev/null +++ b/servers/physics_3d/joints/jacobian_entry_3d_sw.h @@ -0,0 +1,169 @@ +/*************************************************************************/ +/* jacobian_entry_3d_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 JacobianEntry3DSW { +public: + JacobianEntry3DSW(){}; + //constraint between two different rigidbodies + JacobianEntry3DSW( + 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 + JacobianEntry3DSW(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 + JacobianEntry3DSW(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 + JacobianEntry3DSW( + 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 JacobianEntry3DSW &jacB, const real_t massInvA) const { + const JacobianEntry3DSW &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 JacobianEntry3DSW &jacB, const real_t massInvA, const real_t massInvB) const { + const JacobianEntry3DSW &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_3d/joints/pin_joint_3d_sw.cpp b/servers/physics_3d/joints/pin_joint_3d_sw.cpp new file mode 100644 index 0000000000..95c01bc463 --- /dev/null +++ b/servers/physics_3d/joints/pin_joint_3d_sw.cpp @@ -0,0 +1,167 @@ +/*************************************************************************/ +/* pin_joint_3d_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_3d_sw.h" + +bool PinJoint3DSW::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], JacobianEntry3DSW( + 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 PinJoint3DSW::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 PinJoint3DSW::set_param(PhysicsServer3D::PinJointParam p_param, real_t p_value) { + + switch (p_param) { + case PhysicsServer3D::PIN_JOINT_BIAS: m_tau = p_value; break; + case PhysicsServer3D::PIN_JOINT_DAMPING: m_damping = p_value; break; + case PhysicsServer3D::PIN_JOINT_IMPULSE_CLAMP: m_impulseClamp = p_value; break; + } +} + +real_t PinJoint3DSW::get_param(PhysicsServer3D::PinJointParam p_param) const { + + switch (p_param) { + case PhysicsServer3D::PIN_JOINT_BIAS: return m_tau; + case PhysicsServer3D::PIN_JOINT_DAMPING: return m_damping; + case PhysicsServer3D::PIN_JOINT_IMPULSE_CLAMP: return m_impulseClamp; + } + + return 0; +} + +PinJoint3DSW::PinJoint3DSW(Body3DSW *p_body_a, const Vector3 &p_pos_a, Body3DSW *p_body_b, const Vector3 &p_pos_b) : + Joint3DSW(_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); +} + +PinJoint3DSW::~PinJoint3DSW() { +} diff --git a/servers/physics_3d/joints/pin_joint_3d_sw.h b/servers/physics_3d/joints/pin_joint_3d_sw.h new file mode 100644 index 0000000000..8e81ccf5e0 --- /dev/null +++ b/servers/physics_3d/joints/pin_joint_3d_sw.h @@ -0,0 +1,96 @@ +/*************************************************************************/ +/* pin_joint_3d_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_3d/joints/jacobian_entry_3d_sw.h" +#include "servers/physics_3d/joints_3d_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 PinJoint3DSW : public Joint3DSW { + + union { + struct { + Body3DSW *A; + Body3DSW *B; + }; + + Body3DSW *_arr[2]; + }; + + real_t m_tau; //bias + real_t m_damping; + real_t m_impulseClamp; + real_t m_appliedImpulse; + + JacobianEntry3DSW m_jac[3]; //3 orthogonal linear constraints + + Vector3 m_pivotInA; + Vector3 m_pivotInB; + +public: + virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_PIN; } + + virtual bool setup(real_t p_step); + virtual void solve(real_t p_step); + + void set_param(PhysicsServer3D::PinJointParam p_param, real_t p_value); + real_t get_param(PhysicsServer3D::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; } + + PinJoint3DSW(Body3DSW *p_body_a, const Vector3 &p_pos_a, Body3DSW *p_body_b, const Vector3 &p_pos_b); + ~PinJoint3DSW(); +}; + +#endif // PIN_JOINT_SW_H diff --git a/servers/physics_3d/joints/slider_joint_3d_sw.cpp b/servers/physics_3d/joints/slider_joint_3d_sw.cpp new file mode 100644 index 0000000000..066c30e0f3 --- /dev/null +++ b/servers/physics_3d/joints/slider_joint_3d_sw.cpp @@ -0,0 +1,443 @@ +/*************************************************************************/ +/* slider_joint_3d_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_3d_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 SliderJoint3DSW::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() + +//----------------------------------------------------------------------------- + +//----------------------------------------------------------------------------- + +SliderJoint3DSW::SliderJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform &frameInA, const Transform &frameInB) : + Joint3DSW(_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 SliderJoint3DSW::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], JacobianEntry3DSW( + 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], JacobianEntry3DSW( + 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 SliderJoint3DSW::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 SliderJoint3DSW::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 SliderJoint3DSW::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 SliderJoint3DSW::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 SliderJoint3DSW::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 SliderJoint3DSW::getAncorInB(void) { + Vector3 ancorInB; + ancorInB = m_frameInB.origin; + return ancorInB; +} // SliderJointSW::getAncorInB(); + +void SliderJoint3DSW::set_param(PhysicsServer3D::SliderJointParam p_param, real_t p_value) { + + switch (p_param) { + case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_UPPER: m_upperLinLimit = p_value; break; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_LOWER: m_lowerLinLimit = p_value; break; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS: m_softnessLimLin = p_value; break; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION: m_restitutionLimLin = p_value; break; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_DAMPING: m_dampingLimLin = p_value; break; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_SOFTNESS: m_softnessDirLin = p_value; break; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_RESTITUTION: m_restitutionDirLin = p_value; break; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_DAMPING: m_dampingDirLin = p_value; break; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS: m_softnessOrthoLin = p_value; break; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION: m_restitutionOrthoLin = p_value; break; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING: m_dampingOrthoLin = p_value; break; + + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_UPPER: m_upperAngLimit = p_value; break; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_LOWER: m_lowerAngLimit = p_value; break; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS: m_softnessLimAng = p_value; break; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION: m_restitutionLimAng = p_value; break; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_DAMPING: m_dampingLimAng = p_value; break; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS: m_softnessDirAng = p_value; break; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION: m_restitutionDirAng = p_value; break; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_DAMPING: m_dampingDirAng = p_value; break; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS: m_softnessOrthoAng = p_value; break; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION: m_restitutionOrthoAng = p_value; break; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING: m_dampingOrthoAng = p_value; break; + + case PhysicsServer3D::SLIDER_JOINT_MAX: break; // Can't happen, but silences warning + } +} + +real_t SliderJoint3DSW::get_param(PhysicsServer3D::SliderJointParam p_param) const { + + switch (p_param) { + case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_UPPER: return m_upperLinLimit; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_LOWER: return m_lowerLinLimit; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS: return m_softnessLimLin; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION: return m_restitutionLimLin; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_DAMPING: return m_dampingLimLin; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_SOFTNESS: return m_softnessDirLin; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_RESTITUTION: return m_restitutionDirLin; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_DAMPING: return m_dampingDirLin; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS: return m_softnessOrthoLin; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION: return m_restitutionOrthoLin; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING: return m_dampingOrthoLin; + + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_UPPER: return m_upperAngLimit; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_LOWER: return m_lowerAngLimit; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS: return m_softnessLimAng; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION: return m_restitutionLimAng; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_DAMPING: return m_dampingLimAng; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS: return m_softnessDirAng; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION: return m_restitutionDirAng; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_DAMPING: return m_dampingDirAng; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS: return m_softnessOrthoAng; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION: return m_restitutionOrthoAng; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING: return m_dampingOrthoAng; + + case PhysicsServer3D::SLIDER_JOINT_MAX: break; // Can't happen, but silences warning + } + + return 0; +} diff --git a/servers/physics_3d/joints/slider_joint_3d_sw.h b/servers/physics_3d/joints/slider_joint_3d_sw.h new file mode 100644 index 0000000000..18287db9c2 --- /dev/null +++ b/servers/physics_3d/joints/slider_joint_3d_sw.h @@ -0,0 +1,249 @@ +/*************************************************************************/ +/* slider_joint_3d_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_3d/joints/jacobian_entry_3d_sw.h" +#include "servers/physics_3d/joints_3d_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 SliderJoint3DSW : public Joint3DSW { +protected: + union { + struct { + Body3DSW *A; + Body3DSW *B; + }; + + Body3DSW *_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; + + JacobianEntry3DSW m_jacLin[3]; + real_t m_jacLinDiagABInv[3]; + + JacobianEntry3DSW 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 + SliderJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform &frameInA, const Transform &frameInB); + //SliderJointSW(); + // overrides + + // access + const Body3DSW *getRigidBodyA() const { return A; } + const Body3DSW *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(PhysicsServer3D::SliderJointParam p_param, real_t p_value); + real_t get_param(PhysicsServer3D::SliderJointParam p_param) const; + + bool setup(real_t p_step); + void solve(real_t p_step); + + virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_SLIDER; } +}; + +#endif // SLIDER_JOINT_SW_H diff --git a/servers/physics_3d/joints_3d_sw.h b/servers/physics_3d/joints_3d_sw.h new file mode 100644 index 0000000000..0f2d4892a8 --- /dev/null +++ b/servers/physics_3d/joints_3d_sw.h @@ -0,0 +1,46 @@ +/*************************************************************************/ +/* joints_3d_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_3d_sw.h" +#include "constraint_3d_sw.h" + +class Joint3DSW : public Constraint3DSW { + +public: + virtual PhysicsServer3D::JointType get_type() const = 0; + _FORCE_INLINE_ Joint3DSW(Body3DSW **p_body_ptr = nullptr, int p_body_count = 0) : + Constraint3DSW(p_body_ptr, p_body_count) { + } +}; + +#endif // JOINTS_SW_H diff --git a/servers/physics_3d/physics_server_3d_sw.cpp b/servers/physics_3d/physics_server_3d_sw.cpp new file mode 100644 index 0000000000..d8da6e715b --- /dev/null +++ b/servers/physics_3d/physics_server_3d_sw.cpp @@ -0,0 +1,1589 @@ +/*************************************************************************/ +/* physics_server_3d_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_3d_sw.h" + +#include "broad_phase_3d_basic.h" +#include "broad_phase_octree.h" +#include "core/debugger/engine_debugger.h" +#include "core/os/os.h" +#include "joints/cone_twist_joint_3d_sw.h" +#include "joints/generic_6dof_joint_3d_sw.h" +#include "joints/hinge_joint_3d_sw.h" +#include "joints/pin_joint_3d_sw.h" +#include "joints/slider_joint_3d_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 PhysicsServer3DSW::shape_create(ShapeType p_shape) { + + Shape3DSW *shape = nullptr; + switch (p_shape) { + + case SHAPE_PLANE: { + + shape = memnew(PlaneShape3DSW); + } break; + case SHAPE_RAY: { + + shape = memnew(RayShape3DSW); + } break; + case SHAPE_SPHERE: { + + shape = memnew(SphereShape3DSW); + } break; + case SHAPE_BOX: { + + shape = memnew(BoxShape3DSW); + } break; + case SHAPE_CAPSULE: { + + shape = memnew(CapsuleShape3DSW); + } break; + case SHAPE_CYLINDER: { + + ERR_FAIL_V_MSG(RID(), "CylinderShape3D is not supported in GodotPhysics3D. Please switch to Bullet in the Project Settings."); + } break; + case SHAPE_CONVEX_POLYGON: { + + shape = memnew(ConvexPolygonShape3DSW); + } break; + case SHAPE_CONCAVE_POLYGON: { + + shape = memnew(ConcavePolygonShape3DSW); + } break; + case SHAPE_HEIGHTMAP: { + + shape = memnew(HeightMapShape3DSW); + } break; + case SHAPE_CUSTOM: { + + ERR_FAIL_V(RID()); + + } break; + } + + RID id = shape_owner.make_rid(shape); + shape->set_self(id); + + return id; +}; + +void PhysicsServer3DSW::shape_set_data(RID p_shape, const Variant &p_data) { + + Shape3DSW *shape = shape_owner.getornull(p_shape); + ERR_FAIL_COND(!shape); + shape->set_data(p_data); +}; + +void PhysicsServer3DSW::shape_set_custom_solver_bias(RID p_shape, real_t p_bias) { + + Shape3DSW *shape = shape_owner.getornull(p_shape); + ERR_FAIL_COND(!shape); + shape->set_custom_bias(p_bias); +} + +PhysicsServer3D::ShapeType PhysicsServer3DSW::shape_get_type(RID p_shape) const { + + const Shape3DSW *shape = shape_owner.getornull(p_shape); + ERR_FAIL_COND_V(!shape, SHAPE_CUSTOM); + return shape->get_type(); +}; + +Variant PhysicsServer3DSW::shape_get_data(RID p_shape) const { + + const Shape3DSW *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 PhysicsServer3DSW::shape_set_margin(RID p_shape, real_t p_margin) { +} + +real_t PhysicsServer3DSW::shape_get_margin(RID p_shape) const { + return 0.0; +} + +real_t PhysicsServer3DSW::shape_get_custom_solver_bias(RID p_shape) const { + + const Shape3DSW *shape = shape_owner.getornull(p_shape); + ERR_FAIL_COND_V(!shape, 0); + return shape->get_custom_bias(); +} + +RID PhysicsServer3DSW::space_create() { + + Space3DSW *space = memnew(Space3DSW); + RID id = space_owner.make_rid(space); + space->set_self(id); + RID area_id = area_create(); + Area3DSW *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 PhysicsServer3DSW::space_set_active(RID p_space, bool p_active) { + + Space3DSW *space = space_owner.getornull(p_space); + ERR_FAIL_COND(!space); + if (p_active) + active_spaces.insert(space); + else + active_spaces.erase(space); +} + +bool PhysicsServer3DSW::space_is_active(RID p_space) const { + + const Space3DSW *space = space_owner.getornull(p_space); + ERR_FAIL_COND_V(!space, false); + + return active_spaces.has(space); +} + +void PhysicsServer3DSW::space_set_param(RID p_space, SpaceParameter p_param, real_t p_value) { + + Space3DSW *space = space_owner.getornull(p_space); + ERR_FAIL_COND(!space); + + space->set_param(p_param, p_value); +} + +real_t PhysicsServer3DSW::space_get_param(RID p_space, SpaceParameter p_param) const { + + const Space3DSW *space = space_owner.getornull(p_space); + ERR_FAIL_COND_V(!space, 0); + return space->get_param(p_param); +} + +PhysicsDirectSpaceState3D *PhysicsServer3DSW::space_get_direct_state(RID p_space) { + + Space3DSW *space = space_owner.getornull(p_space); + ERR_FAIL_COND_V(!space, nullptr); + ERR_FAIL_COND_V_MSG(!doing_sync || space->is_locked(), nullptr, "Space state is inaccessible right now, wait for iteration or physics process notification."); + + return space->get_direct_state(); +} + +void PhysicsServer3DSW::space_set_debug_contacts(RID p_space, int p_max_contacts) { + + Space3DSW *space = space_owner.getornull(p_space); + ERR_FAIL_COND(!space); + space->set_debug_contacts(p_max_contacts); +} + +Vector<Vector3> PhysicsServer3DSW::space_get_contacts(RID p_space) const { + + Space3DSW *space = space_owner.getornull(p_space); + ERR_FAIL_COND_V(!space, Vector<Vector3>()); + return space->get_debug_contacts(); +} + +int PhysicsServer3DSW::space_get_contact_count(RID p_space) const { + + Space3DSW *space = space_owner.getornull(p_space); + ERR_FAIL_COND_V(!space, 0); + return space->get_debug_contact_count(); +} + +RID PhysicsServer3DSW::area_create() { + + Area3DSW *area = memnew(Area3DSW); + RID rid = area_owner.make_rid(area); + area->set_self(rid); + return rid; +}; + +void PhysicsServer3DSW::area_set_space(RID p_area, RID p_space) { + + Area3DSW *area = area_owner.getornull(p_area); + ERR_FAIL_COND(!area); + + Space3DSW *space = nullptr; + 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 PhysicsServer3DSW::area_get_space(RID p_area) const { + + Area3DSW *area = area_owner.getornull(p_area); + ERR_FAIL_COND_V(!area, RID()); + + Space3DSW *space = area->get_space(); + if (!space) + return RID(); + return space->get_self(); +}; + +void PhysicsServer3DSW::area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode) { + + Area3DSW *area = area_owner.getornull(p_area); + ERR_FAIL_COND(!area); + + area->set_space_override_mode(p_mode); +} + +PhysicsServer3D::AreaSpaceOverrideMode PhysicsServer3DSW::area_get_space_override_mode(RID p_area) const { + + const Area3DSW *area = area_owner.getornull(p_area); + ERR_FAIL_COND_V(!area, AREA_SPACE_OVERRIDE_DISABLED); + + return area->get_space_override_mode(); +} + +void PhysicsServer3DSW::area_add_shape(RID p_area, RID p_shape, const Transform &p_transform, bool p_disabled) { + + Area3DSW *area = area_owner.getornull(p_area); + ERR_FAIL_COND(!area); + + Shape3DSW *shape = shape_owner.getornull(p_shape); + ERR_FAIL_COND(!shape); + + area->add_shape(shape, p_transform, p_disabled); +} + +void PhysicsServer3DSW::area_set_shape(RID p_area, int p_shape_idx, RID p_shape) { + + Area3DSW *area = area_owner.getornull(p_area); + ERR_FAIL_COND(!area); + + Shape3DSW *shape = shape_owner.getornull(p_shape); + ERR_FAIL_COND(!shape); + ERR_FAIL_COND(!shape->is_configured()); + + area->set_shape(p_shape_idx, shape); +} + +void PhysicsServer3DSW::area_set_shape_transform(RID p_area, int p_shape_idx, const Transform &p_transform) { + + Area3DSW *area = area_owner.getornull(p_area); + ERR_FAIL_COND(!area); + + area->set_shape_transform(p_shape_idx, p_transform); +} + +int PhysicsServer3DSW::area_get_shape_count(RID p_area) const { + + Area3DSW *area = area_owner.getornull(p_area); + ERR_FAIL_COND_V(!area, -1); + + return area->get_shape_count(); +} +RID PhysicsServer3DSW::area_get_shape(RID p_area, int p_shape_idx) const { + + Area3DSW *area = area_owner.getornull(p_area); + ERR_FAIL_COND_V(!area, RID()); + + Shape3DSW *shape = area->get_shape(p_shape_idx); + ERR_FAIL_COND_V(!shape, RID()); + + return shape->get_self(); +} +Transform PhysicsServer3DSW::area_get_shape_transform(RID p_area, int p_shape_idx) const { + + Area3DSW *area = area_owner.getornull(p_area); + ERR_FAIL_COND_V(!area, Transform()); + + return area->get_shape_transform(p_shape_idx); +} + +void PhysicsServer3DSW::area_remove_shape(RID p_area, int p_shape_idx) { + + Area3DSW *area = area_owner.getornull(p_area); + ERR_FAIL_COND(!area); + + area->remove_shape(p_shape_idx); +} + +void PhysicsServer3DSW::area_clear_shapes(RID p_area) { + + Area3DSW *area = area_owner.getornull(p_area); + ERR_FAIL_COND(!area); + + while (area->get_shape_count()) + area->remove_shape(0); +} + +void PhysicsServer3DSW::area_set_shape_disabled(RID p_area, int p_shape_idx, bool p_disabled) { + + Area3DSW *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 PhysicsServer3DSW::area_attach_object_instance_id(RID p_area, ObjectID p_id) { + + if (space_owner.owns(p_area)) { + Space3DSW *space = space_owner.getornull(p_area); + p_area = space->get_default_area()->get_self(); + } + Area3DSW *area = area_owner.getornull(p_area); + ERR_FAIL_COND(!area); + area->set_instance_id(p_id); +} +ObjectID PhysicsServer3DSW::area_get_object_instance_id(RID p_area) const { + + if (space_owner.owns(p_area)) { + Space3DSW *space = space_owner.getornull(p_area); + p_area = space->get_default_area()->get_self(); + } + Area3DSW *area = area_owner.getornull(p_area); + ERR_FAIL_COND_V(!area, ObjectID()); + return area->get_instance_id(); +} + +void PhysicsServer3DSW::area_set_param(RID p_area, AreaParameter p_param, const Variant &p_value) { + + if (space_owner.owns(p_area)) { + Space3DSW *space = space_owner.getornull(p_area); + p_area = space->get_default_area()->get_self(); + } + Area3DSW *area = area_owner.getornull(p_area); + ERR_FAIL_COND(!area); + area->set_param(p_param, p_value); +}; + +void PhysicsServer3DSW::area_set_transform(RID p_area, const Transform &p_transform) { + + Area3DSW *area = area_owner.getornull(p_area); + ERR_FAIL_COND(!area); + area->set_transform(p_transform); +}; + +Variant PhysicsServer3DSW::area_get_param(RID p_area, AreaParameter p_param) const { + + if (space_owner.owns(p_area)) { + Space3DSW *space = space_owner.getornull(p_area); + p_area = space->get_default_area()->get_self(); + } + Area3DSW *area = area_owner.getornull(p_area); + ERR_FAIL_COND_V(!area, Variant()); + + return area->get_param(p_param); +}; + +Transform PhysicsServer3DSW::area_get_transform(RID p_area) const { + + Area3DSW *area = area_owner.getornull(p_area); + ERR_FAIL_COND_V(!area, Transform()); + + return area->get_transform(); +}; + +void PhysicsServer3DSW::area_set_collision_layer(RID p_area, uint32_t p_layer) { + + Area3DSW *area = area_owner.getornull(p_area); + ERR_FAIL_COND(!area); + + area->set_collision_layer(p_layer); +} + +void PhysicsServer3DSW::area_set_collision_mask(RID p_area, uint32_t p_mask) { + + Area3DSW *area = area_owner.getornull(p_area); + ERR_FAIL_COND(!area); + + area->set_collision_mask(p_mask); +} + +void PhysicsServer3DSW::area_set_monitorable(RID p_area, bool p_monitorable) { + + Area3DSW *area = area_owner.getornull(p_area); + ERR_FAIL_COND(!area); + FLUSH_QUERY_CHECK(area); + + area->set_monitorable(p_monitorable); +} + +void PhysicsServer3DSW::area_set_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) { + + Area3DSW *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 PhysicsServer3DSW::area_set_ray_pickable(RID p_area, bool p_enable) { + + Area3DSW *area = area_owner.getornull(p_area); + ERR_FAIL_COND(!area); + + area->set_ray_pickable(p_enable); +} + +bool PhysicsServer3DSW::area_is_ray_pickable(RID p_area) const { + + Area3DSW *area = area_owner.getornull(p_area); + ERR_FAIL_COND_V(!area, false); + + return area->is_ray_pickable(); +} + +void PhysicsServer3DSW::area_set_area_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) { + + Area3DSW *area = area_owner.getornull(p_area); + ERR_FAIL_COND(!area); + + area->set_area_monitor_callback(p_receiver ? p_receiver->get_instance_id() : ObjectID(), p_method); +} + +/* BODY API */ + +RID PhysicsServer3DSW::body_create(BodyMode p_mode, bool p_init_sleeping) { + + Body3DSW *body = memnew(Body3DSW); + if (p_mode != BODY_MODE_RIGID) + body->set_mode(p_mode); + if (p_init_sleeping) + body->set_state(BODY_STATE_SLEEPING, p_init_sleeping); + RID rid = body_owner.make_rid(body); + body->set_self(rid); + return rid; +}; + +void PhysicsServer3DSW::body_set_space(RID p_body, RID p_space) { + + Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND(!body); + + Space3DSW *space = nullptr; + 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 PhysicsServer3DSW::body_get_space(RID p_body) const { + + Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND_V(!body, RID()); + + Space3DSW *space = body->get_space(); + if (!space) + return RID(); + return space->get_self(); +}; + +void PhysicsServer3DSW::body_set_mode(RID p_body, BodyMode p_mode) { + + Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND(!body); + + body->set_mode(p_mode); +}; + +PhysicsServer3D::BodyMode PhysicsServer3DSW::body_get_mode(RID p_body) const { + + Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND_V(!body, BODY_MODE_STATIC); + + return body->get_mode(); +}; + +void PhysicsServer3DSW::body_add_shape(RID p_body, RID p_shape, const Transform &p_transform, bool p_disabled) { + + Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND(!body); + + Shape3DSW *shape = shape_owner.getornull(p_shape); + ERR_FAIL_COND(!shape); + + body->add_shape(shape, p_transform, p_disabled); +} + +void PhysicsServer3DSW::body_set_shape(RID p_body, int p_shape_idx, RID p_shape) { + + Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND(!body); + + Shape3DSW *shape = shape_owner.getornull(p_shape); + ERR_FAIL_COND(!shape); + ERR_FAIL_COND(!shape->is_configured()); + + body->set_shape(p_shape_idx, shape); +} +void PhysicsServer3DSW::body_set_shape_transform(RID p_body, int p_shape_idx, const Transform &p_transform) { + + Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND(!body); + + body->set_shape_transform(p_shape_idx, p_transform); +} + +int PhysicsServer3DSW::body_get_shape_count(RID p_body) const { + + Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND_V(!body, -1); + + return body->get_shape_count(); +} +RID PhysicsServer3DSW::body_get_shape(RID p_body, int p_shape_idx) const { + + Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND_V(!body, RID()); + + Shape3DSW *shape = body->get_shape(p_shape_idx); + ERR_FAIL_COND_V(!shape, RID()); + + return shape->get_self(); +} + +void PhysicsServer3DSW::body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled) { + + Body3DSW *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 PhysicsServer3DSW::body_get_shape_transform(RID p_body, int p_shape_idx) const { + + Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND_V(!body, Transform()); + + return body->get_shape_transform(p_shape_idx); +} + +void PhysicsServer3DSW::body_remove_shape(RID p_body, int p_shape_idx) { + + Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND(!body); + + body->remove_shape(p_shape_idx); +} + +void PhysicsServer3DSW::body_clear_shapes(RID p_body) { + + Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND(!body); + + while (body->get_shape_count()) + body->remove_shape(0); +} + +void PhysicsServer3DSW::body_set_enable_continuous_collision_detection(RID p_body, bool p_enable) { + + Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND(!body); + + body->set_continuous_collision_detection(p_enable); +} + +bool PhysicsServer3DSW::body_is_continuous_collision_detection_enabled(RID p_body) const { + + Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND_V(!body, false); + + return body->is_continuous_collision_detection_enabled(); +} + +void PhysicsServer3DSW::body_set_collision_layer(RID p_body, uint32_t p_layer) { + + Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND(!body); + + body->set_collision_layer(p_layer); + body->wakeup(); +} + +uint32_t PhysicsServer3DSW::body_get_collision_layer(RID p_body) const { + + const Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND_V(!body, 0); + + return body->get_collision_layer(); +} + +void PhysicsServer3DSW::body_set_collision_mask(RID p_body, uint32_t p_mask) { + + Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND(!body); + + body->set_collision_mask(p_mask); + body->wakeup(); +} + +uint32_t PhysicsServer3DSW::body_get_collision_mask(RID p_body) const { + + const Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND_V(!body, 0); + + return body->get_collision_mask(); +} + +void PhysicsServer3DSW::body_attach_object_instance_id(RID p_body, ObjectID p_id) { + + Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND(!body); + + body->set_instance_id(p_id); +}; + +ObjectID PhysicsServer3DSW::body_get_object_instance_id(RID p_body) const { + + Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND_V(!body, ObjectID()); + + return body->get_instance_id(); +}; + +void PhysicsServer3DSW::body_set_user_flags(RID p_body, uint32_t p_flags) { + + Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND(!body); +}; + +uint32_t PhysicsServer3DSW::body_get_user_flags(RID p_body) const { + + Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND_V(!body, 0); + + return 0; +}; + +void PhysicsServer3DSW::body_set_param(RID p_body, BodyParameter p_param, real_t p_value) { + + Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND(!body); + + body->set_param(p_param, p_value); +}; + +real_t PhysicsServer3DSW::body_get_param(RID p_body, BodyParameter p_param) const { + + Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND_V(!body, 0); + + return body->get_param(p_param); +}; + +void PhysicsServer3DSW::body_set_kinematic_safe_margin(RID p_body, real_t p_margin) { + Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND(!body); + body->set_kinematic_margin(p_margin); +} + +real_t PhysicsServer3DSW::body_get_kinematic_safe_margin(RID p_body) const { + Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND_V(!body, 0); + + return body->get_kinematic_margin(); +} + +void PhysicsServer3DSW::body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) { + + Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND(!body); + + body->set_state(p_state, p_variant); +}; + +Variant PhysicsServer3DSW::body_get_state(RID p_body, BodyState p_state) const { + + Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND_V(!body, Variant()); + + return body->get_state(p_state); +}; + +void PhysicsServer3DSW::body_set_applied_force(RID p_body, const Vector3 &p_force) { + + Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND(!body); + + body->set_applied_force(p_force); + body->wakeup(); +}; + +Vector3 PhysicsServer3DSW::body_get_applied_force(RID p_body) const { + + Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND_V(!body, Vector3()); + return body->get_applied_force(); +}; + +void PhysicsServer3DSW::body_set_applied_torque(RID p_body, const Vector3 &p_torque) { + + Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND(!body); + + body->set_applied_torque(p_torque); + body->wakeup(); +}; + +Vector3 PhysicsServer3DSW::body_get_applied_torque(RID p_body) const { + + Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND_V(!body, Vector3()); + + return body->get_applied_torque(); +}; + +void PhysicsServer3DSW::body_add_central_force(RID p_body, const Vector3 &p_force) { + Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND(!body); + + body->add_central_force(p_force); + body->wakeup(); +} + +void PhysicsServer3DSW::body_add_force(RID p_body, const Vector3 &p_force, const Vector3 &p_pos) { + Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND(!body); + + body->add_force(p_force, p_pos); + body->wakeup(); +}; + +void PhysicsServer3DSW::body_add_torque(RID p_body, const Vector3 &p_torque) { + Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND(!body); + + body->add_torque(p_torque); + body->wakeup(); +}; + +void PhysicsServer3DSW::body_apply_central_impulse(RID p_body, const Vector3 &p_impulse) { + Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND(!body); + + _update_shapes(); + + body->apply_central_impulse(p_impulse); + body->wakeup(); +} + +void PhysicsServer3DSW::body_apply_impulse(RID p_body, const Vector3 &p_pos, const Vector3 &p_impulse) { + + Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND(!body); + + _update_shapes(); + + body->apply_impulse(p_pos, p_impulse); + body->wakeup(); +}; + +void PhysicsServer3DSW::body_apply_torque_impulse(RID p_body, const Vector3 &p_impulse) { + + Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND(!body); + + _update_shapes(); + + body->apply_torque_impulse(p_impulse); + body->wakeup(); +}; + +void PhysicsServer3DSW::body_set_axis_velocity(RID p_body, const Vector3 &p_axis_velocity) { + + Body3DSW *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 PhysicsServer3DSW::body_set_axis_lock(RID p_body, BodyAxis p_axis, bool p_lock) { + + Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND(!body); + + body->set_axis_lock(p_axis, p_lock); + body->wakeup(); +} + +bool PhysicsServer3DSW::body_is_axis_locked(RID p_body, BodyAxis p_axis) const { + + const Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND_V(!body, 0); + return body->is_axis_locked(p_axis); +} + +void PhysicsServer3DSW::body_add_collision_exception(RID p_body, RID p_body_b) { + + Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND(!body); + + body->add_exception(p_body_b); + body->wakeup(); +}; + +void PhysicsServer3DSW::body_remove_collision_exception(RID p_body, RID p_body_b) { + + Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND(!body); + + body->remove_exception(p_body_b); + body->wakeup(); +}; + +void PhysicsServer3DSW::body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) { + + Body3DSW *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 PhysicsServer3DSW::body_set_contacts_reported_depth_threshold(RID p_body, real_t p_threshold) { + + Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND(!body); +}; + +real_t PhysicsServer3DSW::body_get_contacts_reported_depth_threshold(RID p_body) const { + + Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND_V(!body, 0); + return 0; +}; + +void PhysicsServer3DSW::body_set_omit_force_integration(RID p_body, bool p_omit) { + + Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND(!body); + + body->set_omit_force_integration(p_omit); +}; + +bool PhysicsServer3DSW::body_is_omitting_force_integration(RID p_body) const { + + Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND_V(!body, false); + return body->get_omit_force_integration(); +}; + +void PhysicsServer3DSW::body_set_max_contacts_reported(RID p_body, int p_contacts) { + + Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND(!body); + body->set_max_contacts_reported(p_contacts); +} + +int PhysicsServer3DSW::body_get_max_contacts_reported(RID p_body) const { + + Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND_V(!body, -1); + return body->get_max_contacts_reported(); +} + +void PhysicsServer3DSW::body_set_force_integration_callback(RID p_body, Object *p_receiver, const StringName &p_method, const Variant &p_udata) { + + Body3DSW *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 PhysicsServer3DSW::body_set_ray_pickable(RID p_body, bool p_enable) { + + Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND(!body); + body->set_ray_pickable(p_enable); +} + +bool PhysicsServer3DSW::body_is_ray_pickable(RID p_body) const { + + Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND_V(!body, false); + return body->is_ray_pickable(); +} + +bool PhysicsServer3DSW::body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, MotionResult *r_result, bool p_exclude_raycast_shapes) { + + Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND_V(!body, false); + 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 PhysicsServer3DSW::body_test_ray_separation(RID p_body, const Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, SeparationResult *r_results, int p_result_max, float p_margin) { + + Body3DSW *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); +} + +PhysicsDirectBodyState3D *PhysicsServer3DSW::body_get_direct_state(RID p_body) { + + Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND_V(!body, nullptr); + ERR_FAIL_COND_V_MSG(!doing_sync || body->get_space()->is_locked(), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification."); + + direct_state->body = body; + return direct_state; +} + +/* JOINT API */ + +RID PhysicsServer3DSW::joint_create_pin(RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B) { + + Body3DSW *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(); + } + + Body3DSW *body_B = body_owner.getornull(p_body_B); + ERR_FAIL_COND_V(!body_B, RID()); + + ERR_FAIL_COND_V(body_A == body_B, RID()); + + Joint3DSW *joint = memnew(PinJoint3DSW(body_A, p_local_A, body_B, p_local_B)); + RID rid = joint_owner.make_rid(joint); + joint->set_self(rid); + return rid; +} + +void PhysicsServer3DSW::pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value) { + + Joint3DSW *joint = joint_owner.getornull(p_joint); + ERR_FAIL_COND(!joint); + ERR_FAIL_COND(joint->get_type() != JOINT_PIN); + PinJoint3DSW *pin_joint = static_cast<PinJoint3DSW *>(joint); + pin_joint->set_param(p_param, p_value); +} +real_t PhysicsServer3DSW::pin_joint_get_param(RID p_joint, PinJointParam p_param) const { + + Joint3DSW *joint = joint_owner.getornull(p_joint); + ERR_FAIL_COND_V(!joint, 0); + ERR_FAIL_COND_V(joint->get_type() != JOINT_PIN, 0); + PinJoint3DSW *pin_joint = static_cast<PinJoint3DSW *>(joint); + return pin_joint->get_param(p_param); +} + +void PhysicsServer3DSW::pin_joint_set_local_a(RID p_joint, const Vector3 &p_A) { + + Joint3DSW *joint = joint_owner.getornull(p_joint); + ERR_FAIL_COND(!joint); + ERR_FAIL_COND(joint->get_type() != JOINT_PIN); + PinJoint3DSW *pin_joint = static_cast<PinJoint3DSW *>(joint); + pin_joint->set_pos_a(p_A); +} +Vector3 PhysicsServer3DSW::pin_joint_get_local_a(RID p_joint) const { + + Joint3DSW *joint = joint_owner.getornull(p_joint); + ERR_FAIL_COND_V(!joint, Vector3()); + ERR_FAIL_COND_V(joint->get_type() != JOINT_PIN, Vector3()); + PinJoint3DSW *pin_joint = static_cast<PinJoint3DSW *>(joint); + return pin_joint->get_position_a(); +} + +void PhysicsServer3DSW::pin_joint_set_local_b(RID p_joint, const Vector3 &p_B) { + + Joint3DSW *joint = joint_owner.getornull(p_joint); + ERR_FAIL_COND(!joint); + ERR_FAIL_COND(joint->get_type() != JOINT_PIN); + PinJoint3DSW *pin_joint = static_cast<PinJoint3DSW *>(joint); + pin_joint->set_pos_b(p_B); +} +Vector3 PhysicsServer3DSW::pin_joint_get_local_b(RID p_joint) const { + + Joint3DSW *joint = joint_owner.getornull(p_joint); + ERR_FAIL_COND_V(!joint, Vector3()); + ERR_FAIL_COND_V(joint->get_type() != JOINT_PIN, Vector3()); + PinJoint3DSW *pin_joint = static_cast<PinJoint3DSW *>(joint); + return pin_joint->get_position_b(); +} + +RID PhysicsServer3DSW::joint_create_hinge(RID p_body_A, const Transform &p_frame_A, RID p_body_B, const Transform &p_frame_B) { + + Body3DSW *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(); + } + + Body3DSW *body_B = body_owner.getornull(p_body_B); + ERR_FAIL_COND_V(!body_B, RID()); + + ERR_FAIL_COND_V(body_A == body_B, RID()); + + Joint3DSW *joint = memnew(HingeJoint3DSW(body_A, body_B, p_frame_A, p_frame_B)); + RID rid = joint_owner.make_rid(joint); + joint->set_self(rid); + return rid; +} + +RID PhysicsServer3DSW::joint_create_hinge_simple(RID p_body_A, const Vector3 &p_pivot_A, const Vector3 &p_axis_A, RID p_body_B, const Vector3 &p_pivot_B, const Vector3 &p_axis_B) { + + Body3DSW *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(); + } + + Body3DSW *body_B = body_owner.getornull(p_body_B); + ERR_FAIL_COND_V(!body_B, RID()); + + ERR_FAIL_COND_V(body_A == body_B, RID()); + + Joint3DSW *joint = memnew(HingeJoint3DSW(body_A, body_B, p_pivot_A, p_pivot_B, p_axis_A, p_axis_B)); + RID rid = joint_owner.make_rid(joint); + joint->set_self(rid); + return rid; +} + +void PhysicsServer3DSW::hinge_joint_set_param(RID p_joint, HingeJointParam p_param, real_t p_value) { + + Joint3DSW *joint = joint_owner.getornull(p_joint); + ERR_FAIL_COND(!joint); + ERR_FAIL_COND(joint->get_type() != JOINT_HINGE); + HingeJoint3DSW *hinge_joint = static_cast<HingeJoint3DSW *>(joint); + hinge_joint->set_param(p_param, p_value); +} +real_t PhysicsServer3DSW::hinge_joint_get_param(RID p_joint, HingeJointParam p_param) const { + + Joint3DSW *joint = joint_owner.getornull(p_joint); + ERR_FAIL_COND_V(!joint, 0); + ERR_FAIL_COND_V(joint->get_type() != JOINT_HINGE, 0); + HingeJoint3DSW *hinge_joint = static_cast<HingeJoint3DSW *>(joint); + return hinge_joint->get_param(p_param); +} + +void PhysicsServer3DSW::hinge_joint_set_flag(RID p_joint, HingeJointFlag p_flag, bool p_value) { + + Joint3DSW *joint = joint_owner.getornull(p_joint); + ERR_FAIL_COND(!joint); + ERR_FAIL_COND(joint->get_type() != JOINT_HINGE); + HingeJoint3DSW *hinge_joint = static_cast<HingeJoint3DSW *>(joint); + hinge_joint->set_flag(p_flag, p_value); +} +bool PhysicsServer3DSW::hinge_joint_get_flag(RID p_joint, HingeJointFlag p_flag) const { + + Joint3DSW *joint = joint_owner.getornull(p_joint); + ERR_FAIL_COND_V(!joint, false); + ERR_FAIL_COND_V(joint->get_type() != JOINT_HINGE, false); + HingeJoint3DSW *hinge_joint = static_cast<HingeJoint3DSW *>(joint); + return hinge_joint->get_flag(p_flag); +} + +void PhysicsServer3DSW::joint_set_solver_priority(RID p_joint, int p_priority) { + + Joint3DSW *joint = joint_owner.getornull(p_joint); + ERR_FAIL_COND(!joint); + joint->set_priority(p_priority); +} + +int PhysicsServer3DSW::joint_get_solver_priority(RID p_joint) const { + + Joint3DSW *joint = joint_owner.getornull(p_joint); + ERR_FAIL_COND_V(!joint, 0); + return joint->get_priority(); +} + +void PhysicsServer3DSW::joint_disable_collisions_between_bodies(RID p_joint, const bool p_disable) { + Joint3DSW *joint = joint_owner.getornull(p_joint); + ERR_FAIL_COND(!joint); + + joint->disable_collisions_between_bodies(p_disable); + + if (2 == joint->get_body_count()) { + Body3DSW *body_a = *joint->get_body_ptr(); + Body3DSW *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 PhysicsServer3DSW::joint_is_disabled_collisions_between_bodies(RID p_joint) const { + Joint3DSW *joint = joint_owner.getornull(p_joint); + ERR_FAIL_COND_V(!joint, true); + + return joint->is_disabled_collisions_between_bodies(); +} + +PhysicsServer3DSW::JointType PhysicsServer3DSW::joint_get_type(RID p_joint) const { + + Joint3DSW *joint = joint_owner.getornull(p_joint); + ERR_FAIL_COND_V(!joint, JOINT_PIN); + return joint->get_type(); +} + +RID PhysicsServer3DSW::joint_create_slider(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) { + + Body3DSW *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(); + } + + Body3DSW *body_B = body_owner.getornull(p_body_B); + ERR_FAIL_COND_V(!body_B, RID()); + + ERR_FAIL_COND_V(body_A == body_B, RID()); + + Joint3DSW *joint = memnew(SliderJoint3DSW(body_A, body_B, p_local_frame_A, p_local_frame_B)); + RID rid = joint_owner.make_rid(joint); + joint->set_self(rid); + return rid; +} + +void PhysicsServer3DSW::slider_joint_set_param(RID p_joint, SliderJointParam p_param, real_t p_value) { + + Joint3DSW *joint = joint_owner.getornull(p_joint); + ERR_FAIL_COND(!joint); + ERR_FAIL_COND(joint->get_type() != JOINT_SLIDER); + SliderJoint3DSW *slider_joint = static_cast<SliderJoint3DSW *>(joint); + slider_joint->set_param(p_param, p_value); +} +real_t PhysicsServer3DSW::slider_joint_get_param(RID p_joint, SliderJointParam p_param) const { + + Joint3DSW *joint = joint_owner.getornull(p_joint); + ERR_FAIL_COND_V(!joint, 0); + ERR_FAIL_COND_V(joint->get_type() != JOINT_CONE_TWIST, 0); + SliderJoint3DSW *slider_joint = static_cast<SliderJoint3DSW *>(joint); + return slider_joint->get_param(p_param); +} + +RID PhysicsServer3DSW::joint_create_cone_twist(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) { + + Body3DSW *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(); + } + + Body3DSW *body_B = body_owner.getornull(p_body_B); + ERR_FAIL_COND_V(!body_B, RID()); + + ERR_FAIL_COND_V(body_A == body_B, RID()); + + Joint3DSW *joint = memnew(ConeTwistJoint3DSW(body_A, body_B, p_local_frame_A, p_local_frame_B)); + RID rid = joint_owner.make_rid(joint); + joint->set_self(rid); + return rid; +} + +void PhysicsServer3DSW::cone_twist_joint_set_param(RID p_joint, ConeTwistJointParam p_param, real_t p_value) { + + Joint3DSW *joint = joint_owner.getornull(p_joint); + ERR_FAIL_COND(!joint); + ERR_FAIL_COND(joint->get_type() != JOINT_CONE_TWIST); + ConeTwistJoint3DSW *cone_twist_joint = static_cast<ConeTwistJoint3DSW *>(joint); + cone_twist_joint->set_param(p_param, p_value); +} +real_t PhysicsServer3DSW::cone_twist_joint_get_param(RID p_joint, ConeTwistJointParam p_param) const { + + Joint3DSW *joint = joint_owner.getornull(p_joint); + ERR_FAIL_COND_V(!joint, 0); + ERR_FAIL_COND_V(joint->get_type() != JOINT_CONE_TWIST, 0); + ConeTwistJoint3DSW *cone_twist_joint = static_cast<ConeTwistJoint3DSW *>(joint); + return cone_twist_joint->get_param(p_param); +} + +RID PhysicsServer3DSW::joint_create_generic_6dof(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) { + + Body3DSW *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(); + } + + Body3DSW *body_B = body_owner.getornull(p_body_B); + ERR_FAIL_COND_V(!body_B, RID()); + + ERR_FAIL_COND_V(body_A == body_B, RID()); + + Joint3DSW *joint = memnew(Generic6DOFJoint3DSW(body_A, body_B, p_local_frame_A, p_local_frame_B, true)); + RID rid = joint_owner.make_rid(joint); + joint->set_self(rid); + return rid; +} + +void PhysicsServer3DSW::generic_6dof_joint_set_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParam p_param, real_t p_value) { + + Joint3DSW *joint = joint_owner.getornull(p_joint); + ERR_FAIL_COND(!joint); + ERR_FAIL_COND(joint->get_type() != JOINT_6DOF); + Generic6DOFJoint3DSW *generic_6dof_joint = static_cast<Generic6DOFJoint3DSW *>(joint); + generic_6dof_joint->set_param(p_axis, p_param, p_value); +} +real_t PhysicsServer3DSW::generic_6dof_joint_get_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParam p_param) { + + Joint3DSW *joint = joint_owner.getornull(p_joint); + ERR_FAIL_COND_V(!joint, 0); + ERR_FAIL_COND_V(joint->get_type() != JOINT_6DOF, 0); + Generic6DOFJoint3DSW *generic_6dof_joint = static_cast<Generic6DOFJoint3DSW *>(joint); + return generic_6dof_joint->get_param(p_axis, p_param); +} + +void PhysicsServer3DSW::generic_6dof_joint_set_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlag p_flag, bool p_enable) { + + Joint3DSW *joint = joint_owner.getornull(p_joint); + ERR_FAIL_COND(!joint); + ERR_FAIL_COND(joint->get_type() != JOINT_6DOF); + Generic6DOFJoint3DSW *generic_6dof_joint = static_cast<Generic6DOFJoint3DSW *>(joint); + generic_6dof_joint->set_flag(p_axis, p_flag, p_enable); +} +bool PhysicsServer3DSW::generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlag p_flag) { + + Joint3DSW *joint = joint_owner.getornull(p_joint); + ERR_FAIL_COND_V(!joint, false); + ERR_FAIL_COND_V(joint->get_type() != JOINT_6DOF, false); + Generic6DOFJoint3DSW *generic_6dof_joint = static_cast<Generic6DOFJoint3DSW *>(joint); + return generic_6dof_joint->get_flag(p_axis, p_flag); +} + +void PhysicsServer3DSW::free(RID p_rid) { + + _update_shapes(); //just in case + + if (shape_owner.owns(p_rid)) { + + Shape3DSW *shape = shape_owner.getornull(p_rid); + + while (shape->get_owners().size()) { + ShapeOwner3DSW *so = shape->get_owners().front()->key(); + so->remove_shape(shape); + } + + shape_owner.free(p_rid); + memdelete(shape); + } else if (body_owner.owns(p_rid)) { + + Body3DSW *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(nullptr); + + while (body->get_shape_count()) { + + body->remove_shape(0); + } + + body_owner.free(p_rid); + memdelete(body); + + } else if (area_owner.owns(p_rid)) { + + Area3DSW *area = area_owner.getornull(p_rid); + + /* + if (area->get_monitor_query()) + _clear_query(area->get_monitor_query()); + */ + + area->set_space(nullptr); + + while (area->get_shape_count()) { + + area->remove_shape(0); + } + + area_owner.free(p_rid); + memdelete(area); + } else if (space_owner.owns(p_rid)) { + + Space3DSW *space = space_owner.getornull(p_rid); + + while (space->get_objects().size()) { + CollisionObject3DSW *co = (CollisionObject3DSW *)space->get_objects().front()->get(); + co->set_space(nullptr); + } + + 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)) { + + Joint3DSW *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 PhysicsServer3DSW::set_active(bool p_active) { + + active = p_active; +}; + +void PhysicsServer3DSW::init() { + + doing_sync = true; + last_step = 0.001; + iterations = 8; // 8? + stepper = memnew(Step3DSW); + direct_state = memnew(PhysicsDirectBodyState3DSW); +}; + +void PhysicsServer3DSW::step(real_t p_step) { + +#ifndef _3D_DISABLED + + if (!active) + return; + + _update_shapes(); + + doing_sync = false; + + last_step = p_step; + PhysicsDirectBodyState3DSW::singleton->step = p_step; + + island_count = 0; + active_objects = 0; + collision_pairs = 0; + for (Set<const Space3DSW *>::Element *E = active_spaces.front(); E; E = E->next()) { + + stepper->step((Space3DSW *)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 PhysicsServer3DSW::sync(){ + +}; + +void PhysicsServer3DSW::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 Space3DSW *>::Element *E = active_spaces.front(); E; E = E->next()) { + + Space3DSW *space = (Space3DSW *)E->get(); + space->call_queries(); + } + + flushing_queries = false; + + if (EngineDebugger::is_profiling("servers")) { + + uint64_t total_time[Space3DSW::ELAPSED_TIME_MAX]; + static const char *time_name[Space3DSW::ELAPSED_TIME_MAX] = { + "integrate_forces", + "generate_islands", + "setup_constraints", + "solve_constraints", + "integrate_velocities" + }; + + for (int i = 0; i < Space3DSW::ELAPSED_TIME_MAX; i++) { + total_time[i] = 0; + } + + for (Set<const Space3DSW *>::Element *E = active_spaces.front(); E; E = E->next()) { + + for (int i = 0; i < Space3DSW::ELAPSED_TIME_MAX; i++) { + total_time[i] += E->get()->get_elapsed_time(Space3DSW::ElapsedTime(i)); + } + } + + Array values; + values.resize(Space3DSW::ELAPSED_TIME_MAX * 2); + for (int i = 0; i < Space3DSW::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 PhysicsServer3DSW::finish() { + + memdelete(stepper); + memdelete(direct_state); +}; + +int PhysicsServer3DSW::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 PhysicsServer3DSW::_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 PhysicsServer3DSW::_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++; + } +} + +PhysicsServer3DSW *PhysicsServer3DSW::singleton = nullptr; +PhysicsServer3DSW::PhysicsServer3DSW() { + singleton = this; + BroadPhase3DSW::create_func = BroadPhaseOctree::_create; + island_count = 0; + active_objects = 0; + collision_pairs = 0; + + active = true; + flushing_queries = false; +}; + +PhysicsServer3DSW::~PhysicsServer3DSW(){ + +}; diff --git a/servers/physics_3d/physics_server_3d_sw.h b/servers/physics_3d/physics_server_3d_sw.h new file mode 100644 index 0000000000..6e79d9eceb --- /dev/null +++ b/servers/physics_3d/physics_server_3d_sw.h @@ -0,0 +1,382 @@ +/*************************************************************************/ +/* physics_server_3d_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_3d_sw.h" +#include "servers/physics_server_3d.h" +#include "shape_3d_sw.h" +#include "space_3d_sw.h" +#include "step_3d_sw.h" + +class PhysicsServer3DSW : public PhysicsServer3D { + + GDCLASS(PhysicsServer3DSW, PhysicsServer3D); + + friend class PhysicsDirectSpaceState3DSW; + bool active; + int iterations; + bool doing_sync; + real_t last_step; + + int island_count; + int active_objects; + int collision_pairs; + + bool flushing_queries; + + Step3DSW *stepper; + Set<const Space3DSW *> active_spaces; + + PhysicsDirectBodyState3DSW *direct_state; + + mutable RID_PtrOwner<Shape3DSW> shape_owner; + mutable RID_PtrOwner<Space3DSW> space_owner; + mutable RID_PtrOwner<Area3DSW> area_owner; + mutable RID_PtrOwner<Body3DSW> body_owner; + mutable RID_PtrOwner<Joint3DSW> joint_owner; + + //void _clear_query(QuerySW *p_query); + friend class CollisionObject3DSW; + SelfList<CollisionObject3DSW>::List pending_shape_update_list; + void _update_shapes(); + +public: + static PhysicsServer3DSW *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 PhysicsDirectSpaceState3D *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 = nullptr, bool p_exclude_raycast_shapes = true); + virtual int body_test_ray_separation(RID p_body, const Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, SeparationResult *r_results, int p_result_max, float p_margin = 0.001); + + // this function only works on physics process, errors and returns null otherwise + virtual PhysicsDirectBodyState3D *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_rendering_server(RID p_body, class SoftBodyRenderingServerHandler *p_rendering_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); + + PhysicsServer3DSW(); + ~PhysicsServer3DSW(); +}; + +#endif diff --git a/servers/physics_3d/shape_3d_sw.cpp b/servers/physics_3d/shape_3d_sw.cpp new file mode 100644 index 0000000000..61c32b779a --- /dev/null +++ b/servers/physics_3d/shape_3d_sw.cpp @@ -0,0 +1,1655 @@ +/*************************************************************************/ +/* shape_3d_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_3d_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 Shape3DSW::configure(const AABB &p_aabb) { + aabb = p_aabb; + configured = true; + for (Map<ShapeOwner3DSW *, int>::Element *E = owners.front(); E; E = E->next()) { + ShapeOwner3DSW *co = (ShapeOwner3DSW *)E->key(); + co->_shape_changed(); + } +} + +Vector3 Shape3DSW::get_support(const Vector3 &p_normal) const { + + Vector3 res; + int amnt; + get_supports(p_normal, 1, &res, amnt); + return res; +} + +void Shape3DSW::add_owner(ShapeOwner3DSW *p_owner) { + + Map<ShapeOwner3DSW *, int>::Element *E = owners.find(p_owner); + if (E) { + E->get()++; + } else { + owners[p_owner] = 1; + } +} + +void Shape3DSW::remove_owner(ShapeOwner3DSW *p_owner) { + + Map<ShapeOwner3DSW *, int>::Element *E = owners.find(p_owner); + ERR_FAIL_COND(!E); + E->get()--; + if (E->get() == 0) { + owners.erase(E); + } +} + +bool Shape3DSW::is_owner(ShapeOwner3DSW *p_owner) const { + + return owners.has(p_owner); +} + +const Map<ShapeOwner3DSW *, int> &Shape3DSW::get_owners() const { + return owners; +} + +Shape3DSW::Shape3DSW() { + + custom_bias = 0; + configured = false; +} + +Shape3DSW::~Shape3DSW() { + + ERR_FAIL_COND(owners.size()); +} + +Plane PlaneShape3DSW::get_plane() const { + + return plane; +} + +void PlaneShape3DSW::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 PlaneShape3DSW::get_support(const Vector3 &p_normal) const { + + return p_normal * 1e15; +} + +bool PlaneShape3DSW::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 PlaneShape3DSW::intersect_point(const Vector3 &p_point) const { + + return plane.distance_to(p_point) < 0; +} + +Vector3 PlaneShape3DSW::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 PlaneShape3DSW::get_moment_of_inertia(real_t p_mass) const { + + return Vector3(); //wtf +} + +void PlaneShape3DSW::_setup(const Plane &p_plane) { + + plane = p_plane; + configure(AABB(Vector3(-1e4, -1e4, -1e4), Vector3(1e4 * 2, 1e4 * 2, 1e4 * 2))); +} + +void PlaneShape3DSW::set_data(const Variant &p_data) { + + _setup(p_data); +} + +Variant PlaneShape3DSW::get_data() const { + + return plane; +} + +PlaneShape3DSW::PlaneShape3DSW() { +} + +// + +real_t RayShape3DSW::get_length() const { + + return length; +} + +bool RayShape3DSW::get_slips_on_slope() const { + return slips_on_slope; +} + +void RayShape3DSW::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 RayShape3DSW::get_support(const Vector3 &p_normal) const { + + if (p_normal.z > 0) + return Vector3(0, 0, length); + else + return Vector3(0, 0, 0); +} + +void RayShape3DSW::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 RayShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { + + return false; //simply not possible +} + +bool RayShape3DSW::intersect_point(const Vector3 &p_point) const { + + return false; //simply not possible +} + +Vector3 RayShape3DSW::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 RayShape3DSW::get_moment_of_inertia(real_t p_mass) const { + + return Vector3(); +} + +void RayShape3DSW::_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 RayShape3DSW::set_data(const Variant &p_data) { + + Dictionary d = p_data; + _setup(d["length"], d["slips_on_slope"]); +} + +Variant RayShape3DSW::get_data() const { + + Dictionary d; + d["length"] = length; + d["slips_on_slope"] = slips_on_slope; + return d; +} + +RayShape3DSW::RayShape3DSW() { + + length = 1; + slips_on_slope = false; +} + +/********** SPHERE *************/ + +real_t SphereShape3DSW::get_radius() const { + + return radius; +} + +void SphereShape3DSW::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 SphereShape3DSW::get_support(const Vector3 &p_normal) const { + + return p_normal * radius; +} + +void SphereShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { + + *r_supports = p_normal * radius; + r_amount = 1; +} + +bool SphereShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { + + return Geometry::segment_intersects_sphere(p_begin, p_end, Vector3(), radius, &r_result, &r_normal); +} + +bool SphereShape3DSW::intersect_point(const Vector3 &p_point) const { + + return p_point.length() < radius; +} + +Vector3 SphereShape3DSW::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 SphereShape3DSW::get_moment_of_inertia(real_t p_mass) const { + + real_t s = 0.4 * p_mass * radius * radius; + return Vector3(s, s, s); +} + +void SphereShape3DSW::_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 SphereShape3DSW::set_data(const Variant &p_data) { + + _setup(p_data); +} + +Variant SphereShape3DSW::get_data() const { + + return radius; +} + +SphereShape3DSW::SphereShape3DSW() { + + radius = 0; +} + +/********** BOX *************/ + +void BoxShape3DSW::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 BoxShape3DSW::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 BoxShape3DSW::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 BoxShape3DSW::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 BoxShape3DSW::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 BoxShape3DSW::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 BoxShape3DSW::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 BoxShape3DSW::_setup(const Vector3 &p_half_extents) { + + half_extents = p_half_extents.abs(); + + configure(AABB(-half_extents, half_extents * 2)); +} + +void BoxShape3DSW::set_data(const Variant &p_data) { + + _setup(p_data); +} + +Variant BoxShape3DSW::get_data() const { + + return half_extents; +} + +BoxShape3DSW::BoxShape3DSW() { +} + +/********** CAPSULE *************/ + +void CapsuleShape3DSW::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 CapsuleShape3DSW::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 CapsuleShape3DSW::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 CapsuleShape3DSW::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 CapsuleShape3DSW::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 CapsuleShape3DSW::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 CapsuleShape3DSW::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 CapsuleShape3DSW::_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 CapsuleShape3DSW::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 CapsuleShape3DSW::get_data() const { + + Dictionary d; + d["radius"] = radius; + d["height"] = height; + return d; +} + +CapsuleShape3DSW::CapsuleShape3DSW() { + + height = radius = 0; +} + +/********** CONVEX POLYGON *************/ + +void ConvexPolygonShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { + + 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 ConvexPolygonShape3DSW::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 ConvexPolygonShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { + + const Geometry::MeshData::Face *faces = mesh.faces.ptr(); + 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 ConvexPolygonShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { + + const Geometry::MeshData::Face *faces = mesh.faces.ptr(); + 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 ConvexPolygonShape3DSW::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 ConvexPolygonShape3DSW::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 ConvexPolygonShape3DSW::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 ConvexPolygonShape3DSW::_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 ConvexPolygonShape3DSW::set_data(const Variant &p_data) { + + _setup(p_data); +} + +Variant ConvexPolygonShape3DSW::get_data() const { + + return mesh.vertices; +} + +ConvexPolygonShape3DSW::ConvexPolygonShape3DSW() { +} + +/********** FACE POLYGON *************/ + +void FaceShape3DSW::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 FaceShape3DSW::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 FaceShape3DSW::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 FaceShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { + + bool c = Geometry::segment_intersects_triangle(p_begin, p_end, vertex[0], vertex[1], vertex[2], &r_result); + 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 FaceShape3DSW::intersect_point(const Vector3 &p_point) const { + + return false; //face is flat +} + +Vector3 FaceShape3DSW::get_closest_point_to(const Vector3 &p_point) const { + + return Face3(vertex[0], vertex[1], vertex[2]).get_closest_point_to(p_point); +} + +Vector3 FaceShape3DSW::get_moment_of_inertia(real_t p_mass) const { + + return Vector3(); // Sorry, but i don't think anyone cares, FaceShape! +} + +FaceShape3DSW::FaceShape3DSW() { + + configure(AABB()); +} + +Vector<Vector3> ConcavePolygonShape3DSW::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 ConcavePolygonShape3DSW::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 ConcavePolygonShape3DSW::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 ConcavePolygonShape3DSW::_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 ConcavePolygonShape3DSW::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 ConcavePolygonShape3DSW::intersect_point(const Vector3 &p_point) const { + + return false; //face is flat +} + +Vector3 ConcavePolygonShape3DSW::get_closest_point_to(const Vector3 &p_point) const { + + return Vector3(); +} + +void ConcavePolygonShape3DSW::_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]; + FaceShape3DSW *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 ConcavePolygonShape3DSW::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(); + + FaceShape3DSW 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 ConcavePolygonShape3DSW::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 = nullptr; + bvh->right = nullptr; + 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 ConcavePolygonShape3DSW::_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 ConcavePolygonShape3DSW::_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 ConcavePolygonShape3DSW::set_data(const Variant &p_data) { + + _setup(p_data); +} + +Variant ConcavePolygonShape3DSW::get_data() const { + + return get_faces(); +} + +ConcavePolygonShape3DSW::ConcavePolygonShape3DSW() { +} + +/* HEIGHT MAP SHAPE */ + +Vector<real_t> HeightMapShape3DSW::get_heights() const { + + return heights; +} +int HeightMapShape3DSW::get_width() const { + + return width; +} +int HeightMapShape3DSW::get_depth() const { + + return depth; +} +real_t HeightMapShape3DSW::get_cell_size() const { + + return cell_size; +} + +void HeightMapShape3DSW::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 HeightMapShape3DSW::get_support(const Vector3 &p_normal) const { + + //not very useful, but not very used either + return get_aabb().get_support(p_normal); +} + +bool HeightMapShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal) const { + + return false; +} + +bool HeightMapShape3DSW::intersect_point(const Vector3 &p_point) const { + return false; +} + +Vector3 HeightMapShape3DSW::get_closest_point_to(const Vector3 &p_point) const { + + return Vector3(); +} + +void HeightMapShape3DSW::cull(const AABB &p_local_aabb, Callback p_callback, void *p_userdata) const { +} + +Vector3 HeightMapShape3DSW::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 HeightMapShape3DSW::_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 HeightMapShape3DSW::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 HeightMapShape3DSW::get_data() const { + + ERR_FAIL_V(Variant()); +} + +HeightMapShape3DSW::HeightMapShape3DSW() { + + width = 0; + depth = 0; + cell_size = 0; +} diff --git a/servers/physics_3d/shape_3d_sw.h b/servers/physics_3d/shape_3d_sw.h new file mode 100644 index 0000000000..dd29ec849b --- /dev/null +++ b/servers/physics_3d/shape_3d_sw.h @@ -0,0 +1,470 @@ +/*************************************************************************/ +/* shape_3d_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_3d.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 Shape3DSW; + +class ShapeOwner3DSW { +public: + virtual void _shape_changed() = 0; + virtual void remove_shape(Shape3DSW *p_shape) = 0; + + virtual ~ShapeOwner3DSW() {} +}; + +class Shape3DSW { + + RID self; + AABB aabb; + bool configured; + real_t custom_bias; + + Map<ShapeOwner3DSW *, 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 PhysicsServer3D::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(ShapeOwner3DSW *p_owner); + void remove_owner(ShapeOwner3DSW *p_owner); + bool is_owner(ShapeOwner3DSW *p_owner) const; + const Map<ShapeOwner3DSW *, int> &get_owners() const; + + Shape3DSW(); + virtual ~Shape3DSW(); +}; + +class ConcaveShape3DSW : public Shape3DSW { + +public: + virtual bool is_concave() const { return true; } + typedef void (*Callback)(void *p_userdata, Shape3DSW *p_convex); + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { r_amount = 0; } + + virtual void cull(const AABB &p_local_aabb, Callback p_callback, void *p_userdata) const = 0; + + ConcaveShape3DSW() {} +}; + +class PlaneShape3DSW : public Shape3DSW { + + Plane plane; + + void _setup(const Plane &p_plane); + +public: + Plane get_plane() const; + + virtual real_t get_area() const { return Math_INF; } + virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_PLANE; } + virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; + virtual Vector3 get_support(const Vector3 &p_normal) const; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { r_amount = 0; } + + virtual 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; + + PlaneShape3DSW(); +}; + +class RayShape3DSW : public Shape3DSW { + + 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 PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_RAY; } + virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; + virtual Vector3 get_support(const Vector3 &p_normal) const; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const; + + virtual 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; + + RayShape3DSW(); +}; + +class SphereShape3DSW : public Shape3DSW { + + 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 PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::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; + + SphereShape3DSW(); +}; + +class BoxShape3DSW : public Shape3DSW { + + 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 PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::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; + + BoxShape3DSW(); +}; + +class CapsuleShape3DSW : public Shape3DSW { + + real_t height; + real_t radius; + + void _setup(real_t p_height, real_t p_radius); + +public: + _FORCE_INLINE_ real_t get_height() const { return height; } + _FORCE_INLINE_ real_t get_radius() const { return radius; } + + virtual real_t get_area() const { return 4.0 / 3.0 * Math_PI * radius * radius * radius + height * Math_PI * radius * radius; } + + virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_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; + + CapsuleShape3DSW(); +}; + +struct ConvexPolygonShape3DSW : public Shape3DSW { + + Geometry::MeshData mesh; + + void _setup(const Vector<Vector3> &p_vertices); + +public: + const Geometry::MeshData &get_mesh() const { return mesh; } + + virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_CONVEX_POLYGON; } + + virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; + virtual Vector3 get_support(const Vector3 &p_normal) const; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const; + virtual 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; + + ConvexPolygonShape3DSW(); +}; + +struct _VolumeSW_BVH; +struct FaceShape3DSW; + +struct ConcavePolygonShape3DSW : public ConcaveShape3DSW { + // 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; + FaceShape3DSW *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 PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::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; + + ConcavePolygonShape3DSW(); +}; + +struct HeightMapShape3DSW : public ConcaveShape3DSW { + + 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 PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::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; + + HeightMapShape3DSW(); +}; + +//used internally +struct FaceShape3DSW : public Shape3DSW { + + Vector3 normal; //cache + Vector3 vertex[3]; + + virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::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(); } + + FaceShape3DSW(); +}; + +struct MotionShape3DSW : public Shape3DSW { + + Shape3DSW *shape; + Vector3 motion; + + virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::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(); } + + MotionShape3DSW() { configure(AABB()); } +}; + +#endif // SHAPE_SW_H diff --git a/servers/physics_3d/space_3d_sw.cpp b/servers/physics_3d/space_3d_sw.cpp new file mode 100644 index 0000000000..bd8e89a8f5 --- /dev/null +++ b/servers/physics_3d/space_3d_sw.cpp @@ -0,0 +1,1242 @@ +/*************************************************************************/ +/* space_3d_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_3d_sw.h" + +#include "collision_solver_3d_sw.h" +#include "core/project_settings.h" +#include "physics_server_3d_sw.h" + +_FORCE_INLINE_ static bool _can_collide_with(CollisionObject3DSW *p_object, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { + + if (!(p_object->get_collision_layer() & p_collision_mask)) { + return false; + } + + if (p_object->get_type() == CollisionObject3DSW::TYPE_AREA && !p_collide_with_areas) + return false; + + if (p_object->get_type() == CollisionObject3DSW::TYPE_BODY && !p_collide_with_bodies) + return false; + + return true; +} + +int PhysicsDirectSpaceState3DSW::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, Space3DSW::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 CollisionObject3DSW *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 = nullptr; + r_results[cc].rid = col_obj->get_self(); + r_results[cc].shape = shape_idx; + + cc++; + } + + return cc; +} + +bool PhysicsDirectSpaceState3DSW::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, Space3DSW::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 CollisionObject3DSW *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 CollisionObject3DSW *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 Shape3DSW *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 = nullptr; + 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 PhysicsDirectSpaceState3DSW::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; + + Shape3DSW *shape = static_cast<PhysicsServer3DSW *>(PhysicsServer3D::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, Space3DSW::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 CollisionObject3DSW *col_obj = space->intersection_query_results[i]; + int shape_idx = space->intersection_query_subindex_results[i]; + + if (!CollisionSolver3DSW::solve_static(shape, p_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), nullptr, nullptr, nullptr, p_margin, 0)) + continue; + + 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 = nullptr; + r_results[cc].rid = col_obj->get_self(); + r_results[cc].shape = shape_idx; + } + + cc++; + } + + return cc; +} + +bool PhysicsDirectSpaceState3DSW::cast_motion(const RID &p_shape, const Transform &p_xform, const Vector3 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, ShapeRestInfo *r_info) { + + Shape3DSW *shape = static_cast<PhysicsServer3DSW *>(PhysicsServer3D::get_singleton())->shape_owner.getornull(p_shape); + 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, Space3DSW::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(); + MotionShape3DSW 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 CollisionObject3DSW *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 (CollisionSolver3DSW::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 (!CollisionSolver3DSW::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 = !CollisionSolver3DSW::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() == CollisionObject3DSW::TYPE_BODY) { + const Body3DSW *body = static_cast<const Body3DSW *>(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 PhysicsDirectSpaceState3DSW::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; + + Shape3DSW *shape = static_cast<PhysicsServer3DSW *>(PhysicsServer3D::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, Space3DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); + + bool collided = false; + r_result_count = 0; + + PhysicsServer3DSW::CollCbkData cbk; + cbk.max = p_result_max; + cbk.amount = 0; + cbk.ptr = r_results; + CollisionSolver3DSW::CallbackResult cbkres = PhysicsServer3DSW::_shape_col_cbk; + + PhysicsServer3DSW::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 CollisionObject3DSW *col_obj = space->intersection_query_results[i]; + int shape_idx = space->intersection_query_subindex_results[i]; + + if (p_exclude.has(col_obj->get_self())) { + continue; + } + + if (CollisionSolver3DSW::solve_static(shape, p_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, nullptr, p_margin)) { + collided = true; + } + } + + r_result_count = cbk.amount; + + return collided; +} + +struct _RestCallbackData { + + const CollisionObject3DSW *object; + const CollisionObject3DSW *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 PhysicsDirectSpaceState3DSW::rest_info(RID p_shape, const Transform &p_shape_xform, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { + + Shape3DSW *shape = static_cast<PhysicsServer3DSW *>(PhysicsServer3D::get_singleton())->shape_owner.getornull(p_shape); + 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, Space3DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); + + _RestCallbackData rcd; + rcd.best_len = 0; + rcd.best_object = nullptr; + 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 CollisionObject3DSW *col_obj = space->intersection_query_results[i]; + int shape_idx = space->intersection_query_subindex_results[i]; + + if (p_exclude.has(col_obj->get_self())) + continue; + + rcd.object = col_obj; + rcd.shape = shape_idx; + bool sc = CollisionSolver3DSW::solve_static(shape, p_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), _rest_cbk_result, &rcd, nullptr, p_margin); + 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() == CollisionObject3DSW::TYPE_BODY) { + + const Body3DSW *body = static_cast<const Body3DSW *>(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 PhysicsDirectSpaceState3DSW::get_closest_point_to_object_volume(RID p_object, const Vector3 p_point) const { + + CollisionObject3DSW *obj = PhysicsServer3DSW::singleton->area_owner.getornull(p_object); + if (!obj) { + obj = PhysicsServer3DSW::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); + Shape3DSW *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; + } +} + +PhysicsDirectSpaceState3DSW::PhysicsDirectSpaceState3DSW() { + + space = nullptr; +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////// + +int Space3DSW::_cull_aabb_for_body(Body3DSW *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() == CollisionObject3DSW::TYPE_AREA) + keep = false; + else if ((static_cast<Body3DSW *>(intersection_query_results[i])->test_collision_mask(p_body)) == 0) + keep = false; + else if (static_cast<Body3DSW *>(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<Body3DSW *>(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 Space3DSW::test_body_ray_separation(Body3DSW *p_body, const Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, PhysicsServer3D::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]; + PhysicsServer3DSW::CollCbkData cbk; + cbk.max = max_results; + PhysicsServer3DSW::CollCbkData *cbkptr = &cbk; + CollisionSolver3DSW::CallbackResult cbkres = PhysicsServer3DSW::_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; + + Shape3DSW *body_shape = p_body->get_shape(j); + + if (body_shape->get_type() != PhysicsServer3D::SHAPE_RAY) + continue; + + Transform body_shape_xform = body_transform * p_body->get_shape_transform(j); + + for (int i = 0; i < amount; i++) { + + const CollisionObject3DSW *col_obj = intersection_query_results[i]; + int shape_idx = intersection_query_subindex_results[i]; + + cbk.amount = 0; + cbk.ptr = sr; + + if (CollisionObject3DSW::TYPE_BODY == col_obj->get_type()) { + const Body3DSW *b = static_cast<const Body3DSW *>(col_obj); + if (p_infinite_inertia && PhysicsServer3D::BODY_MODE_STATIC != b->get_mode() && PhysicsServer3D::BODY_MODE_KINEMATIC != b->get_mode()) { + continue; + } + } + + Shape3DSW *against_shape = col_obj->get_shape(shape_idx); + if (CollisionSolver3DSW::solve_static(body_shape, body_shape_xform, against_shape, col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, nullptr, p_margin)) { + if (cbk.amount > 0) { + collided = true; + } + + int ray_index = -1; //reuse shape + for (int k = 0; k < rays_found; k++) { + if (r_results[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) { + PhysicsServer3D::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() == CollisionObject3DSW::TYPE_BODY) { + Body3DSW *body = (Body3DSW *)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 Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, real_t p_margin, PhysicsServer3D::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 = PhysicsServer3D::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 { + + PhysicsServer3DSW::CollCbkData cbk; + cbk.max = max_results; + cbk.amount = 0; + cbk.ptr = sr; + + PhysicsServer3DSW::CollCbkData *cbkptr = &cbk; + CollisionSolver3DSW::CallbackResult cbkres = PhysicsServer3DSW::_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); + Shape3DSW *body_shape = p_body->get_shape(j); + if (p_exclude_raycast_shapes && body_shape->get_type() == PhysicsServer3D::SHAPE_RAY) { + continue; + } + + for (int i = 0; i < amount; i++) { + + const CollisionObject3DSW *col_obj = intersection_query_results[i]; + int shape_idx = intersection_query_subindex_results[i]; + + if (CollisionSolver3DSW::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, nullptr, 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); + Shape3DSW *body_shape = p_body->get_shape(j); + + if (p_exclude_raycast_shapes && body_shape->get_type() == PhysicsServer3D::SHAPE_RAY) { + continue; + } + + Transform body_shape_xform_inv = body_shape_xform.affine_inverse(); + MotionShape3DSW 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 CollisionObject3DSW *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 (CollisionSolver3DSW::solve_distance(&mshape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, motion_aabb, &sep_axis)) { + continue; + } + sep_axis = p_motion.normalized(); + + if (!CollisionSolver3DSW::solve_distance(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, motion_aabb, &sep_axis)) { + stuck = true; + 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 = !CollisionSolver3DSW::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 = nullptr; + rcd.best_shape = 0; + rcd.min_allowed_depth = test_motion_min_contact_depth; + + Transform body_shape_xform = ugt * p_body->get_shape_transform(best_shape); + Shape3DSW *body_shape = p_body->get_shape(best_shape); + + body_aabb.position += p_motion * unsafe; + + int amount = _cull_aabb_for_body(p_body, body_aabb); + + for (int i = 0; i < amount; i++) { + + const CollisionObject3DSW *col_obj = intersection_query_results[i]; + int shape_idx = intersection_query_subindex_results[i]; + + rcd.object = col_obj; + rcd.shape = shape_idx; + bool sc = CollisionSolver3DSW::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), _rest_cbk_result, &rcd, nullptr, p_margin); + if (!sc) + continue; + } + + if (rcd.best_len != 0) { + + 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 Body3DSW *body = static_cast<const Body3DSW *>(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 *Space3DSW::_broadphase_pair(CollisionObject3DSW *A, int p_subindex_A, CollisionObject3DSW *B, int p_subindex_B, void *p_self) { + + CollisionObject3DSW::Type type_A = A->get_type(); + CollisionObject3DSW::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); + } + + Space3DSW *self = (Space3DSW *)p_self; + + self->collision_pairs++; + + if (type_A == CollisionObject3DSW::TYPE_AREA) { + + Area3DSW *area = static_cast<Area3DSW *>(A); + if (type_B == CollisionObject3DSW::TYPE_AREA) { + + Area3DSW *area_b = static_cast<Area3DSW *>(B); + Area2Pair3DSW *area2_pair = memnew(Area2Pair3DSW(area_b, p_subindex_B, area, p_subindex_A)); + return area2_pair; + } else { + + Body3DSW *body = static_cast<Body3DSW *>(B); + AreaPair3DSW *area_pair = memnew(AreaPair3DSW(body, p_subindex_B, area, p_subindex_A)); + return area_pair; + } + } else { + + BodyPair3DSW *b = memnew(BodyPair3DSW((Body3DSW *)A, p_subindex_A, (Body3DSW *)B, p_subindex_B)); + return b; + } + + return nullptr; +} + +void Space3DSW::_broadphase_unpair(CollisionObject3DSW *A, int p_subindex_A, CollisionObject3DSW *B, int p_subindex_B, void *p_data, void *p_self) { + + Space3DSW *self = (Space3DSW *)p_self; + self->collision_pairs--; + Constraint3DSW *c = (Constraint3DSW *)p_data; + memdelete(c); +} + +const SelfList<Body3DSW>::List &Space3DSW::get_active_body_list() const { + + return active_list; +} +void Space3DSW::body_add_to_active_list(SelfList<Body3DSW> *p_body) { + + active_list.add(p_body); +} +void Space3DSW::body_remove_from_active_list(SelfList<Body3DSW> *p_body) { + + active_list.remove(p_body); +} + +void Space3DSW::body_add_to_inertia_update_list(SelfList<Body3DSW> *p_body) { + + inertia_update_list.add(p_body); +} + +void Space3DSW::body_remove_from_inertia_update_list(SelfList<Body3DSW> *p_body) { + + inertia_update_list.remove(p_body); +} + +BroadPhase3DSW *Space3DSW::get_broadphase() { + + return broadphase; +} + +void Space3DSW::add_object(CollisionObject3DSW *p_object) { + + ERR_FAIL_COND(objects.has(p_object)); + objects.insert(p_object); +} + +void Space3DSW::remove_object(CollisionObject3DSW *p_object) { + + ERR_FAIL_COND(!objects.has(p_object)); + objects.erase(p_object); +} + +const Set<CollisionObject3DSW *> &Space3DSW::get_objects() const { + + return objects; +} + +void Space3DSW::body_add_to_state_query_list(SelfList<Body3DSW> *p_body) { + + state_query_list.add(p_body); +} +void Space3DSW::body_remove_from_state_query_list(SelfList<Body3DSW> *p_body) { + + state_query_list.remove(p_body); +} + +void Space3DSW::area_add_to_monitor_query_list(SelfList<Area3DSW> *p_area) { + + monitor_query_list.add(p_area); +} +void Space3DSW::area_remove_from_monitor_query_list(SelfList<Area3DSW> *p_area) { + + monitor_query_list.remove(p_area); +} + +void Space3DSW::area_add_to_moved_list(SelfList<Area3DSW> *p_area) { + + area_moved_list.add(p_area); +} + +void Space3DSW::area_remove_from_moved_list(SelfList<Area3DSW> *p_area) { + + area_moved_list.remove(p_area); +} + +const SelfList<Area3DSW>::List &Space3DSW::get_moved_area_list() const { + + return area_moved_list; +} + +void Space3DSW::call_queries() { + + while (state_query_list.first()) { + + Body3DSW *b = state_query_list.first()->self(); + state_query_list.remove(state_query_list.first()); + b->call_queries(); + } + + while (monitor_query_list.first()) { + + Area3DSW *a = monitor_query_list.first()->self(); + monitor_query_list.remove(monitor_query_list.first()); + a->call_queries(); + } +} + +void Space3DSW::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 Space3DSW::update() { + + broadphase->update(); +} + +void Space3DSW::set_param(PhysicsServer3D::SpaceParameter p_param, real_t p_value) { + + switch (p_param) { + + case PhysicsServer3D::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: contact_recycle_radius = p_value; break; + case PhysicsServer3D::SPACE_PARAM_CONTACT_MAX_SEPARATION: contact_max_separation = p_value; break; + case PhysicsServer3D::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: contact_max_allowed_penetration = p_value; break; + case PhysicsServer3D::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD: body_linear_velocity_sleep_threshold = p_value; break; + case PhysicsServer3D::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD: body_angular_velocity_sleep_threshold = p_value; break; + case PhysicsServer3D::SPACE_PARAM_BODY_TIME_TO_SLEEP: body_time_to_sleep = p_value; break; + case PhysicsServer3D::SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO: body_angular_velocity_damp_ratio = p_value; break; + case PhysicsServer3D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: constraint_bias = p_value; break; + case PhysicsServer3D::SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH: test_motion_min_contact_depth = p_value; break; + } +} + +real_t Space3DSW::get_param(PhysicsServer3D::SpaceParameter p_param) const { + + switch (p_param) { + + case PhysicsServer3D::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: return contact_recycle_radius; + case PhysicsServer3D::SPACE_PARAM_CONTACT_MAX_SEPARATION: return contact_max_separation; + case PhysicsServer3D::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: return contact_max_allowed_penetration; + case PhysicsServer3D::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD: return body_linear_velocity_sleep_threshold; + case PhysicsServer3D::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD: return body_angular_velocity_sleep_threshold; + case PhysicsServer3D::SPACE_PARAM_BODY_TIME_TO_SLEEP: return body_time_to_sleep; + case PhysicsServer3D::SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO: return body_angular_velocity_damp_ratio; + case PhysicsServer3D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: return constraint_bias; + case PhysicsServer3D::SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH: return test_motion_min_contact_depth; + } + return 0; +} + +void Space3DSW::lock() { + + locked = true; +} + +void Space3DSW::unlock() { + + locked = false; +} + +bool Space3DSW::is_locked() const { + + return locked; +} + +PhysicsDirectSpaceState3DSW *Space3DSW::get_direct_state() { + + return direct_access; +} + +Space3DSW::Space3DSW() { + + 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 = BroadPhase3DSW::create_func(); + broadphase->set_pair_callback(_broadphase_pair, this); + broadphase->set_unpair_callback(_broadphase_unpair, this); + area = nullptr; + + direct_access = memnew(PhysicsDirectSpaceState3DSW); + direct_access->space = this; + + for (int i = 0; i < ELAPSED_TIME_MAX; i++) + elapsed_time[i] = 0; +} + +Space3DSW::~Space3DSW() { + + memdelete(broadphase); + memdelete(direct_access); +} diff --git a/servers/physics_3d/space_3d_sw.h b/servers/physics_3d/space_3d_sw.h new file mode 100644 index 0000000000..3634834952 --- /dev/null +++ b/servers/physics_3d/space_3d_sw.h @@ -0,0 +1,208 @@ +/*************************************************************************/ +/* space_3d_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_3d_sw.h" +#include "area_pair_3d_sw.h" +#include "body_3d_sw.h" +#include "body_pair_3d_sw.h" +#include "broad_phase_3d_sw.h" +#include "collision_object_3d_sw.h" +#include "core/hash_map.h" +#include "core/project_settings.h" +#include "core/typedefs.h" + +class PhysicsDirectSpaceState3DSW : public PhysicsDirectSpaceState3D { + + GDCLASS(PhysicsDirectSpaceState3DSW, PhysicsDirectSpaceState3D); + +public: + Space3DSW *space; + + virtual int intersect_point(const Vector3 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false); + virtual bool intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, bool p_pick_ray = false); + virtual int intersect_shape(const RID &p_shape, const Transform &p_xform, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false); + virtual bool cast_motion(const RID &p_shape, const Transform &p_xform, const Vector3 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, ShapeRestInfo *r_info = nullptr); + virtual bool collide_shape(RID p_shape, const Transform &p_shape_xform, real_t p_margin, Vector3 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false); + virtual bool rest_info(RID p_shape, const Transform &p_shape_xform, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false); + virtual Vector3 get_closest_point_to_object_volume(RID p_object, const Vector3 p_point) const; + + PhysicsDirectSpaceState3DSW(); +}; + +class Space3DSW { + +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]; + + PhysicsDirectSpaceState3DSW *direct_access; + RID self; + + BroadPhase3DSW *broadphase; + SelfList<Body3DSW>::List active_list; + SelfList<Body3DSW>::List inertia_update_list; + SelfList<Body3DSW>::List state_query_list; + SelfList<Area3DSW>::List monitor_query_list; + SelfList<Area3DSW>::List area_moved_list; + + static void *_broadphase_pair(CollisionObject3DSW *A, int p_subindex_A, CollisionObject3DSW *B, int p_subindex_B, void *p_self); + static void _broadphase_unpair(CollisionObject3DSW *A, int p_subindex_A, CollisionObject3DSW *B, int p_subindex_B, void *p_data, void *p_self); + + Set<CollisionObject3DSW *> objects; + + Area3DSW *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 + }; + + CollisionObject3DSW *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 PhysicsDirectSpaceState3DSW; + + int _cull_aabb_for_body(Body3DSW *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(Area3DSW *p_area) { area = p_area; } + Area3DSW *get_default_area() const { return area; } + + const SelfList<Body3DSW>::List &get_active_body_list() const; + void body_add_to_active_list(SelfList<Body3DSW> *p_body); + void body_remove_from_active_list(SelfList<Body3DSW> *p_body); + void body_add_to_inertia_update_list(SelfList<Body3DSW> *p_body); + void body_remove_from_inertia_update_list(SelfList<Body3DSW> *p_body); + + void body_add_to_state_query_list(SelfList<Body3DSW> *p_body); + void body_remove_from_state_query_list(SelfList<Body3DSW> *p_body); + + void area_add_to_monitor_query_list(SelfList<Area3DSW> *p_area); + void area_remove_from_monitor_query_list(SelfList<Area3DSW> *p_area); + void area_add_to_moved_list(SelfList<Area3DSW> *p_area); + void area_remove_from_moved_list(SelfList<Area3DSW> *p_area); + const SelfList<Area3DSW>::List &get_moved_area_list() const; + + BroadPhase3DSW *get_broadphase(); + + void add_object(CollisionObject3DSW *p_object); + void remove_object(CollisionObject3DSW *p_object); + const Set<CollisionObject3DSW *> &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(PhysicsServer3D::SpaceParameter p_param, real_t p_value); + real_t get_param(PhysicsServer3D::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; } + + PhysicsDirectSpaceState3DSW *get_direct_state(); + + void set_debug_contacts(int p_amount) { contact_debug.resize(p_amount); } + _FORCE_INLINE_ bool is_debugging_contacts() const { return !contact_debug.empty(); } + _FORCE_INLINE_ 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(Body3DSW *p_body, const Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, PhysicsServer3D::SeparationResult *r_results, int p_result_max, real_t p_margin); + bool test_body_motion(Body3DSW *p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, real_t p_margin, PhysicsServer3D::MotionResult *r_result, bool p_exclude_raycast_shapes); + + Space3DSW(); + ~Space3DSW(); +}; + +#endif // SPACE__SW_H diff --git a/servers/physics_3d/step_3d_sw.cpp b/servers/physics_3d/step_3d_sw.cpp new file mode 100644 index 0000000000..1a7d5f8cec --- /dev/null +++ b/servers/physics_3d/step_3d_sw.cpp @@ -0,0 +1,299 @@ +/*************************************************************************/ +/* step_3d_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_3d_sw.h" +#include "joints_3d_sw.h" + +#include "core/os/os.h" + +void Step3DSW::_populate_island(Body3DSW *p_body, Body3DSW **p_island, Constraint3DSW **p_constraint_island) { + + p_body->set_island_step(_step); + p_body->set_island_next(*p_island); + *p_island = p_body; + + for (Map<Constraint3DSW *, int>::Element *E = p_body->get_constraint_map().front(); E; E = E->next()) { + + Constraint3DSW *c = (Constraint3DSW *)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; + Body3DSW *b = c->get_body_ptr()[i]; + if (b->get_island_step() == _step || b->get_mode() == PhysicsServer3D::BODY_MODE_STATIC || b->get_mode() == PhysicsServer3D::BODY_MODE_KINEMATIC) + continue; //no go + _populate_island(c->get_body_ptr()[i], p_island, p_constraint_island); + } + } +} + +void Step3DSW::_setup_island(Constraint3DSW *p_island, real_t p_delta) { + + Constraint3DSW *ci = p_island; + while (ci) { + ci->setup(p_delta); + //todo remove from island if process fails + ci = ci->get_island_next(); + } +} + +void Step3DSW::_solve_island(Constraint3DSW *p_island, int p_iterations, real_t p_delta) { + + int at_priority = 1; + + while (p_island) { + + for (int i = 0; i < p_iterations; i++) { + + Constraint3DSW *ci = p_island; + while (ci) { + ci->solve(p_delta); + ci = ci->get_island_next(); + } + } + + at_priority++; + + { + Constraint3DSW *ci = p_island; + Constraint3DSW *prev = nullptr; + 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 Step3DSW::_check_suspend(Body3DSW *p_island, real_t p_delta) { + + bool can_sleep = true; + + Body3DSW *b = p_island; + while (b) { + + if (b->get_mode() == PhysicsServer3D::BODY_MODE_STATIC || b->get_mode() == PhysicsServer3D::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() == PhysicsServer3D::BODY_MODE_STATIC || b->get_mode() == PhysicsServer3D::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 Step3DSW::step(Space3DSW *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<Body3DSW>::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<Body3DSW> *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(Space3DSW::ELAPSED_TIME_INTEGRATE_FORCES, profile_endtime - profile_begtime); + profile_begtime = profile_endtime; + } + + /* GENERATE CONSTRAINT ISLANDS */ + + Body3DSW *island_list = nullptr; + Constraint3DSW *constraint_island_list = nullptr; + b = body_list->first(); + + int island_count = 0; + + while (b) { + Body3DSW *body = b->self(); + + if (body->get_island_step() != _step) { + + Body3DSW *island = nullptr; + Constraint3DSW *constraint_island = nullptr; + _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<Area3DSW>::List &aml = p_space->get_moved_area_list(); + + while (aml.first()) { + for (const Set<Constraint3DSW *>::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) { + + Constraint3DSW *c = E->get(); + if (c->get_island_step() == _step) + continue; + c->set_island_step(_step); + c->set_island_next(nullptr); + c->set_island_list_next(constraint_island_list); + constraint_island_list = c; + } + p_space->area_remove_from_moved_list((SelfList<Area3DSW> *)aml.first()); //faster to remove here + } + + { //profile + profile_endtime = OS::get_singleton()->get_ticks_usec(); + p_space->set_elapsed_time(Space3DSW::ELAPSED_TIME_GENERATE_ISLANDS, profile_endtime - profile_begtime); + profile_begtime = profile_endtime; + } + + /* SETUP CONSTRAINT ISLANDS */ + + { + Constraint3DSW *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(Space3DSW::ELAPSED_TIME_SETUP_CONSTRAINTS, profile_endtime - profile_begtime); + profile_begtime = profile_endtime; + } + + /* SOLVE CONSTRAINT ISLANDS */ + + { + Constraint3DSW *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(Space3DSW::ELAPSED_TIME_SOLVE_CONSTRAINTS, profile_endtime - profile_begtime); + profile_begtime = profile_endtime; + } + + /* INTEGRATE VELOCITIES */ + + b = body_list->first(); + while (b) { + const SelfList<Body3DSW> *n = b->next(); + b->self()->integrate_velocities(p_delta); + b = n; + } + + /* SLEEP / WAKE UP ISLANDS */ + + { + Body3DSW *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(Space3DSW::ELAPSED_TIME_INTEGRATE_VELOCITIES, profile_endtime - profile_begtime); + profile_begtime = profile_endtime; + } + + p_space->update(); + p_space->unlock(); + _step++; +} + +Step3DSW::Step3DSW() { + + _step = 1; +} diff --git a/servers/physics_3d/step_3d_sw.h b/servers/physics_3d/step_3d_sw.h new file mode 100644 index 0000000000..c735688a9e --- /dev/null +++ b/servers/physics_3d/step_3d_sw.h @@ -0,0 +1,50 @@ +/*************************************************************************/ +/* step_3d_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_3d_sw.h" + +class Step3DSW { + + uint64_t _step; + + void _populate_island(Body3DSW *p_body, Body3DSW **p_island, Constraint3DSW **p_constraint_island); + void _setup_island(Constraint3DSW *p_island, real_t p_delta); + void _solve_island(Constraint3DSW *p_island, int p_iterations, real_t p_delta); + void _check_suspend(Body3DSW *p_island, real_t p_delta); + +public: + void step(Space3DSW *p_space, real_t p_delta, int p_iterations); + Step3DSW(); +}; + +#endif // STEP__SW_H |