diff options
Diffstat (limited to 'servers/physics_2d')
33 files changed, 9573 insertions, 0 deletions
diff --git a/servers/physics_2d/SCsub b/servers/physics_2d/SCsub new file mode 100644 index 0000000000..a2c2b51a61 --- /dev/null +++ b/servers/physics_2d/SCsub @@ -0,0 +1,4 @@ +Import('env') + +env.add_source_files(env.servers_sources,"*.cpp") + diff --git a/servers/physics_2d/area_2d_sw.cpp b/servers/physics_2d/area_2d_sw.cpp new file mode 100644 index 0000000000..6c391a7aa0 --- /dev/null +++ b/servers/physics_2d/area_2d_sw.cpp @@ -0,0 +1,193 @@ +/*************************************************************************/ +/* area_2d_sw.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#include "area_2d_sw.h" +#include "space_2d_sw.h" +#include "body_2d_sw.h" + +Area2DSW::BodyKey::BodyKey(Body2DSW *p_body, uint32_t p_body_shape,uint32_t p_area_shape) { rid=p_body->get_self(); instance_id=p_body->get_instance_id(); body_shape=p_body_shape; area_shape=p_area_shape; } + +void Area2DSW::_shapes_changed() { + + +} + +void Area2DSW::set_transform(const Matrix32& p_transform) { + + if (!moved_list.in_list() && get_space()) + get_space()->area_add_to_moved_list(&moved_list); + + _set_transform(p_transform); +} + +void Area2DSW::set_space(Space2DSW *p_space) { + + if (get_space()) { + if (monitor_query_list.in_list()) + get_space()->area_remove_from_monitor_query_list(&monitor_query_list); + if (moved_list.in_list()) + get_space()->area_remove_from_moved_list(&moved_list); + + } + + monitored_bodies.clear(); + + _set_space(p_space); +} + + +void Area2DSW::set_monitor_callback(ObjectID p_id, const StringName& p_method) { + + + if (p_id==monitor_callback_id) { + monitor_callback_method=p_method; + return; + } + + _unregister_shapes(); + + monitor_callback_id=p_id; + monitor_callback_method=p_method; + + monitored_bodies.clear(); + + _shape_changed(); + +} + + + +void Area2DSW::set_space_override_mode(Physics2DServer::AreaSpaceOverrideMode p_mode) { + bool do_override=p_mode!=Physics2DServer::AREA_SPACE_OVERRIDE_DISABLED; + if (do_override==space_override_mode!=Physics2DServer::AREA_SPACE_OVERRIDE_DISABLED) + return; + _unregister_shapes(); + space_override_mode=p_mode; + _shape_changed(); +} + +void Area2DSW::set_param(Physics2DServer::AreaParameter p_param, const Variant& p_value) { + + switch(p_param) { + case Physics2DServer::AREA_PARAM_GRAVITY: gravity=p_value; ; break; + case Physics2DServer::AREA_PARAM_GRAVITY_VECTOR: gravity_vector=p_value; ; break; + case Physics2DServer::AREA_PARAM_GRAVITY_IS_POINT: gravity_is_point=p_value; ; break; + case Physics2DServer::AREA_PARAM_GRAVITY_POINT_ATTENUATION: point_attenuation=p_value; ; break; + case Physics2DServer::AREA_PARAM_DENSITY: density=p_value; ; break; + case Physics2DServer::AREA_PARAM_PRIORITY: priority=p_value; ; break; + } + + +} + +Variant Area2DSW::get_param(Physics2DServer::AreaParameter p_param) const { + + + switch(p_param) { + case Physics2DServer::AREA_PARAM_GRAVITY: return gravity; + case Physics2DServer::AREA_PARAM_GRAVITY_VECTOR: return gravity_vector; + case Physics2DServer::AREA_PARAM_GRAVITY_IS_POINT: return gravity_is_point; + case Physics2DServer::AREA_PARAM_GRAVITY_POINT_ATTENUATION: return point_attenuation; + case Physics2DServer::AREA_PARAM_DENSITY: return density; + case Physics2DServer::AREA_PARAM_PRIORITY: return priority; + } + + return Variant(); +} + + +void Area2DSW::_queue_monitor_update() { + + ERR_FAIL_COND(!get_space()); + + if (!monitor_query_list.in_list()) + get_space()->area_add_to_monitor_query_list(&monitor_query_list); + + +} + +void Area2DSW::call_queries() { + + if (monitor_callback_id && !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=0; + 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 ? Physics2DServer::AREA_BODY_ADDED : Physics2DServer::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; + + Variant::CallError ce; + obj->call(monitor_callback_method,(const Variant**)resptr,5,ce); + } + } + + monitored_bodies.clear(); + + //get_space()->area_remove_from_monitor_query_list(&monitor_query_list); + +} + +Area2DSW::Area2DSW() : CollisionObject2DSW(TYPE_AREA), monitor_query_list(this), moved_list(this) { + + _set_static(true); //areas are never active + space_override_mode=Physics2DServer::AREA_SPACE_OVERRIDE_DISABLED; + gravity=9.80665; + gravity_vector=Vector2(0,-1); + gravity_is_point=false; + point_attenuation=1; + density=0.1; + priority=0; + monitor_callback_id=0; + + +} + +Area2DSW::~Area2DSW() { + + +} + diff --git a/servers/physics_2d/area_2d_sw.h b/servers/physics_2d/area_2d_sw.h new file mode 100644 index 0000000000..51e6ccd166 --- /dev/null +++ b/servers/physics_2d/area_2d_sw.h @@ -0,0 +1,172 @@ +/*************************************************************************/ +/* area_2d_sw.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#ifndef AREA_2D_SW_H +#define AREA_2D_SW_H + +#include "servers/physics_2d_server.h" +#include "collision_object_2d_sw.h" +#include "self_list.h" +//#include "servers/physics/query_sw.h" + +class Space2DSW; +class Body2DSW; +class Constraint2DSW; + + +class Area2DSW : public CollisionObject2DSW{ + + + Physics2DServer::AreaSpaceOverrideMode space_override_mode; + float gravity; + Vector2 gravity_vector; + bool gravity_is_point; + float point_attenuation; + float density; + int priority; + + ObjectID monitor_callback_id; + StringName monitor_callback_method; + + SelfList<Area2DSW> monitor_query_list; + SelfList<Area2DSW> 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.area_shape; + } else + return rid < p_key.rid; + + } + + _FORCE_INLINE_ BodyKey() {} + BodyKey(Body2DSW *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; + + //virtual void shape_changed_notify(Shape2DSW *p_shape); + //virtual void shape_deleted_notify(Shape2DSW *p_shape); + Set<Constraint2DSW*> constraints; + + + virtual void _shapes_changed(); + void _queue_monitor_update(); + +public: + + //_FORCE_INLINE_ const Matrix32& 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; } + + _FORCE_INLINE_ void add_body_to_query(Body2DSW *p_body, uint32_t p_body_shape,uint32_t p_area_shape); + _FORCE_INLINE_ void remove_body_from_query(Body2DSW *p_body, uint32_t p_body_shape,uint32_t p_area_shape); + + void set_param(Physics2DServer::AreaParameter p_param, const Variant& p_value); + Variant get_param(Physics2DServer::AreaParameter p_param) const; + + void set_space_override_mode(Physics2DServer::AreaSpaceOverrideMode p_mode); + Physics2DServer::AreaSpaceOverrideMode get_space_override_mode() const { return space_override_mode; } + + _FORCE_INLINE_ void set_gravity(float p_gravity) { gravity=p_gravity; } + _FORCE_INLINE_ float get_gravity() const { return gravity; } + + _FORCE_INLINE_ void set_gravity_vector(const Vector2& p_gravity) { gravity_vector=p_gravity; } + _FORCE_INLINE_ Vector2 get_gravity_vector() const { return gravity_vector; } + + _FORCE_INLINE_ void set_gravity_as_point(bool p_enable) { gravity_is_point=p_enable; } + _FORCE_INLINE_ bool is_gravity_point() const { return gravity_is_point; } + + _FORCE_INLINE_ void set_point_attenuation(float p_point_attenuation) { point_attenuation=p_point_attenuation; } + _FORCE_INLINE_ float get_point_attenuation() const { return point_attenuation; } + + _FORCE_INLINE_ void set_density(float p_density) { density=p_density; } + _FORCE_INLINE_ float get_density() const { return density; } + + _FORCE_INLINE_ void set_priority(int p_priority) { priority=p_priority; } + _FORCE_INLINE_ int get_priority() const { return priority; } + + _FORCE_INLINE_ void add_constraint( Constraint2DSW* p_constraint) { constraints.insert(p_constraint); } + _FORCE_INLINE_ void remove_constraint( Constraint2DSW* p_constraint) { constraints.erase(p_constraint); } + _FORCE_INLINE_ const Set<Constraint2DSW*>& get_constraints() const { return constraints; } + + void set_transform(const Matrix32& p_transform); + + void set_space(Space2DSW *p_space); + + + void call_queries(); + + Area2DSW(); + ~Area2DSW(); +}; + +void Area2DSW::add_body_to_query(Body2DSW *p_body, uint32_t p_body_shape,uint32_t p_area_shape) { + + BodyKey bk(p_body,p_body_shape,p_area_shape); + monitored_bodies[bk].inc(); + if (!monitor_query_list.in_list()) + _queue_monitor_update(); +} +void Area2DSW::remove_body_from_query(Body2DSW *p_body, uint32_t p_body_shape,uint32_t p_area_shape) { + + BodyKey bk(p_body,p_body_shape,p_area_shape); + monitored_bodies[bk].dec(); + if (!monitor_query_list.in_list()) + _queue_monitor_update(); +} + + + + + + +#endif // AREA_2D_SW_H diff --git a/servers/physics_2d/area_pair_2d_sw.cpp b/servers/physics_2d/area_pair_2d_sw.cpp new file mode 100644 index 0000000000..da9a02922c --- /dev/null +++ b/servers/physics_2d/area_pair_2d_sw.cpp @@ -0,0 +1,94 @@ +/*************************************************************************/ +/* area_pair_2d_sw.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#include "area_pair_2d_sw.h" +#include "collision_solver_2d_sw.h" + + +bool AreaPair2DSW::setup(float p_step) { + + bool result = CollisionSolver2DSW::solve_static(body->get_shape(body_shape),body->get_transform() * body->get_shape_transform(body_shape),body->get_shape_inv_transform(body_shape) * body->get_inv_transform(),area->get_shape(area_shape),area->get_transform() * area->get_shape_transform(area_shape),area->get_shape_inv_transform(area_shape) * area->get_inv_transform(),NULL,this); + + if (result!=colliding) { + + if (result) { + + if (area->get_space_override_mode()!=Physics2DServer::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()!=Physics2DServer::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 AreaPair2DSW::solve(float p_step) { + + +} + + +AreaPair2DSW::AreaPair2DSW(Body2DSW *p_body,int p_body_shape, Area2DSW *p_area,int p_area_shape) { + + + body=p_body; + area=p_area; + body_shape=p_body_shape; + area_shape=p_area_shape; + colliding=false; + body->add_constraint(this,0); + area->add_constraint(this); + +} + +AreaPair2DSW::~AreaPair2DSW() { + + if (colliding) { + + if (area->get_space_override_mode()!=Physics2DServer::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); +} diff --git a/servers/physics_2d/area_pair_2d_sw.h b/servers/physics_2d/area_pair_2d_sw.h new file mode 100644 index 0000000000..5dad8c7a12 --- /dev/null +++ b/servers/physics_2d/area_pair_2d_sw.h @@ -0,0 +1,53 @@ +/*************************************************************************/ +/* area_pair_2d_sw.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#ifndef AREA_PAIR_2D_SW_H +#define AREA_PAIR_2D_SW_H + +#include "constraint_2d_sw.h" +#include "body_2d_sw.h" +#include "area_2d_sw.h" + +class AreaPair2DSW : public Constraint2DSW { + + Body2DSW *body; + Area2DSW *area; + int body_shape; + int area_shape; + bool colliding; +public: + + bool setup(float p_step); + void solve(float p_step); + + AreaPair2DSW(Body2DSW *p_body,int p_body_shape, Area2DSW *p_area,int p_area_shape); + ~AreaPair2DSW(); +}; + +#endif // AREA_PAIR_2D_SW_H + diff --git a/servers/physics_2d/body_2d_sw.cpp b/servers/physics_2d/body_2d_sw.cpp new file mode 100644 index 0000000000..f0e96ae5a6 --- /dev/null +++ b/servers/physics_2d/body_2d_sw.cpp @@ -0,0 +1,609 @@ +/*************************************************************************/ +/* body_2d_sw.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#include "body_2d_sw.h"
+#include "space_2d_sw.h"
+#include "area_2d_sw.h"
+
+void Body2DSW::_update_inertia() {
+
+ if (get_space() && !inertia_update_list.in_list())
+ get_space()->body_add_to_inertia_update_list(&inertia_update_list);
+
+}
+
+
+
+void Body2DSW::update_inertias() {
+
+ //update shapes and motions
+
+ switch(mode) {
+
+ case Physics2DServer::BODY_MODE_RIGID: {
+
+ //update tensor for allshapes, not the best way but should be somehow OK. (inspired from bullet)
+ float total_area=0;
+
+ for (int i=0;i<get_shape_count();i++) {
+
+ total_area+=get_shape_aabb(i).get_area();
+ }
+
+ real_t _inertia=0;
+
+ for (int i=0;i<get_shape_count();i++) {
+
+ const Shape2DSW* shape=get_shape(i);
+
+ float area=get_shape_aabb(i).get_area();
+
+ float mass = area * this->mass / total_area;
+
+ _inertia += shape->get_moment_of_inertia(mass) + mass * get_shape_transform(i).get_origin().length_squared();
+
+ }
+
+ if (_inertia!=0)
+ _inv_inertia=1.0/_inertia;
+ else
+ _inv_inertia=0.0; //wathever
+
+ if (mass)
+ _inv_mass=1.0/mass;
+ else
+ _inv_mass=0;
+
+ } break;
+ case Physics2DServer::BODY_MODE_STATIC_ACTIVE:
+ case Physics2DServer::BODY_MODE_STATIC: {
+
+ _inv_inertia=0;
+ _inv_mass=0;
+ } break;
+ case Physics2DServer::BODY_MODE_CHARACTER: {
+
+ _inv_inertia=0;
+ _inv_mass=1.0/mass;
+
+ } break;
+ }
+ //_update_inertia_tensor();
+
+ //_update_shapes();
+
+}
+
+
+
+void Body2DSW::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==Physics2DServer::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 Body2DSW::set_param(Physics2DServer::BodyParameter p_param, float p_value) {
+
+ switch(p_param) {
+ case Physics2DServer::BODY_PARAM_BOUNCE: {
+
+ bounce=p_value;
+ } break;
+ case Physics2DServer::BODY_PARAM_FRICTION: {
+
+ friction=p_value;
+ } break;
+ case Physics2DServer::BODY_PARAM_MASS: {
+ ERR_FAIL_COND(p_value<=0);
+ mass=p_value;
+ _update_inertia();
+
+ } break;
+ default:{}
+ }
+}
+
+float Body2DSW::get_param(Physics2DServer::BodyParameter p_param) const {
+
+ switch(p_param) {
+ case Physics2DServer::BODY_PARAM_BOUNCE: {
+
+ return bounce;
+ } break;
+ case Physics2DServer::BODY_PARAM_FRICTION: {
+
+ return friction;
+ } break;
+ case Physics2DServer::BODY_PARAM_MASS: {
+ return mass;
+ } break;
+ default:{}
+ }
+
+ return 0;
+}
+
+void Body2DSW::set_mode(Physics2DServer::BodyMode p_mode) {
+
+ mode=p_mode;
+
+ switch(p_mode) {
+ //CLEAR UP EVERYTHING IN CASE IT NOT WORKS!
+ case Physics2DServer::BODY_MODE_STATIC:
+ case Physics2DServer::BODY_MODE_STATIC_ACTIVE: {
+
+ _set_inv_transform(get_transform().affine_inverse());
+ _inv_mass=0;
+ _set_static(p_mode==Physics2DServer::BODY_MODE_STATIC);
+ set_active(p_mode==Physics2DServer::BODY_MODE_STATIC_ACTIVE);
+ linear_velocity=Vector2();
+ angular_velocity=0;
+ } break;
+ case Physics2DServer::BODY_MODE_RIGID: {
+
+ _inv_mass=mass>0?(1.0/mass):0;
+ _set_static(false);
+ simulated_motion=false; //jic
+
+ } break;
+ case Physics2DServer::BODY_MODE_CHARACTER: {
+
+ _inv_mass=mass>0?(1.0/mass):0;
+ _set_static(false);
+ simulated_motion=false; //jic
+ } break;
+ }
+
+ _update_inertia();
+ //if (get_space())
+// _update_queries();
+
+}
+Physics2DServer::BodyMode Body2DSW::get_mode() const {
+
+ return mode;
+}
+
+void Body2DSW::_shapes_changed() {
+
+ _update_inertia();
+ wakeup_neighbours();
+}
+
+void Body2DSW::set_state(Physics2DServer::BodyState p_state, const Variant& p_variant) {
+
+ switch(p_state) {
+ case Physics2DServer::BODY_STATE_TRANSFORM: {
+
+
+ if (mode==Physics2DServer::BODY_MODE_STATIC || mode==Physics2DServer::BODY_MODE_STATIC_ACTIVE) {
+ _set_transform(p_variant);
+ _set_inv_transform(get_transform().affine_inverse());
+ wakeup_neighbours();
+ } else {
+ Matrix32 t = p_variant;
+ t.orthonormalize();
+ _set_transform(t);
+ _set_inv_transform(get_transform().inverse());
+
+ }
+
+ } break;
+ case Physics2DServer::BODY_STATE_LINEAR_VELOCITY: {
+
+ //if (mode==Physics2DServer::BODY_MODE_STATIC)
+ // break;
+ linear_velocity=p_variant;
+
+ } break;
+ case Physics2DServer::BODY_STATE_ANGULAR_VELOCITY: {
+ //if (mode!=Physics2DServer::BODY_MODE_RIGID)
+ // break;
+ angular_velocity=p_variant;
+
+ } break;
+ case Physics2DServer::BODY_STATE_SLEEPING: {
+ //?
+ if (mode==Physics2DServer::BODY_MODE_STATIC || mode==Physics2DServer::BODY_MODE_STATIC_ACTIVE)
+ break;
+ bool do_sleep=p_variant;
+ if (do_sleep) {
+ linear_velocity=Vector2();
+ //biased_linear_velocity=Vector3();
+ angular_velocity=0;
+ //biased_angular_velocity=Vector3();
+ set_active(false);
+ } else {
+ if (mode!=Physics2DServer::BODY_MODE_STATIC)
+ set_active(true);
+ }
+ } break;
+ case Physics2DServer::BODY_STATE_CAN_SLEEP: {
+ can_sleep=p_variant;
+ if (mode==Physics2DServer::BODY_MODE_RIGID && !active && !can_sleep)
+ set_active(true);
+
+ } break;
+ }
+
+}
+Variant Body2DSW::get_state(Physics2DServer::BodyState p_state) const {
+
+ switch(p_state) {
+ case Physics2DServer::BODY_STATE_TRANSFORM: {
+ return get_transform();
+ } break;
+ case Physics2DServer::BODY_STATE_LINEAR_VELOCITY: {
+ return linear_velocity;
+ } break;
+ case Physics2DServer::BODY_STATE_ANGULAR_VELOCITY: {
+ return angular_velocity;
+ } break;
+ case Physics2DServer::BODY_STATE_SLEEPING: {
+ return !is_active();
+ } break;
+ case Physics2DServer::BODY_STATE_CAN_SLEEP: {
+ return can_sleep;
+ } break;
+ }
+
+ return Variant();
+}
+
+
+void Body2DSW::set_space(Space2DSW *p_space){
+
+ if (get_space()) {
+
+ wakeup_neighbours();
+
+ 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);
+ //}
+
+ }
+
+}
+
+void Body2DSW::_compute_area_gravity(const Area2DSW *p_area) {
+
+ if (p_area->is_gravity_point()) {
+
+ gravity = (p_area->get_transform().get_origin()+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();
+ }
+}
+
+void Body2DSW::integrate_forces(real_t p_step) {
+
+ if (mode==Physics2DServer::BODY_MODE_STATIC || mode==Physics2DServer::BODY_MODE_STATIC_ACTIVE)
+ return;
+
+ Area2DSW *current_area = get_space()->get_default_area();
+ ERR_FAIL_COND(!current_area);
+
+ int prio = current_area->get_priority();
+ int ac = areas.size();
+ if (ac) {
+ const AreaCMP *aa = &areas[0];
+ for(int i=0;i<ac;i++) {
+ if (aa[i].area->get_priority() > prio) {
+ current_area=aa[i].area;
+ prio=current_area->get_priority();
+ }
+ }
+ }
+
+ _compute_area_gravity(current_area);
+ density=current_area->get_density();
+
+ if (!omit_force_integration) {
+ //overriden by direct state query
+
+ Vector2 force=gravity*mass;
+ force+=applied_force;
+ real_t torque=applied_torque;
+
+ real_t damp = 1.0 - p_step * density;
+
+ if (damp<0) // reached zero in the given time
+ damp=0;
+
+ real_t angular_damp = 1.0 - p_step * density * get_space()->get_body_angular_velocity_damp_ratio();
+
+ 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 * torque * p_step;
+ }
+
+
+ //motion=linear_velocity*p_step;
+
+ biased_angular_velocity=0;
+ biased_linear_velocity=Vector2();
+
+ if (continuous_cd) //shapes temporarily extend for raycast
+ _update_shapes_with_motion(linear_velocity*p_step);
+
+ current_area=NULL; // clear the area, so it is set in the next frame
+ contact_count=0;
+
+}
+
+void Body2DSW::integrate_velocities(real_t p_step) {
+
+ if (mode==Physics2DServer::BODY_MODE_STATIC)
+ return;
+ if (mode==Physics2DServer::BODY_MODE_STATIC_ACTIVE) {
+ if (fi_callback)
+ get_space()->body_add_to_state_query_list(&direct_state_query_list);
+ return;
+ }
+
+ real_t total_angular_velocity = angular_velocity+biased_angular_velocity;
+ Vector2 total_linear_velocity=linear_velocity+biased_linear_velocity;
+
+ real_t angle = get_transform().get_rotation() - total_angular_velocity * p_step;
+ Vector2 pos = get_transform().get_origin() + total_linear_velocity * p_step;
+
+
+ _set_transform(Matrix32(angle,pos));
+ _set_inv_transform(get_transform().inverse());
+
+
+ if (fi_callback) {
+
+ get_space()->body_add_to_state_query_list(&direct_state_query_list);
+ }
+
+ //_update_inertia_tensor();
+}
+
+
+void Body2DSW::simulate_motion(const Matrix32& p_xform,real_t p_step) {
+
+ Matrix32 inv_xform = p_xform.affine_inverse();
+ if (!get_space()) {
+ _set_transform(p_xform);
+ _set_inv_transform(inv_xform);
+
+ return;
+ }
+
+
+
+ linear_velocity = (p_xform.elements[2] - get_transform().elements[2])/p_step;
+
+ real_t rot = inv_xform.basis_xform(get_transform().elements[1]).atan2();
+ angular_velocity = rot / 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 Body2DSW::wakeup_neighbours() {
+
+
+
+ for(Map<Constraint2DSW*,int>::Element *E=constraint_map.front();E;E=E->next()) {
+
+ const Constraint2DSW *c=E->key();
+ Body2DSW **n = c->get_body_ptr();
+ int bc=c->get_body_count();
+
+ for(int i=0;i<bc;i++) {
+
+ if (i==E->get())
+ continue;
+ Body2DSW *b = n[i];
+ if (b->mode!=Physics2DServer::BODY_MODE_RIGID)
+ continue;
+
+ if (!b->is_active())
+ b->set_active(true);
+ }
+ }
+}
+
+void Body2DSW::call_queries() {
+
+
+ if (fi_callback) {
+
+ Physics2DDirectBodyStateSW *dbs = Physics2DDirectBodyStateSW::singleton;
+ dbs->body=this;
+
+ Variant v=dbs;
+ const Variant *vp[2]={&v,&fi_callback->callback_udata};
+
+ Object *obj = ObjectDB::get_instance(fi_callback->id);
+ if (!obj) {
+
+ set_force_integration_callback(NULL,StringName());
+ } else {
+ Variant::CallError ce;
+ if (fi_callback->callback_udata.get_type()) {
+
+ obj->call(fi_callback->method,vp,2,ce);
+
+ } else {
+ obj->call(fi_callback->method,vp,1,ce);
+ }
+ }
+
+
+ }
+
+ if (simulated_motion) {
+
+ // linear_velocity=Vector2();
+ // angular_velocity=0;
+ simulated_motion=false;
+ }
+}
+
+
+bool Body2DSW::sleep_test(real_t p_step) {
+
+ if (mode==Physics2DServer::BODY_MODE_STATIC || mode==Physics2DServer::BODY_MODE_STATIC_ACTIVE)
+ return true; //
+ else if (mode==Physics2DServer::BODY_MODE_CHARACTER)
+ return !active; // characters don't sleep unless asked to sleep
+ else if (!can_sleep)
+ return false;
+
+
+
+
+ if (Math::abs(angular_velocity)<get_space()->get_body_angular_velocity_sleep_treshold() && Math::abs(linear_velocity.length_squared()) < get_space()->get_body_linear_velocity_sleep_treshold()*get_space()->get_body_linear_velocity_sleep_treshold()) {
+
+ still_time+=p_step;
+
+ return still_time > get_space()->get_body_time_to_sleep();
+ } else {
+
+ still_time=0; //maybe this should be set to 0 on set_active?
+ return false;
+ }
+}
+
+
+void Body2DSW::set_force_integration_callback(ObjectID p_id,const StringName& p_method,const Variant& p_udata) {
+
+ if (fi_callback) {
+
+ memdelete(fi_callback);
+ fi_callback=NULL;
+ }
+
+
+ if (p_id!=0) {
+
+ fi_callback=memnew(ForceIntegrationCallback);
+ fi_callback->id=p_id;
+ fi_callback->method=p_method;
+ fi_callback->callback_udata=p_udata;
+ }
+
+}
+
+Body2DSW::Body2DSW() : CollisionObject2DSW(TYPE_BODY), active_list(this), inertia_update_list(this), direct_state_query_list(this) {
+
+
+ mode=Physics2DServer::BODY_MODE_RIGID;
+ active=true;
+ angular_velocity=0;
+ biased_angular_velocity=0;
+ mass=1;
+ _inv_inertia=0;
+ _inv_mass=1;
+ bounce=0;
+ friction=1;
+ omit_force_integration=false;
+ applied_torque=0;
+ island_step=0;
+ island_next=NULL;
+ island_list_next=NULL;
+ _set_static(false);
+ density=0;
+ contact_count=0;
+ simulated_motion=false;
+ still_time=0;
+ continuous_cd=false;
+ can_sleep=false;
+ fi_callback=NULL;
+
+}
+
+Body2DSW::~Body2DSW() {
+
+ if (fi_callback)
+ memdelete(fi_callback);
+}
+
+Physics2DDirectBodyStateSW *Physics2DDirectBodyStateSW::singleton=NULL;
+
+Physics2DDirectSpaceState* Physics2DDirectBodyStateSW::get_space_state() {
+
+ return body->get_space()->get_direct_state();
+}
diff --git a/servers/physics_2d/body_2d_sw.h b/servers/physics_2d/body_2d_sw.h new file mode 100644 index 0000000000..55b84ce7a7 --- /dev/null +++ b/servers/physics_2d/body_2d_sw.h @@ -0,0 +1,334 @@ +/*************************************************************************/ +/* body_2d_sw.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#ifndef BODY_2D_SW_H
+#define BODY_2D_SW_H
+
+#include "collision_object_2d_sw.h"
+#include "vset.h"
+#include "area_2d_sw.h"
+
+class Constraint2DSW;
+
+
+class Body2DSW : public CollisionObject2DSW {
+
+
+ Physics2DServer::BodyMode mode;
+
+ Vector2 biased_linear_velocity;
+ real_t biased_angular_velocity;
+
+ Vector2 linear_velocity;
+ real_t angular_velocity;
+
+ real_t mass;
+ real_t bounce;
+ real_t friction;
+
+ real_t _inv_mass;
+ real_t _inv_inertia;
+
+ Vector2 gravity;
+ real_t density;
+
+ real_t still_time;
+
+ Vector2 applied_force;
+ real_t applied_torque;
+
+ SelfList<Body2DSW> active_list;
+ SelfList<Body2DSW> inertia_update_list;
+ SelfList<Body2DSW> direct_state_query_list;
+
+ VSet<RID> exceptions;
+ bool omit_force_integration;
+ bool active;
+ bool simulated_motion;
+ bool continuous_cd;
+ bool can_sleep;
+ void _update_inertia();
+ virtual void _shapes_changed();
+
+
+ Map<Constraint2DSW*,int> constraint_map;
+
+ struct AreaCMP {
+
+ Area2DSW *area;
+ _FORCE_INLINE_ bool operator<(const AreaCMP& p_cmp) const { return area->get_self() < p_cmp.area->get_self() ; }
+ _FORCE_INLINE_ AreaCMP() {}
+ _FORCE_INLINE_ AreaCMP(Area2DSW *p_area) { area=p_area;}
+ };
+
+
+ VSet<AreaCMP> areas;
+
+ struct Contact {
+
+
+ Vector2 local_pos;
+ Vector2 local_normal;
+ float depth;
+ int local_shape;
+ Vector2 collider_pos;
+ int collider_shape;
+ ObjectID collider_instance_id;
+ RID collider;
+ Vector2 collider_velocity_at_pos;
+ };
+
+ Vector<Contact> contacts; //no contacts by default
+ int contact_count;
+
+ struct ForceIntegrationCallback {
+
+ ObjectID id;
+ StringName method;
+ Variant callback_udata;
+ };
+
+ ForceIntegrationCallback *fi_callback;
+
+
+ uint64_t island_step;
+ Body2DSW *island_next;
+ Body2DSW *island_list_next;
+
+ _FORCE_INLINE_ void _compute_area_gravity(const Area2DSW *p_area);
+
+friend class Physics2DDirectBodyStateSW; // 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());
+
+
+ _FORCE_INLINE_ void add_area(Area2DSW *p_area) { areas.insert(AreaCMP(p_area)); }
+ _FORCE_INLINE_ void remove_area(Area2DSW *p_area) { areas.erase(AreaCMP(p_area)); }
+
+ _FORCE_INLINE_ void set_max_contacts_reported(int p_size) { contacts.resize(p_size); contact_count=0; }
+ _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 Vector2& p_local_pos,const Vector2& p_local_normal, float p_depth, int p_local_shape, const Vector2& p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID& p_collider,const Vector2& p_collider_velocity_at_pos);
+
+
+ _FORCE_INLINE_ void add_exception(const RID& p_exception) { exceptions.insert(p_exception);}
+ _FORCE_INLINE_ void remove_exception(const RID& p_exception) { exceptions.erase(p_exception);}
+ _FORCE_INLINE_ bool has_exception(const RID& p_exception) const { return exceptions.has(p_exception);}
+ _FORCE_INLINE_ const VSet<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_ Body2DSW* get_island_next() const { return island_next; }
+ _FORCE_INLINE_ void set_island_next(Body2DSW* p_next) { island_next=p_next; }
+
+ _FORCE_INLINE_ Body2DSW* get_island_list_next() const { return island_list_next; }
+ _FORCE_INLINE_ void set_island_list_next(Body2DSW* p_next) { island_list_next=p_next; }
+
+ _FORCE_INLINE_ void add_constraint(Constraint2DSW* p_constraint, int p_pos) { constraint_map[p_constraint]=p_pos; }
+ _FORCE_INLINE_ void remove_constraint(Constraint2DSW* p_constraint) { constraint_map.erase(p_constraint); }
+ const Map<Constraint2DSW*,int>& get_constraint_map() const { return constraint_map; }
+
+ _FORCE_INLINE_ void set_omit_force_integration(bool p_omit_force_integration) { omit_force_integration=p_omit_force_integration; }
+ _FORCE_INLINE_ bool get_omit_force_integration() const { return omit_force_integration; }
+
+ _FORCE_INLINE_ void set_linear_velocity(const Vector2& p_velocity) {linear_velocity=p_velocity; }
+ _FORCE_INLINE_ Vector2 get_linear_velocity() const { return linear_velocity; }
+
+ _FORCE_INLINE_ void set_angular_velocity(real_t p_velocity) { angular_velocity=p_velocity; }
+ _FORCE_INLINE_ real_t get_angular_velocity() const { return angular_velocity; }
+
+ _FORCE_INLINE_ void set_biased_linear_velocity(const Vector2& p_velocity) {biased_linear_velocity=p_velocity; }
+ _FORCE_INLINE_ Vector2 get_biased_linear_velocity() const { return biased_linear_velocity; }
+
+ _FORCE_INLINE_ void set_biased_angular_velocity(real_t p_velocity) { biased_angular_velocity=p_velocity; }
+ _FORCE_INLINE_ real_t get_biased_angular_velocity() const { return biased_angular_velocity; }
+
+ _FORCE_INLINE_ void apply_impulse(const Vector2& p_pos, const Vector2& p_j) {
+
+ linear_velocity += p_j * _inv_mass;
+ angular_velocity += _inv_inertia * p_pos.cross(p_j);
+ }
+
+ _FORCE_INLINE_ void apply_bias_impulse(const Vector2& p_pos, const Vector2& p_j) {
+
+ biased_linear_velocity += p_j * _inv_mass;
+ biased_angular_velocity += _inv_inertia * p_pos.cross(p_j);
+ }
+
+ void set_active(bool p_active);
+ _FORCE_INLINE_ bool is_active() const { return active; }
+
+ void set_param(Physics2DServer::BodyParameter p_param, float);
+ float get_param(Physics2DServer::BodyParameter p_param) const;
+
+ void set_mode(Physics2DServer::BodyMode p_mode);
+ Physics2DServer::BodyMode get_mode() const;
+
+ void set_state(Physics2DServer::BodyState p_state, const Variant& p_variant);
+ Variant get_state(Physics2DServer::BodyState p_state) const;
+
+ void set_applied_force(const Vector2& p_force) { applied_force=p_force; }
+ Vector2 get_applied_force() const { return applied_force; }
+
+ void set_applied_torque(real_t p_torque) { applied_torque=p_torque; }
+ real_t get_applied_torque() const { return applied_torque; }
+
+ _FORCE_INLINE_ void 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(Space2DSW *p_space);
+
+ void update_inertias();
+
+ _FORCE_INLINE_ real_t get_inv_mass() const { return _inv_mass; }
+ _FORCE_INLINE_ real_t get_inv_inertia() const { return _inv_inertia; }
+ _FORCE_INLINE_ real_t get_friction() const { return friction; }
+ _FORCE_INLINE_ Vector2 get_gravity() const { return gravity; }
+ _FORCE_INLINE_ real_t get_density() const { return density; }
+
+ void integrate_forces(real_t p_step);
+ void integrate_velocities(real_t p_step);
+
+ void simulate_motion(const Matrix32& p_xform,real_t p_step);
+ void call_queries();
+ void wakeup_neighbours();
+
+ bool sleep_test(real_t p_step);
+
+ Body2DSW();
+ ~Body2DSW();
+
+};
+
+
+//add contact inline
+
+void Body2DSW::add_contact(const Vector2& p_local_pos,const Vector2& p_local_normal, float p_depth, int p_local_shape, const Vector2& p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID& p_collider,const Vector2& p_collider_velocity_at_pos) {
+
+ int c_max=contacts.size();
+
+ if (c_max==0)
+ return;
+
+ Contact *c = &contacts[0];
+
+
+ int idx=-1;
+
+ if (contact_count<c_max) {
+ idx=contact_count++;
+ } else {
+
+ float 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 Physics2DDirectBodyStateSW : public Physics2DDirectBodyState {
+
+ OBJ_TYPE( Physics2DDirectBodyStateSW, Physics2DDirectBodyState );
+
+public:
+
+ static Physics2DDirectBodyStateSW *singleton;
+ Body2DSW *body;
+ real_t step;
+
+ virtual Vector2 get_total_gravity() const { return body->get_gravity(); } // get gravity vector working on this body space/area
+ virtual float get_total_density() const { return body->get_density(); } // get density of this body space/area
+
+ virtual float get_inverse_mass() const { return body->get_inv_mass(); } // get the mass
+ virtual real_t get_inverse_inertia() const { return body->get_inv_inertia(); } // get density of this body space
+
+ virtual void set_linear_velocity(const Vector2& p_velocity) { body->set_linear_velocity(p_velocity); }
+ virtual Vector2 get_linear_velocity() const { return body->get_linear_velocity(); }
+
+ virtual void set_angular_velocity(real_t p_velocity) { body->set_angular_velocity(p_velocity); }
+ virtual real_t get_angular_velocity() const { return body->get_angular_velocity(); }
+
+ virtual void set_transform(const Matrix32& p_transform) { body->set_state(Physics2DServer::BODY_STATE_TRANSFORM,p_transform); }
+ virtual Matrix32 get_transform() const { return body->get_transform(); }
+
+ virtual void set_sleep_state(bool p_enable) { body->set_active(!p_enable); }
+ virtual bool is_sleeping() const { return !body->is_active(); }
+
+ virtual int get_contact_count() const { return body->contact_count; }
+
+ virtual Vector2 get_contact_local_pos(int p_contact_idx) const {
+ ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector2());
+ return body->contacts[p_contact_idx].local_pos;
+ }
+ virtual Vector2 get_contact_local_normal(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector2()); return body->contacts[p_contact_idx].local_normal; }
+ 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 Vector2 get_contact_collider_pos(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector2()); 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,0); 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 Vector2 get_contact_collider_velocity_at_pos(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector2()); return body->contacts[p_contact_idx].collider_velocity_at_pos; }
+
+ virtual Physics2DDirectSpaceState* get_space_state();
+
+
+ virtual real_t get_step() const { return step; }
+ Physics2DDirectBodyStateSW() { singleton=this; body=NULL; }
+};
+
+
+#endif // BODY_2D_SW_H
diff --git a/servers/physics_2d/body_pair_2d_sw.cpp b/servers/physics_2d/body_pair_2d_sw.cpp new file mode 100644 index 0000000000..6d8215840a --- /dev/null +++ b/servers/physics_2d/body_pair_2d_sw.cpp @@ -0,0 +1,435 @@ +/*************************************************************************/ +/* body_pair_2d_sw.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#include "body_pair_2d_sw.h" +#include "collision_solver_2d_sw.h" +#include "space_2d_sw.h" + + +#define POSITION_CORRECTION +#define ACCUMULATE_IMPULSES + +void BodyPair2DSW::_add_contact(const Vector2& p_point_A,const Vector2& p_point_B,void *p_self) { + + BodyPair2DSW *self = (BodyPair2DSW *)p_self; + + self->_contact_added_callback(p_point_A,p_point_B); + +} + +void BodyPair2DSW::_contact_added_callback(const Vector2& p_point_A,const Vector2& p_point_B) { + + // check if we already have the contact + + + Vector2 local_A = A->get_inv_transform().basis_xform(p_point_A); + Vector2 local_B = B->get_inv_transform().basis_xform(p_point_B-offset_B); + + int new_index = contact_count; + + ERR_FAIL_COND( new_index >= (MAX_CONTACTS+1) ); + + Contact contact; + + contact.acc_normal_impulse=0; + contact.acc_bias_impulse=0; + contact.acc_tangent_impulse=0; + contact.local_A=local_A; + contact.local_B=local_B; + contact.normal=(p_point_A-p_point_B).normalized(); + + // attempt to determine if the contact will be reused + + real_t recycle_radius_2 = space->get_contact_recycle_radius() * space->get_contact_recycle_radius(); + + for (int i=0;i<contact_count;i++) { + + Contact& c = contacts[i]; + if ( + c.local_A.distance_squared_to( local_A ) < (recycle_radius_2) && + c.local_B.distance_squared_to( local_B ) < (recycle_radius_2) ) { + + contact.acc_normal_impulse=c.acc_normal_impulse; + contact.acc_tangent_impulse=c.acc_tangent_impulse; + contact.acc_bias_impulse=c.acc_bias_impulse; + new_index=i; + break; + } + } + + // figure out if the contact amount must be reduced to fit the new contact + + if (new_index == MAX_CONTACTS) { + + // remove the contact with the minimum depth + + int least_deep=-1; + real_t min_depth=1e10; + + + for (int i=0;i<=contact_count;i++) { + + Contact& c = (i==contact_count)?contact:contacts[i]; + Vector2 global_A = A->get_transform().basis_xform(c.local_A); + Vector2 global_B = B->get_transform().basis_xform(c.local_B)+offset_B; + + Vector2 axis = global_A - global_B; + float depth = axis.dot( c.normal ); + + + if (depth<min_depth) { + + min_depth=depth; + least_deep=i; + } + } + + ERR_FAIL_COND(least_deep==-1); + + if (least_deep < contact_count) { //replace the last deep contact by the new one + + contacts[least_deep]=contact; + } + + return; + } + + contacts[new_index]=contact; + + if (new_index==contact_count) { + + contact_count++; + } + +} + +void BodyPair2DSW::_validate_contacts() { + + //make sure to erase contacts that are no longer valid + + real_t max_separation = space->get_contact_max_separation(); + real_t max_separation2 = max_separation*max_separation; + + for (int i=0;i<contact_count;i++) { + + Contact& c = contacts[i]; + + Vector2 global_A = A->get_transform().basis_xform(c.local_A); + Vector2 global_B = B->get_transform().basis_xform(c.local_B)+offset_B; + Vector2 axis = global_A - global_B; + float depth = axis.dot( c.normal ); + + if (depth < -max_separation || (global_B + c.normal * depth - global_A).length_squared() > max_separation2) { + // contact no longer needed, remove + + + if ((i+1) < contact_count) { + // swap with the last one + SWAP( contacts[i], contacts[ contact_count-1 ] ); + + } + + i--; + contact_count--; + } + } +} + + +bool BodyPair2DSW::_test_ccd(float p_step,Body2DSW *p_A, int p_shape_A,const Matrix32& p_xform_A,const Matrix32& p_xform_inv_A,Body2DSW *p_B, int p_shape_B,const Matrix32& p_xform_B,const Matrix32& p_xform_inv_B,bool p_swap_result) { + + Vector2 motion = p_A->get_linear_velocity()*p_step; + real_t mlen = motion.length(); + if (mlen<CMP_EPSILON) + return false; + + Vector2 mnormal = motion / mlen; + + real_t min,max; + p_A->get_shape(p_shape_A)->project_rangev(mnormal,p_xform_A,min,max); + if (mlen < (max-min)*0.3) //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 + int a; + Vector2 s[2]; + p_A->get_shape(p_shape_A)->get_supports(p_xform_A.basis_xform(mnormal).normalized(),s,a); + Vector2 from = p_xform_A.xform(s[0]); + Vector2 to = from + motion; + + Vector2 local_from = p_xform_inv_B.xform(from); + Vector2 local_to = p_xform_inv_B.xform(to); + + Vector2 rpos,rnorm; + if (!p_B->get_shape(p_shape_B)->intersect_segment(local_from,local_to,rpos,rnorm)) + return false; + + //ray hit something + + Vector2 contact_A = to; + Vector2 contact_B = p_xform_B.xform(rpos); + + //create a contact + + if (p_swap_result) + _contact_added_callback(contact_B,contact_A); + else + _contact_added_callback(contact_A,contact_B); + + + return true; +} + +bool BodyPair2DSW::setup(float p_step) { + + + //use local A coordinates to avoid numerical issues on collision detection + offset_B = B->get_transform().get_origin() - A->get_transform().get_origin(); + + _validate_contacts(); + + //cannot collide + if (A->is_shape_set_as_trigger(shape_A) || B->is_shape_set_as_trigger(shape_B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self()) || (A->get_mode()<=Physics2DServer::BODY_MODE_STATIC_ACTIVE && B->get_mode()<=Physics2DServer::BODY_MODE_STATIC_ACTIVE)) { + collided=false; + + return false; + } + Vector2 offset_A = A->get_transform().get_origin(); + Matrix32 xform_Au = A->get_transform().untranslated(); + Matrix32 xform_A = xform_Au * A->get_shape_transform(shape_A); + Matrix32 xform_inv_A = xform_A.affine_inverse(); + + Matrix32 xform_Bu = B->get_transform(); + xform_Bu.elements[2]-=A->get_transform().get_origin(); + Matrix32 xform_B = xform_Bu * B->get_shape_transform(shape_B); + Matrix32 xform_inv_B = xform_B.affine_inverse(); + + Shape2DSW *shape_A_ptr=A->get_shape(shape_A); + Shape2DSW *shape_B_ptr=B->get_shape(shape_B); + + collided = CollisionSolver2DSW::solve_static(shape_A_ptr,xform_A,xform_inv_A,shape_B_ptr,xform_B,xform_inv_B,_add_contact,this,&sep_axis); + if (!collided) { + + //test ccd (currently just a raycast) + if (A->is_continuous_collision_detection_enabled() && A->get_type()>Physics2DServer::BODY_MODE_STATIC_ACTIVE) { + if (_test_ccd(p_step,A,shape_A,xform_A,xform_inv_A,B,shape_B,xform_B,xform_inv_B)) + collided=true; + } + + if (B->is_continuous_collision_detection_enabled() && B->get_type()>Physics2DServer::BODY_MODE_STATIC_ACTIVE) { + if (_test_ccd(p_step,B,shape_B,xform_B,xform_inv_B,A,shape_A,xform_A,xform_inv_A,true)) + collided=true; + } + + if (!collided) + return false; + + } + + + + real_t max_penetration = space->get_contact_max_allowed_penetration(); + + float bias = 0.3f; + 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; + } + + + cc=0; + + + real_t inv_dt = 1.0/p_step; + for (int i = 0; i < contact_count; i++) { + + Contact& c = contacts[i]; + + Vector2 global_A = xform_Au.xform(c.local_A); + Vector2 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; + + int gather_A = A->can_report_contacts(); + int gather_B = B->can_report_contacts(); + + c.rA = global_A; + c.rB = global_B-offset_B; + + if (gather_A | gather_B) { + + //Vector2 crB( -B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x ); + + global_A+=offset_A; + global_B+=offset_A; + + if (gather_A) { + Vector2 crB( -B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x ); + A->add_contact(global_A,-c.normal,depth,shape_A,global_B,shape_B,B->get_instance_id(),B->get_self(),crB+B->get_linear_velocity()); + } + if (gather_B) { + + Vector2 crA( -A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x ); + B->add_contact(global_B,c.normal,depth,shape_B,global_A,shape_A,A->get_instance_id(),A->get_self(),crA+A->get_linear_velocity()); + } + } + + + // Precompute normal mass, tangent mass, and bias. + real_t rnA = c.rA.dot(c.normal); + real_t rnB = c.rB.dot(c.normal); + real_t kNormal = A->get_inv_mass() + B->get_inv_mass(); + kNormal += A->get_inv_inertia() * (c.rA.dot(c.rA) - rnA * rnA) + B->get_inv_inertia() * (c.rB.dot(c.rB) - rnB * rnB); + c.mass_normal = 1.0f / kNormal; + + Vector2 tangent = c.normal.tangent(); + real_t rtA = c.rA.dot(tangent); + real_t rtB = c.rB.dot(tangent); + real_t kTangent = A->get_inv_mass() + B->get_inv_mass(); + kTangent += A->get_inv_inertia() * (c.rA.dot(c.rA) - rtA * rtA) + B->get_inv_inertia() * (c.rB.dot(c.rB) - rtB * rtB); + c.mass_tangent = 1.0f / kTangent; + + + + c.bias = -bias * inv_dt * MIN(0.0f, -depth + max_penetration); + c.depth=depth; + //c.acc_bias_impulse=0; + + +#ifdef ACCUMULATE_IMPULSES + { + // Apply normal + friction impulse + Vector2 P = c.acc_normal_impulse * c.normal + c.acc_tangent_impulse * tangent; + + + A->apply_impulse(c.rA,-P); + B->apply_impulse(c.rB, P); + } + +#endif + } + + return true; +} + +void BodyPair2DSW::solve(float p_step) { + + if (!collided) + return; + + for (int i = 0; i < contact_count; ++i) { + + Contact& c = contacts[i]; + cc++; + + if (!c.active) + continue; + + + // Relative velocity at contact + + Vector2 crA( -A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x ); + Vector2 crB( -B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x ); + Vector2 dv = B->get_linear_velocity() + crB - A->get_linear_velocity() - crA; + + Vector2 crbA( -A->get_biased_angular_velocity() * c.rA.y, A->get_biased_angular_velocity() * c.rA.x ); + Vector2 crbB( -B->get_biased_angular_velocity() * c.rB.y, B->get_biased_angular_velocity() * c.rB.x ); + Vector2 dbv = B->get_biased_linear_velocity() + crbB - A->get_biased_linear_velocity() - crbA; + + real_t vn = dv.dot(c.normal); + real_t vbn = dbv.dot(c.normal); + Vector2 tangent = c.normal.tangent(); + real_t vt = dv.dot(tangent); + + + real_t jbn = (c.bias - vbn)*c.mass_normal; + real_t jbnOld = c.acc_bias_impulse; + c.acc_bias_impulse = MAX(jbnOld + jbn, 0.0f); + + Vector2 jb = c.normal * (c.acc_bias_impulse - jbnOld); + + A->apply_bias_impulse(c.rA,-jb); + B->apply_bias_impulse(c.rB, jb); + + real_t bounce=0; + real_t jn = -(bounce + vn)*c.mass_normal; + real_t jnOld = c.acc_normal_impulse; + c.acc_normal_impulse = MAX(jnOld + jn, 0.0f); + + + real_t friction = A->get_friction() * B->get_friction(); + + real_t jtMax = friction*c.acc_normal_impulse; + real_t jt = -vt*c.mass_tangent; + real_t jtOld = c.acc_tangent_impulse; + c.acc_tangent_impulse = CLAMP(jtOld + jt, -jtMax, jtMax); + + Vector2 j =c.normal * (c.acc_normal_impulse - jnOld) + tangent * ( c.acc_tangent_impulse - jtOld ); + + + A->apply_impulse(c.rA,-j); + B->apply_impulse(c.rB, j); + + + } +} + + +BodyPair2DSW::BodyPair2DSW(Body2DSW *p_A, int p_shape_A,Body2DSW *p_B, int p_shape_B) : Constraint2DSW(_arr,2) { + + A=p_A; + B=p_B; + shape_A=p_shape_A; + shape_B=p_shape_B; + space=A->get_space(); + A->add_constraint(this,0); + B->add_constraint(this,1); + contact_count=0; + collided=false; + +} + + +BodyPair2DSW::~BodyPair2DSW() { + + A->remove_constraint(this); + B->remove_constraint(this); + +} diff --git a/servers/physics_2d/body_pair_2d_sw.h b/servers/physics_2d/body_pair_2d_sw.h new file mode 100644 index 0000000000..26278f87cd --- /dev/null +++ b/servers/physics_2d/body_pair_2d_sw.h @@ -0,0 +1,94 @@ +/*************************************************************************/ +/* body_pair_2d_sw.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#ifndef BODY_PAIR_2D_SW_H +#define BODY_PAIR_2D_SW_H + +#include "body_2d_sw.h" +#include "constraint_2d_sw.h" + +class BodyPair2DSW : public Constraint2DSW { + + enum { + MAX_CONTACTS=2 + }; + union { + struct { + Body2DSW *A; + Body2DSW *B; + }; + + Body2DSW *_arr[2]; + }; + + int shape_A; + int shape_B; + + Space2DSW *space; + + struct Contact { + + Vector2 position; + Vector2 normal; + Vector2 local_A, local_B; + real_t acc_normal_impulse; // accumulated normal impulse (Pn) + real_t acc_tangent_impulse; // accumulated tangent impulse (Pt) + real_t acc_bias_impulse; // accumulated normal impulse for position bias (Pnb) + real_t mass_normal, mass_tangent; + real_t bias; + + real_t depth; + bool active; + Vector2 rA,rB; + }; + + Vector2 offset_B; //use local A coordinates to avoid numerical issues on collision detection + + Vector2 sep_axis; + Contact contacts[MAX_CONTACTS]; + int contact_count; + bool collided; + int cc; + + + bool _test_ccd(float p_step,Body2DSW *p_A, int p_shape_A,const Matrix32& p_xform_A,const Matrix32& p_xform_inv_A,Body2DSW *p_B, int p_shape_B,const Matrix32& p_xform_B,const Matrix32& p_xform_inv_B,bool p_swap_result=false); + void _validate_contacts(); + static void _add_contact(const Vector2& p_point_A,const Vector2& p_point_B,void *p_self); + _FORCE_INLINE_ void _contact_added_callback(const Vector2& p_point_A,const Vector2& p_point_B); + +public: + + bool setup(float p_step); + void solve(float p_step); + + BodyPair2DSW(Body2DSW *p_A, int p_shape_A,Body2DSW *p_B, int p_shape_B); + ~BodyPair2DSW(); + +}; + +#endif // BODY_PAIR_2D_SW_H diff --git a/servers/physics_2d/broad_phase_2d_basic.cpp b/servers/physics_2d/broad_phase_2d_basic.cpp new file mode 100644 index 0000000000..9641a986e8 --- /dev/null +++ b/servers/physics_2d/broad_phase_2d_basic.cpp @@ -0,0 +1,192 @@ +/*************************************************************************/ +/* broad_phase_2d_basic.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#include "broad_phase_2d_basic.h" + +ID BroadPhase2DBasic::create(CollisionObject2DSW *p_object_, int p_subindex) { + + + current++; + + Element e; + e.owner=p_object_; + e._static=false; + e.subindex=p_subindex; + + element_map[current]=e; + return current; +} + +void BroadPhase2DBasic::move(ID p_id, const Rect2& p_aabb) { + + Map<ID,Element>::Element *E=element_map.find(p_id); + ERR_FAIL_COND(!E); + E->get().aabb=p_aabb; + +} +void BroadPhase2DBasic::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 BroadPhase2DBasic::remove(ID p_id) { + + Map<ID,Element>::Element *E=element_map.find(p_id); + ERR_FAIL_COND(!E); + element_map.erase(E); + +} + +CollisionObject2DSW *BroadPhase2DBasic::get_object(ID p_id) const { + + const Map<ID,Element>::Element *E=element_map.find(p_id); + ERR_FAIL_COND_V(!E,NULL); + return E->get().owner; + +} +bool BroadPhase2DBasic::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 BroadPhase2DBasic::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 BroadPhase2DBasic::cull_segment(const Vector2& p_from, const Vector2& p_to,CollisionObject2DSW** 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 Rect2 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 BroadPhase2DBasic::cull_aabb(const Rect2& p_aabb,CollisionObject2DSW** 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 Rect2 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 BroadPhase2DBasic::set_pair_callback(PairCallback p_pair_callback,void *p_userdata) { + + pair_userdata=p_userdata; + pair_callback=p_pair_callback; + +} +void BroadPhase2DBasic::set_unpair_callback(UnpairCallback p_pair_callback,void *p_userdata) { + + unpair_userdata=p_userdata; + unpair_callback=p_pair_callback; + +} + +void BroadPhase2DBasic::update() { + + // recompute pairs + for(Map<ID,Element>::Element *I=element_map.front();I;I=I->next()) { + + for(Map<ID,Element>::Element *J=I->next();J;J=J->next()) { + + Element *elem_A=&I->get(); + Element *elem_B=&J->get(); + + if (elem_A->owner == elem_B->owner) + continue; + + + bool pair_ok=elem_A->aabb.intersects( elem_B->aabb ) && (!elem_A->_static || !elem_B->_static ); + + PairKey key(I->key(),J->key()); + + Map<PairKey,void*>::Element *E=pair_map.find(key); + + if (!pair_ok && E) { + if (unpair_callback) + unpair_callback(elem_A->owner,elem_A->subindex,elem_B->owner,elem_B->subindex,E->get(),unpair_userdata); + pair_map.erase(key); + } + + if (pair_ok && !E) { + + void *data=NULL; + if (pair_callback) + data=pair_callback(elem_A->owner,elem_A->subindex,elem_B->owner,elem_B->subindex,unpair_userdata); + pair_map.insert(key,data); + } + } + } + +} + +BroadPhase2DSW *BroadPhase2DBasic::_create() { + + return memnew( BroadPhase2DBasic ); +} + +BroadPhase2DBasic::BroadPhase2DBasic() { + + current=1; + unpair_callback=NULL; + unpair_userdata=NULL; + pair_callback=NULL; + pair_userdata=NULL; + +} diff --git a/servers/physics_2d/broad_phase_2d_basic.h b/servers/physics_2d/broad_phase_2d_basic.h new file mode 100644 index 0000000000..ce15752251 --- /dev/null +++ b/servers/physics_2d/broad_phase_2d_basic.h @@ -0,0 +1,100 @@ +/*************************************************************************/ +/* broad_phase_2d_basic.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#ifndef BROAD_PHASE_2D_BASIC_H +#define BROAD_PHASE_2D_BASIC_H + +#include "space_2d_sw.h" +#include "map.h" +class BroadPhase2DBasic : public BroadPhase2DSW { + + struct Element { + + CollisionObject2DSW *owner; + bool _static; + Rect2 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(CollisionObject2DSW *p_object_, int p_subindex=0); + virtual void move(ID p_id, const Rect2& p_aabb); + virtual void set_static(ID p_id, bool p_static); + virtual void remove(ID p_id); + + virtual CollisionObject2DSW *get_object(ID p_id) const; + virtual bool is_static(ID p_id) const; + virtual int get_subindex(ID p_id) const; + + virtual int cull_segment(const Vector2& p_from, const Vector2& p_to,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices=NULL); + virtual int cull_aabb(const Rect2& p_aabb,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices=NULL); + + virtual void set_pair_callback(PairCallback p_pair_callback,void *p_userdata); + virtual void set_unpair_callback(UnpairCallback p_unpair_callback,void *p_userdata); + + virtual void update(); + + static BroadPhase2DSW *_create(); + BroadPhase2DBasic(); +}; + +#endif // BROAD_PHASE_2D_BASIC_H diff --git a/servers/physics_2d/broad_phase_2d_hash_grid.cpp b/servers/physics_2d/broad_phase_2d_hash_grid.cpp new file mode 100644 index 0000000000..10da376dfd --- /dev/null +++ b/servers/physics_2d/broad_phase_2d_hash_grid.cpp @@ -0,0 +1,665 @@ +/*************************************************************************/ +/* broad_phase_2d_hash_grid.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#include "broad_phase_2d_hash_grid.h" +#include "globals.h" + +void BroadPhase2DHashGrid::_pair_attempt(Element *p_elem, Element* p_with) { + + Map<Element*,PairData*>::Element *E=p_elem->paired.find(p_with); + + ERR_FAIL_COND(p_elem->_static && p_with->_static); + + if (!E) { + + PairData *pd = memnew( PairData ); + p_elem->paired[p_with]=pd; + p_with->paired[p_elem]=pd; + } else { + E->get()->rc++; + } + +} + +void BroadPhase2DHashGrid::_unpair_attempt(Element *p_elem, Element* p_with) { + + Map<Element*,PairData*>::Element *E=p_elem->paired.find(p_with); + + ERR_FAIL_COND(!E); //this should really be paired.. + + E->get()->rc--; + + if (E->get()->rc==0) { + + if (E->get()->colliding) { + //uncollide + if (unpair_callback) { + unpair_callback(p_elem->owner,p_elem->subindex,p_with->owner,p_with->subindex,E->get()->ud,unpair_userdata); + } + + + } + + memdelete(E->get()); + p_elem->paired.erase(E); + p_with->paired.erase(p_elem); + } + + +} + +void BroadPhase2DHashGrid::_check_motion(Element *p_elem) { + + for (Map<Element*,PairData*>::Element *E=p_elem->paired.front();E;E=E->next()) { + + bool pairing = p_elem->aabb.intersects( E->key()->aabb ); + + if (pairing!=E->get()->colliding) { + + if (pairing) { + + if (pair_callback) { + E->get()->ud=pair_callback(p_elem->owner,p_elem->subindex,E->key()->owner,E->key()->subindex,pair_userdata); + } + } else { + + if (unpair_callback) { + unpair_callback(p_elem->owner,p_elem->subindex,E->key()->owner,E->key()->subindex,E->get()->ud,unpair_userdata); + } + + } + + E->get()->colliding=pairing; + } + } +} + +void BroadPhase2DHashGrid::_enter_grid( Element* p_elem, const Rect2& p_rect,bool p_static) { + + + Point2i from = (p_rect.pos/cell_size).floor(); + Point2i to = ((p_rect.pos+p_rect.size)/cell_size).floor(); + + for(int i=from.x;i<=to.x;i++) { + + + for(int j=from.y;j<=to.y;j++) { + + PosKey pk; + pk.x=i; + pk.y=j; + + uint32_t idx = pk.hash() % hash_table_size; + PosBin *pb = hash_table[idx]; + + while (pb) { + + if (pb->key == pk) { + break; + } + + pb=pb->next; + } + + + bool entered=false; + + if (!pb) { + //does not exist, create! + pb = memnew( PosBin ); + pb->key=pk; + pb->next=hash_table[idx]; + hash_table[idx]=pb; + } + + + + if (p_static) { + if (pb->static_object_set[p_elem].inc()==1) { + entered=true; + } + } else { + if (pb->object_set[p_elem].inc()==1) { + + entered=true; + } + } + + if (entered) { + + for(Map<Element*,RC>::Element *E=pb->object_set.front();E;E=E->next()) { + + if (E->key()->owner==p_elem->owner) + continue; + _pair_attempt(p_elem,E->key()); + } + + if (!p_static) { + + for(Map<Element*,RC>::Element *E=pb->static_object_set.front();E;E=E->next()) { + + if (E->key()->owner==p_elem->owner) + continue; + _pair_attempt(p_elem,E->key()); + } + } + } + + } + + } + + +} + + +void BroadPhase2DHashGrid::_exit_grid( Element* p_elem, const Rect2& p_rect,bool p_static) { + + + Point2i from = (p_rect.pos/cell_size).floor(); + Point2i to = ((p_rect.pos+p_rect.size)/cell_size).floor(); + + for(int i=from.x;i<=to.x;i++) { + + for(int j=from.y;j<=to.y;j++) { + + PosKey pk; + pk.x=i; + pk.y=j; + + uint32_t idx = pk.hash() % hash_table_size; + PosBin *pb = hash_table[idx]; + + while (pb) { + + if (pb->key == pk) { + break; + } + + pb=pb->next; + } + + ERR_CONTINUE(!pb); //should exist!! + + bool exited=false; + + + if (p_static) { + if (pb->static_object_set[p_elem].dec()==0) { + + pb->static_object_set.erase(p_elem); + exited=true; + + } + } else { + if (pb->object_set[p_elem].dec()==0) { + + pb->object_set.erase(p_elem); + exited=true; + + } + } + + if (exited) { + + for(Map<Element*,RC>::Element *E=pb->object_set.front();E;E=E->next()) { + + if (E->key()->owner==p_elem->owner) + continue; + _unpair_attempt(p_elem,E->key()); + + } + + if (!p_static) { + + for(Map<Element*,RC>::Element *E=pb->static_object_set.front();E;E=E->next()) { + + if (E->key()->owner==p_elem->owner) + continue; + _unpair_attempt(p_elem,E->key()); + } + } + } + + if (pb->object_set.empty() && pb->static_object_set.empty()) { + + if (hash_table[idx]==pb) { + hash_table[idx]=pb->next; + } else { + + PosBin *px = hash_table[idx]; + + while (px) { + + if (px->next==pb) { + px->next=pb->next; + break; + } + + px=px->next; + } + + ERR_CONTINUE(!px); + } + + memdelete(pb); + + } + } + + } + +} + + +BroadPhase2DHashGrid::ID BroadPhase2DHashGrid::create(CollisionObject2DSW *p_object, int p_subindex) { + + current++; + + Element e; + e.owner=p_object; + e._static=false; + e.subindex=p_subindex; + e.self=current; + e.pass=0; + + element_map[current]=e; + return current; + +} + +void BroadPhase2DHashGrid::move(ID p_id, const Rect2& p_aabb) { + + + Map<ID,Element>::Element *E=element_map.find(p_id); + ERR_FAIL_COND(!E); + + Element &e=E->get(); + + if (p_aabb==e.aabb) + return; + + + if (p_aabb!=Rect2()) { + + _enter_grid(&e,p_aabb,e._static); + } + + if (e.aabb!=Rect2()) { + + _exit_grid(&e,e.aabb,e._static); + } + + e.aabb=p_aabb; + + _check_motion(&e); + + e.aabb=p_aabb; + +} +void BroadPhase2DHashGrid::set_static(ID p_id, bool p_static) { + + Map<ID,Element>::Element *E=element_map.find(p_id); + ERR_FAIL_COND(!E); + + Element &e=E->get(); + + if (e._static==p_static) + return; + + if (e.aabb!=Rect2()) + _exit_grid(&e,e.aabb,e._static); + + e._static=p_static; + + if (e.aabb!=Rect2()) { + _enter_grid(&e,e.aabb,e._static); + _check_motion(&e); + } + +} +void BroadPhase2DHashGrid::remove(ID p_id) { + + Map<ID,Element>::Element *E=element_map.find(p_id); + ERR_FAIL_COND(!E); + + Element &e=E->get(); + + if (e.aabb!=Rect2()) + _exit_grid(&e,e.aabb,e._static); + + element_map.erase(p_id); + +} + +CollisionObject2DSW *BroadPhase2DHashGrid::get_object(ID p_id) const { + + const Map<ID,Element>::Element *E=element_map.find(p_id); + ERR_FAIL_COND_V(!E,NULL); + return E->get().owner; + +} +bool BroadPhase2DHashGrid::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 BroadPhase2DHashGrid::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; +} + +void BroadPhase2DHashGrid::_cull(const Point2i p_cell,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices,int &index) { + + + PosKey pk; + pk.x=p_cell.x; + pk.y=p_cell.y; + + uint32_t idx = pk.hash() % hash_table_size; + PosBin *pb = hash_table[idx]; + + while (pb) { + + if (pb->key == pk) { + break; + } + + pb=pb->next; + } + + if (!pb) + return; + + + + for(Map<Element*,RC>::Element *E=pb->object_set.front();E;E=E->next()) { + + + if (index>=p_max_results) + break; + if (E->key()->pass==pass) + continue; + + E->key()->pass=pass; + p_results[index]=E->key()->owner; + p_result_indices[index]=E->key()->subindex; + index++; + + } + + for(Map<Element*,RC>::Element *E=pb->static_object_set.front();E;E=E->next()) { + + + if (index>=p_max_results) + break; + if (E->key()->pass==pass) + continue; + + E->key()->pass=pass; + p_results[index]=E->key()->owner; + p_result_indices[index]=E->key()->subindex; + index++; + + } +} + +int BroadPhase2DHashGrid::cull_segment(const Vector2& p_from, const Vector2& p_to,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices) { + + pass++; + + Vector2 dir = (p_to-p_from); + if (dir==Vector2()) + return 0; + //avoid divisions by zero + dir.normalize(); + if (dir.x==0.0) + dir.x=0.000001; + if (dir.y==0.0) + dir.y=0.000001; + Vector2 delta = dir.abs(); + + delta.x=cell_size/delta.x; + delta.y=cell_size/delta.y; + + Point2i pos = p_from.floor() / cell_size; + Point2i end = p_to.floor() / cell_size; + Point2i step = Vector2( SGN(dir.x), SGN(dir.y)); + + Vector2 max; + + if (dir.x<0) + max.x= (Math::floor(pos.x)*cell_size - p_from.x) / dir.x; + else + max.x= (Math::floor(pos.x + 1)*cell_size - p_from.x) / dir.x; + + if (dir.y<0) + max.y= (Math::floor(pos.y)*cell_size - p_from.y) / dir.y; + else + max.y= (Math::floor(pos.y + 1)*cell_size - p_from.y) / dir.y; + + int cullcount=0; + _cull(pos,p_results,p_max_results,p_result_indices,cullcount); + + bool reached_x=false; + bool reached_y=false; + + while(true) { + + if (max.x < max.y) { + + max.x+=delta.x; + pos.x+=step.x; + } else { + + max.y+=delta.y; + pos.y+=step.y; + + } + + if (step.x>0) { + if (pos.x>=end.x) + reached_x=true; + } else if (pos.x<=end.x) { + + reached_x=true; + } + + if (step.y>0) { + if (pos.y>=end.y) + reached_y=true; + } else if (pos.y<=end.y) { + + reached_y=true; + } + + _cull(pos,p_results,p_max_results,p_result_indices,cullcount); + + if (reached_x && reached_y) + break; + + } + + return cullcount; +} + + +int BroadPhase2DHashGrid::cull_aabb(const Rect2& p_aabb,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices) { + + + return 0; +} + +void BroadPhase2DHashGrid::set_pair_callback(PairCallback p_pair_callback,void *p_userdata) { + + pair_callback=p_pair_callback; + pair_userdata=p_userdata; + +} +void BroadPhase2DHashGrid::set_unpair_callback(UnpairCallback p_unpair_callback,void *p_userdata) { + + unpair_callback=p_unpair_callback; + unpair_userdata=p_userdata; + +} + +void BroadPhase2DHashGrid::update() { + + +} + +BroadPhase2DSW *BroadPhase2DHashGrid::_create() { + + return memnew( BroadPhase2DHashGrid ); +} + + +BroadPhase2DHashGrid::BroadPhase2DHashGrid() { + + hash_table_size = GLOBAL_DEF("physics_2d/bp_hash_table_size",4096); + hash_table_size = Math::larger_prime(hash_table_size); + hash_table = memnew_arr( PosBin*, hash_table_size); + + cell_size = GLOBAL_DEF("physics_2d/cell_size",128); + + for(int i=0;i<hash_table_size;i++) + hash_table[i]=NULL; + pass=1; + + current=0; +} + +BroadPhase2DHashGrid::~BroadPhase2DHashGrid() { + + for(int i=0;i<hash_table_size;i++) { + while(hash_table[i]) { + PosBin *pb=hash_table[i]; + hash_table[i]=pb->next; + memdelete(pb); + } + } + + memdelete_arr( hash_table ); + + +} + + + +/* 3D version of voxel traversal: + +public IEnumerable<Point3D> GetCellsOnRay(Ray ray, int maxDepth) +{ + // Implementation is based on: + // "A Fast Voxel Traversal Algorithm for Ray Tracing" + // John Amanatides, Andrew Woo + // http://www.cse.yorku.ca/~amana/research/grid.pdf + // http://www.devmaster.net/articles/raytracing_series/A%20faster%20voxel%20traversal%20algorithm%20for%20ray%20tracing.pdf + + // NOTES: + // * This code assumes that the ray's position and direction are in 'cell coordinates', which means + // that one unit equals one cell in all directions. + // * When the ray doesn't start within the voxel grid, calculate the first position at which the + // ray could enter the grid. If it never enters the grid, there is nothing more to do here. + // * Also, it is important to test when the ray exits the voxel grid when the grid isn't infinite. + // * The Point3D structure is a simple structure having three integer fields (X, Y and Z). + + // The cell in which the ray starts. + Point3D start = GetCellAt(ray.Position); // Rounds the position's X, Y and Z down to the nearest integer values. + int x = start.X; + int y = start.Y; + int z = start.Z; + + // Determine which way we go. + int stepX = Math.Sign(ray.Direction.X); + int stepY = Math.Sign(ray.Direction.Y); + int stepZ = Math.Sign(ray.Direction.Z); + + // Calculate cell boundaries. When the step (i.e. direction sign) is positive, + // the next boundary is AFTER our current position, meaning that we have to add 1. + // Otherwise, it is BEFORE our current position, in which case we add nothing. + Point3D cellBoundary = new Point3D( + x + (stepX > 0 ? 1 : 0), + y + (stepY > 0 ? 1 : 0), + z + (stepZ > 0 ? 1 : 0)); + + // NOTE: For the following calculations, the result will be Single.PositiveInfinity + // when ray.Direction.X, Y or Z equals zero, which is OK. However, when the left-hand + // value of the division also equals zero, the result is Single.NaN, which is not OK. + + // Determine how far we can travel along the ray before we hit a voxel boundary. + Vector3 tMax = new Vector3( + (cellBoundary.X - ray.Position.X) / ray.Direction.X, // Boundary is a plane on the YZ axis. + (cellBoundary.Y - ray.Position.Y) / ray.Direction.Y, // Boundary is a plane on the XZ axis. + (cellBoundary.Z - ray.Position.Z) / ray.Direction.Z); // Boundary is a plane on the XY axis. + if (Single.IsNaN(tMax.X)) tMax.X = Single.PositiveInfinity; + if (Single.IsNaN(tMax.Y)) tMax.Y = Single.PositiveInfinity; + if (Single.IsNaN(tMax.Z)) tMax.Z = Single.PositiveInfinity; + + // Determine how far we must travel along the ray before we have crossed a gridcell. + Vector3 tDelta = new Vector3( + stepX / ray.Direction.X, // Crossing the width of a cell. + stepY / ray.Direction.Y, // Crossing the height of a cell. + stepZ / ray.Direction.Z); // Crossing the depth of a cell. + if (Single.IsNaN(tDelta.X)) tDelta.X = Single.PositiveInfinity; + if (Single.IsNaN(tDelta.Y)) tDelta.Y = Single.PositiveInfinity; + if (Single.IsNaN(tDelta.Z)) tDelta.Z = Single.PositiveInfinity; + + // For each step, determine which distance to the next voxel boundary is lowest (i.e. + // which voxel boundary is nearest) and walk that way. + for (int i = 0; i < maxDepth; i++) + { + // Return it. + yield return new Point3D(x, y, z); + + // Do the next step. + if (tMax.X < tMax.Y && tMax.X < tMax.Z) + { + // tMax.X is the lowest, an YZ cell boundary plane is nearest. + x += stepX; + tMax.X += tDelta.X; + } + else if (tMax.Y < tMax.Z) + { + // tMax.Y is the lowest, an XZ cell boundary plane is nearest. + y += stepY; + tMax.Y += tDelta.Y; + } + else + { + // tMax.Z is the lowest, an XY cell boundary plane is nearest. + z += stepZ; + tMax.Z += tDelta.Z; + } + } + + */ diff --git a/servers/physics_2d/broad_phase_2d_hash_grid.h b/servers/physics_2d/broad_phase_2d_hash_grid.h new file mode 100644 index 0000000000..713d960487 --- /dev/null +++ b/servers/physics_2d/broad_phase_2d_hash_grid.h @@ -0,0 +1,192 @@ +/*************************************************************************/ +/* broad_phase_2d_hash_grid.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#ifndef BROAD_PHASE_2D_HASH_GRID_H +#define BROAD_PHASE_2D_HASH_GRID_H + +#include "broad_phase_2d_sw.h" +#include "map.h" + +class BroadPhase2DHashGrid : public BroadPhase2DSW { + + + struct PairData { + + bool colliding; + int rc; + void *ud; + PairData() { colliding=false; rc=1; ud=NULL; } + }; + + struct Element { + + ID self; + CollisionObject2DSW *owner; + bool _static; + Rect2 aabb; + int subindex; + uint64_t pass; + Map<Element*,PairData*> paired; + + }; + + + Map<ID,Element> element_map; + + ID current; + + uint64_t pass; + + + 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,PairData> pair_map; + + int cell_size; + + PairCallback pair_callback; + void *pair_userdata; + UnpairCallback unpair_callback; + void *unpair_userdata; + + void _enter_grid(Element* p_elem, const Rect2& p_rect,bool p_static); + void _exit_grid(Element* p_elem, const Rect2& p_rect,bool p_static); + _FORCE_INLINE_ void _cull(const Point2i p_cell,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices,int &index); + + + struct PosKey { + + union { + struct { + int32_t x; + int32_t y; + }; + uint64_t key; + }; + + + _FORCE_INLINE_ uint32_t hash() const { + uint64_t k=key; + k = (~k) + (k << 18); // k = (k << 18) - k - 1; + k = k ^ (k >> 31); + k = k * 21; // k = (k + (k << 2)) + (k << 4); + k = k ^ (k >> 11); + k = k + (k << 6); + k = k ^ (k >> 22); + return k; + } + + bool operator==(const PosKey& p_key) const { return key==p_key.key; } + _FORCE_INLINE_ bool operator<(const PosKey& p_key) const { + return key < p_key.key; + } + + }; + + struct RC { + + int ref; + + _FORCE_INLINE_ int inc() { + ref++; + return ref; + } + _FORCE_INLINE_ int dec() { + ref--; + return ref; + } + + _FORCE_INLINE_ RC() { + ref=0; + } + }; + + struct PosBin { + + PosKey key; + Map<Element*,RC> object_set; + Map<Element*,RC> static_object_set; + PosBin *next; + }; + + + uint32_t hash_table_size; + PosBin **hash_table; + + void _pair_attempt(Element *p_elem, Element* p_with); + void _unpair_attempt(Element *p_elem, Element* p_with); + void _check_motion(Element *p_elem); + + +public: + + virtual ID create(CollisionObject2DSW *p_object_, int p_subindex=0); + virtual void move(ID p_id, const Rect2& p_aabb); + virtual void set_static(ID p_id, bool p_static); + virtual void remove(ID p_id); + + virtual CollisionObject2DSW *get_object(ID p_id) const; + virtual bool is_static(ID p_id) const; + virtual int get_subindex(ID p_id) const; + + virtual int cull_segment(const Vector2& p_from, const Vector2& p_to,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices=NULL); + virtual int cull_aabb(const Rect2& p_aabb,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices=NULL); + + virtual void set_pair_callback(PairCallback p_pair_callback,void *p_userdata); + virtual void set_unpair_callback(UnpairCallback p_unpair_callback,void *p_userdata); + + virtual void update(); + + + static BroadPhase2DSW *_create(); + + BroadPhase2DHashGrid(); + ~BroadPhase2DHashGrid(); + + +}; + +#endif // BROAD_PHASE_2D_HASH_GRID_H diff --git a/servers/physics_2d/broad_phase_2d_sw.cpp b/servers/physics_2d/broad_phase_2d_sw.cpp new file mode 100644 index 0000000000..7ded6ed01e --- /dev/null +++ b/servers/physics_2d/broad_phase_2d_sw.cpp @@ -0,0 +1,35 @@ +/*************************************************************************/ +/* broad_phase_2d_sw.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#include "broad_phase_2d_sw.h" + +BroadPhase2DSW::CreateFunction BroadPhase2DSW::create_func=NULL; + +BroadPhase2DSW::~BroadPhase2DSW() +{ +} diff --git a/servers/physics_2d/broad_phase_2d_sw.h b/servers/physics_2d/broad_phase_2d_sw.h new file mode 100644 index 0000000000..510f7db112 --- /dev/null +++ b/servers/physics_2d/broad_phase_2d_sw.h @@ -0,0 +1,73 @@ +/*************************************************************************/ +/* broad_phase_2d_sw.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#ifndef BROAD_PHASE_2D_SW_H +#define BROAD_PHASE_2D_SW_H + +#include "math_funcs.h" +#include "math_2d.h" + +class CollisionObject2DSW; + + +class BroadPhase2DSW { + +public: + typedef BroadPhase2DSW* (*CreateFunction)(); + + static CreateFunction create_func; + + typedef uint32_t ID; + + + typedef void* (*PairCallback)(CollisionObject2DSW *A,int p_subindex_A,CollisionObject2DSW *B,int p_subindex_B,void *p_userdata); + typedef void (*UnpairCallback)(CollisionObject2DSW *A,int p_subindex_A,CollisionObject2DSW *B,int p_subindex_B,void *p_data,void *p_userdata); + + // 0 is an invalid ID + virtual ID create(CollisionObject2DSW *p_object_, int p_subindex=0)=0; + virtual void move(ID p_id, const Rect2& p_aabb)=0; + virtual void set_static(ID p_id, bool p_static)=0; + virtual void remove(ID p_id)=0; + + virtual CollisionObject2DSW *get_object(ID p_id) const=0; + virtual bool is_static(ID p_id) const=0; + virtual int get_subindex(ID p_id) const=0; + + virtual int cull_segment(const Vector2& p_from, const Vector2& p_to,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices=NULL)=0; + virtual int cull_aabb(const Rect2& p_aabb,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices=NULL)=0; + + virtual void set_pair_callback(PairCallback p_pair_callback,void *p_userdata)=0; + virtual void set_unpair_callback(UnpairCallback p_unpair_callback,void *p_userdata)=0; + + virtual void update()=0; + + virtual ~BroadPhase2DSW(); + +}; + +#endif // BROAD_PHASE_2D_SW_H diff --git a/servers/physics_2d/collision_object_2d_sw.cpp b/servers/physics_2d/collision_object_2d_sw.cpp new file mode 100644 index 0000000000..6e5a703aa2 --- /dev/null +++ b/servers/physics_2d/collision_object_2d_sw.cpp @@ -0,0 +1,220 @@ +/*************************************************************************/ +/* collision_object_2d_sw.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#include "collision_object_2d_sw.h" +#include "space_2d_sw.h" + +void CollisionObject2DSW::add_shape(Shape2DSW *p_shape,const Matrix32& p_transform) { + + Shape s; + s.shape=p_shape; + s.xform=p_transform; + s.xform_inv=s.xform.affine_inverse(); + s.bpid=0; //needs update + s.trigger=false; + shapes.push_back(s); + p_shape->add_owner(this); + _update_shapes(); + _shapes_changed(); + +} + +void CollisionObject2DSW::set_shape(int p_index,Shape2DSW *p_shape){ + + ERR_FAIL_INDEX(p_index,shapes.size()); + shapes[p_index].shape->remove_owner(this); + shapes[p_index].shape=p_shape; + + p_shape->add_owner(this); + _update_shapes(); + _shapes_changed(); + +} +void CollisionObject2DSW::set_shape_transform(int p_index,const Matrix32& p_transform){ + + ERR_FAIL_INDEX(p_index,shapes.size()); + + shapes[p_index].xform=p_transform; + shapes[p_index].xform_inv=p_transform.affine_inverse(); + _update_shapes(); + _shapes_changed(); +} + +void CollisionObject2DSW::remove_shape(Shape2DSW *p_shape) { + + //remove a shape, all the times it appears + for(int i=0;i<shapes.size();i++) { + + if (shapes[i].shape==p_shape) { + remove_shape(i); + i--; + } + } +} + +void CollisionObject2DSW::remove_shape(int p_index){ + + //remove anything from shape to be erased to end, so subindices don't change + ERR_FAIL_INDEX(p_index,shapes.size()); + for(int i=p_index;i<shapes.size();i++) { + + if (shapes[i].bpid==0) + continue; + //should never get here with a null owner + space->get_broadphase()->remove(shapes[i].bpid); + shapes[i].bpid=0; + } + shapes[p_index].shape->remove_owner(this); + shapes.remove(p_index); + + _shapes_changed(); + +} + +void CollisionObject2DSW::_set_static(bool p_static) { + if (_static==p_static) + return; + _static=p_static; + + if (!space) + return; + for(int i=0;i<get_shape_count();i++) { + Shape &s=shapes[i]; + if (s.bpid>0) { + space->get_broadphase()->set_static(s.bpid,_static); + } + } + +} + +void CollisionObject2DSW::_unregister_shapes() { + + for(int i=0;i<shapes.size();i++) { + + Shape &s=shapes[i]; + if (s.bpid>0) { + space->get_broadphase()->remove(s.bpid); + s.bpid=0; + } + } + +} + +void CollisionObject2DSW::_update_shapes() { + + if (!space) + return; + + for(int i=0;i<shapes.size();i++) { + + Shape &s=shapes[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.. + Rect2 shape_aabb=s.shape->get_aabb(); + Matrix32 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 ); + + + space->get_broadphase()->move(s.bpid,s.aabb_cache); + } + +} + +void CollisionObject2DSW::_update_shapes_with_motion(const Vector2& p_motion) { + + + if (!space) + return; + + for(int i=0;i<shapes.size();i++) { + + Shape &s=shapes[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.. + Rect2 shape_aabb=s.shape->get_aabb(); + Matrix32 xform = transform * s.xform; + shape_aabb=xform.xform(shape_aabb); + shape_aabb=shape_aabb.merge(Rect2( shape_aabb.pos+p_motion,shape_aabb.size)); //use motion + s.aabb_cache=shape_aabb; + + space->get_broadphase()->move(s.bpid,shape_aabb); + } + + +} + +void CollisionObject2DSW::_set_space(Space2DSW *p_space) { + + if (space) { + + space->remove_object(this); + + for(int i=0;i<shapes.size();i++) { + + Shape &s=shapes[i]; + if (s.bpid) { + space->get_broadphase()->remove(s.bpid); + s.bpid=0; + } + } + + } + + space=p_space; + + if (space) { + + space->add_object(this); + _update_shapes(); + } + +} + +void CollisionObject2DSW::_shape_changed() { + + _update_shapes(); + _shapes_changed(); +} + +CollisionObject2DSW::CollisionObject2DSW(Type p_type) { + + _static=true; + type=p_type; + space=NULL; + instance_id=0; +} diff --git a/servers/physics_2d/collision_object_2d_sw.h b/servers/physics_2d/collision_object_2d_sw.h new file mode 100644 index 0000000000..b2d2c25451 --- /dev/null +++ b/servers/physics_2d/collision_object_2d_sw.h @@ -0,0 +1,123 @@ +/*************************************************************************/ +/* collision_object_2d_sw.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#ifndef COLLISION_OBJECT_2D_SW_H +#define COLLISION_OBJECT_2D_SW_H + +#include "shape_2d_sw.h" +#include "servers/physics_2d_server.h" +#include "self_list.h" +#include "broad_phase_2d_sw.h" + +class Space2DSW; + +class CollisionObject2DSW : public ShapeOwner2DSW { +public: + enum Type { + TYPE_AREA, + TYPE_BODY + }; +private: + + Type type; + RID self; + ObjectID instance_id; + + struct Shape { + + Matrix32 xform; + Matrix32 xform_inv; + BroadPhase2DSW::ID bpid; + Rect2 aabb_cache; //for rayqueries + Shape2DSW *shape; + bool trigger; + Shape() { trigger=false; } + }; + + Vector<Shape> shapes; + Space2DSW *space; + Matrix32 transform; + Matrix32 inv_transform; + bool _static; + + void _update_shapes(); + +protected: + + + void _update_shapes_with_motion(const Vector2& p_motion); + void _unregister_shapes(); + + _FORCE_INLINE_ void _set_transform(const Matrix32& p_transform) { transform=p_transform; _update_shapes(); } + _FORCE_INLINE_ void _set_inv_transform(const Matrix32& p_transform) { inv_transform=p_transform; } + void _set_static(bool p_static); + + virtual void _shapes_changed()=0; + void _set_space(Space2DSW *space); + + CollisionObject2DSW(Type p_type); +public: + + _FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; } + _FORCE_INLINE_ RID get_self() const { return self; } + + _FORCE_INLINE_ void set_instance_id(const ObjectID& p_instance_id) { instance_id=p_instance_id; } + _FORCE_INLINE_ ObjectID get_instance_id() const { return instance_id; } + + void _shape_changed(); + + _FORCE_INLINE_ Type get_type() const { return type; } + void add_shape(Shape2DSW *p_shape,const Matrix32& p_transform=Matrix32()); + void set_shape(int p_index,Shape2DSW *p_shape); + void set_shape_transform(int p_index,const Matrix32& p_transform); + _FORCE_INLINE_ int get_shape_count() const { return shapes.size(); } + _FORCE_INLINE_ Shape2DSW *get_shape(int p_index) const { return shapes[p_index].shape; } + _FORCE_INLINE_ const Matrix32& get_shape_transform(int p_index) const { return shapes[p_index].xform; } + _FORCE_INLINE_ const Matrix32& get_shape_inv_transform(int p_index) const { return shapes[p_index].xform_inv; } + _FORCE_INLINE_ const Rect2& get_shape_aabb(int p_index) const { return shapes[p_index].aabb_cache; } + + _FORCE_INLINE_ Matrix32 get_transform() const { return transform; } + _FORCE_INLINE_ Matrix32 get_inv_transform() const { return inv_transform; } + _FORCE_INLINE_ Space2DSW* get_space() const { return space; } + + _FORCE_INLINE_ void set_shape_as_trigger(int p_idx,bool p_enable) { shapes[p_idx].trigger=p_enable; } + _FORCE_INLINE_ bool is_shape_set_as_trigger(int p_idx) const { return shapes[p_idx].trigger; } + + + void remove_shape(Shape2DSW *p_shape); + void remove_shape(int p_index); + + virtual void set_space(Space2DSW *p_space)=0; + + _FORCE_INLINE_ bool is_static() const { return _static; } + + virtual ~CollisionObject2DSW() {} + +}; + +#endif // COLLISION_OBJECT_2D_SW_H diff --git a/servers/physics_2d/collision_solver_2d_sat.cpp b/servers/physics_2d/collision_solver_2d_sat.cpp new file mode 100644 index 0000000000..4b87ff02a2 --- /dev/null +++ b/servers/physics_2d/collision_solver_2d_sat.cpp @@ -0,0 +1,1034 @@ +/*************************************************************************/ +/* collision_solver_2d_sat.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#include "collision_solver_2d_sat.h" +#include "geometry.h" + +struct _CollectorCallback2D { + + CollisionSolver2DSW::CallbackResult callback; + void *userdata; + bool swap; + bool collided; + Vector2 normal; + Vector2 *sep_axis; + + _FORCE_INLINE_ void call(const Vector2& p_point_A, const Vector2& p_point_B) { + + //if (normal.dot(p_point_A) >= normal.dot(p_point_B)) + // return; + if (swap) + callback(p_point_B,p_point_A,userdata); + else + callback(p_point_A,p_point_B,userdata); + } + +}; + +typedef void (*GenerateContactsFunc)(const Vector2 *,int, const Vector2 *,int ,_CollectorCallback2D *); + + +_FORCE_INLINE_ static void _generate_contacts_point_point(const Vector2 * p_points_A,int p_point_count_A, const Vector2 * p_points_B,int p_point_count_B,_CollectorCallback2D *p_collector) { + +#ifdef DEBUG_ENABLED + ERR_FAIL_COND( p_point_count_A != 1 ); + ERR_FAIL_COND( p_point_count_B != 1 ); +#endif + + p_collector->call(*p_points_A,*p_points_B); +} + +_FORCE_INLINE_ static void _generate_contacts_point_edge(const Vector2 * p_points_A,int p_point_count_A, const Vector2 * p_points_B,int p_point_count_B,_CollectorCallback2D *p_collector) { + +#ifdef DEBUG_ENABLED + ERR_FAIL_COND( p_point_count_A != 1 ); + ERR_FAIL_COND( p_point_count_B != 2 ); +#endif + + Vector2 closest_B = Geometry::get_closest_point_to_segment_uncapped_2d(*p_points_A, p_points_B ); + p_collector->call(*p_points_A,closest_B); + +} + + +struct _generate_contacts_Pair { + int idx; + float d; + _FORCE_INLINE_ bool operator <(const _generate_contacts_Pair& l) const { return d< l.d; } +}; + +_FORCE_INLINE_ static void _generate_contacts_edge_edge(const Vector2 * p_points_A,int p_point_count_A, const Vector2 * p_points_B,int p_point_count_B,_CollectorCallback2D *p_collector) { + +#ifdef DEBUG_ENABLED + ERR_FAIL_COND( p_point_count_A != 2 ); + ERR_FAIL_COND( p_point_count_B != 2 ); // circle is actually a 4x3 matrix +#endif + + + Vector2 rel_A=p_points_A[1]-p_points_A[0]; + Vector2 rel_B=p_points_B[1]-p_points_B[0]; + + Vector2 t = p_collector->normal.tangent(); + + real_t dA[2]={t.dot(p_points_A[0]),t.dot(p_points_A[1])}; + Vector2 pA[2]={p_points_A[0],p_points_A[1]}; + + if (dA[0]>dA[1]) { + SWAP(dA[0],dA[1]); + SWAP(pA[0],pA[1]); + } + + float dB[2]={t.dot(p_points_B[0]),t.dot(p_points_B[1])}; + Vector2 pB[2]={p_points_B[0],p_points_B[1]}; + if (dB[0]>dB[1]) { + SWAP(dB[0],dB[1]); + SWAP(pB[0],pB[1]); + } + + + if (dA[0]<dB[0]) { + + Vector2 n = (p_points_A[1]-p_points_A[0]).normalized().tangent(); + real_t d = n.dot(p_points_A[1]); + + if (dA[1]>dB[1]) { + //A contains B + for(int i=0;i<2;i++) { + + Vector2 b = p_points_B[i]; + Vector2 a = n.plane_project(d,b); + if (p_collector->normal.dot(a) > p_collector->normal.dot(b)-CMP_EPSILON) + continue; + p_collector->call(a,b); + + } + } else { + + // B0,A1 containment + + Vector2 n_B = (p_points_B[1]-p_points_B[0]).normalized().tangent(); + real_t d_B = n_B.dot(p_points_B[1]); + + // first, B on A + + { + Vector2 b = p_points_B[0]; + Vector2 a = n.plane_project(d,b); + if (p_collector->normal.dot(a) < p_collector->normal.dot(b)-CMP_EPSILON) + p_collector->call(a,b); + } + + // second, A on B + + { + Vector2 a = p_points_A[1]; + Vector2 b = n_B.plane_project(d_B,a); + if (p_collector->normal.dot(a) < p_collector->normal.dot(b)-CMP_EPSILON) + p_collector->call(a,b); + } + + + + } + + + } else { + + Vector2 n = (p_points_B[1]-p_points_B[0]).normalized().tangent(); + real_t d = n.dot(p_points_B[1]); + + if (dB[1]>dA[1]) { + //B contains A + for(int i=0;i<2;i++) { + + Vector2 a = p_points_A[i]; + Vector2 b = n.plane_project(d,a); + if (p_collector->normal.dot(a) > p_collector->normal.dot(b)-CMP_EPSILON) + continue; + p_collector->call(a,b); + } + } else { + + // A0,B1 containment + Vector2 n_A = (p_points_A[1]-p_points_A[0]).normalized().tangent(); + real_t d_A = n_A.dot(p_points_A[1]); + + // first A on B + + { + Vector2 a = p_points_A[0]; + Vector2 b = n.plane_project(d,a); + if (p_collector->normal.dot(a) < p_collector->normal.dot(b)-CMP_EPSILON) + p_collector->call(a,b); + + } + + //second, B on A + + { + + Vector2 b = p_points_B[1]; + Vector2 a = n_A.plane_project(d_A,b); + if (p_collector->normal.dot(a) < p_collector->normal.dot(b)-CMP_EPSILON) + p_collector->call(a,b); + } + + } + } + + +#if 0 + + Vector2 axis = rel_A.normalized(); + Vector2 axis_B = rel_B.normalized(); + if (axis.dot(axis_B)<0) + axis_B=-axis_B; + axis=(axis+axis_B)*0.5; + + Vector2 normal_A = axis.tangent(); + real_t dA = normal_A.dot(p_points_A[0]); + Vector2 normal_B = rel_B.tangent().normalized(); + real_t dB = normal_A.dot(p_points_B[0]); + + Vector2 A[4]={ normal_A.plane_project(dA,p_points_B[0]), normal_A.plane_project(dA,p_points_B[1]), p_points_A[0], p_points_A[1] }; + Vector2 B[4]={ p_points_B[0], p_points_B[1], normal_B.plane_project(dB,p_points_A[0]), normal_B.plane_project(dB,p_points_A[1]) }; + + _generate_contacts_Pair dvec[4]; + for(int i=0;i<4;i++) { + dvec[i].d=axis.dot(p_points_A[0]-A[i]); + dvec[i].idx=i; + } + + SortArray<_generate_contacts_Pair> sa; + sa.sort(dvec,4); + + for(int i=1;i<=2;i++) { + + Vector2 a = A[i]; + Vector2 b = B[i]; + if (p_collector->normal.dot(a) > p_collector->normal.dot(b)-CMP_EPSILON) + continue; + p_collector->call(a,b); + } + +#elif 0 + Vector2 axis = rel_A.normalized(); //make an axis + Vector2 axis_B = rel_B.normalized(); + if (axis.dot(axis_B)<0) + axis_B=-axis_B; + axis=(axis+axis_B)*0.5; + Vector2 base_A = p_points_A[0] - axis * axis.dot(p_points_A[0]); + Vector2 base_B = p_points_B[0] - axis * axis.dot(p_points_B[0]); + + //sort all 4 points in axis + float 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]) }; + + //todo , find max/min and then use 2 central points + SortArray<float> sa; + sa.sort(dvec,4); + + //use the middle ones as contacts + for (int i=1;i<=2;i++) { + + Vector2 a = base_A+axis*dvec[i]; + Vector2 b = base_B+axis*dvec[i]; + if (p_collector->normal.dot(a) > p_collector->normal.dot(b)-0.01) { + print_line("fail a: "+a); + print_line("fail b: "+b); + continue; + } + print_line("res a: "+a); + print_line("res b: "+b); + p_collector->call(a,b); + } +#endif +} + +static void _generate_contacts_from_supports(const Vector2 * p_points_A,int p_point_count_A, const Vector2 * p_points_B,int p_point_count_B,_CollectorCallback2D *p_collector) { + + +#ifdef DEBUG_ENABLED + ERR_FAIL_COND( p_point_count_A <1 ); + ERR_FAIL_COND( p_point_count_B <1 ); +#endif + + + static const GenerateContactsFunc generate_contacts_func_table[2][2]={ + { + _generate_contacts_point_point, + _generate_contacts_point_edge, + },{ + 0, + _generate_contacts_edge_edge, + } + }; + + int pointcount_B; + int pointcount_A; + const Vector2 *points_A; + const Vector2 *points_B; + + if (p_point_count_A > p_point_count_B) { + //swap + p_collector->swap = !p_collector->swap; + p_collector->normal = -p_collector->normal; + + pointcount_B = p_point_count_A; + pointcount_A = p_point_count_B; + points_A=p_points_B; + points_B=p_points_A; + } else { + + pointcount_B = p_point_count_B; + pointcount_A = p_point_count_A; + points_A=p_points_A; + points_B=p_points_B; + } + + int version_A = (pointcount_A > 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_collector); + +} + + + +template<class ShapeA, class ShapeB> +class SeparatorAxisTest2D { + + const ShapeA *shape_A; + const ShapeB *shape_B; + const Matrix32 *transform_A; + const Matrix32 *transform_B; + const Matrix32 *transform_inv_A; + const Matrix32 *transform_inv_B; + real_t best_depth; + Vector2 best_axis; + int best_axis_count; + int best_axis_index; + _CollectorCallback2D *callback; + +public: + + _FORCE_INLINE_ bool test_previous_axis() { + + if (callback && callback->sep_axis && *callback->sep_axis!=Vector2()) { + return test_axis(*callback->sep_axis); + } else { +#ifdef DEBUG_ENABLED + best_axis_count++; +#endif + + } + return true; + } + + _FORCE_INLINE_ bool test_axis(const Vector2& p_axis) { + + Vector2 axis=p_axis; + + + if ( Math::abs(axis.x)<CMP_EPSILON && + Math::abs(axis.y)<CMP_EPSILON) { + // strange case, try an upwards separator + axis=Vector2(0.0,1.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); + + min_B -= ( max_A - min_A ) * 0.5; + max_B += ( max_A - min_A ) * 0.5; + + real_t dmin = min_B - ( min_A + max_A ) * 0.5; + real_t dmax = max_B - ( min_A + max_A ) * 0.5; + + if (dmin > 0.0 || dmax < 0.0) { + if (callback && callback->sep_axis) + *callback->sep_axis=axis; +#ifdef DEBUG_ENABLED + best_axis_count++; +#endif + + return false; // doesn't contain 0 + } + + //use the smallest depth + + dmin = Math::abs(dmin); + + if ( dmax < dmin ) { + if ( dmax < best_depth ) { + best_depth=dmax; + best_axis=axis; +#ifdef DEBUG_ENABLED + best_axis_index=best_axis_count; +#endif + + } + } else { + if ( dmin < best_depth ) { + best_depth=dmin; + best_axis=-axis; // keep it as A axis +#ifdef DEBUG_ENABLED + best_axis_index=best_axis_count; +#endif + } + } + + // print_line("test axis: "+p_axis+" depth: "+rtos(best_depth)); +#ifdef DEBUG_ENABLED + best_axis_count++; +#endif + + return true; + } + + + _FORCE_INLINE_ void generate_contacts() { + + // nothing to do, don't generate + if (best_axis==Vector2(0.0,0.0)) + return; + + callback->collided=true; + + if (!callback->callback) + return; //only collide, no callback + static const int max_supports=2; + + + Vector2 supports_A[max_supports]; + int support_count_A; + shape_A->get_supports(transform_A->basis_xform_inv(-best_axis).normalized(),supports_A,support_count_A); + for(int i=0;i<support_count_A;i++) { + supports_A[i] = transform_A->xform(supports_A[i]); + } + + + + Vector2 supports_B[max_supports]; + int support_count_B; + shape_B->get_supports(transform_B->basis_xform_inv(best_axis).normalized(),supports_B,support_count_B); + for(int i=0;i<support_count_B;i++) { + supports_B[i] = transform_B->xform(supports_B[i]); + } +/* + + + print_line("**************************"); + printf("CBK: %p\n",callback->userdata); + print_line("type A: "+itos(shape_A->get_type())); + print_line("type B: "+itos(shape_B->get_type())); + print_line("xform A: "+*transform_A); + print_line("xform B: "+*transform_B); + print_line("normal: "+best_axis); + print_line("depth: "+rtos(best_depth)); + print_line("index: "+itos(best_axis_index)); + + for(int i=0;i<support_count_A;i++) { + + print_line("A-"+itos(i)+": "+supports_A[i]); + } + + for(int i=0;i<support_count_B;i++) { + + print_line("B-"+itos(i)+": "+supports_B[i]); + } +//*/ + + + + + callback->normal=best_axis; + _generate_contacts_from_supports(supports_A,support_count_A,supports_B,support_count_B,callback); + + if (callback && callback->sep_axis && *callback->sep_axis!=Vector2()) + *callback->sep_axis=Vector2(); //invalidate previous axis (no test) + //CollisionSolver2DSW::CallbackResult cbk=NULL; + //cbk(Vector2(),Vector2(),NULL); + + } + + _FORCE_INLINE_ SeparatorAxisTest2D(const ShapeA *p_shape_A,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a, const ShapeB *p_shape_B,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) { + best_depth=1e15; + shape_A=p_shape_A; + shape_B=p_shape_B; + transform_A=&p_transform_a; + transform_B=&p_transform_b; + transform_inv_A=&p_transform_inv_a; + transform_inv_B=&p_transform_inv_b; + callback=p_collector; +#ifdef DEBUG_ENABLED + best_axis_count=0; + best_axis_index=-1; +#endif + } + +}; + +/****** SAT TESTS *******/ +/****** SAT TESTS *******/ +/****** SAT TESTS *******/ +/****** SAT TESTS *******/ + + +typedef void (*CollisionFunc)(const Shape2DSW*,const Matrix32&,const Matrix32&,const Shape2DSW*,const Matrix32&,const Matrix32&,_CollectorCallback2D *p_collector); + +static void _collision_segment_segment(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) { + + const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a); + const SegmentShape2DSW *segment_B = static_cast<const SegmentShape2DSW*>(p_b); + + SeparatorAxisTest2D<SegmentShape2DSW,SegmentShape2DSW> separator(segment_A,p_transform_a,p_transform_inv_a,segment_B,p_transform_b,p_transform_inv_b,p_collector); + + if (!separator.test_previous_axis()) + return; + + if (!separator.test_axis(p_transform_inv_a.basis_xform_inv(segment_A->get_normal()).normalized())) + return; + if (!separator.test_axis(p_transform_inv_a.basis_xform_inv(segment_B->get_normal()).normalized())) + return; + + separator.generate_contacts(); + +} + +static void _collision_segment_circle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) { + + + const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a); + const CircleShape2DSW *circle_B = static_cast<const CircleShape2DSW*>(p_b); + + + SeparatorAxisTest2D<SegmentShape2DSW,CircleShape2DSW> separator(segment_A,p_transform_a,p_transform_inv_a,circle_B,p_transform_b,p_transform_inv_b,p_collector); + + if (!separator.test_previous_axis()) + return; + + if (!separator.test_axis( + (p_transform_a.xform(segment_A->get_b())-p_transform_a.xform(segment_A->get_a())).normalized().tangent() + )) + return; + +// if (!separator.test_axis(p_transform_inv_a.basis_xform_inv(segment_A->get_normal()).normalized())) +// return; + if (!separator.test_axis((p_transform_a.xform(segment_A->get_a())-p_transform_b.get_origin()).normalized())) + return; + if (!separator.test_axis((p_transform_a.xform(segment_A->get_b())-p_transform_b.get_origin()).normalized())) + return; + + + separator.generate_contacts(); + + +} + +static void _collision_segment_rectangle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) { + + const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a); + const RectangleShape2DSW *rectangle_B = static_cast<const RectangleShape2DSW*>(p_b); + + SeparatorAxisTest2D<SegmentShape2DSW,RectangleShape2DSW> separator(segment_A,p_transform_a,p_transform_inv_a,rectangle_B,p_transform_b,p_transform_inv_b,p_collector); + + if (!separator.test_previous_axis()) + return; + + if (!separator.test_axis(p_transform_inv_a.basis_xform_inv(segment_A->get_normal()).normalized())) + return; + + if (!separator.test_axis(p_transform_b.elements[0].normalized())) + return; + + if (!separator.test_axis(p_transform_b.elements[1].normalized())) + return; + + separator.generate_contacts(); + +} + +static void _collision_segment_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) { + + const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a); + const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b); + + SeparatorAxisTest2D<SegmentShape2DSW,CapsuleShape2DSW> separator(segment_A,p_transform_a,p_transform_inv_a,capsule_B,p_transform_b,p_transform_inv_b,p_collector); + + if (!separator.test_previous_axis()) + return; + + if (!separator.test_axis(p_transform_inv_a.basis_xform_inv(segment_A->get_normal()).normalized())) + return; + + if (!separator.test_axis(p_transform_b.elements[0].normalized())) + return; + + if (!separator.test_axis((p_transform_a.xform(segment_A->get_a())-(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*0.5)).normalized())) + return; + if (!separator.test_axis((p_transform_a.xform(segment_A->get_a())-(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*-0.5)).normalized())) + return; + if (!separator.test_axis((p_transform_a.xform(segment_A->get_b())-(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*0.5)).normalized())) + return; + if (!separator.test_axis((p_transform_a.xform(segment_A->get_b())-(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*-0.5)).normalized())) + return; + + separator.generate_contacts(); +} + +static void _collision_segment_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) { + + const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a); + const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b); + + SeparatorAxisTest2D<SegmentShape2DSW,ConvexPolygonShape2DSW> separator(segment_A,p_transform_a,p_transform_inv_a,convex_B,p_transform_b,p_transform_inv_b,p_collector); + + if (!separator.test_previous_axis()) + return; + + if (!separator.test_axis(p_transform_inv_a.basis_xform_inv(segment_A->get_normal()).normalized())) + return; + + for(int i=0;i<convex_B->get_point_count();i++) { + + if (!separator.test_axis( p_transform_inv_b.basis_xform_inv(convex_B->get_segment_normal(i)).normalized() )) + return; + } + + separator.generate_contacts(); + +} + + +///////// + +static void _collision_circle_circle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) { + + const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW*>(p_a); + const CircleShape2DSW *circle_B = static_cast<const CircleShape2DSW*>(p_b); + + + SeparatorAxisTest2D<CircleShape2DSW,CircleShape2DSW> separator(circle_A,p_transform_a,p_transform_inv_a,circle_B,p_transform_b,p_transform_inv_b,p_collector); + + if (!separator.test_previous_axis()) + return; + + if (!separator.test_axis((p_transform_a.get_origin()-p_transform_b.get_origin()).normalized())) + return; + + separator.generate_contacts(); + +} + +static void _collision_circle_rectangle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) { + + const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW*>(p_a); + const RectangleShape2DSW *rectangle_B = static_cast<const RectangleShape2DSW*>(p_b); + + + SeparatorAxisTest2D<CircleShape2DSW,RectangleShape2DSW> separator(circle_A,p_transform_a,p_transform_inv_a,rectangle_B,p_transform_b,p_transform_inv_b,p_collector); + + if (!separator.test_previous_axis()) + return; + + const Vector2 &sphere=p_transform_a.elements[2]; + const Vector2 *axis=&p_transform_b.elements[0]; + const Vector2& half_extents = rectangle_B->get_half_extents(); + + if (!separator.test_axis(axis[0].normalized())) + return; + + if (!separator.test_axis(axis[1].normalized())) + return; + + Vector2 local_v = p_transform_inv_b.xform(p_transform_a.get_origin()); + + Vector2 he( + (local_v.x<0) ? -half_extents.x : half_extents.x, + (local_v.y<0) ? -half_extents.y : half_extents.y + ); + + + if (!separator.test_axis((p_transform_b.xform(he)-sphere).normalized())) + return; + + separator.generate_contacts(); +} + +static void _collision_circle_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) { + + const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW*>(p_a); + const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b); + + + SeparatorAxisTest2D<CircleShape2DSW,CapsuleShape2DSW> separator(circle_A,p_transform_a,p_transform_inv_a,capsule_B,p_transform_b,p_transform_inv_b,p_collector); + + if (!separator.test_previous_axis()) + return; + + //capsule axis + if (!separator.test_axis(p_transform_b.elements[0].normalized())) + return; + + //capsule endpoints + if (!separator.test_axis((p_transform_a.get_origin()-(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*0.5)).normalized())) + return; + if (!separator.test_axis((p_transform_a.get_origin()-(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*-0.5)).normalized())) + return; + + + separator.generate_contacts(); + + +} + +static void _collision_circle_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) { + + const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW*>(p_a); + const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b); + + + SeparatorAxisTest2D<CircleShape2DSW,ConvexPolygonShape2DSW> separator(circle_A,p_transform_a,p_transform_inv_a,convex_B,p_transform_b,p_transform_inv_b,p_collector); + + if (!separator.test_previous_axis()) + return; + + //poly faces and poly points vs circle + for(int i=0;i<convex_B->get_point_count();i++) { + + if (!separator.test_axis( (p_transform_b.xform(convex_B->get_point(i))-p_transform_a.get_origin()).normalized() )) + return; + + if (!separator.test_axis( p_transform_inv_b.basis_xform_inv(convex_B->get_segment_normal(i)).normalized() )) + return; + } + + separator.generate_contacts(); +} + + +///////// + +static void _collision_rectangle_rectangle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) { + + const RectangleShape2DSW *rectangle_A = static_cast<const RectangleShape2DSW*>(p_a); + const RectangleShape2DSW *rectangle_B = static_cast<const RectangleShape2DSW*>(p_b); + + + SeparatorAxisTest2D<RectangleShape2DSW,RectangleShape2DSW> separator(rectangle_A,p_transform_a,p_transform_inv_a,rectangle_B,p_transform_b,p_transform_inv_b,p_collector); + + if (!separator.test_previous_axis()) + return; + + //box faces A + if (!separator.test_axis(p_transform_a.elements[0].normalized())) + return; + + if (!separator.test_axis(p_transform_a.elements[1].normalized())) + return; + + //box faces B + if (!separator.test_axis(p_transform_b.elements[0].normalized())) + return; + + if (!separator.test_axis(p_transform_b.elements[1].normalized())) + return; + + separator.generate_contacts(); +} + +static void _collision_rectangle_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) { + + const RectangleShape2DSW *rectangle_A = static_cast<const RectangleShape2DSW*>(p_a); + const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b); + + + SeparatorAxisTest2D<RectangleShape2DSW,CapsuleShape2DSW> separator(rectangle_A,p_transform_a,p_transform_inv_a,capsule_B,p_transform_b,p_transform_inv_b,p_collector); + + if (!separator.test_previous_axis()) + return; + + //box faces + if (!separator.test_axis(p_transform_a.elements[0].normalized())) + return; + + if (!separator.test_axis(p_transform_a.elements[1].normalized())) + return; + + //capsule axis + if (!separator.test_axis(p_transform_b.elements[0].normalized())) + return; + + + //box endpoints to capsule circles + + for(int i=0;i<2;i++) { + + Vector2 capsule_endpoint = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(i==0?0.5:-0.5); + + const Vector2& half_extents = rectangle_A->get_half_extents(); + Vector2 local_v = p_transform_inv_a.xform(capsule_endpoint); + + Vector2 he( + (local_v.x<0) ? -half_extents.x : half_extents.x, + (local_v.y<0) ? -half_extents.y : half_extents.y + ); + + + if (!separator.test_axis(p_transform_a.xform(he).normalized())) + return; + + } + + + separator.generate_contacts(); +} + +static void _collision_rectangle_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) { + + const RectangleShape2DSW *rectangle_A = static_cast<const RectangleShape2DSW*>(p_a); + const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b); + + SeparatorAxisTest2D<RectangleShape2DSW,ConvexPolygonShape2DSW> separator(rectangle_A,p_transform_a,p_transform_inv_a,convex_B,p_transform_b,p_transform_inv_b,p_collector); + + if (!separator.test_previous_axis()) + return; + + //box faces + if (!separator.test_axis(p_transform_a.elements[0].normalized())) + return; + + if (!separator.test_axis(p_transform_a.elements[1].normalized())) + return; + + //convex faces + for(int i=0;i<convex_B->get_point_count();i++) { + + if (!separator.test_axis( p_transform_inv_b.basis_xform_inv(convex_B->get_segment_normal(i)).normalized() )) + return; + } + + separator.generate_contacts(); + +} + + +///////// + +static void _collision_capsule_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) { + + const CapsuleShape2DSW *capsule_A = static_cast<const CapsuleShape2DSW*>(p_a); + const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b); + + + SeparatorAxisTest2D<CapsuleShape2DSW,CapsuleShape2DSW> separator(capsule_A,p_transform_a,p_transform_inv_a,capsule_B,p_transform_b,p_transform_inv_b,p_collector); + + if (!separator.test_previous_axis()) + return; + + //capsule axis + + if (!separator.test_axis(p_transform_b.elements[0].normalized())) + return; + + if (!separator.test_axis(p_transform_a.elements[0].normalized())) + return; + + //capsule endpoints + + for(int i=0;i<2;i++) { + + Vector2 capsule_endpoint_A = p_transform_a.get_origin()+p_transform_a.elements[1]*capsule_A->get_height()*(i==0?0.5:-0.5); + + for(int j=0;j<2;j++) { + + Vector2 capsule_endpoint_B = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(j==0?0.5:-0.5); + + if (!separator.test_axis( (capsule_endpoint_A-capsule_endpoint_B).normalized() )) + return; + + } + } + + separator.generate_contacts(); + +} + +static void _collision_capsule_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) { + + const CapsuleShape2DSW *capsule_A = static_cast<const CapsuleShape2DSW*>(p_a); + const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b); + + + SeparatorAxisTest2D<CapsuleShape2DSW,ConvexPolygonShape2DSW> separator(capsule_A,p_transform_a,p_transform_inv_a,convex_B,p_transform_b,p_transform_inv_b,p_collector); + + if (!separator.test_previous_axis()) + return; + + //capsule axis + + if (!separator.test_axis(p_transform_a.elements[0].normalized())) + return; + + + //poly vs capsule + for(int i=0;i<convex_B->get_point_count();i++) { + + Vector2 cpoint = p_transform_b.xform(convex_B->get_point(i)); + + for(int j=0;j<2;j++) { + + Vector2 capsule_endpoint_A = p_transform_a.get_origin()+p_transform_a.elements[1]*capsule_A->get_height()*(j==0?0.5:-0.5); + + if (!separator.test_axis( (cpoint - capsule_endpoint_A).normalized() )) + return; + + } + + if (!separator.test_axis( p_transform_inv_b.basis_xform_inv(convex_B->get_segment_normal(i)).normalized() )) + return; + } + + separator.generate_contacts(); +} + + +///////// + + +static void _collision_convex_polygon_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) { + + + const ConvexPolygonShape2DSW *convex_A = static_cast<const ConvexPolygonShape2DSW*>(p_a); + const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b); + + SeparatorAxisTest2D<ConvexPolygonShape2DSW,ConvexPolygonShape2DSW> separator(convex_A,p_transform_a,p_transform_inv_a,convex_B,p_transform_b,p_transform_inv_b,p_collector); + + if (!separator.test_previous_axis()) + return; + + for(int i=0;i<convex_A->get_point_count();i++) { + + if (!separator.test_axis( p_transform_inv_a.basis_xform_inv(convex_A->get_segment_normal(i)).normalized() )) + return; + } + + for(int i=0;i<convex_B->get_point_count();i++) { + + if (!separator.test_axis( p_transform_inv_b.basis_xform_inv(convex_B->get_segment_normal(i)).normalized() )) + return; + } + + separator.generate_contacts(); + +} + + +//////// + +bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Matrix32& p_transform_A, const Matrix32& p_transform_inv_A, const Shape2DSW *p_shape_B, const Matrix32& p_transform_B, const Matrix32& p_transform_inv_B, CollisionSolver2DSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap,Vector2 *sep_axis) { + + Physics2DServer::ShapeType type_A=p_shape_A->get_type(); + + ERR_FAIL_COND_V(type_A==Physics2DServer::SHAPE_LINE,false); + //ERR_FAIL_COND_V(type_A==Physics2DServer::SHAPE_RAY,false); + ERR_FAIL_COND_V(p_shape_A->is_concave(),false); + + Physics2DServer::ShapeType type_B=p_shape_B->get_type(); + + ERR_FAIL_COND_V(type_B==Physics2DServer::SHAPE_LINE,false); + //ERR_FAIL_COND_V(type_B==Physics2DServer::SHAPE_RAY,false); + ERR_FAIL_COND_V(p_shape_B->is_concave(),false); + + + static const CollisionFunc collision_table[5][5]={ + {_collision_segment_segment, + _collision_segment_circle, + _collision_segment_rectangle, + _collision_segment_capsule, + _collision_segment_convex_polygon}, + {0, + _collision_circle_circle, + _collision_circle_rectangle, + _collision_circle_capsule, + _collision_circle_convex_polygon}, + {0, + 0, + _collision_rectangle_rectangle, + _collision_rectangle_capsule, + _collision_rectangle_convex_polygon}, + {0, + 0, + 0, + _collision_capsule_capsule, + _collision_capsule_convex_polygon}, + {0, + 0, + 0, + 0, + _collision_convex_polygon_convex_polygon} + + }; + + _CollectorCallback2D callback; + callback.callback=p_result_callback; + callback.swap=p_swap; + callback.userdata=p_userdata; + callback.collided=false; + callback.sep_axis=sep_axis; + + const Shape2DSW *A=p_shape_A; + const Shape2DSW *B=p_shape_B; + const Matrix32 *transform_A=&p_transform_A; + const Matrix32 *transform_B=&p_transform_B; + const Matrix32 *transform_inv_A=&p_transform_inv_A; + const Matrix32 *transform_inv_B=&p_transform_inv_B; + + if (type_A > type_B) { + SWAP(A,B); + SWAP(transform_A,transform_B); + SWAP(transform_inv_A,transform_inv_B); + SWAP(type_A,type_B); + callback.swap = !callback.swap; + } + + + CollisionFunc collision_func = collision_table[type_A-2][type_B-2]; + ERR_FAIL_COND_V(!collision_func,false); + + + collision_func(A,*transform_A,*transform_inv_A,B,*transform_B,*transform_inv_B,&callback); + + return callback.collided; + + +} diff --git a/servers/physics_2d/collision_solver_2d_sat.h b/servers/physics_2d/collision_solver_2d_sat.h new file mode 100644 index 0000000000..dc6767d651 --- /dev/null +++ b/servers/physics_2d/collision_solver_2d_sat.h @@ -0,0 +1,37 @@ +/*************************************************************************/ +/* collision_solver_2d_sat.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#ifndef COLLISION_SOLVER_2D_SAT_H +#define COLLISION_SOLVER_2D_SAT_H + +#include "collision_solver_2d_sw.h" + + +bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Matrix32& p_transform_A, const Matrix32& p_transform_inv_A, const Shape2DSW *p_shape_B, const Matrix32& p_transform_B, const Matrix32& p_transform_inv_B, CollisionSolver2DSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap=false,Vector2 *sep_axis=NULL); + +#endif // COLLISION_SOLVER_2D_SAT_H diff --git a/servers/physics_2d/collision_solver_2d_sw.cpp b/servers/physics_2d/collision_solver_2d_sw.cpp new file mode 100644 index 0000000000..cee5582c6f --- /dev/null +++ b/servers/physics_2d/collision_solver_2d_sw.cpp @@ -0,0 +1,309 @@ +/*************************************************************************/ +/* collision_solver_2d_sw.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#include "collision_solver_2d_sw.h" +#include "collision_solver_2d_sat.h" + + +#define collision_solver sat_2d_calculate_penetration +//#define collision_solver gjk_epa_calculate_penetration + + +bool CollisionSolver2DSW::solve_static_line(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Matrix32& p_transform_inv_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Matrix32& p_transform_inv_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result) { + + + const LineShape2DSW *line = static_cast<const LineShape2DSW*>(p_shape_A); + if (p_shape_B->get_type()==Physics2DServer::SHAPE_LINE) + return false; + + + Vector2 n = p_transform_A.basis_xform(line->get_normal()).normalized(); + Vector2 p = p_transform_A.xform(line->get_normal()*line->get_d()); + real_t d = n.dot(p); + + Vector2 supports[2]; + int support_count; + + p_shape_B->get_supports(p_transform_inv_B.basis_xform(-n).normalized(),supports,support_count); + + bool found=false; + + + for(int i=0;i<support_count;i++) { + + supports[i] = p_transform_B.xform( supports[i] ); + real_t pd = n.dot(supports[i]); + if (pd>=d) + continue; + found=true; + + Vector2 support_A = supports[i] - n*(pd-d); + + if (p_result_callback) { + if (p_swap_result) + p_result_callback(supports[i],support_A,p_userdata); + else + p_result_callback(support_A,supports[i],p_userdata); + } + + } + + + return found; +} + +bool CollisionSolver2DSW::solve_raycast(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Matrix32& p_transform_inv_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Matrix32& p_transform_inv_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis) { + + + + const RayShape2DSW *ray = static_cast<const RayShape2DSW*>(p_shape_A); + if (p_shape_B->get_type()==Physics2DServer::SHAPE_RAY) + return false; + + Vector2 from = p_transform_A.get_origin(); + Vector2 to = from+p_transform_A[1]*ray->get_length(); + Vector2 support_A=to; + + from = p_transform_inv_B.xform(from); + to = p_transform_inv_B.xform(to); + + Vector2 p,n; + if (!p_shape_B->intersect_segment(from,to,p,n)) { + + if (sep_axis) + *sep_axis=p_transform_A[1].normalized(); + return false; + } + + + Vector2 support_B=p_transform_B.xform(p); + + 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; + +} + +/* +bool CollisionSolver2DSW::solve_ray(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Matrix32& p_inverse_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result) { + + + const RayShape2DSW *ray = static_cast<const RayShape2DSW*>(p_shape_A); + + Vector2 from = p_transform_A.origin; + Vector2 to = from+p_transform_A.basis.get_axis(2)*ray->get_length(); + Vector2 support_A=to; + + from = p_inverse_B.xform(from); + to = p_inverse_B.xform(to); + + Vector2 p,n; + if (!p_shape_B->intersect_segment(from,to,&p,&n)) + return false; + + Vector2 support_B=p_transform_B.xform(p); + + if (p_result_callback) { + if (p_swap_result) + p_result_callback(support_B,support_A,p_userdata); + else + p_result_callback(support_A,support_B,p_userdata); + } + return true; +} +*/ + +struct _ConcaveCollisionInfo2D { + + const Matrix32 *transform_A; + const Matrix32 *transform_inv_A; + const Shape2DSW *shape_A; + const Matrix32 *transform_B; + const Matrix32 *transform_inv_B; + CollisionSolver2DSW::CallbackResult result_callback; + void *userdata; + bool swap_result; + bool collided; + int aabb_tests; + int collisions; + Vector2 *sep_axis; + +}; + +void CollisionSolver2DSW::concave_callback(void *p_userdata, Shape2DSW *p_convex) { + + + + _ConcaveCollisionInfo2D &cinfo = *(_ConcaveCollisionInfo2D*)(p_userdata); + cinfo.aabb_tests++; + if (!cinfo.result_callback && cinfo.collided) + return; //already collided and no contacts requested, don't test anymore + + bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, *cinfo.transform_inv_A, p_convex,*cinfo.transform_B,*cinfo.transform_inv_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result,cinfo.sep_axis ); + if (!collided) + return; + + + cinfo.collided=true; + cinfo.collisions++; + +} + +bool CollisionSolver2DSW::solve_concave(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Matrix32& p_transform_inv_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Matrix32& p_transform_inv_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis) { + + + const ConcaveShape2DSW *concave_B=static_cast<const ConcaveShape2DSW*>(p_shape_B); + + _ConcaveCollisionInfo2D cinfo; + cinfo.transform_A=&p_transform_A; + cinfo.transform_inv_A=&p_transform_inv_A; + cinfo.shape_A=p_shape_A; + cinfo.transform_B=&p_transform_B; + cinfo.transform_inv_B=&p_transform_inv_B; + cinfo.result_callback=p_result_callback; + cinfo.userdata=p_userdata; + cinfo.swap_result=p_swap_result; + cinfo.collided=false; + cinfo.collisions=0; + cinfo.sep_axis=sep_axis; + + cinfo.aabb_tests=0; + + Matrix32 rel_transform = p_transform_A; + rel_transform.elements[2]-=p_transform_B.elements[2]; + + //quickly compute a local Rect2 + + Rect2 local_aabb; + for(int i=0;i<2;i++) { + + Vector2 axis( p_transform_B.elements[i] ); + float axis_scale = 1.0/axis.length(); + axis*=axis_scale; + + float smin,smax; + p_shape_A->project_rangev(axis,rel_transform,smin,smax); + smin*=axis_scale; + smax*=axis_scale; + + local_aabb.pos[i]=smin; + local_aabb.size[i]=smax-smin; + } + + concave_B->cull(local_aabb,concave_callback,&cinfo); + + +// print_line("Rect2 TESTS: "+itos(cinfo.aabb_tests)); + return cinfo.collided; +} + + +bool CollisionSolver2DSW::solve_static(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Matrix32& p_transform_inv_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Matrix32& p_transform_inv_B,CallbackResult p_result_callback,void *p_userdata,Vector2 *sep_axis) { + + + + Physics2DServer::ShapeType type_A=p_shape_A->get_type(); + Physics2DServer::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==Physics2DServer::SHAPE_LINE) { + + if (type_B==Physics2DServer::SHAPE_LINE || type_B==Physics2DServer::SHAPE_RAY) { + return false; + //if (type_B==Physics2DServer::SHAPE_RAY) { + // return false; + } + + if (swap) { + return solve_static_line(p_shape_B,p_transform_B,p_transform_inv_B,p_shape_A,p_transform_A,p_transform_inv_A,p_result_callback,p_userdata,true); + } else { + return solve_static_line(p_shape_A,p_transform_A,p_transform_inv_A,p_shape_B,p_transform_B,p_transform_inv_B,p_result_callback,p_userdata,false); + } + + /*} else if (type_A==Physics2DServer::SHAPE_RAY) { + + if (type_B==Physics2DServer::SHAPE_RAY) + return false; + + if (swap) { + return solve_ray(p_shape_B,p_transform_B,p_shape_A,p_transform_A,p_inverse_A,p_result_callback,p_userdata,true); + } else { + return solve_ray(p_shape_A,p_transform_A,p_shape_B,p_transform_B,p_inverse_B,p_result_callback,p_userdata,false); + } +*/ + } else if (type_A==Physics2DServer::SHAPE_RAY) { + + if (type_B==Physics2DServer::SHAPE_RAY) { + + return false; //no ray-ray + } + + + if (swap) { + return solve_raycast(p_shape_B,p_transform_B,p_transform_inv_B,p_shape_A,p_transform_A,p_transform_inv_A,p_result_callback,p_userdata,true,sep_axis); + } else { + return solve_raycast(p_shape_A,p_transform_A,p_transform_inv_A,p_shape_B,p_transform_B,p_transform_inv_B,p_result_callback,p_userdata,false,sep_axis); + } + + + } else if (concave_B) { + + + if (concave_A) + return false; + + if (!swap) + return solve_concave(p_shape_A,p_transform_A,p_transform_inv_A,p_shape_B,p_transform_B,p_transform_inv_B,p_result_callback,p_userdata,false,sep_axis); + else + return solve_concave(p_shape_B,p_transform_B,p_transform_inv_B,p_shape_A,p_transform_A,p_transform_inv_A,p_result_callback,p_userdata,true,sep_axis); + + + + } else { + + + return collision_solver(p_shape_A, p_transform_A, p_transform_inv_A, p_shape_B, p_transform_B, p_transform_inv_B, p_result_callback,p_userdata,false,sep_axis); + } + + + return false; +} + diff --git a/servers/physics_2d/collision_solver_2d_sw.h b/servers/physics_2d/collision_solver_2d_sw.h new file mode 100644 index 0000000000..fc5500bfb9 --- /dev/null +++ b/servers/physics_2d/collision_solver_2d_sw.h @@ -0,0 +1,52 @@ +/*************************************************************************/ +/* collision_solver_2d_sw.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#ifndef COLLISION_SOLVER_2D_SW_H +#define COLLISION_SOLVER_2D_SW_H + +#include "shape_2d_sw.h" + +class CollisionSolver2DSW { +public: + typedef void (*CallbackResult)(const Vector2& p_point_A,const Vector2& p_point_B,void *p_userdata); +private: + static bool solve_static_line(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Matrix32& p_transform_inv_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Matrix32& p_transform_inv_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result); + static void concave_callback(void *p_userdata, Shape2DSW *p_convex); + static bool solve_concave(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Matrix32& p_transform_inv_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Matrix32& p_transform_inv_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis=NULL); + static bool solve_raycast(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Matrix32& p_transform_inv_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Matrix32& p_transform_inv_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis=NULL); + + + +public: + + static bool solve_static(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Matrix32& p_inverse_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Matrix32& p_inverse_B,CallbackResult p_result_callback,void *p_userdata,Vector2 *sep_axis=NULL); + + +}; + +#endif // COLLISION_SOLVER_2D_SW_H diff --git a/servers/physics_2d/constraint_2d_sw.cpp b/servers/physics_2d/constraint_2d_sw.cpp new file mode 100644 index 0000000000..e97b5d794e --- /dev/null +++ b/servers/physics_2d/constraint_2d_sw.cpp @@ -0,0 +1,30 @@ +/*************************************************************************/ +/* constraint_2d_sw.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 "constraint_2d_sw.h" + diff --git a/servers/physics_2d/constraint_2d_sw.h b/servers/physics_2d/constraint_2d_sw.h new file mode 100644 index 0000000000..7abe49f5b4 --- /dev/null +++ b/servers/physics_2d/constraint_2d_sw.h @@ -0,0 +1,72 @@ +/*************************************************************************/ +/* constraint_2d_sw.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#ifndef CONSTRAINT_2D_SW_H +#define CONSTRAINT_2D_SW_H + +#include "body_2d_sw.h" + +class Constraint2DSW { + + Body2DSW **_body_ptr; + int _body_count; + uint64_t island_step; + Constraint2DSW *island_next; + Constraint2DSW *island_list_next; + + + RID self; + +protected: + Constraint2DSW(Body2DSW **p_body_ptr=NULL,int p_body_count=0) { _body_ptr=p_body_ptr; _body_count=p_body_count; island_step=0; } +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_ Constraint2DSW* get_island_next() const { return island_next; } + _FORCE_INLINE_ void set_island_next(Constraint2DSW* p_next) { island_next=p_next; } + + _FORCE_INLINE_ Constraint2DSW* get_island_list_next() const { return island_list_next; } + _FORCE_INLINE_ void set_island_list_next(Constraint2DSW* p_next) { island_list_next=p_next; } + + _FORCE_INLINE_ Body2DSW **get_body_ptr() const { return _body_ptr; } + _FORCE_INLINE_ int get_body_count() const { return _body_count; } + + + virtual bool setup(float p_step)=0; + virtual void solve(float p_step)=0; + + virtual ~Constraint2DSW() {} +}; + +#endif // CONSTRAINT_2D_SW_H diff --git a/servers/physics_2d/joints_2d_sw.cpp b/servers/physics_2d/joints_2d_sw.cpp new file mode 100644 index 0000000000..fea58b6e8d --- /dev/null +++ b/servers/physics_2d/joints_2d_sw.cpp @@ -0,0 +1,574 @@ +/*************************************************************************/ +/* joints_2d_sw.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#include "joints_2d_sw.h" +#include "space_2d_sw.h" + +//based on chipmunk joint constraints + +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +static inline real_t k_scalar(Body2DSW *a,Body2DSW *b,const Vector2& rA, const Vector2& rB, const Vector2& n) { + + + real_t value=0; + + + { + value+=a->get_inv_mass(); + real_t rcn = rA.cross(n); + value+=a->get_inv_inertia() * rcn * rcn; + } + + if (b) { + + value+=b->get_inv_mass(); + real_t rcn = rB.cross(n); + value+=b->get_inv_inertia() * rcn * rcn; + } + + return value; + +} + +static inline Vector2 +relative_velocity(Body2DSW *a, Body2DSW *b, Vector2 rA, Vector2 rB){ + Vector2 sum = a->get_linear_velocity() -rA.tangent() * a->get_angular_velocity(); + if (b) + return (b->get_linear_velocity() -rB.tangent() * b->get_angular_velocity()) - sum; + else + return -sum; +} + +static inline real_t +normal_relative_velocity(Body2DSW *a, Body2DSW *b, Vector2 rA, Vector2 rB, Vector2 n){ + return relative_velocity(a, b, rA, rB).dot(n); +} + +#if 0 + +bool PinJoint2DSW::setup(float p_step) { + + Space2DSW *space = A->get_space(); + ERR_FAIL_COND_V(!space,false;) + rA = A->get_transform().basis_xform(anchor_A); + rB = B?B->get_transform().basis_xform(anchor_B):anchor_B; + + Vector2 gA = A->get_transform().get_origin(); + Vector2 gB = B?B->get_transform().get_origin():Vector2(); + + Vector2 delta = gB - gA; + delta = (delta+rB) -rA; + + real_t jdist = delta.length(); + correct=false; + if (jdist==0) + return false; // do not correct + + correct=true; + + n = delta / jdist; + + // calculate mass normal + mass_normal = 1.0f/k_scalar(A, B, rA, rB, n); + + // calculate bias velocity + //real_t maxBias = joint->constraint.maxBias; + bias = -(get_bias()==0?space->get_constraint_bias():get_bias())*(1.0/p_step)*(jdist-dist); + bias = CLAMP(bias, -get_max_bias(), +get_max_bias()); + + // compute max impulse + jn_max = get_max_force() * p_step; + + // apply accumulated impulse + Vector2 j = n * jn_acc; + A->apply_impulse(rA,-j); + if (B) + B->apply_impulse(rB,j); + + print_line("setup"); + return true; +} + + + +void PinJoint2DSW::solve(float p_step){ + + if (!correct) + return; + + Vector2 ln = n; + + // compute relative velocity + real_t vrn = normal_relative_velocity(A,B, rA, rB, ln); + + // compute normal impulse + real_t jn = (bias - vrn)*mass_normal; + real_t jnOld = jn_acc; + jn_acc = CLAMP(jnOld + jn,-jn_max,jn_max); //cpfclamp(jnOld + jn, -joint->jnMax, joint->jnMax); + jn = jn_acc - jnOld; + print_line("jn_acc: "+rtos(jn_acc)); + Vector2 j = jn*ln; + + A->apply_impulse(rA,-j); + if (B) + B->apply_impulse(rB,j); + +} + + +PinJoint2DSW::PinJoint2DSW(const Vector2& p_pos,Body2DSW* p_body_a,Body2DSW* p_body_b) : Joint2DSW(_arr,p_body_b?2:1) { + + A=p_body_a; + B=p_body_b; + anchor_A = p_body_a->get_inv_transform().xform(p_pos); + anchor_B = p_body_b?p_body_b->get_inv_transform().xform(p_pos):p_pos; + + jn_acc=0; + dist=0; + + p_body_a->add_constraint(this,0); + if (p_body_b) + p_body_b->add_constraint(this,1); + +} + +PinJoint2DSW::~PinJoint2DSW() { + + if (A) + A->remove_constraint(this); + if (B) + B->remove_constraint(this); + +} + +#else + + +bool PinJoint2DSW::setup(float p_step) { + + Space2DSW *space = A->get_space(); + ERR_FAIL_COND_V(!space,false;) + rA = A->get_transform().basis_xform(anchor_A); + rB = B?B->get_transform().basis_xform(anchor_B):anchor_B; +#if 0 + Vector2 gA = rA+A->get_transform().get_origin(); + Vector2 gB = B?rB+B->get_transform().get_origin():rB; + + VectorB delta = gB - gA; + + real_t jdist = delta.length(); + correct=false; + if (jdist==0) + return false; // do not correct +#endif + + // deltaV = deltaV0 + K * impulse + // invM = [(1/m1 + 1/m2) * eye(2) - skew(rA) * invI1 * skew(rA) - skew(rB) * invI2 * skew(rB)] + // = [1/m1+1/m2 0 ] + invI1 * [rA.y*rA.y -rA.x*rA.y] + invI2 * [rA.y*rA.y -rA.x*rA.y] + // [ 0 1/m1+1/m2] [-rA.x*rA.y rA.x*rA.x] [-rA.x*rA.y rA.x*rA.x] + + real_t B_inv_mass = B?B->get_inv_mass():0.0; + + + Matrix32 K1; + K1[0].x = A->get_inv_mass() + B_inv_mass; K1[1].x = 0.0f; + K1[0].y = 0.0f; K1[1].y = A->get_inv_mass() + B_inv_mass; + + Matrix32 K2; + K2[0].x = A->get_inv_inertia() * rA.y * rA.y; K2[1].x = -A->get_inv_inertia() * rA.x * rA.y; + K2[0].y = -A->get_inv_inertia() * rA.x * rA.y; K2[1].y = A->get_inv_inertia() * rA.x * rA.x; + + Matrix32 K; + K[0]= K1[0] + K2[0]; + K[1]= K1[1] + K2[1]; + + if (B) { + + Matrix32 K3; + K3[0].x = B->get_inv_inertia() * rB.y * rB.y; K3[1].x = -B->get_inv_inertia() * rB.x * rB.y; + K3[0].y = -B->get_inv_inertia() * rB.x * rB.y; K3[1].y = B->get_inv_inertia() * rB.x * rB.x; + + K[0]+=K3[0]; + K[1]+=K3[1]; + } + + K[0].x += softness; + K[1].y += softness; + + M = K.affine_inverse(); + + Vector2 gA = rA+A->get_transform().get_origin(); + Vector2 gB = B?rB+B->get_transform().get_origin():rB; + + Vector2 delta = gB - gA; + + bias = delta*-(get_bias()==0?space->get_constraint_bias():get_bias())*(1.0/p_step); + + // apply accumulated impulse + A->apply_impulse(rA,-P); + if (B) + B->apply_impulse(rB,P); + + return true; +} + +void PinJoint2DSW::solve(float p_step){ + + + // compute relative velocity + Vector2 vA = A->get_linear_velocity() - rA.cross(A->get_angular_velocity()); + + Vector2 rel_vel; + if (B) + rel_vel = B->get_linear_velocity() - rB.cross(B->get_angular_velocity()) - vA; + else + rel_vel = -vA; + + Vector2 impulse = M.basis_xform(bias - rel_vel - Vector2(softness,softness) * P); + + A->apply_impulse(rA,-impulse); + if (B) + B->apply_impulse(rB,impulse); + + + P += impulse; +} + + +PinJoint2DSW::PinJoint2DSW(const Vector2& p_pos,Body2DSW* p_body_a,Body2DSW* p_body_b) : Joint2DSW(_arr,p_body_b?2:1) { + + A=p_body_a; + B=p_body_b; + anchor_A = p_body_a->get_inv_transform().xform(p_pos); + anchor_B = p_body_b?p_body_b->get_inv_transform().xform(p_pos):p_pos; + + softness=0; + + p_body_a->add_constraint(this,0); + if (p_body_b) + p_body_b->add_constraint(this,1); + +} + + + +PinJoint2DSW::~PinJoint2DSW() { + + if (A) + A->remove_constraint(this); + if (B) + B->remove_constraint(this); + +} + + + +#endif + +////////////////////////////////////////////// +////////////////////////////////////////////// +////////////////////////////////////////////// + + +static inline void +k_tensor(Body2DSW *a, Body2DSW *b, Vector2 r1, Vector2 r2, Vector2 *k1, Vector2 *k2) +{ + // calculate mass matrix + // If I wasn't lazy and wrote a proper matrix class, this wouldn't be so gross... + real_t k11, k12, k21, k22; + real_t m_sum = a->get_inv_mass() + b->get_inv_mass(); + + // start with I*m_sum + k11 = m_sum; k12 = 0.0f; + k21 = 0.0f; k22 = m_sum; + + // add the influence from r1 + real_t a_i_inv = a->get_inv_inertia(); + real_t r1xsq = r1.x * r1.x * a_i_inv; + real_t r1ysq = r1.y * r1.y * a_i_inv; + real_t r1nxy = -r1.x * r1.y * a_i_inv; + k11 += r1ysq; k12 += r1nxy; + k21 += r1nxy; k22 += r1xsq; + + // add the influnce from r2 + real_t b_i_inv = b->get_inv_inertia(); + real_t r2xsq = r2.x * r2.x * b_i_inv; + real_t r2ysq = r2.y * r2.y * b_i_inv; + real_t r2nxy = -r2.x * r2.y * b_i_inv; + k11 += r2ysq; k12 += r2nxy; + k21 += r2nxy; k22 += r2xsq; + + // invert + real_t determinant = k11*k22 - k12*k21; + ERR_FAIL_COND(determinant== 0.0); + + real_t det_inv = 1.0f/determinant; + *k1 = Vector2( k22*det_inv, -k12*det_inv); + *k2 = Vector2(-k21*det_inv, k11*det_inv); +} + +static _FORCE_INLINE_ Vector2 +mult_k(const Vector2& vr, const Vector2 &k1, const Vector2 &k2) +{ + return Vector2(vr.dot(k1), vr.dot(k2)); +} + +bool GrooveJoint2DSW::setup(float p_step) { + + + // calculate endpoints in worldspace + Vector2 ta = A->get_transform().xform(A_groove_1); + Vector2 tb = A->get_transform().xform(A_groove_2); + Space2DSW *space=A->get_space(); + + // calculate axis + Vector2 n = -(tb - ta).tangent().normalized(); + real_t d = ta.dot(n); + + xf_normal = n; + rB = B->get_transform().basis_xform(B_anchor); + + // calculate tangential distance along the axis of rB + real_t td = (B->get_transform().get_origin() + rB).cross(n); + // calculate clamping factor and rB + if(td <= ta.cross(n)){ + clamp = 1.0f; + rA = ta - A->get_transform().get_origin(); + } else if(td >= tb.cross(n)){ + clamp = -1.0f; + rA = tb - A->get_transform().get_origin(); + } else { + clamp = 0.0f; + //joint->r1 = cpvsub(cpvadd(cpvmult(cpvperp(n), -td), cpvmult(n, d)), a->p); + rA = ((-n.tangent() * -td) + n*d) - A->get_transform().get_origin(); + } + + // Calculate mass tensor + k_tensor(A, B, rA, rB, &k1, &k2); + + // compute max impulse + jn_max = get_max_force() * p_step; + + // calculate bias velocity +// cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1)); +// joint->bias = cpvclamp(cpvmult(delta, -joint->constraint.biasCoef*dt_inv), joint->constraint.maxBias); + + + Vector2 delta = (B->get_transform().get_origin() +rB) - (A->get_transform().get_origin() + rA); + float _b = get_bias(); + _b=0.001; + gbias=(delta*-(_b==0?space->get_constraint_bias():_b)*(1.0/p_step)).clamped(get_max_bias()); + + // apply accumulated impulse + A->apply_impulse(rA,-jn_acc); + B->apply_impulse(rB,jn_acc); + + correct=true; + return true; +} + +void GrooveJoint2DSW::solve(float p_step){ + + + // compute impulse + Vector2 vr = relative_velocity(A, B, rA,rB); + + Vector2 j = mult_k(gbias-vr, k1, k2); + Vector2 jOld = jn_acc; + j+=jOld; + + jn_acc = (((clamp * j.cross(xf_normal)) > 0) ? j : xf_normal.project(j)).clamped(jn_max); + + j = jn_acc - jOld; + + A->apply_impulse(rA,-j); + B->apply_impulse(rB,j); +} + + +GrooveJoint2DSW::GrooveJoint2DSW(const Vector2& p_a_groove1,const Vector2& p_a_groove2, const Vector2& p_b_anchor, Body2DSW* p_body_a,Body2DSW* p_body_b) : Joint2DSW(_arr,2) { + + A=p_body_a; + B=p_body_b; + + A_groove_1 = A->get_inv_transform().xform(p_a_groove1); + A_groove_2 = A->get_inv_transform().xform(p_a_groove2); + B_anchor=B->get_inv_transform().xform(p_b_anchor); + A_groove_normal = -(A_groove_2 - A_groove_1).normalized().tangent(); + + A->add_constraint(this,0); + B->add_constraint(this,1); + +} + +GrooveJoint2DSW::~GrooveJoint2DSW() { + + A->remove_constraint(this); + B->remove_constraint(this); +} + + +////////////////////////////////////////////// +////////////////////////////////////////////// +////////////////////////////////////////////// + + +bool DampedSpringJoint2DSW::setup(float p_step) { + + rA = A->get_transform().basis_xform(anchor_A); + rB = B->get_transform().basis_xform(anchor_B); + + Vector2 delta = (B->get_transform().get_origin() + rB) - (A->get_transform().get_origin() + rA) ; + real_t dist = delta.length(); + + if (dist) + n=delta/dist; + else + n=Vector2(); + + real_t k = k_scalar(A, B, rA, rB, n); + n_mass = 1.0f/k; + + target_vrn = 0.0f; + v_coef = 1.0f - Math::exp(-damping*(p_step)*k); + + // apply spring force + real_t f_spring = (rest_length - dist) * stiffness; + Vector2 j = n * f_spring*(p_step); + + A->apply_impulse(rA,-j); + B->apply_impulse(rB,j); + + + return true; +} + +void DampedSpringJoint2DSW::solve(float p_step) { + + // compute relative velocity + real_t vrn = normal_relative_velocity(A, B, rA, rB, n) - target_vrn; + + // compute velocity loss from drag + // not 100% certain this is derived correctly, though it makes sense + real_t v_damp = -vrn*v_coef; + target_vrn = vrn + v_damp; + Vector2 j=n*v_damp*n_mass; + + A->apply_impulse(rA,-j); + B->apply_impulse(rB,j); + +} + +void DampedSpringJoint2DSW::set_param(Physics2DServer::DampedStringParam p_param, real_t p_value) { + + switch(p_param) { + + case Physics2DServer::DAMPED_STRING_REST_LENGTH: { + + rest_length=p_value; + } break; + case Physics2DServer::DAMPED_STRING_DAMPING: { + + damping=p_value; + } break; + case Physics2DServer::DAMPED_STRING_STIFFNESS: { + + stiffness=p_value; + } break; + } + +} + +real_t DampedSpringJoint2DSW::get_param(Physics2DServer::DampedStringParam p_param) const{ + + switch(p_param) { + + case Physics2DServer::DAMPED_STRING_REST_LENGTH: { + + return rest_length; + } break; + case Physics2DServer::DAMPED_STRING_DAMPING: { + + return damping; + } break; + case Physics2DServer::DAMPED_STRING_STIFFNESS: { + + return stiffness; + } break; + } + + ERR_FAIL_V(0); +} + + +DampedSpringJoint2DSW::DampedSpringJoint2DSW(const Vector2& p_anchor_a,const Vector2& p_anchor_b, Body2DSW* p_body_a,Body2DSW* p_body_b) : Joint2DSW(_arr,2) { + + + A=p_body_a; + B=p_body_b; + anchor_A = A->get_inv_transform().xform(p_anchor_a); + anchor_B = B->get_inv_transform().xform(p_anchor_b); + + rest_length=p_anchor_a.distance_to(p_anchor_b); + stiffness=20; + damping=1.5; + + + A->add_constraint(this,0); + B->add_constraint(this,1); + +} + +DampedSpringJoint2DSW::~DampedSpringJoint2DSW() { + + A->remove_constraint(this); + B->remove_constraint(this); + +} + + diff --git a/servers/physics_2d/joints_2d_sw.h b/servers/physics_2d/joints_2d_sw.h new file mode 100644 index 0000000000..0a9bf34250 --- /dev/null +++ b/servers/physics_2d/joints_2d_sw.h @@ -0,0 +1,210 @@ +/*************************************************************************/ +/* joints_2d_sw.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#ifndef JOINTS_2D_SW_H +#define JOINTS_2D_SW_H + +#include "constraint_2d_sw.h" +#include "body_2d_sw.h" + + + +class Joint2DSW : public Constraint2DSW { + + real_t max_force; + real_t bias; + real_t max_bias; +public: + + _FORCE_INLINE_ void set_max_force(real_t p_force) { max_force=p_force; } + _FORCE_INLINE_ real_t get_max_force() const { return max_force; } + + _FORCE_INLINE_ void set_bias(real_t p_bias) { bias=p_bias; } + _FORCE_INLINE_ real_t get_bias() const { return bias; } + + _FORCE_INLINE_ void set_max_bias(real_t p_bias) { max_bias=p_bias; } + _FORCE_INLINE_ real_t get_max_bias() const { return max_bias; } + + virtual Physics2DServer::JointType get_type() const=0; + Joint2DSW(Body2DSW **p_body_ptr=NULL,int p_body_count=0) : Constraint2DSW(p_body_ptr,p_body_count) { bias=0; max_force=max_bias=3.40282e+38; }; + +}; +#if 0 + +class PinJoint2DSW : public Joint2DSW { + + union { + struct { + Body2DSW *A; + Body2DSW *B; + }; + + Body2DSW *_arr[2]; + }; + + Vector2 anchor_A; + Vector2 anchor_B; + real_t dist; + real_t jn_acc; + real_t jn_max; + real_t max_distance; + real_t mass_normal; + real_t bias; + + Vector2 rA,rB; + Vector2 n; //normal + bool correct; + + +public: + + virtual Physics2DServer::JointType get_type() const { return Physics2DServer::JOINT_PIN; } + + virtual bool setup(float p_step); + virtual void solve(float p_step); + + + PinJoint2DSW(const Vector2& p_pos,Body2DSW* p_body_a,Body2DSW* p_body_b=NULL); + ~PinJoint2DSW(); +}; + +#else + +class PinJoint2DSW : public Joint2DSW { + + union { + struct { + Body2DSW *A; + Body2DSW *B; + }; + + Body2DSW *_arr[2]; + }; + + Matrix32 M; + Vector2 rA,rB; + Vector2 anchor_A; + Vector2 anchor_B; + Vector2 bias; + Vector2 P; + real_t softness; + +public: + + virtual Physics2DServer::JointType get_type() const { return Physics2DServer::JOINT_PIN; } + + virtual bool setup(float p_step); + virtual void solve(float p_step); + + + PinJoint2DSW(const Vector2& p_pos,Body2DSW* p_body_a,Body2DSW* p_body_b=NULL); + ~PinJoint2DSW(); +}; + + +#endif +class GrooveJoint2DSW : public Joint2DSW { + + union { + struct { + Body2DSW *A; + Body2DSW *B; + }; + + Body2DSW *_arr[2]; + }; + + Vector2 A_groove_1; + Vector2 A_groove_2; + Vector2 A_groove_normal; + Vector2 B_anchor; + Vector2 jn_acc; + Vector2 gbias; + real_t jn_max; + real_t clamp; + Vector2 xf_normal; + Vector2 rA,rB; + Vector2 k1,k2; + + + bool correct; + +public: + + virtual Physics2DServer::JointType get_type() const { return Physics2DServer::JOINT_GROOVE; } + + virtual bool setup(float p_step); + virtual void solve(float p_step); + + + GrooveJoint2DSW(const Vector2& p_a_groove1,const Vector2& p_a_groove2, const Vector2& p_b_anchor, Body2DSW* p_body_a,Body2DSW* p_body_b); + ~GrooveJoint2DSW(); +}; + + +class DampedSpringJoint2DSW : public Joint2DSW { + + union { + struct { + Body2DSW *A; + Body2DSW *B; + }; + + Body2DSW *_arr[2]; + }; + + + Vector2 anchor_A; + Vector2 anchor_B; + + real_t rest_length; + real_t damping; + real_t stiffness; + + Vector2 rA,rB; + Vector2 n; + real_t n_mass; + real_t target_vrn; + real_t v_coef; + +public: + + virtual Physics2DServer::JointType get_type() const { return Physics2DServer::JOINT_DAMPED_SPRING; } + + virtual bool setup(float p_step); + virtual void solve(float p_step); + + void set_param(Physics2DServer::DampedStringParam p_param, real_t p_value); + real_t get_param(Physics2DServer::DampedStringParam p_param) const; + + DampedSpringJoint2DSW(const Vector2& p_anchor_a,const Vector2& p_anchor_b, Body2DSW* p_body_a,Body2DSW* p_body_b); + ~DampedSpringJoint2DSW(); +}; + + +#endif // JOINTS_2D_SW_H diff --git a/servers/physics_2d/physics_2d_server_sw.cpp b/servers/physics_2d/physics_2d_server_sw.cpp new file mode 100644 index 0000000000..70e3d843b4 --- /dev/null +++ b/servers/physics_2d/physics_2d_server_sw.cpp @@ -0,0 +1,1046 @@ +/*************************************************************************/ +/* physics_2d_server_sw.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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_2d_server_sw.h" +#include "broad_phase_2d_basic.h" +#include "broad_phase_2d_hash_grid.h" + +RID Physics2DServerSW::shape_create(ShapeType p_shape) { + + Shape2DSW *shape=NULL; + switch(p_shape) { + + case SHAPE_LINE: { + + shape=memnew( LineShape2DSW ); + } break; + case SHAPE_RAY: { + + shape=memnew( RayShape2DSW ); + } break; + case SHAPE_SEGMENT: { + + shape=memnew( SegmentShape2DSW); + } break; + case SHAPE_CIRCLE: { + + shape=memnew( CircleShape2DSW); + } break; + case SHAPE_RECTANGLE: { + + shape=memnew( RectangleShape2DSW); + } break; + case SHAPE_CAPSULE: { + + shape=memnew( CapsuleShape2DSW ); + } break; + case SHAPE_CONVEX_POLYGON: { + + shape=memnew( ConvexPolygonShape2DSW ); + } break; + case SHAPE_CONCAVE_POLYGON: { + + shape=memnew( ConcavePolygonShape2DSW ); + } break; + case SHAPE_CUSTOM: { + + ERR_FAIL_V(RID()); + + } break; + + } + + RID id = shape_owner.make_rid(shape); + shape->set_self(id); + + return id; +}; + +void Physics2DServerSW::shape_set_data(RID p_shape, const Variant& p_data) { + + Shape2DSW *shape = shape_owner.get(p_shape); + ERR_FAIL_COND(!shape); + shape->set_data(p_data); + + +}; + + +void Physics2DServerSW::shape_set_custom_solver_bias(RID p_shape, real_t p_bias) { + + Shape2DSW *shape = shape_owner.get(p_shape); + ERR_FAIL_COND(!shape); + shape->set_custom_bias(p_bias); + +} + + +Physics2DServer::ShapeType Physics2DServerSW::shape_get_type(RID p_shape) const { + + const Shape2DSW *shape = shape_owner.get(p_shape); + ERR_FAIL_COND_V(!shape,SHAPE_CUSTOM); + return shape->get_type(); + +}; + +Variant Physics2DServerSW::shape_get_data(RID p_shape) const { + + const Shape2DSW *shape = shape_owner.get(p_shape); + ERR_FAIL_COND_V(!shape,Variant()); + ERR_FAIL_COND_V(!shape->is_configured(),Variant()); + return shape->get_data(); + +}; + +real_t Physics2DServerSW::shape_get_custom_solver_bias(RID p_shape) const { + + const Shape2DSW *shape = shape_owner.get(p_shape); + ERR_FAIL_COND_V(!shape,0); + return shape->get_custom_bias(); + +} + + +RID Physics2DServerSW::space_create() { + + Space2DSW *space = memnew( Space2DSW ); + RID id = space_owner.make_rid(space); + space->set_self(id); + RID area_id = area_create(); + Area2DSW *area = area_owner.get(area_id); + ERR_FAIL_COND_V(!area,RID()); + space->set_default_area(area); + area->set_space(space); + area->set_priority(-1); + + return id; +}; + +void Physics2DServerSW::space_set_active(RID p_space,bool p_active) { + + Space2DSW *space = space_owner.get(p_space); + ERR_FAIL_COND(!space); + if (p_active) + active_spaces.insert(space); + else + active_spaces.erase(space); +} + +bool Physics2DServerSW::space_is_active(RID p_space) const { + + const Space2DSW *space = space_owner.get(p_space); + ERR_FAIL_COND_V(!space,false); + + return active_spaces.has(space); + +} + +void Physics2DServerSW::space_set_param(RID p_space,SpaceParameter p_param, real_t p_value) { + + Space2DSW *space = space_owner.get(p_space); + ERR_FAIL_COND(!space); + + space->set_param(p_param,p_value); + +} + +real_t Physics2DServerSW::space_get_param(RID p_space,SpaceParameter p_param) const { + + const Space2DSW *space = space_owner.get(p_space); + ERR_FAIL_COND_V(!space,0); + return space->get_param(p_param); +} + +Physics2DDirectSpaceState* Physics2DServerSW::space_get_direct_state(RID p_space) { + + Space2DSW *space = space_owner.get(p_space); + ERR_FAIL_COND_V(!space,NULL); + if (/*doing_sync ||*/ space->is_locked()) { + + ERR_EXPLAIN("Space state is inaccesible right now, wait for iteration or fixed process notification."); + ERR_FAIL_V(NULL); + } + + return space->get_direct_state(); +} + +RID Physics2DServerSW::area_create() { + + Area2DSW *area = memnew( Area2DSW ); + RID rid = area_owner.make_rid(area); + area->set_self(rid); + return rid; +}; + +void Physics2DServerSW::area_set_space(RID p_area, RID p_space) { + + Area2DSW *area = area_owner.get(p_area); + ERR_FAIL_COND(!area); + Space2DSW *space=NULL; + if (p_space.is_valid()) { + space = space_owner.get(p_space); + ERR_FAIL_COND(!space); + } + + area->set_space(space); + +}; + +RID Physics2DServerSW::area_get_space(RID p_area) const { + + Area2DSW *area = area_owner.get(p_area); + ERR_FAIL_COND_V(!area,RID()); + + Space2DSW *space = area->get_space(); + if (!space) + return RID(); + return space->get_self(); +}; + +void Physics2DServerSW::area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode) { + + + Area2DSW *area = area_owner.get(p_area); + ERR_FAIL_COND(!area); + + area->set_space_override_mode(p_mode); +} + +Physics2DServer::AreaSpaceOverrideMode Physics2DServerSW::area_get_space_override_mode(RID p_area) const { + + const Area2DSW *area = area_owner.get(p_area); + ERR_FAIL_COND_V(!area,AREA_SPACE_OVERRIDE_DISABLED); + + return area->get_space_override_mode(); +} + + +void Physics2DServerSW::area_add_shape(RID p_area, RID p_shape, const Matrix32& p_transform) { + + Area2DSW *area = area_owner.get(p_area); + ERR_FAIL_COND(!area); + + Shape2DSW *shape = shape_owner.get(p_shape); + ERR_FAIL_COND(!shape); + + area->add_shape(shape,p_transform); + +} + +void Physics2DServerSW::area_set_shape(RID p_area, int p_shape_idx,RID p_shape) { + + Area2DSW *area = area_owner.get(p_area); + ERR_FAIL_COND(!area); + + Shape2DSW *shape = shape_owner.get(p_shape); + ERR_FAIL_COND(!shape); + ERR_FAIL_COND(!shape->is_configured()); + + area->set_shape(p_shape_idx,shape); + +} +void Physics2DServerSW::area_set_shape_transform(RID p_area, int p_shape_idx, const Matrix32& p_transform) { + + Area2DSW *area = area_owner.get(p_area); + ERR_FAIL_COND(!area); + + area->set_shape_transform(p_shape_idx,p_transform); +} + +int Physics2DServerSW::area_get_shape_count(RID p_area) const { + + Area2DSW *area = area_owner.get(p_area); + ERR_FAIL_COND_V(!area,-1); + + return area->get_shape_count(); + +} +RID Physics2DServerSW::area_get_shape(RID p_area, int p_shape_idx) const { + + Area2DSW *area = area_owner.get(p_area); + ERR_FAIL_COND_V(!area,RID()); + + Shape2DSW *shape = area->get_shape(p_shape_idx); + ERR_FAIL_COND_V(!shape,RID()); + + return shape->get_self(); +} +Matrix32 Physics2DServerSW::area_get_shape_transform(RID p_area, int p_shape_idx) const { + + Area2DSW *area = area_owner.get(p_area); + ERR_FAIL_COND_V(!area,Matrix32()); + + return area->get_shape_transform(p_shape_idx); +} + +void Physics2DServerSW::area_remove_shape(RID p_area, int p_shape_idx) { + + Area2DSW *area = area_owner.get(p_area); + ERR_FAIL_COND(!area); + + area->remove_shape(p_shape_idx); +} + +void Physics2DServerSW::area_clear_shapes(RID p_area) { + + Area2DSW *area = area_owner.get(p_area); + ERR_FAIL_COND(!area); + + while(area->get_shape_count()) + area->remove_shape(0); + +} + +void Physics2DServerSW::area_attach_object_instance_ID(RID p_area,ObjectID p_ID) { + + if (space_owner.owns(p_area)) { + Space2DSW *space=space_owner.get(p_area); + p_area=space->get_default_area()->get_self(); + } + Area2DSW *area = area_owner.get(p_area); + ERR_FAIL_COND(!area); + area->set_instance_id(p_ID); + +} +ObjectID Physics2DServerSW::area_get_object_instance_ID(RID p_area) const { + + if (space_owner.owns(p_area)) { + Space2DSW *space=space_owner.get(p_area); + p_area=space->get_default_area()->get_self(); + } + Area2DSW *area = area_owner.get(p_area); + ERR_FAIL_COND_V(!area,0); + return area->get_instance_id(); + + +} + + +void Physics2DServerSW::area_set_param(RID p_area,AreaParameter p_param,const Variant& p_value) { + + if (space_owner.owns(p_area)) { + Space2DSW *space=space_owner.get(p_area); + p_area=space->get_default_area()->get_self(); + } + Area2DSW *area = area_owner.get(p_area); + ERR_FAIL_COND(!area); + area->set_param(p_param,p_value); + +}; + + +void Physics2DServerSW::area_set_transform(RID p_area, const Matrix32& p_transform) { + + Area2DSW *area = area_owner.get(p_area); + ERR_FAIL_COND(!area); + area->set_transform(p_transform); + +}; + +Variant Physics2DServerSW::area_get_param(RID p_area,AreaParameter p_param) const { + + if (space_owner.owns(p_area)) { + Space2DSW *space=space_owner.get(p_area); + p_area=space->get_default_area()->get_self(); + } + Area2DSW *area = area_owner.get(p_area); + ERR_FAIL_COND_V(!area,Variant()); + + return area->get_param(p_param); +}; + +Matrix32 Physics2DServerSW::area_get_transform(RID p_area) const { + + Area2DSW *area = area_owner.get(p_area); + ERR_FAIL_COND_V(!area,Matrix32()); + + return area->get_transform(); +}; + +void Physics2DServerSW::area_set_monitor_callback(RID p_area,Object *p_receiver,const StringName& p_method) { + + Area2DSW *area = area_owner.get(p_area); + ERR_FAIL_COND(!area); + + area->set_monitor_callback(p_receiver?p_receiver->get_instance_ID():0,p_method); + + +} + + +/* BODY API */ + +RID Physics2DServerSW::body_create(BodyMode p_mode,bool p_init_sleeping) { + + Body2DSW *body = memnew( Body2DSW ); + 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 Physics2DServerSW::body_set_space(RID p_body, RID p_space) { + + Body2DSW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + Space2DSW *space=NULL; + if (p_space.is_valid()) { + space = space_owner.get(p_space); + ERR_FAIL_COND(!space); + } + + body->set_space(space); + +}; + +RID Physics2DServerSW::body_get_space(RID p_body) const { + + Body2DSW *body = body_owner.get(p_body); + ERR_FAIL_COND_V(!body,RID()); + + Space2DSW *space = body->get_space(); + if (!space) + return RID(); + return space->get_self(); +}; + + +void Physics2DServerSW::body_set_mode(RID p_body, BodyMode p_mode) { + + Body2DSW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + + body->set_mode(p_mode); +}; + +Physics2DServer::BodyMode Physics2DServerSW::body_get_mode(RID p_body, BodyMode p_mode) const { + + Body2DSW *body = body_owner.get(p_body); + ERR_FAIL_COND_V(!body,BODY_MODE_STATIC); + + return body->get_mode(); +}; + +void Physics2DServerSW::body_add_shape(RID p_body, RID p_shape, const Matrix32& p_transform) { + + Body2DSW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + + Shape2DSW *shape = shape_owner.get(p_shape); + ERR_FAIL_COND(!shape); + + body->add_shape(shape,p_transform); + +} + +void Physics2DServerSW::body_set_shape(RID p_body, int p_shape_idx,RID p_shape) { + + Body2DSW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + + Shape2DSW *shape = shape_owner.get(p_shape); + ERR_FAIL_COND(!shape); + ERR_FAIL_COND(!shape->is_configured()); + + body->set_shape(p_shape_idx,shape); + +} +void Physics2DServerSW::body_set_shape_transform(RID p_body, int p_shape_idx, const Matrix32& p_transform) { + + Body2DSW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + + body->set_shape_transform(p_shape_idx,p_transform); +} + +int Physics2DServerSW::body_get_shape_count(RID p_body) const { + + Body2DSW *body = body_owner.get(p_body); + ERR_FAIL_COND_V(!body,-1); + + return body->get_shape_count(); + +} +RID Physics2DServerSW::body_get_shape(RID p_body, int p_shape_idx) const { + + Body2DSW *body = body_owner.get(p_body); + ERR_FAIL_COND_V(!body,RID()); + + Shape2DSW *shape = body->get_shape(p_shape_idx); + ERR_FAIL_COND_V(!shape,RID()); + + return shape->get_self(); +} +Matrix32 Physics2DServerSW::body_get_shape_transform(RID p_body, int p_shape_idx) const { + + Body2DSW *body = body_owner.get(p_body); + ERR_FAIL_COND_V(!body,Matrix32()); + + return body->get_shape_transform(p_shape_idx); +} + +void Physics2DServerSW::body_remove_shape(RID p_body, int p_shape_idx) { + + Body2DSW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + + body->remove_shape(p_shape_idx); +} + +void Physics2DServerSW::body_clear_shapes(RID p_body) { + + Body2DSW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + + while(body->get_shape_count()) + body->remove_shape(0); + +} + + +void Physics2DServerSW::body_set_shape_as_trigger(RID p_body, int p_shape_idx,bool p_enable) { + + Body2DSW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + + ERR_FAIL_INDEX(p_shape_idx,body->get_shape_count()); + + body->set_shape_as_trigger(p_shape_idx,p_enable); + +} + +bool Physics2DServerSW::body_is_shape_set_as_trigger(RID p_body, int p_shape_idx) const { + + const Body2DSW *body = body_owner.get(p_body); + ERR_FAIL_COND_V(!body,false); + + ERR_FAIL_INDEX_V(p_shape_idx,body->get_shape_count(),false); + + return body->is_shape_set_as_trigger(p_shape_idx); + +} + + +void Physics2DServerSW::body_set_enable_continuous_collision_detection(RID p_body,bool p_enable) { + + Body2DSW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + + body->set_continuous_collision_detection(p_enable); + +} + +bool Physics2DServerSW::body_is_continuous_collision_detection_enabled(RID p_body) const { + + Body2DSW *body = body_owner.get(p_body); + ERR_FAIL_COND_V(!body,false); + + return body->is_continuous_collision_detection_enabled(); +} + +void Physics2DServerSW::body_attach_object_instance_ID(RID p_body,uint32_t p_ID) { + + Body2DSW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + + body->set_instance_id(p_ID); + +}; + +uint32_t Physics2DServerSW::body_get_object_instance_ID(RID p_body) const { + + Body2DSW *body = body_owner.get(p_body); + ERR_FAIL_COND_V(!body,0); + + return body->get_instance_id(); +}; + + +void Physics2DServerSW::body_set_user_flags(RID p_body, uint32_t p_flags) { + + Body2DSW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + +}; + +uint32_t Physics2DServerSW::body_get_user_flags(RID p_body, uint32_t p_flags) const { + + Body2DSW *body = body_owner.get(p_body); + ERR_FAIL_COND_V(!body,0); + + return 0; +}; + +void Physics2DServerSW::body_set_param(RID p_body, BodyParameter p_param, float p_value) { + + Body2DSW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + + body->set_param(p_param,p_value); +}; + +float Physics2DServerSW::body_get_param(RID p_body, BodyParameter p_param) const { + + Body2DSW *body = body_owner.get(p_body); + ERR_FAIL_COND_V(!body,0); + + return body->get_param(p_param); +}; + + +void Physics2DServerSW::body_static_simulate_motion(RID p_body,const Matrix32& p_new_transform) { + + Body2DSW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + body->simulate_motion(p_new_transform,last_step); + +}; + +void Physics2DServerSW::body_set_state(RID p_body, BodyState p_state, const Variant& p_variant) { + + Body2DSW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + + body->set_state(p_state,p_variant); +}; + +Variant Physics2DServerSW::body_get_state(RID p_body, BodyState p_state) const { + + Body2DSW *body = body_owner.get(p_body); + ERR_FAIL_COND_V(!body,Variant()); + + return body->get_state(p_state); +}; + + +void Physics2DServerSW::body_set_applied_force(RID p_body, const Vector2& p_force) { + + Body2DSW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + + body->set_applied_force(p_force); +}; + +Vector2 Physics2DServerSW::body_get_applied_force(RID p_body) const { + + Body2DSW *body = body_owner.get(p_body); + ERR_FAIL_COND_V(!body,Vector2()); + return body->get_applied_force(); +}; + +void Physics2DServerSW::body_set_applied_torque(RID p_body, float p_torque) { + + Body2DSW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + + body->set_applied_torque(p_torque); +}; + +float Physics2DServerSW::body_get_applied_torque(RID p_body) const { + + Body2DSW *body = body_owner.get(p_body); + ERR_FAIL_COND_V(!body,0); + + return body->get_applied_torque(); +}; + +void Physics2DServerSW::body_apply_impulse(RID p_body, const Vector2& p_pos, const Vector2& p_impulse) { + + Body2DSW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + + body->apply_impulse(p_pos,p_impulse); +}; + +void Physics2DServerSW::body_set_axis_velocity(RID p_body, const Vector2& p_axis_velocity) { + + Body2DSW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + + Vector2 v = body->get_linear_velocity(); + Vector2 axis = p_axis_velocity.normalized(); + v-=axis*axis.dot(v); + v+=p_axis_velocity; + body->set_linear_velocity(v); + +}; + +void Physics2DServerSW::body_add_collision_exception(RID p_body, RID p_body_b) { + + Body2DSW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + + body->add_exception(p_body_b); + +}; + +void Physics2DServerSW::body_remove_collision_exception(RID p_body, RID p_body_b) { + + Body2DSW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + + body->remove_exception(p_body); + +}; + +void Physics2DServerSW::body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) { + + Body2DSW *body = body_owner.get(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 Physics2DServerSW::body_set_contacts_reported_depth_treshold(RID p_body, float p_treshold) { + + Body2DSW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + +}; + +float Physics2DServerSW::body_get_contacts_reported_depth_treshold(RID p_body) const { + + Body2DSW *body = body_owner.get(p_body); + ERR_FAIL_COND_V(!body,0); + return 0; +}; + +void Physics2DServerSW::body_set_omit_force_integration(RID p_body,bool p_omit) { + + Body2DSW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + + body->set_omit_force_integration(p_omit); +}; + +bool Physics2DServerSW::body_is_omitting_force_integration(RID p_body) const { + + Body2DSW *body = body_owner.get(p_body); + ERR_FAIL_COND_V(!body,false); + return body->get_omit_force_integration(); +}; + +void Physics2DServerSW::body_set_max_contacts_reported(RID p_body, int p_contacts) { + + Body2DSW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + body->set_max_contacts_reported(p_contacts); +} + +int Physics2DServerSW::body_get_max_contacts_reported(RID p_body) const { + + Body2DSW *body = body_owner.get(p_body); + ERR_FAIL_COND_V(!body,-1); + return body->get_max_contacts_reported(); +} + +void Physics2DServerSW::body_set_force_integration_callback(RID p_body,Object *p_receiver,const StringName& p_method,const Variant& p_udata) { + + + Body2DSW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + body->set_force_integration_callback(p_receiver?p_receiver->get_instance_ID():ObjectID(0),p_method,p_udata); + +} + + +/* JOINT API */ + +void Physics2DServerSW::joint_set_param(RID p_joint, JointParam p_param, real_t p_value) { + + Joint2DSW *joint = joint_owner.get(p_joint); + ERR_FAIL_COND(!joint); + + switch(p_param) { + case JOINT_PARAM_BIAS: joint->set_bias(p_value); break; + case JOINT_PARAM_MAX_BIAS: joint->set_max_bias(p_value); break; + case JOINT_PARAM_MAX_FORCE: joint->set_max_force(p_value); break; + } + + +} + +real_t Physics2DServerSW::joint_get_param(RID p_joint,JointParam p_param) const { + + const Joint2DSW *joint = joint_owner.get(p_joint); + ERR_FAIL_COND_V(!joint,-1); + + switch(p_param) { + case JOINT_PARAM_BIAS: return joint->get_bias(); break; + case JOINT_PARAM_MAX_BIAS: return joint->get_max_bias(); break; + case JOINT_PARAM_MAX_FORCE: return joint->get_max_force(); break; + } + + return 0; +} + + +RID Physics2DServerSW::pin_joint_create(const Vector2& p_pos,RID p_body_a,RID p_body_b) { + + Body2DSW *A=body_owner.get(p_body_a); + ERR_FAIL_COND_V(!A,RID()); + Body2DSW *B=NULL; + if (body_owner.owns(p_body_b)) { + B=body_owner.get(p_body_b); + ERR_FAIL_COND_V(!B,RID()); + } + + Joint2DSW *joint = memnew( PinJoint2DSW(p_pos,A,B) ); + RID self = joint_owner.make_rid(joint); + joint->set_self(self); + + return self; +} + +RID Physics2DServerSW::groove_joint_create(const Vector2& p_a_groove1,const Vector2& p_a_groove2, const Vector2& p_b_anchor, RID p_body_a,RID p_body_b) { + + + Body2DSW *A=body_owner.get(p_body_a); + ERR_FAIL_COND_V(!A,RID()); + + Body2DSW *B=body_owner.get(p_body_b); + ERR_FAIL_COND_V(!B,RID()); + + Joint2DSW *joint = memnew( GrooveJoint2DSW(p_a_groove1,p_a_groove2,p_b_anchor,A,B) ); + RID self = joint_owner.make_rid(joint); + joint->set_self(self); + return self; + + +} + +RID Physics2DServerSW::damped_spring_joint_create(const Vector2& p_anchor_a,const Vector2& p_anchor_b,RID p_body_a,RID p_body_b) { + + Body2DSW *A=body_owner.get(p_body_a); + ERR_FAIL_COND_V(!A,RID()); + + Body2DSW *B=body_owner.get(p_body_b); + ERR_FAIL_COND_V(!B,RID()); + + Joint2DSW *joint = memnew( DampedSpringJoint2DSW(p_anchor_a,p_anchor_b,A,B) ); + RID self = joint_owner.make_rid(joint); + joint->set_self(self); + return self; + +} + +void Physics2DServerSW::damped_string_joint_set_param(RID p_joint, DampedStringParam p_param, real_t p_value) { + + + Joint2DSW *j = joint_owner.get(p_joint); + ERR_FAIL_COND(!j); + ERR_FAIL_COND(j->get_type()!=JOINT_DAMPED_SPRING); + + DampedSpringJoint2DSW *dsj = static_cast<DampedSpringJoint2DSW*>(j); + dsj->set_param(p_param,p_value); +} + +real_t Physics2DServerSW::damped_string_joint_get_param(RID p_joint, DampedStringParam p_param) const { + + Joint2DSW *j = joint_owner.get(p_joint); + ERR_FAIL_COND_V(!j,0); + ERR_FAIL_COND_V(j->get_type()!=JOINT_DAMPED_SPRING,0); + + DampedSpringJoint2DSW *dsj = static_cast<DampedSpringJoint2DSW*>(j); + return dsj->get_param(p_param); +} + +Physics2DServer::JointType Physics2DServerSW::joint_get_type(RID p_joint) const { + + + Joint2DSW *joint = joint_owner.get(p_joint); + ERR_FAIL_COND_V(!joint,JOINT_PIN); + + return joint->get_type(); +} + + + +void Physics2DServerSW::free(RID p_rid) { + + if (shape_owner.owns(p_rid)) { + + Shape2DSW *shape = shape_owner.get(p_rid); + + while(shape->get_owners().size()) { + ShapeOwner2DSW *so=shape->get_owners().front()->key(); + so->remove_shape(shape); + } + + shape_owner.free(p_rid); + memdelete(shape); + } else if (body_owner.owns(p_rid)) { + + Body2DSW *body = body_owner.get(p_rid); + +// if (body->get_state_query()) +// _clear_query(body->get_state_query()); + +// if (body->get_direct_state_query()) +// _clear_query(body->get_direct_state_query()); + + body->set_space(NULL); + + + while( body->get_shape_count() ) { + + body->remove_shape(0); + } + + while (body->get_constraint_map().size()) { + RID self = body->get_constraint_map().front()->key()->get_self(); + ERR_FAIL_COND(!self.is_valid()); + free(self); + } + + body_owner.free(p_rid); + memdelete(body); + + } else if (area_owner.owns(p_rid)) { + + Area2DSW *area = area_owner.get(p_rid); + +// if (area->get_monitor_query()) +// _clear_query(area->get_monitor_query()); + + area->set_space(NULL); + + while( area->get_shape_count() ) { + + area->remove_shape(0); + } + + area_owner.free(p_rid); + memdelete(area); + } else if (space_owner.owns(p_rid)) { + + Space2DSW *space = space_owner.get(p_rid); + + while(space->get_objects().size()) { + CollisionObject2DSW *co = (CollisionObject2DSW *)space->get_objects().front()->get(); + co->set_space(NULL); + } + + active_spaces.erase(space); + free(space->get_default_area()->get_self()); + space_owner.free(p_rid); + memdelete(space); + } else if (joint_owner.owns(p_rid)) { + + Joint2DSW *joint = joint_owner.get(p_rid); + + joint_owner.free(p_rid); + memdelete(joint); + + } else { + + ERR_EXPLAIN("Invalid ID"); + ERR_FAIL(); + } + + +}; + +void Physics2DServerSW::set_active(bool p_active) { + + active=p_active; +}; + +void Physics2DServerSW::init() { + + doing_sync=true; + last_step=0.001; + iterations=8;// 8? + stepper = memnew( Step2DSW ); + direct_state = memnew( Physics2DDirectBodyStateSW ); +}; + + +void Physics2DServerSW::step(float p_step) { + + + if (!active) + return; + + doing_sync=false; + + last_step=p_step; + Physics2DDirectBodyStateSW::singleton->step=p_step; + for( Set<const Space2DSW*>::Element *E=active_spaces.front();E;E=E->next()) { + + stepper->step((Space2DSW*)E->get(),p_step,iterations); + } +}; + +void Physics2DServerSW::sync() { + +}; + +void Physics2DServerSW::flush_queries() { + + if (!active) + return; + + doing_sync=true; + for( Set<const Space2DSW*>::Element *E=active_spaces.front();E;E=E->next()) { + + Space2DSW *space=(Space2DSW *)E->get(); + space->call_queries(); + } + +}; + + + +void Physics2DServerSW::finish() { + + memdelete(stepper); + memdelete(direct_state); +}; + + +Physics2DServerSW::Physics2DServerSW() { + + BroadPhase2DSW::create_func=BroadPhase2DHashGrid::_create; +// BroadPhase2DSW::create_func=BroadPhase2DBasic::_create; + + active=true; +}; + +Physics2DServerSW::~Physics2DServerSW() { + +}; + + diff --git a/servers/physics_2d/physics_2d_server_sw.h b/servers/physics_2d/physics_2d_server_sw.h new file mode 100644 index 0000000000..f6665da73f --- /dev/null +++ b/servers/physics_2d/physics_2d_server_sw.h @@ -0,0 +1,215 @@ +/*************************************************************************/ +/* physics_2d_server_sw.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#ifndef PHYSICS_2D_SERVER_SW +#define PHYSICS_2D_SERVER_SW + + +#include "servers/physics_2d_server.h" +#include "shape_2d_sw.h" +#include "space_2d_sw.h" +#include "step_2d_sw.h" +#include "joints_2d_sw.h" + + +class Physics2DServerSW : public Physics2DServer { + + OBJ_TYPE( Physics2DServerSW, Physics2DServer ); + +friend class Physics2DDirectSpaceStateSW; + bool active; + int iterations; + bool doing_sync; + real_t last_step; + + Step2DSW *stepper; + Set<const Space2DSW*> active_spaces; + + Physics2DDirectBodyStateSW *direct_state; + + mutable RID_Owner<Shape2DSW> shape_owner; + mutable RID_Owner<Space2DSW> space_owner; + mutable RID_Owner<Area2DSW> area_owner; + mutable RID_Owner<Body2DSW> body_owner; + mutable RID_Owner<Joint2DSW> joint_owner; + +// void _clear_query(Query2DSW *p_query); +public: + + 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 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 fixed process, errors and returns null otherwise + virtual Physics2DDirectSpaceState* space_get_direct_state(RID p_space); + + + /* 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 Matrix32& p_transform=Matrix32()); + 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 Matrix32& 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 Matrix32 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_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 Matrix32& p_transform); + + virtual Variant area_get_param(RID p_parea,AreaParameter p_param) const; + virtual Matrix32 area_get_transform(RID p_area) const; + + virtual void area_set_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, BodyMode p_mode) const; + + virtual void body_add_shape(RID p_body, RID p_shape, const Matrix32& p_transform=Matrix32()); + 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 Matrix32& 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 Matrix32 body_get_shape_transform(RID p_body, int p_shape_idx) const; + + virtual void body_remove_shape(RID p_body, int p_shape_idx); + virtual void body_clear_shapes(RID p_body); + + virtual void body_set_shape_as_trigger(RID p_body, int p_shape_idx,bool p_enable); + virtual bool body_is_shape_set_as_trigger(RID p_body, int p_shape_idx) const; + + virtual void body_attach_object_instance_ID(RID p_body,uint32_t p_ID); + virtual uint32_t 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_user_flags(RID p_body, uint32_t p_flags); + virtual uint32_t body_get_user_flags(RID p_body, uint32_t p_flags) const; + + virtual void body_set_param(RID p_body, BodyParameter p_param, float p_value); + virtual float body_get_param(RID p_body, BodyParameter p_param) const; + + //advanced simulation + virtual void body_static_simulate_motion(RID p_body,const Matrix32& p_new_transform); + + 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 Vector2& p_force); + virtual Vector2 body_get_applied_force(RID p_body) const; + + virtual void body_set_applied_torque(RID p_body, float p_torque); + virtual float body_get_applied_torque(RID p_body) const; + + virtual void body_apply_impulse(RID p_body, const Vector2& p_pos, const Vector2& p_impulse); + virtual void body_set_axis_velocity(RID p_body, const Vector2& p_axis_velocity); + + 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_treshold(RID p_body, float p_treshold); + virtual float body_get_contacts_reported_depth_treshold(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()); + + /* JOINT API */ + + virtual void joint_set_param(RID p_joint, JointParam p_param, real_t p_value); + virtual real_t joint_get_param(RID p_joint,JointParam p_param) const; + + virtual RID pin_joint_create(const Vector2& p_pos,RID p_body_a,RID p_body_b=RID()); + virtual RID groove_joint_create(const Vector2& p_a_groove1,const Vector2& p_a_groove2, const Vector2& p_b_anchor, RID p_body_a,RID p_body_b); + virtual RID damped_spring_joint_create(const Vector2& p_anchor_a,const Vector2& p_anchor_b,RID p_body_a,RID p_body_b=RID()); + virtual void damped_string_joint_set_param(RID p_joint, DampedStringParam p_param, real_t p_value); + virtual real_t damped_string_joint_get_param(RID p_joint, DampedStringParam p_param) const; + + virtual JointType joint_get_type(RID p_joint) const; + + /* MISC */ + + virtual void free(RID p_rid); + + virtual void set_active(bool p_active); + virtual void init(); + virtual void step(float p_step); + virtual void sync(); + virtual void flush_queries(); + virtual void finish(); + + Physics2DServerSW(); + ~Physics2DServerSW(); + +}; + +#endif + diff --git a/servers/physics_2d/shape_2d_sw.cpp b/servers/physics_2d/shape_2d_sw.cpp new file mode 100644 index 0000000000..5ad05a18ac --- /dev/null +++ b/servers/physics_2d/shape_2d_sw.cpp @@ -0,0 +1,1085 @@ +/*************************************************************************/ +/* shape_2d_sw.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#include "shape_2d_sw.h"
+#include "geometry.h"
+#include "sort.h"
+
+#define _SEGMENT_IS_VALID_SUPPORT_TRESHOLD 0.99998
+
+
+void Shape2DSW::configure(const Rect2& p_aabb) {
+ aabb=p_aabb;
+ configured=true;
+ for (Map<ShapeOwner2DSW*,int>::Element *E=owners.front();E;E=E->next()) {
+ ShapeOwner2DSW* co=(ShapeOwner2DSW*)E->key();
+ co->_shape_changed();
+ }
+}
+
+
+Vector2 Shape2DSW::get_support(const Vector2& p_normal) const {
+
+ Vector2 res[2];
+ int amnt;
+ get_supports(p_normal,res,amnt);
+ return res[0];
+}
+
+void Shape2DSW::add_owner(ShapeOwner2DSW *p_owner) {
+
+ Map<ShapeOwner2DSW*,int>::Element *E=owners.find(p_owner);
+ if (E) {
+ E->get()++;
+ } else {
+ owners[p_owner]=1;
+ }
+}
+
+void Shape2DSW::remove_owner(ShapeOwner2DSW *p_owner){
+
+ Map<ShapeOwner2DSW*,int>::Element *E=owners.find(p_owner);
+ ERR_FAIL_COND(!E);
+ E->get()--;
+ if (E->get()==0) {
+ owners.erase(E);
+ }
+
+}
+
+bool Shape2DSW::is_owner(ShapeOwner2DSW *p_owner) const{
+
+ return owners.has(p_owner);
+
+}
+
+const Map<ShapeOwner2DSW*,int>& Shape2DSW::get_owners() const{
+ return owners;
+}
+
+
+Shape2DSW::Shape2DSW() {
+
+ custom_bias=0;
+ configured=false;
+}
+
+
+Shape2DSW::~Shape2DSW() {
+
+ ERR_FAIL_COND(owners.size());
+}
+
+
+/*********************************************************/
+/*********************************************************/
+/*********************************************************/
+
+
+
+void LineShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const {
+
+ r_amount=0;
+}
+
+bool LineShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const {
+
+ Vector2 segment= p_begin - p_end;
+ real_t den=normal.dot( segment );
+
+ //printf("den is %i\n",den);
+ if (Math::abs(den)<=CMP_EPSILON) {
+
+ return false;
+ }
+
+ real_t dist=(normal.dot( p_begin ) - d) / den;
+ //printf("dist is %i\n",dist);
+
+ if (dist<-CMP_EPSILON || dist > (1.0 +CMP_EPSILON)) {
+
+ return false;
+ }
+
+ r_point = p_begin + segment * -dist;
+ r_normal=normal;
+
+ return true;
+}
+
+real_t LineShape2DSW::get_moment_of_inertia(float p_mass) const {
+
+ return 0;
+}
+
+void LineShape2DSW::set_data(const Variant& p_data) {
+
+ ERR_FAIL_COND(p_data.get_type()!=Variant::ARRAY);
+ Array arr = p_data;
+ ERR_FAIL_COND(arr.size()!=2);
+ normal=arr[0];
+ d=arr[1];
+ configure(Rect2(Vector2(-1e4,-1e4),Vector2(1e4*2,1e4*2)));
+
+}
+
+Variant LineShape2DSW::get_data() const {
+
+ Array arr;
+ arr.resize(2);
+ arr[0]=normal;
+ arr[1]=d;
+ return arr;
+}
+
+/*********************************************************/
+/*********************************************************/
+/*********************************************************/
+
+
+
+void RayShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const {
+
+
+ r_amount=1;
+
+ if (p_normal.y>0)
+ *r_supports=Vector2(0,length);
+ else
+ *r_supports=Vector2();
+
+}
+
+bool RayShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const {
+
+ return false; //rays can't be intersected
+
+}
+
+real_t RayShape2DSW::get_moment_of_inertia(float p_mass) const {
+
+ return 0; //rays are mass-less
+}
+
+void RayShape2DSW::set_data(const Variant& p_data) {
+
+ length=p_data;
+ configure(Rect2(0,0,0.001,length));
+}
+
+Variant RayShape2DSW::get_data() const {
+
+ return length;
+}
+
+
+/*********************************************************/
+/*********************************************************/
+/*********************************************************/
+
+
+
+void SegmentShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const {
+
+ if (Math::abs(p_normal.dot(n))>_SEGMENT_IS_VALID_SUPPORT_TRESHOLD) {
+ r_supports[0]=a;
+ r_supports[1]=b;
+ r_amount=2;
+ return;
+
+ }
+
+ float dp=p_normal.dot(b-a);
+ if (dp>0)
+ *r_supports=b;
+ else
+ *r_supports=a;
+ r_amount=1;
+
+}
+
+bool SegmentShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const {
+
+ if (!Geometry::segment_intersects_segment_2d(p_begin,p_end,a,b,&r_point))
+ return false;
+
+ Vector2 d = p_end-p_begin;
+ if (n.dot(p_begin) > n.dot(a)) {
+ r_normal=n;
+ } else {
+ r_normal=-n;
+ }
+
+ return true;
+}
+
+real_t SegmentShape2DSW::get_moment_of_inertia(float p_mass) const {
+
+ real_t l = b.distance_to(a);
+ Vector2 ofs = (a+b)*0.5;
+
+ return p_mass*(l*l/12.0f + ofs.length_squared());
+}
+
+void SegmentShape2DSW::set_data(const Variant& p_data) {
+
+ ERR_FAIL_COND(p_data.get_type()!=Variant::RECT2);
+
+ Rect2 r = p_data;
+ a=r.pos;
+ b=r.size;
+ n=(b-a).tangent();
+
+ Rect2 aabb;
+ aabb.pos=a;
+ aabb.expand_to(b);
+ if (aabb.size.x==0)
+ aabb.size.x=0.001;
+ if (aabb.size.y==0)
+ aabb.size.y=0.001;
+ configure(aabb);
+}
+
+Variant SegmentShape2DSW::get_data() const {
+
+ Rect2 r;
+ r.pos=a;
+ r.size=b;
+ return r;
+}
+
+/*********************************************************/
+/*********************************************************/
+/*********************************************************/
+
+
+
+void CircleShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const {
+
+ r_amount=1;
+ *r_supports=p_normal*radius;
+
+}
+
+bool CircleShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const {
+
+
+ Vector2 line_vec = p_end - p_begin;
+
+ real_t a, b, c;
+
+ a = line_vec.dot(line_vec);
+ b = 2 * p_begin.dot(line_vec);
+ c = p_begin.dot(p_begin) - radius * radius;
+
+ real_t sqrtterm = b*b - 4*a*c;
+
+ if(sqrtterm < 0)
+ return false;
+ sqrtterm = Math::sqrt(sqrtterm);
+ real_t res = ( -b - sqrtterm ) / (2 * a);
+
+ if (res <0 || res >1+CMP_EPSILON) {
+ return false;
+ }
+
+ r_point=p_begin+line_vec*res;
+ r_normal=r_point.normalized();
+ return true;
+}
+
+real_t CircleShape2DSW::get_moment_of_inertia(float p_mass) const {
+
+ return radius*radius;
+}
+
+void CircleShape2DSW::set_data(const Variant& p_data) {
+
+ ERR_FAIL_COND(!p_data.is_num());
+ radius=p_data;
+ configure(Rect2(-radius,-radius,radius*2,radius*2));
+}
+
+Variant CircleShape2DSW::get_data() const {
+
+ return radius;
+}
+
+
+/*********************************************************/
+/*********************************************************/
+/*********************************************************/
+
+
+
+void RectangleShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const {
+
+ for(int i=0;i<2;i++) {
+
+ Vector2 ag;
+ ag[i]=1.0;
+ float dp = ag.dot(p_normal);
+ if (Math::abs(dp)<_SEGMENT_IS_VALID_SUPPORT_TRESHOLD)
+ continue;
+
+ float sgn = dp>0 ? 1.0 : -1.0;
+
+ r_amount=2;
+
+ r_supports[0][i]=half_extents[i]*sgn;
+ r_supports[0][i^1]=half_extents[i^1];
+
+ r_supports[1][i]=half_extents[i]*sgn;
+ r_supports[1][i^1]=-half_extents[i^1];
+
+ return;
+
+
+ }
+
+ /* USE POINT */
+
+ r_amount=1;
+ r_supports[0]=Vector2(
+ (p_normal.x<0) ? -half_extents.x : half_extents.x,
+ (p_normal.y<0) ? -half_extents.y : half_extents.y
+ );
+
+}
+
+bool RectangleShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const {
+
+
+ return get_aabb().intersects_segment(p_begin,p_end,&r_point,&r_normal);
+}
+
+real_t RectangleShape2DSW::get_moment_of_inertia(float p_mass) const {
+
+ Vector2 he2=half_extents*2;
+ return p_mass*he2.dot(he2)/12.0f;
+}
+
+void RectangleShape2DSW::set_data(const Variant& p_data) {
+
+ ERR_FAIL_COND(p_data.get_type()!=Variant::VECTOR2);
+
+ half_extents=p_data;
+ configure(Rect2(-half_extents,half_extents*2.0));
+}
+
+Variant RectangleShape2DSW::get_data() const {
+
+ return half_extents;
+}
+
+
+
+/*********************************************************/
+/*********************************************************/
+/*********************************************************/
+
+
+
+void CapsuleShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const {
+
+ Vector2 n=p_normal;
+
+ float d = n.y;
+
+ if (Math::abs( d )<(1.0-_SEGMENT_IS_VALID_SUPPORT_TRESHOLD) ) {
+
+ // make it flat
+ n.y=0.0;
+ n.normalize();
+ n*=radius;
+
+ r_amount=2;
+ r_supports[0]=n;
+ r_supports[0].y+=height*0.5;
+ r_supports[1]=n;
+ r_supports[1].y-=height*0.5;
+
+ } else {
+
+ float h = (d > 0) ? height : -height;
+
+ n*=radius;
+ n.y += h*0.5;
+ r_amount=1;
+ *r_supports=n;
+
+ }
+}
+
+bool CapsuleShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const {
+
+
+ float d = 1e10;
+ Vector2 n = (p_end-p_begin).normalized();
+ bool collided=false;
+
+ //try spheres
+ for(int i=0;i<2;i++) {
+
+ Vector2 begin = p_begin;
+ Vector2 end = p_end;
+ float ofs = (i==0)?-height*0.5:height*0.5;
+ begin.y+=ofs;
+ end.y+=ofs;
+
+ Vector2 line_vec = end - begin;
+
+ real_t a, b, c;
+
+ a = line_vec.dot(line_vec);
+ b = 2 * begin.dot(line_vec);
+ c = begin.dot(begin) - radius * radius;
+
+ real_t sqrtterm = b*b - 4*a*c;
+
+ if(sqrtterm < 0)
+ continue;
+
+ sqrtterm = Math::sqrt(sqrtterm);
+ real_t res = ( -b - sqrtterm ) / (2 * a);
+
+ if (res <0 || res >1+CMP_EPSILON) {
+ continue;
+ }
+
+ Vector2 point = begin+line_vec*res;
+ Vector2 pointf(point.x,point.y-ofs);
+ real_t pd = n.dot(pointf);
+ if (pd<d) {
+ r_point=pointf;
+ r_normal=point.normalized();
+ d=pd;
+ collided=true;
+ }
+ }
+
+
+ Vector2 rpos,rnorm;
+ if (Rect2( Point2(-radius,-height*0.5), Size2(radius*2.0,height) ).intersects_segment(p_begin,p_end,&rpos,&rnorm)) {
+
+ real_t pd = n.dot(rpos);
+ if (pd<d) {
+ r_point=rpos;
+ r_normal=rnorm;
+ d=pd;
+ collided=true;
+ }
+ }
+
+ //return get_aabb().intersects_segment(p_begin,p_end,&r_point,&r_normal);
+ return collided; //todo
+}
+
+real_t CapsuleShape2DSW::get_moment_of_inertia(float p_mass) const {
+
+ Vector2 he2(radius*2,height+radius*2);
+ return p_mass*he2.dot(he2)/12.0f;
+}
+
+void CapsuleShape2DSW::set_data(const Variant& p_data) {
+
+ ERR_FAIL_COND(p_data.get_type()!=Variant::ARRAY && p_data.get_type()!=Variant::VECTOR2);
+
+ if (p_data.get_type()==Variant::ARRAY) {
+ Array arr=p_data;
+ ERR_FAIL_COND(arr.size()!=2);
+ height=arr[0];
+ radius=arr[1];
+ } else {
+
+ Point2 p = p_data;
+ radius=p.x;
+ height=p.y;
+ }
+
+ Point2 he(radius,height*0.5+radius);
+ configure(Rect2(-he,he*2));
+
+}
+
+Variant CapsuleShape2DSW::get_data() const {
+
+ return Point2(height,radius);
+}
+
+
+
+/*********************************************************/
+/*********************************************************/
+/*********************************************************/
+
+
+
+void ConvexPolygonShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const {
+
+ int support_idx=-1;
+ real_t d=-1e10;
+
+ for(int i=0;i<point_count;i++) {
+
+ //test point
+ real_t ld = p_normal.dot(points[i].pos);
+ if (ld>d) {
+ support_idx=i;
+ d=ld;
+ }
+
+ //test segment
+ if (points[i].normal.dot(p_normal)>_SEGMENT_IS_VALID_SUPPORT_TRESHOLD) {
+
+ r_amount=2;
+ r_supports[0]=points[i].pos;
+ r_supports[1]=points[(i+1)%point_count].pos;
+ return;
+ }
+ }
+
+ ERR_FAIL_COND(support_idx==-1);
+
+ r_amount=1;
+ r_supports[0]=points[support_idx].pos;
+
+}
+
+bool ConvexPolygonShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const {
+
+ Vector2 n = (p_end-p_begin).normalized();
+ real_t d=1e10;
+ bool inters=false;
+
+ for(int i=0;i<point_count;i++) {
+
+ //hmm crap.. no can do..
+ //if (d.dot(points[i].normal)>=0)
+ // continue;
+
+
+ Vector2 res;
+
+ if (!Geometry::segment_intersects_segment_2d(p_begin,p_end,points[i].pos,points[(i+1)%point_count].pos,&res))
+ continue;
+
+ float nd = n.dot(res);
+ if (nd<d) {
+
+ d=nd;
+ r_point=res;
+ r_normal=points[i].normal;
+ inters=true;
+ }
+
+ }
+
+
+ if (inters) {
+
+ if (n.dot(r_normal)>0)
+ r_normal=-r_normal;
+ }
+
+ //return get_aabb().intersects_segment(p_begin,p_end,&r_point,&r_normal);
+ return inters; //todo
+}
+
+real_t ConvexPolygonShape2DSW::get_moment_of_inertia(float p_mass) const {
+
+ Rect2 aabb;
+ aabb.pos=points[0].pos;
+ for(int i=0;i<point_count;i++) {
+
+ aabb.expand_to(points[i].pos);
+ }
+
+ return p_mass*aabb.size.dot(aabb.size)/12.0f;
+}
+
+void ConvexPolygonShape2DSW::set_data(const Variant& p_data) {
+
+ ERR_FAIL_COND(p_data.get_type()!=Variant::VECTOR2_ARRAY && p_data.get_type()!=Variant::REAL_ARRAY);
+
+
+ if (points)
+ memdelete_arr(points);
+ points=NULL;
+ point_count=0;
+
+ if (p_data.get_type()==Variant::VECTOR2_ARRAY) {
+ DVector<Vector2> arr=p_data;
+ ERR_FAIL_COND(arr.size()==0);
+ point_count=arr.size();
+ points = memnew_arr(Point,point_count);
+ DVector<Vector2>::Read r = arr.read();
+
+ for(int i=0;i<point_count;i++) {
+ points[i].pos=r[i];
+ }
+
+ for(int i=0;i<point_count;i++) {
+
+ Vector2 p = points[i].pos;
+ Vector2 pn = points[(i+1)%point_count].pos;
+ points[i].normal=(pn-p).tangent().normalized();
+ }
+ } else {
+
+ DVector<real_t> dvr = p_data;
+ point_count=dvr.size()/4;
+ ERR_FAIL_COND(point_count==0);
+
+ points = memnew_arr(Point,point_count);
+ DVector<real_t>::Read r = dvr.read();
+
+ for(int i=0;i<point_count;i++) {
+
+ int idx=i<<2;
+ points[i].pos.x=r[idx+0];
+ points[i].pos.y=r[idx+1];
+ points[i].normal.x=r[idx+2];
+ points[i].normal.y=r[idx+3];
+
+ }
+ }
+
+
+ ERR_FAIL_COND(point_count==0);
+ Rect2 aabb;
+ aabb.pos=points[0].pos;
+ for(int i=1;i<point_count;i++)
+ aabb.expand_to(points[i].pos);
+
+ configure(aabb);
+}
+
+Variant ConvexPolygonShape2DSW::get_data() const {
+
+ DVector<Vector2> dvr;
+
+ dvr.resize(point_count);
+
+ for(int i=0;i<point_count;i++) {
+ dvr.set(i,points[i].pos);
+ }
+
+ return dvr;
+}
+
+
+ConvexPolygonShape2DSW::ConvexPolygonShape2DSW() {
+
+ points=NULL;
+ point_count=0;
+
+}
+
+ConvexPolygonShape2DSW::~ConvexPolygonShape2DSW(){
+
+ if (points)
+ memdelete_arr(points);
+
+}
+
+//////////////////////////////////////////////////
+
+
+void ConcavePolygonShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const {
+
+ real_t d=-1e10;
+ int idx=-1;
+ for(int i=0;i<points.size();i++) {
+
+ real_t ld = p_normal.dot(points[i]);
+ if (ld>d) {
+ d=ld;
+ idx=i;
+ }
+ }
+
+
+ r_amount=1;
+ ERR_FAIL_COND(idx==-1);
+ *r_supports=points[idx];
+
+}
+
+bool ConcavePolygonShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const{
+
+ uint32_t* stack = (uint32_t*)alloca(sizeof(int)*bvh_depth);
+
+ enum {
+ TEST_AABB_BIT=0,
+ VISIT_LEFT_BIT=1,
+ VISIT_RIGHT_BIT=2,
+ VISIT_DONE_BIT=3,
+ VISITED_BIT_SHIFT=29,
+ NODE_IDX_MASK=(1<<VISITED_BIT_SHIFT)-1,
+ VISITED_BIT_MASK=~NODE_IDX_MASK,
+
+
+ };
+
+ Vector2 n = (p_end-p_begin).normalized();
+ real_t d=1e10;
+ bool inters=false;
+
+ //for(int i=0;i<bvh_depth;i++)
+ // stack[i]=0;
+
+ int level=0;
+
+ const Segment *segmentptr=&segments[0];
+ const Vector2 *pointptr=&points[0];
+ const BVH *bvhptr = &bvh[0];
+ int pos=bvh.size()-1;
+
+
+ stack[0]=0;
+ while(true) {
+
+ uint32_t node = stack[level]&NODE_IDX_MASK;
+ const BVH &b = bvhptr[ node ];
+ bool done=false;
+
+ switch(stack[level]>>VISITED_BIT_SHIFT) {
+ case TEST_AABB_BIT: {
+
+
+ bool valid = b.aabb.intersects_segment(p_begin,p_end);
+ if (!valid) {
+
+ stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node;
+
+ } else {
+
+ if (b.left<0) {
+
+ const Segment &s=segmentptr[ b.right ];
+ Vector2 a = pointptr[ s.points[0] ];
+ Vector2 b = pointptr[ s.points[1] ];
+
+
+ Vector2 res;
+
+ if (Geometry::segment_intersects_segment_2d(p_begin,p_end,a,b,&res)) {
+
+ float nd = n.dot(res);
+ if (nd<d) {
+
+ d=nd;
+ r_point=res;
+ r_normal=(b-a).tangent().normalized();
+ inters=true;
+ }
+
+ }
+
+ stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node;
+
+ } else {
+
+ stack[level]=(VISIT_LEFT_BIT<<VISITED_BIT_SHIFT)|node;
+ }
+ }
+
+ } continue;
+ case VISIT_LEFT_BIT: {
+
+ stack[level]=(VISIT_RIGHT_BIT<<VISITED_BIT_SHIFT)|node;
+ stack[level+1]=b.left|TEST_AABB_BIT;
+ level++;
+
+ } continue;
+ case VISIT_RIGHT_BIT: {
+
+ stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node;
+ stack[level+1]=b.right|TEST_AABB_BIT;
+ level++;
+ } continue;
+ case VISIT_DONE_BIT: {
+
+ if (level==0) {
+ done=true;
+ break;
+ } else
+ level--;
+
+ } continue;
+ }
+
+
+ if (done)
+ break;
+ }
+
+
+ if (inters) {
+
+ if (n.dot(r_normal)>0)
+ r_normal=-r_normal;
+ }
+
+ return inters;
+
+}
+
+
+
+int ConcavePolygonShape2DSW::_generate_bvh(BVH *p_bvh,int p_len,int p_depth) {
+
+ if (p_len==1) {
+
+ bvh_depth=MAX(p_depth,bvh_depth);
+ bvh.push_back(*p_bvh);
+ return bvh.size()-1;
+ }
+
+ //else sort best
+
+ Rect2 global_aabb=p_bvh[0].aabb;
+ for(int i=1;i<p_len;i++) {
+ global_aabb=global_aabb.merge(p_bvh[i].aabb);
+ }
+
+ if (global_aabb.size.x > global_aabb.size.y) {
+
+ SortArray<BVH,BVH_CompareX> sort;
+ sort.sort(p_bvh,p_len);
+
+ } else {
+
+ SortArray<BVH,BVH_CompareY> sort;
+ sort.sort(p_bvh,p_len);
+
+ }
+
+ int median = p_len/2;
+
+
+ BVH node;
+ node.aabb=global_aabb;
+ int node_idx = bvh.size();
+ bvh.push_back(node);
+
+ int l = _generate_bvh(p_bvh,median,p_depth+1);
+ int r = _generate_bvh(&p_bvh[median],p_len-median,p_depth+1);
+ bvh[node_idx].left=l;
+ bvh[node_idx].right=r;
+
+ return node_idx;
+
+}
+
+void ConcavePolygonShape2DSW::set_data(const Variant& p_data) {
+
+ ERR_FAIL_COND(p_data.get_type()!=Variant::VECTOR2_ARRAY && p_data.get_type()!=Variant::REAL_ARRAY);
+
+ segments.clear();;
+ points.clear();;
+ bvh.clear();;
+ bvh_depth=1;
+
+ Rect2 aabb;
+
+ if (p_data.get_type()==Variant::VECTOR2_ARRAY) {
+
+ DVector<Vector2> p2arr = p_data;
+ int len = p2arr.size();
+ DVector<Vector2>::Read arr = p2arr.read();
+
+
+ Map<Point2,int> pointmap;
+ for(int i=0;i<len;i+=2) {
+
+ Point2 p1 =arr[i];
+ Point2 p2 =arr[i+1];
+ int idx_p1,idx_p2;
+ if (p1==p2)
+ continue; //don't want it
+
+ if (pointmap.has(p1)) {
+ idx_p1=pointmap[p1];
+ } else {
+ idx_p1=pointmap.size();
+ pointmap[p1]=idx_p1;
+ }
+
+ if (pointmap.has(p2)) {
+ idx_p2=pointmap[p2];
+ } else {
+ idx_p2=pointmap.size();
+ pointmap[p2]=idx_p2;
+ }
+
+ Segment s;
+ s.points[0]=idx_p1;
+ s.points[1]=idx_p2;
+ segments.push_back(s);
+ }
+
+ points.resize(pointmap.size());
+ aabb.pos=pointmap.front()->key();
+ for(Map<Point2,int>::Element *E=pointmap.front();E;E=E->next()) {
+
+ aabb.expand_to(E->key());
+ points[E->get()]=E->key();
+ }
+
+ Vector<BVH> main_vbh;
+ main_vbh.resize(segments.size());
+ for(int i=0;i<main_vbh.size();i++) {
+
+
+ main_vbh[i].aabb.pos=points[segments[i].points[0]];
+ main_vbh[i].aabb.expand_to(points[segments[i].points[1]]);
+ main_vbh[i].left=-1;
+ main_vbh[i].right=i;
+ }
+
+ _generate_bvh(&main_vbh[0],main_vbh.size(),1);
+
+
+ } else {
+ //dictionary with arrays
+
+ }
+
+
+ configure(aabb);
+}
+Variant ConcavePolygonShape2DSW::get_data() const {
+
+
+ DVector<Vector2> rsegments;
+ int len = segments.size();
+ rsegments.resize(len*2);
+ DVector<Vector2>::Write w = rsegments.write();
+ for(int i=0;i<len;i++) {
+
+ w[(i<<1)+0]=points[segments[i].points[0]];
+ w[(i<<1)+1]=points[segments[i].points[1]];
+ }
+
+ w=DVector<Vector2>::Write();
+
+ return rsegments;
+}
+
+void ConcavePolygonShape2DSW::cull(const Rect2& p_local_aabb,Callback p_callback,void* p_userdata) const {
+
+ uint32_t* stack = (uint32_t*)alloca(sizeof(int)*bvh_depth);
+
+ enum {
+ TEST_AABB_BIT=0,
+ VISIT_LEFT_BIT=1,
+ VISIT_RIGHT_BIT=2,
+ VISIT_DONE_BIT=3,
+ VISITED_BIT_SHIFT=29,
+ NODE_IDX_MASK=(1<<VISITED_BIT_SHIFT)-1,
+ VISITED_BIT_MASK=~NODE_IDX_MASK,
+
+
+ };
+
+ //for(int i=0;i<bvh_depth;i++)
+ // stack[i]=0;
+
+
+ int level=0;
+
+ const Segment *segmentptr=&segments[0];
+ const Vector2 *pointptr=&points[0];
+ const BVH *bvhptr = &bvh[0];
+ int pos=bvh.size()-1;
+
+
+ stack[0]=0;
+ while(true) {
+
+ uint32_t node = stack[level]&NODE_IDX_MASK;
+ const BVH &b = bvhptr[ node ];
+
+ switch(stack[level]>>VISITED_BIT_SHIFT) {
+ case TEST_AABB_BIT: {
+
+
+ bool valid = p_local_aabb.intersects(b.aabb);
+ if (!valid) {
+
+ stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node;
+
+ } else {
+
+ if (b.left<0) {
+
+ const Segment &s=segmentptr[ b.right ];
+ Vector2 a = pointptr[ s.points[0] ];
+ Vector2 b = pointptr[ s.points[1] ];
+
+ SegmentShape2DSW ss(a,b,(b-a).tangent().normalized());
+
+ p_callback(p_userdata,&ss);
+ stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node;
+
+ } else {
+
+ stack[level]=(VISIT_LEFT_BIT<<VISITED_BIT_SHIFT)|node;
+ }
+ }
+
+ } continue;
+ case VISIT_LEFT_BIT: {
+
+ stack[level]=(VISIT_RIGHT_BIT<<VISITED_BIT_SHIFT)|node;
+ stack[level+1]=b.left|TEST_AABB_BIT;
+ level++;
+
+ } continue;
+ case VISIT_RIGHT_BIT: {
+
+ stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node;
+ stack[level+1]=b.right|TEST_AABB_BIT;
+ level++;
+ } continue;
+ case VISIT_DONE_BIT: {
+
+ if (level==0)
+ return;
+ else
+ level--;
+
+ } continue;
+ }
+ }
+
+}
+
+
diff --git a/servers/physics_2d/shape_2d_sw.h b/servers/physics_2d/shape_2d_sw.h new file mode 100644 index 0000000000..20e76fad5a --- /dev/null +++ b/servers/physics_2d/shape_2d_sw.h @@ -0,0 +1,442 @@ +/*************************************************************************/ +/* shape_2d_sw.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#ifndef SHAPE_2D_2DSW_H
+#define SHAPE_2D_2DSW_H
+
+#include "servers/physics_2d_server.h"
+/*
+
+SHAPE_LINE, ///< plane:"plane"
+SHAPE_SEGMENT, ///< float:"length"
+SHAPE_CIRCLE, ///< float:"radius"
+SHAPE_RECTANGLE, ///< vec3:"extents"
+SHAPE_CONVEX_POLYGON, ///< array of planes:"planes"
+SHAPE_CONCAVE_POLYGON, ///< Vector2 array:"triangles" , or Dictionary with "indices" (int array) and "triangles" (Vector2 array)
+SHAPE_CUSTOM, ///< Server-Implementation based custom shape, calling shape_create() with this value will result in an error
+
+*/
+
+class Shape2DSW;
+
+class ShapeOwner2DSW {
+public:
+
+ virtual void _shape_changed()=0;
+ virtual void remove_shape(Shape2DSW *p_shape)=0;
+
+ virtual ~ShapeOwner2DSW() {}
+};
+
+class Shape2DSW {
+
+ RID self;
+ Rect2 aabb;
+ bool configured;
+ real_t custom_bias;
+
+ Map<ShapeOwner2DSW*,int> owners;
+protected:
+
+ void configure(const Rect2& p_aabb);
+public:
+
+ _FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; }
+ _FORCE_INLINE_ RID get_self() const {return self; }
+
+ virtual Physics2DServer::ShapeType get_type() const=0;
+
+ _FORCE_INLINE_ Rect2 get_aabb() const { return aabb; }
+ _FORCE_INLINE_ bool is_configured() const { return configured; }
+
+ virtual bool is_concave() const { return false; }
+
+ virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const=0;
+ virtual Vector2 get_support(const Vector2& p_normal) const;
+ virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const=0;
+
+ virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const=0;
+ virtual real_t get_moment_of_inertia(float 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(ShapeOwner2DSW *p_owner);
+ void remove_owner(ShapeOwner2DSW *p_owner);
+ bool is_owner(ShapeOwner2DSW *p_owner) const;
+ const Map<ShapeOwner2DSW*,int>& get_owners() const;
+
+ Shape2DSW();
+ virtual ~Shape2DSW();
+};
+
+
+class LineShape2DSW : public Shape2DSW {
+
+
+ Vector2 normal;
+ real_t d;
+
+public:
+
+ _FORCE_INLINE_ Vector2 get_normal() const { return normal; }
+ _FORCE_INLINE_ real_t get_d() const { return d; }
+
+ virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_LINE; }
+
+ virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }
+ virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;
+
+ virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;
+ virtual real_t get_moment_of_inertia(float p_mass) const;
+
+ virtual void set_data(const Variant& p_data);
+ virtual Variant get_data() const;
+
+ _FORCE_INLINE_ void project_range(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const {
+ //real large
+ r_min=-1e10;
+ r_max=1e10;
+ }
+
+};
+
+
+class RayShape2DSW : public Shape2DSW {
+
+
+ real_t length;
+
+public:
+
+
+ _FORCE_INLINE_ real_t get_length() const { return length; }
+
+ virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_RAY; }
+
+ virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }
+ virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;
+
+ virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;
+ virtual real_t get_moment_of_inertia(float p_mass) const;
+
+ virtual void set_data(const Variant& p_data);
+ virtual Variant get_data() const;
+
+ _FORCE_INLINE_ void project_range(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const {
+ //real large
+ r_max = p_normal.dot(p_transform.get_origin());
+ r_min = p_normal.dot(p_transform.xform(Vector2(0,length)));
+ if (r_max<r_min) {
+
+ SWAP(r_max,r_min);
+ }
+ }
+
+ _FORCE_INLINE_ RayShape2DSW() {}
+ _FORCE_INLINE_ RayShape2DSW(real_t p_length) { length=p_length; }
+};
+
+
+class SegmentShape2DSW : public Shape2DSW {
+
+
+ Vector2 a;
+ Vector2 b;
+ Vector2 n;
+
+public:
+
+
+ _FORCE_INLINE_ const Vector2& get_a() const { return a; }
+ _FORCE_INLINE_ const Vector2& get_b() const { return b; }
+ _FORCE_INLINE_ const Vector2& get_normal() const { return n; }
+
+ virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_SEGMENT; }
+
+ virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }
+ virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;
+
+ virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;
+ virtual real_t get_moment_of_inertia(float p_mass) const;
+
+ virtual void set_data(const Variant& p_data);
+ virtual Variant get_data() const;
+
+ _FORCE_INLINE_ void project_range(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const {
+ //real large
+ r_max = p_normal.dot(p_transform.xform(a));
+ r_min = p_normal.dot(p_transform.xform(b));
+ if (r_max<r_min) {
+
+ SWAP(r_max,r_min);
+ }
+ }
+
+ _FORCE_INLINE_ SegmentShape2DSW() {}
+ _FORCE_INLINE_ SegmentShape2DSW(const Vector2& p_a,const Vector2& p_b,const Vector2& p_n) { a=p_a; b=p_b; n=p_n; }
+};
+
+
+class CircleShape2DSW : public Shape2DSW {
+
+
+ real_t radius;
+
+public:
+
+ _FORCE_INLINE_ const real_t& get_radius() const { return radius; }
+
+ virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_CIRCLE; }
+
+ virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }
+ virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;
+
+ virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;
+ virtual real_t get_moment_of_inertia(float p_mass) const;
+
+ virtual void set_data(const Variant& p_data);
+ virtual Variant get_data() const;
+
+ _FORCE_INLINE_ void project_range(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const {
+ //real large
+ real_t d = p_normal.dot( p_transform.get_origin() );
+
+ // figure out scale at point
+ Vector2 local_normal = p_transform.basis_xform_inv(p_normal);
+ real_t scale = local_normal.length();
+
+ r_min = d - (radius) * scale;
+ r_max = d + (radius) * scale;
+ }
+
+};
+
+
+
+class RectangleShape2DSW : public Shape2DSW {
+
+
+ Vector2 half_extents;
+
+public:
+
+ _FORCE_INLINE_ const Vector2& get_half_extents() const { return half_extents; }
+
+ virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_RECTANGLE; }
+
+ virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }
+ virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;
+
+ virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;
+ virtual real_t get_moment_of_inertia(float p_mass) const;
+
+ virtual void set_data(const Variant& p_data);
+ virtual Variant get_data() const;
+
+ _FORCE_INLINE_ void project_range(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const {
+ // no matter the angle, the box is mirrored anyway
+ Vector2 local_normal=p_transform.basis_xform_inv(p_normal);
+
+ float length = local_normal.abs().dot(half_extents);
+ float distance = p_normal.dot( p_transform.get_origin() );
+
+ r_min = distance - length;
+ r_max = distance + length;
+ }
+
+};
+
+class CapsuleShape2DSW : public Shape2DSW {
+
+
+ real_t radius;
+ real_t height;
+
+public:
+
+ _FORCE_INLINE_ const real_t& get_radius() const { return radius; }
+ _FORCE_INLINE_ const real_t& get_height() const { return height; }
+
+ virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_CAPSULE; }
+
+ virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }
+ virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;
+
+ virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;
+ virtual real_t get_moment_of_inertia(float p_mass) const;
+
+ virtual void set_data(const Variant& p_data);
+ virtual Variant get_data() const;
+
+ _FORCE_INLINE_ void project_range(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const {
+ // no matter the angle, the box is mirrored anyway
+ Vector2 n=p_transform.basis_xform_inv(p_normal).normalized();
+ float h = (n.y > 0) ? height : -height;
+
+ n *= radius;
+ n.y += h * 0.5;
+
+ r_max = p_normal.dot(p_transform.xform(n));
+ r_min = p_normal.dot(p_transform.xform(-n));
+
+ if (r_max<r_min) {
+
+ SWAP(r_max,r_min);
+ }
+
+ //ERR_FAIL_COND( r_max < r_min );
+ }
+
+};
+
+
+class ConvexPolygonShape2DSW : public Shape2DSW {
+
+
+ struct Point {
+
+ Vector2 pos;
+ Vector2 normal; //normal to next segment
+ };
+
+ Point *points;
+ int point_count;
+
+public:
+
+ _FORCE_INLINE_ int get_point_count() const { return point_count; }
+ _FORCE_INLINE_ const Vector2& get_point(int p_idx) const { return points[p_idx].pos; }
+ _FORCE_INLINE_ const Vector2& get_segment_normal(int p_idx) const { return points[p_idx].normal; }
+
+ virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_CONVEX_POLYGON; }
+
+ virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }
+ virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;
+
+ virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;
+ virtual real_t get_moment_of_inertia(float p_mass) const;
+
+ virtual void set_data(const Variant& p_data);
+ virtual Variant get_data() const;
+
+ _FORCE_INLINE_ void project_range(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const {
+ // no matter the angle, the box is mirrored anyway
+
+ r_min = r_max = p_normal.dot(p_transform.xform(points[0].pos));
+ for(int i=1;i<point_count;i++) {
+
+ float d = p_normal.dot(p_transform.xform(points[i].pos));
+ if (d>r_max)
+ r_max=d;
+ if (d<r_min)
+ r_min=d;
+
+ }
+
+ }
+
+
+ ConvexPolygonShape2DSW();
+ ~ConvexPolygonShape2DSW();
+};
+
+
+class ConcaveShape2DSW : public Shape2DSW {
+
+public:
+
+ virtual bool is_concave() const { return true; }
+ typedef void (*Callback)(void* p_userdata,Shape2DSW *p_convex);
+
+ virtual void cull(const Rect2& p_local_aabb,Callback p_callback,void* p_userdata) const=0;
+
+};
+
+class ConcavePolygonShape2DSW : public ConcaveShape2DSW {
+
+ struct Segment {
+
+ int points[2];
+ };
+
+ Vector<Segment> segments;
+ Vector<Point2> points;
+
+ struct BVH {
+
+ Rect2 aabb;
+ int left,right;
+ };
+
+
+ Vector<BVH> bvh;
+ int bvh_depth;
+
+
+ struct BVH_CompareX {
+
+ _FORCE_INLINE_ bool operator ()(const BVH& a, const BVH& b) const {
+
+ return (a.aabb.pos.x+a.aabb.size.x*0.5) < (b.aabb.pos.x+b.aabb.size.x*0.5);
+ }
+ };
+
+ struct BVH_CompareY {
+
+ _FORCE_INLINE_ bool operator ()(const BVH& a, const BVH& b) const {
+
+ return (a.aabb.pos.y+a.aabb.size.y*0.5) < (b.aabb.pos.y+b.aabb.size.y*0.5);
+ }
+ };
+
+ int _generate_bvh(BVH *p_bvh,int p_len,int p_depth);
+
+public:
+
+ virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_CONCAVE_POLYGON; }
+
+ virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { /*project_range(p_normal,p_transform,r_min,r_max);*/ }
+ virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;
+
+ virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;
+
+ virtual real_t get_moment_of_inertia(float p_mass) const { return 0; }
+
+ virtual void set_data(const Variant& p_data);
+ virtual Variant get_data() const;
+
+ virtual void cull(const Rect2& p_local_aabb,Callback p_callback,void* p_userdata) const;
+
+};
+
+
+#endif // SHAPE_2D_2DSW_H
diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp new file mode 100644 index 0000000000..4fe53aeb4d --- /dev/null +++ b/servers/physics_2d/space_2d_sw.cpp @@ -0,0 +1,432 @@ +/*************************************************************************/ +/* space_2d_sw.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#include "space_2d_sw.h"
+#include "collision_solver_2d_sw.h"
+#include "physics_2d_server_sw.h"
+
+
+bool Physics2DDirectSpaceStateSW::intersect_ray(const Vector2& p_from, const Vector2& p_to,RayResult &r_result,const Set<RID>& p_exclude,uint32_t p_user_mask) {
+
+
+ ERR_FAIL_COND_V(space->locked,false);
+
+ Vector2 begin,end;
+ Vector2 normal;
+ begin=p_from;
+ end=p_to;
+ normal=(end-begin).normalized();
+
+
+ int amount = space->broadphase->cull_segment(begin,end,space->intersection_query_results,Space2DSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results);
+
+ //todo, create another array tha references results, compute AABBs and check closest point to ray origin, sort, and stop evaluating results when beyond first collision
+
+ bool collided=false;
+ Vector2 res_point,res_normal;
+ int res_shape;
+ const CollisionObject2DSW *res_obj;
+ real_t min_d=1e10;
+
+
+ for(int i=0;i<amount;i++) {
+
+ if (space->intersection_query_results[i]->get_type()==CollisionObject2DSW::TYPE_AREA)
+ continue; //ignore area
+
+ if (p_exclude.has( space->intersection_query_results[i]->get_self()))
+ continue;
+
+ const CollisionObject2DSW *col_obj=space->intersection_query_results[i];
+
+ int shape_idx=space->intersection_query_subindex_results[i];
+ Matrix32 inv_xform = col_obj->get_shape_inv_transform(shape_idx) * col_obj->get_inv_transform();
+
+ Vector2 local_from = inv_xform.xform(begin);
+ Vector2 local_to = inv_xform.xform(end);
+
+ /*local_from = col_obj->get_inv_transform().xform(begin);
+ local_from = col_obj->get_shape_inv_transform(shape_idx).xform(local_from);
+
+ local_to = col_obj->get_inv_transform().xform(end);
+ local_to = col_obj->get_shape_inv_transform(shape_idx).xform(local_to);*/
+
+ const Shape2DSW *shape = col_obj->get_shape(shape_idx);
+
+ Vector2 shape_point,shape_normal;
+
+
+ if (shape->intersect_segment(local_from,local_to,shape_point,shape_normal)) {
+
+
+ //print_line("inters sgment!");
+ Matrix32 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!=0)
+ r_result.collider=ObjectDB::get_instance(r_result.collider_id);
+ r_result.normal=res_normal;
+ r_result.position=res_point;
+ r_result.rid=res_obj->get_self();
+ r_result.shape=res_shape;
+
+ return true;
+
+}
+
+
+int Physics2DDirectSpaceStateSW::intersect_shape(const RID& p_shape, const Matrix32& p_xform,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude,uint32_t p_user_mask) {
+
+ if (p_result_max<=0)
+ return 0;
+
+ Shape2DSW *shape = static_cast<Physics2DServerSW*>(Physics2DServer::get_singleton())->shape_owner.get(p_shape);
+ ERR_FAIL_COND_V(!shape,0);
+
+ Rect2 aabb = p_xform.xform(shape->get_aabb());
+
+ int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,Space2DSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results);
+
+ bool collided=false;
+ int cc=0;
+
+ for(int i=0;i<amount;i++) {
+
+ if (cc>=p_result_max)
+ break;
+
+ if (space->intersection_query_results[i]->get_type()==CollisionObject2DSW::TYPE_AREA)
+ continue; //ignore area
+
+ if (p_exclude.has( space->intersection_query_results[i]->get_self()))
+ continue;
+
+
+ const CollisionObject2DSW *col_obj=space->intersection_query_results[i];
+ int shape_idx=space->intersection_query_subindex_results[i];
+
+ if (!CollisionSolver2DSW::solve_static(shape,p_xform,p_xform.affine_inverse(),col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), col_obj->get_inv_transform() * col_obj->get_shape_inv_transform(shape_idx),NULL,NULL,NULL))
+ continue;
+
+ r_results[cc].collider_id=col_obj->get_instance_id();
+ if (r_results[cc].collider_id!=0)
+ r_results[cc].collider=ObjectDB::get_instance(r_results[cc].collider_id);
+ r_results[cc].rid=col_obj->get_self();
+ r_results[cc].shape=shape_idx;
+
+ cc++;
+
+ }
+
+ return cc;
+
+}
+
+Physics2DDirectSpaceStateSW::Physics2DDirectSpaceStateSW() {
+
+
+ space=NULL;
+}
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+
+
+
+
+
+
+
+
+
+void* Space2DSW::_broadphase_pair(CollisionObject2DSW *A,int p_subindex_A,CollisionObject2DSW *B,int p_subindex_B,void *p_self) {
+
+ CollisionObject2DSW::Type type_A=A->get_type();
+ CollisionObject2DSW::Type type_B=B->get_type();
+ if (type_A>type_B) {
+
+ SWAP(A,B);
+ SWAP(p_subindex_A,p_subindex_B);
+ SWAP(type_A,type_B);
+ }
+
+ Space2DSW *self = (Space2DSW*)p_self;
+
+ if (type_A==CollisionObject2DSW::TYPE_AREA) {
+
+
+ ERR_FAIL_COND_V(type_B!=CollisionObject2DSW::TYPE_BODY,NULL);
+ Area2DSW *area=static_cast<Area2DSW*>(A);
+ Body2DSW *body=static_cast<Body2DSW*>(B);
+
+ AreaPair2DSW *area_pair = memnew(AreaPair2DSW(body,p_subindex_B,area,p_subindex_A) );
+
+ return area_pair;
+ } else {
+
+
+ BodyPair2DSW *b = memnew( BodyPair2DSW((Body2DSW*)A,p_subindex_A,(Body2DSW*)B,p_subindex_B) );
+ return b;
+
+ }
+
+ return NULL;
+}
+
+void Space2DSW::_broadphase_unpair(CollisionObject2DSW *A,int p_subindex_A,CollisionObject2DSW *B,int p_subindex_B,void *p_data,void *p_self) {
+
+
+
+ Space2DSW *self = (Space2DSW*)p_self;
+ Constraint2DSW *c = (Constraint2DSW*)p_data;
+ memdelete(c);
+}
+
+
+const SelfList<Body2DSW>::List& Space2DSW::get_active_body_list() const {
+
+ return active_list;
+}
+void Space2DSW::body_add_to_active_list(SelfList<Body2DSW>* p_body) {
+
+ active_list.add(p_body);
+}
+void Space2DSW::body_remove_from_active_list(SelfList<Body2DSW>* p_body) {
+
+ active_list.remove(p_body);
+
+}
+
+void Space2DSW::body_add_to_inertia_update_list(SelfList<Body2DSW>* p_body) {
+
+
+ inertia_update_list.add(p_body);
+}
+
+void Space2DSW::body_remove_from_inertia_update_list(SelfList<Body2DSW>* p_body) {
+
+ inertia_update_list.remove(p_body);
+}
+
+BroadPhase2DSW *Space2DSW::get_broadphase() {
+
+ return broadphase;
+}
+
+void Space2DSW::add_object(CollisionObject2DSW *p_object) {
+
+ ERR_FAIL_COND( objects.has(p_object) );
+ objects.insert(p_object);
+}
+
+void Space2DSW::remove_object(CollisionObject2DSW *p_object) {
+
+ ERR_FAIL_COND( !objects.has(p_object) );
+ objects.erase(p_object);
+}
+
+const Set<CollisionObject2DSW*> &Space2DSW::get_objects() const {
+
+ return objects;
+}
+
+void Space2DSW::body_add_to_state_query_list(SelfList<Body2DSW>* p_body) {
+
+ state_query_list.add(p_body);
+}
+void Space2DSW::body_remove_from_state_query_list(SelfList<Body2DSW>* p_body) {
+
+ state_query_list.remove(p_body);
+}
+
+void Space2DSW::area_add_to_monitor_query_list(SelfList<Area2DSW>* p_area) {
+
+ monitor_query_list.add(p_area);
+}
+void Space2DSW::area_remove_from_monitor_query_list(SelfList<Area2DSW>* p_area) {
+
+ monitor_query_list.remove(p_area);
+}
+
+void Space2DSW::area_add_to_moved_list(SelfList<Area2DSW>* p_area) {
+
+ area_moved_list.add(p_area);
+}
+
+void Space2DSW::area_remove_from_moved_list(SelfList<Area2DSW>* p_area) {
+
+ area_moved_list.remove(p_area);
+}
+
+const SelfList<Area2DSW>::List& Space2DSW::get_moved_area_list() const {
+
+ return area_moved_list;
+}
+
+
+void Space2DSW::call_queries() {
+
+ while(state_query_list.first()) {
+
+ Body2DSW * b = state_query_list.first()->self();
+ b->call_queries();
+ state_query_list.remove(state_query_list.first());
+ }
+
+ while(monitor_query_list.first()) {
+
+ Area2DSW * a = monitor_query_list.first()->self();
+ a->call_queries();
+ monitor_query_list.remove(monitor_query_list.first());
+ }
+
+}
+
+void Space2DSW::setup() {
+
+
+ while(inertia_update_list.first()) {
+ inertia_update_list.first()->self()->update_inertias();
+ inertia_update_list.remove(inertia_update_list.first());
+ }
+
+
+}
+
+void Space2DSW::update() {
+
+ broadphase->update();
+
+}
+
+
+void Space2DSW::set_param(Physics2DServer::SpaceParameter p_param, real_t p_value) {
+
+ switch(p_param) {
+
+ case Physics2DServer::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: contact_recycle_radius=p_value; break;
+ case Physics2DServer::SPACE_PARAM_CONTACT_MAX_SEPARATION: contact_max_separation=p_value; break;
+ case Physics2DServer::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: contact_max_allowed_penetration=p_value; break;
+ case Physics2DServer::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_TRESHOLD: body_linear_velocity_sleep_treshold=p_value; break;
+ case Physics2DServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_TRESHOLD: body_angular_velocity_sleep_treshold=p_value; break;
+ case Physics2DServer::SPACE_PARAM_BODY_TIME_TO_SLEEP: body_time_to_sleep=p_value; break;
+ case Physics2DServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO: body_angular_velocity_damp_ratio=p_value; break;
+ case Physics2DServer::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: constraint_bias=p_value; break;
+ }
+}
+
+real_t Space2DSW::get_param(Physics2DServer::SpaceParameter p_param) const {
+
+ switch(p_param) {
+
+ case Physics2DServer::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: return contact_recycle_radius;
+ case Physics2DServer::SPACE_PARAM_CONTACT_MAX_SEPARATION: return contact_max_separation;
+ case Physics2DServer::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: return contact_max_allowed_penetration;
+ case Physics2DServer::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_TRESHOLD: return body_linear_velocity_sleep_treshold;
+ case Physics2DServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_TRESHOLD: return body_angular_velocity_sleep_treshold;
+ case Physics2DServer::SPACE_PARAM_BODY_TIME_TO_SLEEP: return body_time_to_sleep;
+ case Physics2DServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO: return body_angular_velocity_damp_ratio;
+ case Physics2DServer::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: return constraint_bias;
+ }
+ return 0;
+}
+
+void Space2DSW::lock() {
+
+ locked=true;
+}
+
+void Space2DSW::unlock() {
+
+ locked=false;
+}
+
+bool Space2DSW::is_locked() const {
+
+ return locked;
+}
+
+Physics2DDirectSpaceStateSW *Space2DSW::get_direct_state() {
+
+ return direct_access;
+}
+
+Space2DSW::Space2DSW() {
+
+
+ locked=false;
+ contact_recycle_radius=0.01;
+ contact_max_separation=0.05;
+ contact_max_allowed_penetration= 0.01;
+
+ constraint_bias = 0.01;
+ body_linear_velocity_sleep_treshold=0.01;
+ body_angular_velocity_sleep_treshold=(8.0 / 180.0 * Math_PI);
+ body_time_to_sleep=0.5;
+ body_angular_velocity_damp_ratio=15;
+
+
+ broadphase = BroadPhase2DSW::create_func();
+ broadphase->set_pair_callback(_broadphase_pair,this);
+ broadphase->set_unpair_callback(_broadphase_unpair,this);
+ area=NULL;
+
+ direct_access = memnew( Physics2DDirectSpaceStateSW );
+ direct_access->space=this;
+}
+
+Space2DSW::~Space2DSW() {
+
+ memdelete(broadphase);
+ memdelete( direct_access );
+}
+
+
+
diff --git a/servers/physics_2d/space_2d_sw.h b/servers/physics_2d/space_2d_sw.h new file mode 100644 index 0000000000..f65ec14427 --- /dev/null +++ b/servers/physics_2d/space_2d_sw.h @@ -0,0 +1,160 @@ +/*************************************************************************/ +/* space_2d_sw.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#ifndef SPACE_2D_SW_H
+#define SPACE_2D_SW_H
+
+#include "typedefs.h"
+#include "hash_map.h"
+#include "body_2d_sw.h"
+#include "area_2d_sw.h"
+#include "body_pair_2d_sw.h"
+#include "area_pair_2d_sw.h"
+#include "broad_phase_2d_sw.h"
+#include "collision_object_2d_sw.h"
+
+
+class Physics2DDirectSpaceStateSW : public Physics2DDirectSpaceState {
+
+ OBJ_TYPE( Physics2DDirectSpaceStateSW, Physics2DDirectSpaceState );
+public:
+
+ Space2DSW *space;
+
+ bool intersect_ray(const Vector2& p_from, const Vector2& p_to,RayResult &r_result,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0);
+ int intersect_shape(const RID& p_shape, const Matrix32& p_xform,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0);
+
+ Physics2DDirectSpaceStateSW();
+};
+
+
+
+class Space2DSW {
+
+
+ Physics2DDirectSpaceStateSW *direct_access;
+ RID self;
+
+ BroadPhase2DSW *broadphase;
+ SelfList<Body2DSW>::List active_list;
+ SelfList<Body2DSW>::List inertia_update_list;
+ SelfList<Body2DSW>::List state_query_list;
+ SelfList<Area2DSW>::List monitor_query_list;
+ SelfList<Area2DSW>::List area_moved_list;
+
+ static void* _broadphase_pair(CollisionObject2DSW *A,int p_subindex_A,CollisionObject2DSW *B,int p_subindex_B,void *p_self);
+ static void _broadphase_unpair(CollisionObject2DSW *A,int p_subindex_A,CollisionObject2DSW *B,int p_subindex_B,void *p_data,void *p_self);
+
+ Set<CollisionObject2DSW*> objects;
+
+ Area2DSW *area;
+
+ real_t contact_recycle_radius;
+ real_t contact_max_separation;
+ real_t contact_max_allowed_penetration;
+ real_t constraint_bias;
+
+ enum {
+
+ INTERSECTION_QUERY_MAX=2048
+ };
+
+ CollisionObject2DSW *intersection_query_results[INTERSECTION_QUERY_MAX];
+ int intersection_query_subindex_results[INTERSECTION_QUERY_MAX];
+
+ float body_linear_velocity_sleep_treshold;
+ float body_angular_velocity_sleep_treshold;
+ float body_time_to_sleep;
+ float body_angular_velocity_damp_ratio;
+
+ bool locked;
+
+friend class Physics2DDirectSpaceStateSW;
+
+public:
+
+ _FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; }
+ _FORCE_INLINE_ RID get_self() const { return self; }
+
+ void set_default_area(Area2DSW *p_area) { area=p_area; }
+ Area2DSW *get_default_area() const { return area; }
+
+ const SelfList<Body2DSW>::List& get_active_body_list() const;
+ void body_add_to_active_list(SelfList<Body2DSW>* p_body);
+ void body_remove_from_active_list(SelfList<Body2DSW>* p_body);
+ void body_add_to_inertia_update_list(SelfList<Body2DSW>* p_body);
+ void body_remove_from_inertia_update_list(SelfList<Body2DSW>* p_body);
+ void area_add_to_moved_list(SelfList<Area2DSW>* p_area);
+ void area_remove_from_moved_list(SelfList<Area2DSW>* p_area);
+ const SelfList<Area2DSW>::List& get_moved_area_list() const;
+
+
+
+
+ void body_add_to_state_query_list(SelfList<Body2DSW>* p_body);
+ void body_remove_from_state_query_list(SelfList<Body2DSW>* p_body);
+
+ void area_add_to_monitor_query_list(SelfList<Area2DSW>* p_area);
+ void area_remove_from_monitor_query_list(SelfList<Area2DSW>* p_area);
+
+ BroadPhase2DSW *get_broadphase();
+
+ void add_object(CollisionObject2DSW *p_object);
+ void remove_object(CollisionObject2DSW *p_object);
+ const Set<CollisionObject2DSW*> &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_treshold() const { return body_linear_velocity_sleep_treshold; }
+ _FORCE_INLINE_ real_t get_body_angular_velocity_sleep_treshold() const { return body_angular_velocity_sleep_treshold; }
+ _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(Physics2DServer::SpaceParameter p_param, real_t p_value);
+ real_t get_param(Physics2DServer::SpaceParameter p_param) const;
+
+ Physics2DDirectSpaceStateSW *get_direct_state();
+
+ Space2DSW();
+ ~Space2DSW();
+};
+
+
+#endif // SPACE_2D_SW_H
diff --git a/servers/physics_2d/step_2d_sw.cpp b/servers/physics_2d/step_2d_sw.cpp new file mode 100644 index 0000000000..9f41fc94eb --- /dev/null +++ b/servers/physics_2d/step_2d_sw.cpp @@ -0,0 +1,239 @@ +/*************************************************************************/ +/* step_2d_sw.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#include "step_2d_sw.h" + + +void Step2DSW::_populate_island(Body2DSW* p_body,Body2DSW** p_island,Constraint2DSW **p_constraint_island) { + + p_body->set_island_step(_step); + p_body->set_island_next(*p_island); + *p_island=p_body; + + for(Map<Constraint2DSW*,int>::Element *E=p_body->get_constraint_map().front();E;E=E->next()) { + + Constraint2DSW *c=(Constraint2DSW*)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; + Body2DSW *b = c->get_body_ptr()[i]; + if (b->get_island_step()==_step || b->get_mode()==Physics2DServer::BODY_MODE_STATIC) + continue; //no go + _populate_island(c->get_body_ptr()[i],p_island,p_constraint_island); + } + } +} + +void Step2DSW::_setup_island(Constraint2DSW *p_island,float p_delta) { + + Constraint2DSW *ci=p_island; + while(ci) { + bool process = ci->setup(p_delta); + //todo remove from island if process fails + ci=ci->get_island_next(); + } +} + +void Step2DSW::_solve_island(Constraint2DSW *p_island,int p_iterations,float p_delta){ + + + for(int i=0;i<p_iterations;i++) { + + Constraint2DSW *ci=p_island; + while(ci) { + ci->solve(p_delta); + ci=ci->get_island_next(); + } + } +} + +void Step2DSW::_check_suspend(Body2DSW *p_island,float p_delta) { + + + bool can_sleep=true; + + Body2DSW *b = p_island; + while(b) { + + if (b->get_mode()==Physics2DServer::BODY_MODE_STATIC) + 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()==Physics2DServer::BODY_MODE_STATIC) + continue; //ignore for static + + bool active = b->is_active(); + + if (active==can_sleep) + b->set_active(!can_sleep); + + b=b->get_island_next(); + } +} + +void Step2DSW::step(Space2DSW* p_space,float p_delta,int p_iterations) { + + + p_space->lock(); // can't access space during this + + p_space->setup(); //update inertias, etc + + const SelfList<Body2DSW>::List * body_list = &p_space->get_active_body_list(); + + /* INTEGRATE FORCES */ + int active_count=0; + + const SelfList<Body2DSW>*b = body_list->first(); + while(b) { + + b->self()->integrate_forces(p_delta); + b=b->next(); + active_count++; + } + + /* GENERATE CONSTRAINT ISLANDS */ + + Body2DSW *island_list=NULL; + Constraint2DSW *constraint_island_list=NULL; + b = body_list->first(); + + int island_count=0; + + while(b) { + Body2DSW *body = b->self(); + + + if (body->get_island_step()!=_step) { + + Body2DSW *island=NULL; + Constraint2DSW *constraint_island=NULL; + _populate_island(body,&island,&constraint_island); + + island->set_island_list_next(island_list); + island_list=island; + + if (constraint_island) { + constraint_island->set_island_list_next(constraint_island_list); + constraint_island_list=constraint_island; + island_count++; + } + + } + b=b->next(); + } + + const SelfList<Area2DSW>::List &aml = p_space->get_moved_area_list(); + + + + while(aml.first()) { + for(const Set<Constraint2DSW*>::Element *E=aml.first()->self()->get_constraints().front();E;E=E->next()) { + + Constraint2DSW*c=E->get(); + if (c->get_island_step()==_step) + continue; + c->set_island_step(_step); + c->set_island_next(NULL); + c->set_island_list_next(constraint_island_list); + constraint_island_list=c; + } + p_space->area_remove_from_moved_list((SelfList<Area2DSW>*)aml.first()); //faster to remove here + } + +// print_line("island count: "+itos(island_count)+" active count: "+itos(active_count)); + /* SETUP CONSTRAINT ISLANDS */ + + { + Constraint2DSW *ci=constraint_island_list; + while(ci) { + + _setup_island(ci,p_delta); + ci=ci->get_island_list_next(); + } + } + + /* SOLVE CONSTRAINT ISLANDS */ + + { + Constraint2DSW *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(); + } + } + + /* INTEGRATE VELOCITIES */ + + b = body_list->first(); + while(b) { + + b->self()->integrate_velocities(p_delta); + b=b->next(); + } + + /* SLEEP / WAKE UP ISLANDS */ + + { + Body2DSW *bi=island_list; + while(bi) { + + _check_suspend(bi,p_delta); + bi=bi->get_island_list_next(); + } + } + + p_space->update(); + p_space->unlock(); + _step++; + + + +} + +Step2DSW::Step2DSW() { + + _step=1; +} diff --git a/servers/physics_2d/step_2d_sw.h b/servers/physics_2d/step_2d_sw.h new file mode 100644 index 0000000000..91ac9d8584 --- /dev/null +++ b/servers/physics_2d/step_2d_sw.h @@ -0,0 +1,48 @@ +/*************************************************************************/ +/* step_2d_sw.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#ifndef STEP_2D_SW_H +#define STEP_2D_SW_H + +#include "space_2d_sw.h" + +class Step2DSW { + + uint64_t _step; + + void _populate_island(Body2DSW* p_body,Body2DSW** p_island,Constraint2DSW **p_constraint_island); + void _setup_island(Constraint2DSW *p_island,float p_delta); + void _solve_island(Constraint2DSW *p_island,int p_iterations,float p_delta); + void _check_suspend(Body2DSW *p_island,float p_delta); +public: + + void step(Space2DSW* p_space,float p_delta,int p_iterations); + Step2DSW(); +}; + +#endif // STEP_2D_SW_H |