diff options
| author | Rémi Verschelde <rverschelde@gmail.com> | 2017-03-05 16:44:50 +0100 |
|---|---|---|
| committer | Rémi Verschelde <rverschelde@gmail.com> | 2017-03-05 16:44:50 +0100 |
| commit | 5dbf1809c6e3e905b94b8764e99491e608122261 (patch) | |
| tree | 5e5a5360db15d86d59ec8c6e4f7eb511388c5a9a /servers/physics_2d | |
| parent | 45438e9918d421b244bfd7776a30e67dc7f2d3e3 (diff) | |
A Whole New World (clang-format edition)
I can show you the code
Pretty, with proper whitespace
Tell me, coder, now when did
You last write readable code?
I can open your eyes
Make you see your bad indent
Force you to respect the style
The core devs agreed upon
A whole new world
A new fantastic code format
A de facto standard
With some sugar
Enforced with clang-format
A whole new world
A dazzling style we all dreamed of
And when we read it through
It's crystal clear
That now we're in a whole new world of code
Diffstat (limited to 'servers/physics_2d')
33 files changed, 3421 insertions, 4215 deletions
diff --git a/servers/physics_2d/area_2d_sw.cpp b/servers/physics_2d/area_2d_sw.cpp index 2625502717..885ede7cbf 100644 --- a/servers/physics_2d/area_2d_sw.cpp +++ b/servers/physics_2d/area_2d_sw.cpp @@ -27,18 +27,26 @@ /* 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" +#include "space_2d_sw.h" -Area2DSW::BodyKey::BodyKey(Body2DSW *p_body, uint32_t p_body_shape,uint32_t p_area_shape) { rid=p_body->get_self(); instance_id=p_body->get_instance_id(); body_shape=p_body_shape; area_shape=p_area_shape; } -Area2DSW::BodyKey::BodyKey(Area2DSW *p_body, uint32_t p_body_shape,uint32_t p_area_shape) { rid=p_body->get_self(); instance_id=p_body->get_instance_id(); body_shape=p_body_shape; area_shape=p_area_shape; } +Area2DSW::BodyKey::BodyKey(Body2DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { + rid = p_body->get_self(); + instance_id = p_body->get_instance_id(); + body_shape = p_body_shape; + area_shape = p_area_shape; +} +Area2DSW::BodyKey::BodyKey(Area2DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { + rid = p_body->get_self(); + instance_id = p_body->get_instance_id(); + body_shape = p_body_shape; + area_shape = p_area_shape; +} void Area2DSW::_shapes_changed() { - - } -void Area2DSW::set_transform(const Transform2D& p_transform) { +void Area2DSW::set_transform(const Transform2D &p_transform) { if (!moved_list.in_list() && get_space()) get_space()->area_add_to_moved_list(&moved_list); @@ -54,7 +62,6 @@ void Area2DSW::set_space(Space2DSW *p_space) { 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(); @@ -63,19 +70,17 @@ void Area2DSW::set_space(Space2DSW *p_space) { _set_space(p_space); } +void Area2DSW::set_monitor_callback(ObjectID p_id, const StringName &p_method) { -void Area2DSW::set_monitor_callback(ObjectID p_id, const StringName& p_method) { - - - if (p_id==monitor_callback_id) { - monitor_callback_method=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; + monitor_callback_id = p_id; + monitor_callback_method = p_method; monitored_bodies.clear(); monitored_areas.clear(); @@ -84,22 +89,19 @@ void Area2DSW::set_monitor_callback(ObjectID p_id, const StringName& p_method) { if (!moved_list.in_list() && get_space()) get_space()->area_add_to_moved_list(&moved_list); - - } -void Area2DSW::set_area_monitor_callback(ObjectID p_id, const StringName& p_method) { +void Area2DSW::set_area_monitor_callback(ObjectID p_id, const StringName &p_method) { - - if (p_id==area_monitor_callback_id) { - area_monitor_callback_method=p_method; + if (p_id == area_monitor_callback_id) { + area_monitor_callback_method = p_method; return; } _unregister_shapes(); - area_monitor_callback_id=p_id; - area_monitor_callback_method=p_method; + area_monitor_callback_id = p_id; + area_monitor_callback_method = p_method; monitored_bodies.clear(); monitored_areas.clear(); @@ -108,45 +110,39 @@ void Area2DSW::set_area_monitor_callback(ObjectID p_id, const StringName& p_meth if (!moved_list.in_list() && get_space()) get_space()->area_add_to_moved_list(&moved_list); - - } - 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)) + 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; + 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_DISTANCE_SCALE: gravity_distance_scale=p_value; break; - case Physics2DServer::AREA_PARAM_GRAVITY_POINT_ATTENUATION: point_attenuation=p_value; break; - case Physics2DServer::AREA_PARAM_LINEAR_DAMP: linear_damp=p_value; break; - case Physics2DServer::AREA_PARAM_ANGULAR_DAMP: angular_damp=p_value; break; - case Physics2DServer::AREA_PARAM_PRIORITY: priority=p_value; break; +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_DISTANCE_SCALE: gravity_distance_scale = p_value; break; + case Physics2DServer::AREA_PARAM_GRAVITY_POINT_ATTENUATION: point_attenuation = p_value; break; + case Physics2DServer::AREA_PARAM_LINEAR_DAMP: linear_damp = p_value; break; + case Physics2DServer::AREA_PARAM_ANGULAR_DAMP: angular_damp = p_value; break; + case Physics2DServer::AREA_PARAM_PRIORITY: priority = p_value; break; } - - } Variant Area2DSW::get_param(Physics2DServer::AreaParameter p_param) const { - - switch(p_param) { + 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_DISTANCE_SCALE: return gravity_distance_scale; - case Physics2DServer::AREA_PARAM_GRAVITY_POINT_ATTENUATION: return point_attenuation; + case Physics2DServer::AREA_PARAM_GRAVITY_POINT_ATTENUATION: return point_attenuation; case Physics2DServer::AREA_PARAM_LINEAR_DAMP: return linear_damp; case Physics2DServer::AREA_PARAM_ANGULAR_DAMP: return angular_damp; case Physics2DServer::AREA_PARAM_PRIORITY: return priority; @@ -155,23 +151,20 @@ Variant Area2DSW::get_param(Physics2DServer::AreaParameter p_param) const { return Variant(); } - void Area2DSW::_queue_monitor_update() { ERR_FAIL_COND(!get_space()); if (!monitor_query_list.in_list()) get_space()->area_add_to_monitor_query_list(&monitor_query_list); - - } void Area2DSW::set_monitorable(bool p_monitorable) { - if (monitorable==p_monitorable) + if (monitorable == p_monitorable) return; - monitorable=p_monitorable; + monitorable = p_monitorable; _set_static(!monitorable); } @@ -181,31 +174,29 @@ void Area2DSW::call_queries() { Variant res[5]; Variant *resptr[5]; - for(int i=0;i<5;i++) - resptr[i]=&res[i]; + 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; + monitor_callback_id = 0; return; } + for (Map<BodyKey, BodyState>::Element *E = monitored_bodies.front(); E; E = E->next()) { - - for (Map<BodyKey,BodyState>::Element *E=monitored_bodies.front();E;E=E->next()) { - - if (E->get().state==0) + 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; + 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); + obj->call(monitor_callback_method, (const Variant **)resptr, 5, ce); } } @@ -213,66 +204,57 @@ void Area2DSW::call_queries() { if (area_monitor_callback_id && !monitored_areas.empty()) { - Variant res[5]; Variant *resptr[5]; - for(int i=0;i<5;i++) - resptr[i]=&res[i]; + for (int i = 0; i < 5; i++) + resptr[i] = &res[i]; Object *obj = ObjectDB::get_instance(area_monitor_callback_id); if (!obj) { monitored_areas.clear(); - area_monitor_callback_id=0; + area_monitor_callback_id = 0; return; } + for (Map<BodyKey, BodyState>::Element *E = monitored_areas.front(); E; E = E->next()) { - - for (Map<BodyKey,BodyState>::Element *E=monitored_areas.front();E;E=E->next()) { - - if (E->get().state==0) + 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; - + 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(area_monitor_callback_method,(const Variant**)resptr,5,ce); + obj->call(area_monitor_callback_method, (const Variant **)resptr, 5, ce); } } monitored_areas.clear(); //get_space()->area_remove_from_monitor_query_list(&monitor_query_list); - } -Area2DSW::Area2DSW() : CollisionObject2DSW(TYPE_AREA), monitor_query_list(this), moved_list(this) { +Area2DSW::Area2DSW() + : CollisionObject2DSW(TYPE_AREA), monitor_query_list(this), moved_list(this) { _set_static(true); //areas are not active by default - space_override_mode=Physics2DServer::AREA_SPACE_OVERRIDE_DISABLED; - gravity=9.80665; - gravity_vector=Vector2(0,-1); - gravity_is_point=false; - gravity_distance_scale=0; - point_attenuation=1; - - angular_damp=1.0; - linear_damp=0.1; - priority=0; - monitor_callback_id=0; - area_monitor_callback_id=0; - monitorable=false; - - + space_override_mode = Physics2DServer::AREA_SPACE_OVERRIDE_DISABLED; + gravity = 9.80665; + gravity_vector = Vector2(0, -1); + gravity_is_point = false; + gravity_distance_scale = 0; + point_attenuation = 1; + + angular_damp = 1.0; + linear_damp = 0.1; + priority = 0; + monitor_callback_id = 0; + area_monitor_callback_id = 0; + monitorable = false; } Area2DSW::~Area2DSW() { - - } - diff --git a/servers/physics_2d/area_2d_sw.h b/servers/physics_2d/area_2d_sw.h index 02a65962e5..8c52c96527 100644 --- a/servers/physics_2d/area_2d_sw.h +++ b/servers/physics_2d/area_2d_sw.h @@ -29,18 +29,16 @@ #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_2d_server.h" //#include "servers/physics/query_sw.h" class Space2DSW; class Body2DSW; class Constraint2DSW; - -class Area2DSW : public CollisionObject2DSW{ - +class Area2DSW : public CollisionObject2DSW { Physics2DServer::AreaSpaceOverrideMode space_override_mode; real_t gravity; @@ -69,23 +67,22 @@ class Area2DSW : public CollisionObject2DSW{ uint32_t body_shape; uint32_t area_shape; - _FORCE_INLINE_ bool operator<( const BodyKey& p_key) const { + _FORCE_INLINE_ bool operator<(const BodyKey &p_key) const { - if (rid==p_key.rid) { + if (rid == p_key.rid) { - if (body_shape==p_key.body_shape) { + if (body_shape == p_key.body_shape) { return area_shape < p_key.area_shape; } else return body_shape < p_key.body_shape; } else return rid < p_key.rid; - } _FORCE_INLINE_ BodyKey() {} - BodyKey(Body2DSW *p_body, uint32_t p_body_shape,uint32_t p_area_shape); - BodyKey(Area2DSW *p_body, uint32_t p_body_shape,uint32_t p_area_shape); + BodyKey(Body2DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape); + BodyKey(Area2DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape); }; struct BodyState { @@ -93,122 +90,110 @@ class Area2DSW : public CollisionObject2DSW{ int state; _FORCE_INLINE_ void inc() { state++; } _FORCE_INLINE_ void dec() { state--; } - _FORCE_INLINE_ BodyState() { state=0; } + _FORCE_INLINE_ BodyState() { state = 0; } }; - Map<BodyKey,BodyState> monitored_bodies; - Map<BodyKey,BodyState> monitored_areas; + Map<BodyKey, BodyState> monitored_bodies; + Map<BodyKey, BodyState> monitored_areas; //virtual void shape_changed_notify(Shape2DSW *p_shape); //virtual void shape_deleted_notify(Shape2DSW *p_shape); - Set<Constraint2DSW*> constraints; - + 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); + void set_monitor_callback(ObjectID p_id, const StringName &p_method); _FORCE_INLINE_ bool has_monitor_callback() const { return monitor_callback_id; } - void set_area_monitor_callback(ObjectID p_id, const StringName& p_method); + void set_area_monitor_callback(ObjectID p_id, const StringName &p_method); _FORCE_INLINE_ bool has_area_monitor_callback() const { return area_monitor_callback_id; } + _FORCE_INLINE_ void add_body_to_query(Body2DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape); + _FORCE_INLINE_ void remove_body_from_query(Body2DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape); - _FORCE_INLINE_ void add_body_to_query(Body2DSW *p_body, uint32_t p_body_shape,uint32_t p_area_shape); - _FORCE_INLINE_ void remove_body_from_query(Body2DSW *p_body, uint32_t p_body_shape,uint32_t p_area_shape); + _FORCE_INLINE_ void add_area_to_query(Area2DSW *p_area, uint32_t p_area_shape, uint32_t p_self_shape); + _FORCE_INLINE_ void remove_area_from_query(Area2DSW *p_area, uint32_t p_area_shape, uint32_t p_self_shape); - _FORCE_INLINE_ void add_area_to_query(Area2DSW *p_area, uint32_t p_area_shape,uint32_t p_self_shape); - _FORCE_INLINE_ void remove_area_from_query(Area2DSW *p_area, uint32_t p_area_shape,uint32_t p_self_shape); - - void set_param(Physics2DServer::AreaParameter p_param, const Variant& p_value); + 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(real_t p_gravity) { gravity=p_gravity; } + _FORCE_INLINE_ void set_gravity(real_t p_gravity) { gravity = p_gravity; } _FORCE_INLINE_ real_t get_gravity() const { return gravity; } - _FORCE_INLINE_ void set_gravity_vector(const Vector2& p_gravity) { gravity_vector=p_gravity; } + _FORCE_INLINE_ 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_ void set_gravity_as_point(bool p_enable) { gravity_is_point = p_enable; } _FORCE_INLINE_ bool is_gravity_point() const { return gravity_is_point; } - _FORCE_INLINE_ void set_gravity_distance_scale(real_t scale) { gravity_distance_scale=scale; } + _FORCE_INLINE_ void set_gravity_distance_scale(real_t scale) { gravity_distance_scale = scale; } _FORCE_INLINE_ real_t get_gravity_distance_scale() const { return gravity_distance_scale; } - _FORCE_INLINE_ void set_point_attenuation(real_t p_point_attenuation) { point_attenuation=p_point_attenuation; } + _FORCE_INLINE_ void set_point_attenuation(real_t p_point_attenuation) { point_attenuation = p_point_attenuation; } _FORCE_INLINE_ real_t get_point_attenuation() const { return point_attenuation; } - _FORCE_INLINE_ void set_linear_damp(real_t p_linear_damp) { linear_damp=p_linear_damp; } + _FORCE_INLINE_ void set_linear_damp(real_t p_linear_damp) { linear_damp = p_linear_damp; } _FORCE_INLINE_ real_t get_linear_damp() const { return linear_damp; } - _FORCE_INLINE_ void set_angular_damp(real_t p_angular_damp) { angular_damp=p_angular_damp; } + _FORCE_INLINE_ void set_angular_damp(real_t p_angular_damp) { angular_damp = p_angular_damp; } _FORCE_INLINE_ real_t get_angular_damp() const { return angular_damp; } - _FORCE_INLINE_ void set_priority(int p_priority) { priority=p_priority; } + _FORCE_INLINE_ 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; } + _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_monitorable(bool p_monitorable); _FORCE_INLINE_ bool is_monitorable() const { return monitorable; } - void set_transform(const Transform2D& p_transform); + void set_transform(const Transform2D &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) { +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); + 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) { +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); + BodyKey bk(p_body, p_body_shape, p_area_shape); monitored_bodies[bk].dec(); if (!monitor_query_list.in_list()) _queue_monitor_update(); } -void Area2DSW::add_area_to_query(Area2DSW *p_area, uint32_t p_area_shape,uint32_t p_self_shape) { +void Area2DSW::add_area_to_query(Area2DSW *p_area, uint32_t p_area_shape, uint32_t p_self_shape) { - - BodyKey bk(p_area,p_area_shape,p_self_shape); + BodyKey bk(p_area, p_area_shape, p_self_shape); monitored_areas[bk].inc(); if (!monitor_query_list.in_list()) _queue_monitor_update(); - - } -void Area2DSW::remove_area_from_query(Area2DSW *p_area, uint32_t p_area_shape,uint32_t p_self_shape) { - +void Area2DSW::remove_area_from_query(Area2DSW *p_area, uint32_t p_area_shape, uint32_t p_self_shape) { - BodyKey bk(p_area,p_area_shape,p_self_shape); + BodyKey bk(p_area, p_area_shape, p_self_shape); monitored_areas[bk].dec(); if (!monitor_query_list.in_list()) _queue_monitor_update(); } - - - - #endif // AREA_2D_SW_H diff --git a/servers/physics_2d/area_pair_2d_sw.cpp b/servers/physics_2d/area_pair_2d_sw.cpp index cb91caf626..769db8eb35 100644 --- a/servers/physics_2d/area_pair_2d_sw.cpp +++ b/servers/physics_2d/area_pair_2d_sw.cpp @@ -29,124 +29,105 @@ #include "area_pair_2d_sw.h" #include "collision_solver_2d_sw.h" - bool AreaPair2DSW::setup(real_t p_step) { - bool result = area->test_collision_mask(body) && CollisionSolver2DSW::solve(body->get_shape(body_shape),body->get_transform() * body->get_shape_transform(body_shape),Vector2(),area->get_shape(area_shape),area->get_transform() * area->get_shape_transform(area_shape),Vector2(),NULL,this); + bool result = area->test_collision_mask(body) && CollisionSolver2DSW::solve(body->get_shape(body_shape), body->get_transform() * body->get_shape_transform(body_shape), Vector2(), area->get_shape(area_shape), area->get_transform() * area->get_shape_transform(area_shape), Vector2(), NULL, this); - if (result!=colliding) { + if (result != colliding) { if (result) { - if (area->get_space_override_mode()!=Physics2DServer::AREA_SPACE_OVERRIDE_DISABLED) + 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); + area->add_body_to_query(body, body_shape, area_shape); } else { - if (area->get_space_override_mode()!=Physics2DServer::AREA_SPACE_OVERRIDE_DISABLED) + 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); - + area->remove_body_from_query(body, body_shape, area_shape); } - colliding=result; - + colliding = result; } return false; //never do any post solving } void AreaPair2DSW::solve(real_t p_step) { - - } +AreaPair2DSW::AreaPair2DSW(Body2DSW *p_body, int p_body_shape, Area2DSW *p_area, int p_area_shape) { -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); + body = p_body; + area = p_area; + body_shape = p_body_shape; + area_shape = p_area_shape; + colliding = false; + body->add_constraint(this, 0); area->add_constraint(this); - if (p_body->get_mode()==Physics2DServer::BODY_MODE_KINEMATIC) //need to be active to process pair + if (p_body->get_mode() == Physics2DServer::BODY_MODE_KINEMATIC) //need to be active to process pair p_body->set_active(true); - } AreaPair2DSW::~AreaPair2DSW() { if (colliding) { - if (area->get_space_override_mode()!=Physics2DServer::AREA_SPACE_OVERRIDE_DISABLED) + 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); - - + area->remove_body_from_query(body, body_shape, area_shape); } body->remove_constraint(this); area->remove_constraint(this); } - ////////////////////////////////// - - bool Area2Pair2DSW::setup(real_t p_step) { - bool result = area_a->test_collision_mask(area_b) && CollisionSolver2DSW::solve(area_a->get_shape(shape_a),area_a->get_transform() * area_a->get_shape_transform(shape_a),Vector2(),area_b->get_shape(shape_b),area_b->get_transform() * area_b->get_shape_transform(shape_b),Vector2(),NULL,this); + bool result = area_a->test_collision_mask(area_b) && CollisionSolver2DSW::solve(area_a->get_shape(shape_a), area_a->get_transform() * area_a->get_shape_transform(shape_a), Vector2(), area_b->get_shape(shape_b), area_b->get_transform() * area_b->get_shape_transform(shape_b), Vector2(), NULL, this); - if (result!=colliding) { + if (result != colliding) { if (result) { if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) - area_b->add_area_to_query(area_a,shape_a,shape_b); + area_b->add_area_to_query(area_a, shape_a, shape_b); if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) - area_a->add_area_to_query(area_b,shape_b,shape_a); + area_a->add_area_to_query(area_b, shape_b, shape_a); } else { if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) - area_b->remove_area_from_query(area_a,shape_a,shape_b); + area_b->remove_area_from_query(area_a, shape_a, shape_b); if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) - area_a->remove_area_from_query(area_b,shape_b,shape_a); + area_a->remove_area_from_query(area_b, shape_b, shape_a); } - colliding=result; - + colliding = result; } return false; //never do any post solving } void Area2Pair2DSW::solve(real_t p_step) { - - } +Area2Pair2DSW::Area2Pair2DSW(Area2DSW *p_area_a, int p_shape_a, Area2DSW *p_area_b, int p_shape_b) { -Area2Pair2DSW::Area2Pair2DSW(Area2DSW *p_area_a,int p_shape_a, Area2DSW *p_area_b,int p_shape_b) { - - - area_a=p_area_a; - area_b=p_area_b; - shape_a=p_shape_a; - shape_b=p_shape_b; - colliding=false; + area_a = p_area_a; + area_b = p_area_b; + shape_a = p_shape_a; + shape_b = p_shape_b; + colliding = false; area_a->add_constraint(this); area_b->add_constraint(this); - } Area2Pair2DSW::~Area2Pair2DSW() { @@ -154,10 +135,10 @@ Area2Pair2DSW::~Area2Pair2DSW() { if (colliding) { if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) - area_b->remove_area_from_query(area_a,shape_a,shape_b); + area_b->remove_area_from_query(area_a, shape_a, shape_b); if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) - area_a->remove_area_from_query(area_b,shape_b,shape_a); + area_a->remove_area_from_query(area_b, shape_b, shape_a); } area_a->remove_constraint(this); diff --git a/servers/physics_2d/area_pair_2d_sw.h b/servers/physics_2d/area_pair_2d_sw.h index 219ca30c4b..78be9572bf 100644 --- a/servers/physics_2d/area_pair_2d_sw.h +++ b/servers/physics_2d/area_pair_2d_sw.h @@ -29,9 +29,9 @@ #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" +#include "body_2d_sw.h" +#include "constraint_2d_sw.h" class AreaPair2DSW : public Constraint2DSW { @@ -40,16 +40,15 @@ class AreaPair2DSW : public Constraint2DSW { int body_shape; int area_shape; bool colliding; -public: +public: bool setup(real_t p_step); void solve(real_t p_step); - AreaPair2DSW(Body2DSW *p_body,int p_body_shape, Area2DSW *p_area,int p_area_shape); + AreaPair2DSW(Body2DSW *p_body, int p_body_shape, Area2DSW *p_area, int p_area_shape); ~AreaPair2DSW(); }; - class Area2Pair2DSW : public Constraint2DSW { Area2DSW *area_a; @@ -57,15 +56,13 @@ class Area2Pair2DSW : public Constraint2DSW { int shape_a; int shape_b; bool colliding; -public: +public: bool setup(real_t p_step); void solve(real_t p_step); - Area2Pair2DSW(Area2DSW *p_area_a,int p_shape_a, Area2DSW *p_area_b,int p_shape_b); + Area2Pair2DSW(Area2DSW *p_area_a, int p_shape_a, Area2DSW *p_area_b, int p_shape_b); ~Area2Pair2DSW(); }; - #endif // AREA_PAIR_2D_SW_H - diff --git a/servers/physics_2d/body_2d_sw.cpp b/servers/physics_2d/body_2d_sw.cpp index 0dab534ef9..d49dd35ade 100644 --- a/servers/physics_2d/body_2d_sw.cpp +++ b/servers/physics_2d/body_2d_sw.cpp @@ -27,107 +27,98 @@ /* 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" #include "physics_2d_server_sw.h" +#include "space_2d_sw.h" void Body2DSW::_update_inertia() { if (!user_inertia && 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) { + switch (mode) { case Physics2DServer::BODY_MODE_RIGID: { - if(user_inertia) break; + if (user_inertia) break; //update tensor for allshapes, not the best way but should be somehow OK. (inspired from bullet) - real_t total_area=0; + real_t total_area = 0; - for (int i=0;i<get_shape_count();i++) { + for (int i = 0; i < get_shape_count(); i++) { - total_area+=get_shape_aabb(i).get_area(); + total_area += get_shape_aabb(i).get_area(); } - real_t _inertia=0; + real_t _inertia = 0; - for (int i=0;i<get_shape_count();i++) { + for (int i = 0; i < get_shape_count(); i++) { - const Shape2DSW* shape=get_shape(i); + const Shape2DSW *shape = get_shape(i); - real_t area=get_shape_aabb(i).get_area(); + real_t area = get_shape_aabb(i).get_area(); real_t mass = area * this->mass / total_area; Transform2D mtx = get_shape_transform(i); Vector2 scale = mtx.get_scale(); - _inertia += shape->get_moment_of_inertia(mass,scale) + mass * mtx.get_origin().length_squared(); + _inertia += shape->get_moment_of_inertia(mass, scale) + mass * mtx.get_origin().length_squared(); //Rect2 ab = get_shape_aabb(i); //_inertia+=mass*ab.size.dot(ab.size)/12.0f; - - - } - if (_inertia!=0) - _inv_inertia=1.0/_inertia; + if (_inertia != 0) + _inv_inertia = 1.0 / _inertia; else - _inv_inertia=0.0; //wathever + _inv_inertia = 0.0; //wathever if (mass) - _inv_mass=1.0/mass; + _inv_mass = 1.0 / mass; else - _inv_mass=0; + _inv_mass = 0; } break; case Physics2DServer::BODY_MODE_KINEMATIC: case Physics2DServer::BODY_MODE_STATIC: { - _inv_inertia=0; - _inv_mass=0; + _inv_inertia = 0; + _inv_mass = 0; } break; case Physics2DServer::BODY_MODE_CHARACTER: { - _inv_inertia=0; - _inv_mass=1.0/mass; + _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) + if (active == p_active) return; - active=p_active; + 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) + 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; @@ -140,27 +131,25 @@ void Body2DSW::set_active(bool p_active) { */ } - - void Body2DSW::set_param(Physics2DServer::BodyParameter p_param, real_t p_value) { - switch(p_param) { + switch (p_param) { case Physics2DServer::BODY_PARAM_BOUNCE: { - bounce=p_value; + bounce = p_value; } break; case Physics2DServer::BODY_PARAM_FRICTION: { - friction=p_value; + friction = p_value; } break; case Physics2DServer::BODY_PARAM_MASS: { - ERR_FAIL_COND(p_value<=0); - mass=p_value; + ERR_FAIL_COND(p_value <= 0); + mass = p_value; _update_inertia(); } break; case Physics2DServer::BODY_PARAM_INERTIA: { - if(p_value<=0) { + if (p_value <= 0) { user_inertia = false; _update_inertia(); } else { @@ -169,23 +158,23 @@ void Body2DSW::set_param(Physics2DServer::BodyParameter p_param, real_t p_value) } } break; case Physics2DServer::BODY_PARAM_GRAVITY_SCALE: { - gravity_scale=p_value; + gravity_scale = p_value; } break; case Physics2DServer::BODY_PARAM_LINEAR_DAMP: { - linear_damp=p_value; + linear_damp = p_value; } break; case Physics2DServer::BODY_PARAM_ANGULAR_DAMP: { - angular_damp=p_value; + angular_damp = p_value; } break; - default:{} + default: {} } } real_t Body2DSW::get_param(Physics2DServer::BodyParameter p_param) const { - switch(p_param) { + switch (p_param) { case Physics2DServer::BODY_PARAM_BOUNCE: { return bounce; @@ -198,7 +187,7 @@ real_t Body2DSW::get_param(Physics2DServer::BodyParameter p_param) const { return mass; } break; case Physics2DServer::BODY_PARAM_INERTIA: { - return _inv_inertia==0 ? 0 : 1.0 / _inv_inertia; + return _inv_inertia == 0 ? 0 : 1.0 / _inv_inertia; } break; case Physics2DServer::BODY_PARAM_GRAVITY_SCALE: { return gravity_scale; @@ -211,7 +200,7 @@ real_t Body2DSW::get_param(Physics2DServer::BodyParameter p_param) const { return angular_damp; } break; - default:{} + default: {} } return 0; @@ -219,33 +208,33 @@ real_t Body2DSW::get_param(Physics2DServer::BodyParameter p_param) const { void Body2DSW::set_mode(Physics2DServer::BodyMode p_mode) { - Physics2DServer::BodyMode prev=mode; - mode=p_mode; + Physics2DServer::BodyMode prev = mode; + mode = p_mode; - switch(p_mode) { - //CLEAR UP EVERYTHING IN CASE IT NOT WORKS! + switch (p_mode) { + //CLEAR UP EVERYTHING IN CASE IT NOT WORKS! case Physics2DServer::BODY_MODE_STATIC: case Physics2DServer::BODY_MODE_KINEMATIC: { _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_KINEMATIC && contacts.size()); - linear_velocity=Vector2(); - angular_velocity=0; - if (mode==Physics2DServer::BODY_MODE_KINEMATIC && prev!=mode) { - first_time_kinematic=true; + _inv_mass = 0; + _set_static(p_mode == Physics2DServer::BODY_MODE_STATIC); + set_active(p_mode == Physics2DServer::BODY_MODE_KINEMATIC && contacts.size()); + linear_velocity = Vector2(); + angular_velocity = 0; + if (mode == Physics2DServer::BODY_MODE_KINEMATIC && prev != mode) { + first_time_kinematic = true; } } break; case Physics2DServer::BODY_MODE_RIGID: { - _inv_mass=mass>0?(1.0/mass):0; + _inv_mass = mass > 0 ? (1.0 / mass) : 0; _set_static(false); } break; case Physics2DServer::BODY_MODE_CHARACTER: { - _inv_mass=mass>0?(1.0/mass):0; + _inv_mass = mass > 0 ? (1.0 / mass) : 0; _set_static(false); } break; } @@ -255,7 +244,6 @@ void Body2DSW::set_mode(Physics2DServer::BodyMode p_mode) { if (get_space()) _update_queries(); */ - } Physics2DServer::BodyMode Body2DSW::get_mode() const { @@ -268,35 +256,33 @@ void Body2DSW::_shapes_changed() { wakeup_neighbours(); } -void Body2DSW::set_state(Physics2DServer::BodyState p_state, const Variant& p_variant) { +void Body2DSW::set_state(Physics2DServer::BodyState p_state, const Variant &p_variant) { - switch(p_state) { + switch (p_state) { case Physics2DServer::BODY_STATE_TRANSFORM: { + if (mode == Physics2DServer::BODY_MODE_KINEMATIC) { - if (mode==Physics2DServer::BODY_MODE_KINEMATIC) { - - new_transform=p_variant; + new_transform = p_variant; //wakeup_neighbours(); set_active(true); if (first_time_kinematic) { _set_transform(p_variant); _set_inv_transform(get_transform().affine_inverse()); - first_time_kinematic=false; + first_time_kinematic = false; } - } else if (mode==Physics2DServer::BODY_MODE_STATIC) { + } else if (mode == Physics2DServer::BODY_MODE_STATIC) { _set_transform(p_variant); _set_inv_transform(get_transform().affine_inverse()); wakeup_neighbours(); } else { Transform2D t = p_variant; t.orthonormalize(); - new_transform=get_transform(); //used as old to compute motion - if (t==new_transform) + new_transform = get_transform(); //used as old to compute motion + if (t == new_transform) break; _set_transform(t); _set_inv_transform(get_transform().inverse()); - } wakeup(); @@ -307,7 +293,7 @@ void Body2DSW::set_state(Physics2DServer::BodyState p_state, const Variant& p_va if (mode==Physics2DServer::BODY_MODE_STATIC) break; */ - linear_velocity=p_variant; + linear_velocity = p_variant; wakeup(); } break; @@ -316,38 +302,37 @@ void Body2DSW::set_state(Physics2DServer::BodyState p_state, const Variant& p_va if (mode!=Physics2DServer::BODY_MODE_RIGID) break; */ - angular_velocity=p_variant; + angular_velocity = p_variant; wakeup(); } break; case Physics2DServer::BODY_STATE_SLEEPING: { //? - if (mode==Physics2DServer::BODY_MODE_STATIC || mode==Physics2DServer::BODY_MODE_KINEMATIC) + if (mode == Physics2DServer::BODY_MODE_STATIC || mode == Physics2DServer::BODY_MODE_KINEMATIC) break; - bool do_sleep=p_variant; + bool do_sleep = p_variant; if (do_sleep) { - linear_velocity=Vector2(); + linear_velocity = Vector2(); //biased_linear_velocity=Vector3(); - angular_velocity=0; + angular_velocity = 0; //biased_angular_velocity=Vector3(); set_active(false); } else { - if (mode!=Physics2DServer::BODY_MODE_STATIC) + 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) + 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) { + switch (p_state) { case Physics2DServer::BODY_STATE_TRANSFORM: { return get_transform(); } break; @@ -368,8 +353,7 @@ Variant Body2DSW::get_state(Physics2DServer::BodyState p_state) const { return Variant(); } - -void Body2DSW::set_space(Space2DSW *p_space){ +void Body2DSW::set_space(Space2DSW *p_space) { if (get_space()) { @@ -381,7 +365,6 @@ void Body2DSW::set_space(Space2DSW *p_space){ 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); @@ -398,18 +381,17 @@ void Body2DSW::set_space(Space2DSW *p_space){ set_active(true); } */ - } - first_integration=false; + first_integration = false; } void Body2DSW::_compute_area_gravity_and_dampenings(const Area2DSW *p_area) { if (p_area->is_gravity_point()) { - if(p_area->get_gravity_distance_scale() > 0) { + if (p_area->get_gravity_distance_scale() > 0) { Vector2 v = p_area->get_transform().xform(p_area->get_gravity_vector()) - get_transform().get_origin(); - gravity += v.normalized() * (p_area->get_gravity() / Math::pow(v.length() * p_area->get_gravity_distance_scale()+1, 2) ); + gravity += v.normalized() * (p_area->get_gravity() / Math::pow(v.length() * p_area->get_gravity_distance_scale() + 1, 2)); } else { gravity += (p_area->get_transform().xform(p_area->get_gravity_vector()) - get_transform().get_origin()).normalized() * p_area->get_gravity(); } @@ -423,7 +405,7 @@ void Body2DSW::_compute_area_gravity_and_dampenings(const Area2DSW *p_area) { void Body2DSW::integrate_forces(real_t p_step) { - if (mode==Physics2DServer::BODY_MODE_STATIC) + if (mode == Physics2DServer::BODY_MODE_STATIC) return; Area2DSW *def_area = get_space()->get_default_area(); @@ -432,47 +414,47 @@ void Body2DSW::integrate_forces(real_t p_step) { int ac = areas.size(); bool stopped = false; - gravity = Vector2(0,0); + gravity = Vector2(0, 0); area_angular_damp = 0; area_linear_damp = 0; if (ac) { areas.sort(); const AreaCMP *aa = &areas[0]; // damp_area = aa[ac-1].area; - for(int i=ac-1;i>=0 && !stopped;i--) { - Physics2DServer::AreaSpaceOverrideMode mode=aa[i].area->get_space_override_mode(); + for (int i = ac - 1; i >= 0 && !stopped; i--) { + Physics2DServer::AreaSpaceOverrideMode mode = aa[i].area->get_space_override_mode(); switch (mode) { case Physics2DServer::AREA_SPACE_OVERRIDE_COMBINE: case Physics2DServer::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: { _compute_area_gravity_and_dampenings(aa[i].area); - stopped = mode==Physics2DServer::AREA_SPACE_OVERRIDE_COMBINE_REPLACE; + stopped = mode == Physics2DServer::AREA_SPACE_OVERRIDE_COMBINE_REPLACE; } break; case Physics2DServer::AREA_SPACE_OVERRIDE_REPLACE: case Physics2DServer::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: { - gravity = Vector2(0,0); + gravity = Vector2(0, 0); area_angular_damp = 0; area_linear_damp = 0; _compute_area_gravity_and_dampenings(aa[i].area); - stopped = mode==Physics2DServer::AREA_SPACE_OVERRIDE_REPLACE; + stopped = mode == Physics2DServer::AREA_SPACE_OVERRIDE_REPLACE; } break; default: {} } } } - if( !stopped ) { + if (!stopped) { _compute_area_gravity_and_dampenings(def_area); } - gravity*=gravity_scale; + gravity *= gravity_scale; // If less than 0, override dampenings with that of the Body2D - if (angular_damp>=0) + if (angular_damp >= 0) area_angular_damp = angular_damp; /* else area_angular_damp=damp_area->get_angular_damp(); */ - if (linear_damp>=0) + if (linear_damp >= 0) area_linear_damp = linear_damp; /* else @@ -480,19 +462,18 @@ void Body2DSW::integrate_forces(real_t p_step) { */ Vector2 motion; - bool do_motion=false; + bool do_motion = false; - if (mode==Physics2DServer::BODY_MODE_KINEMATIC) { + if (mode == Physics2DServer::BODY_MODE_KINEMATIC) { //compute motion, angular and etc. velocities from prev transform motion = new_transform.get_origin() - get_transform().get_origin(); - linear_velocity = motion/p_step; + linear_velocity = motion / p_step; real_t rot = new_transform.get_rotation() - get_transform().get_rotation(); angular_velocity = rot / p_step; - - do_motion=true; + do_motion = true; /* for(int i=0;i<get_shape_count();i++) { @@ -505,102 +486,96 @@ void Body2DSW::integrate_forces(real_t p_step) { if (!omit_force_integration && !first_integration) { //overriden by direct state query - Vector2 force=gravity*mass; - force+=applied_force; - real_t torque=applied_torque; + Vector2 force = gravity * mass; + force += applied_force; + real_t torque = applied_torque; real_t damp = 1.0 - p_step * area_linear_damp; - if (damp<0) // reached zero in the given time - damp=0; + if (damp < 0) // reached zero in the given time + damp = 0; real_t angular_damp = 1.0 - p_step * area_angular_damp; - if (angular_damp<0) // reached zero in the given time - angular_damp=0; + if (angular_damp < 0) // reached zero in the given time + angular_damp = 0; - linear_velocity*=damp; - angular_velocity*=angular_damp; + linear_velocity *= damp; + angular_velocity *= angular_damp; - linear_velocity+=_inv_mass * force * p_step; - angular_velocity+=_inv_inertia * torque * p_step; + linear_velocity += _inv_mass * force * p_step; + angular_velocity += _inv_inertia * torque * p_step; } - if (continuous_cd_mode!=Physics2DServer::CCD_MODE_DISABLED) { + if (continuous_cd_mode != Physics2DServer::CCD_MODE_DISABLED) { motion = new_transform.get_origin() - get_transform().get_origin(); //linear_velocity*p_step; - do_motion=true; + do_motion = true; } } - //motion=linear_velocity*p_step; - first_integration=false; - biased_angular_velocity=0; - biased_linear_velocity=Vector2(); + first_integration = false; + biased_angular_velocity = 0; + biased_linear_velocity = Vector2(); - if (do_motion) {//shapes temporarily extend for raycast + if (do_motion) { //shapes temporarily extend for raycast _update_shapes_with_motion(motion); } // damp_area=NULL; // clear the area, so it is set in the next frame - def_area=NULL; // clear the area, so it is set in the next frame - contact_count=0; - + def_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) + if (mode == Physics2DServer::BODY_MODE_STATIC) return; if (fi_callback) get_space()->body_add_to_state_query_list(&direct_state_query_list); - if (mode==Physics2DServer::BODY_MODE_KINEMATIC) { + if (mode == Physics2DServer::BODY_MODE_KINEMATIC) { - _set_transform(new_transform,false); + _set_transform(new_transform, false); _set_inv_transform(new_transform.affine_inverse()); - if (contacts.size()==0 && linear_velocity==Vector2() && angular_velocity==0) + if (contacts.size() == 0 && linear_velocity == Vector2() && angular_velocity == 0) set_active(false); //stopped moving, deactivate return; } - real_t total_angular_velocity = angular_velocity+biased_angular_velocity; - Vector2 total_linear_velocity=linear_velocity+biased_linear_velocity; + real_t 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(Transform2D(angle,pos),continuous_cd_mode==Physics2DServer::CCD_MODE_DISABLED); + _set_transform(Transform2D(angle, pos), continuous_cd_mode == Physics2DServer::CCD_MODE_DISABLED); _set_inv_transform(get_transform().inverse()); - if (continuous_cd_mode!=Physics2DServer::CCD_MODE_DISABLED) - new_transform=get_transform(); + if (continuous_cd_mode != Physics2DServer::CCD_MODE_DISABLED) + new_transform = get_transform(); //_update_inertia_tensor(); } - - void Body2DSW::wakeup_neighbours() { + for (Map<Constraint2DSW *, int>::Element *E = constraint_map.front(); E; E = E->next()) { - - for(Map<Constraint2DSW*,int>::Element *E=constraint_map.front();E;E=E->next()) { - - const Constraint2DSW *c=E->key(); + const Constraint2DSW *c = E->key(); Body2DSW **n = c->get_body_ptr(); - int bc=c->get_body_count(); + int bc = c->get_body_count(); - for(int i=0;i<bc;i++) { + for (int i = 0; i < bc; i++) { - if (i==E->get()) + if (i == E->get()) continue; Body2DSW *b = n[i]; - if (b->mode!=Physics2DServer::BODY_MODE_RIGID) + if (b->mode != Physics2DServer::BODY_MODE_RIGID) continue; if (!b->is_active()) @@ -611,116 +586,103 @@ void Body2DSW::wakeup_neighbours() { 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}; + 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(0,StringName()); + set_force_integration_callback(0, StringName()); } else { Variant::CallError ce; if (fi_callback->callback_udata.get_type()) { - obj->call(fi_callback->method,vp,2,ce); + obj->call(fi_callback->method, vp, 2, ce); } else { - obj->call(fi_callback->method,vp,1,ce); + obj->call(fi_callback->method, vp, 1, ce); } } - - } - } +bool Body2DSW::sleep_test(real_t p_step) { -bool Body2DSW::sleep_test(real_t p_step) { - - if (mode==Physics2DServer::BODY_MODE_STATIC || mode==Physics2DServer::BODY_MODE_KINEMATIC) + if (mode == Physics2DServer::BODY_MODE_STATIC || mode == Physics2DServer::BODY_MODE_KINEMATIC) return true; // - else if (mode==Physics2DServer::BODY_MODE_CHARACTER) + else if (mode == Physics2DServer::BODY_MODE_CHARACTER) return !active; // characters and kinematic bodies 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()) { - - - 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; + 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? + 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) { +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; + fi_callback = NULL; } + if (p_id != 0) { - if (p_id!=0) { - - fi_callback=memnew(ForceIntegrationCallback); - fi_callback->id=p_id; - fi_callback->method=p_method; - fi_callback->callback_udata=p_udata; + 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; - user_inertia=false; - _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; +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; + user_inertia = false; + _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); - first_time_kinematic=false; - linear_damp=-1; - angular_damp=-1; - area_angular_damp=0; - area_linear_damp=0; - contact_count=0; - gravity_scale=1.0; - using_one_way_cache=false; - one_way_collision_max_depth=0.1; - first_integration=false; - - still_time=0; - continuous_cd_mode=Physics2DServer::CCD_MODE_DISABLED; - can_sleep=false; - fi_callback=NULL; - + first_time_kinematic = false; + linear_damp = -1; + angular_damp = -1; + area_angular_damp = 0; + area_linear_damp = 0; + contact_count = 0; + gravity_scale = 1.0; + using_one_way_cache = false; + one_way_collision_max_depth = 0.1; + first_integration = false; + + still_time = 0; + continuous_cd_mode = Physics2DServer::CCD_MODE_DISABLED; + can_sleep = false; + fi_callback = NULL; } Body2DSW::~Body2DSW() { @@ -729,17 +691,16 @@ Body2DSW::~Body2DSW() { memdelete(fi_callback); } -Physics2DDirectBodyStateSW *Physics2DDirectBodyStateSW::singleton=NULL; +Physics2DDirectBodyStateSW *Physics2DDirectBodyStateSW::singleton = NULL; -Physics2DDirectSpaceState* Physics2DDirectBodyStateSW::get_space_state() { +Physics2DDirectSpaceState *Physics2DDirectBodyStateSW::get_space_state() { return body->get_space()->get_direct_state(); } - Variant Physics2DDirectBodyStateSW::get_contact_collider_shape_metadata(int p_contact_idx) const { - ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Variant()); + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Variant()); if (!Physics2DServerSW::singletonsw->body_owner.owns(body->contacts[p_contact_idx].collider)) { @@ -748,11 +709,10 @@ Variant Physics2DDirectBodyStateSW::get_contact_collider_shape_metadata(int p_co Body2DSW *other = Physics2DServerSW::singletonsw->body_owner.get(body->contacts[p_contact_idx].collider); int sidx = body->contacts[p_contact_idx].collider_shape; - if (sidx<0 || sidx>=other->get_shape_count()) { + if (sidx < 0 || sidx >= other->get_shape_count()) { return Variant(); } - return other->get_shape_metadata(sidx); } diff --git a/servers/physics_2d/body_2d_sw.h b/servers/physics_2d/body_2d_sw.h index 3fb01959a9..7e4fef8df3 100644 --- a/servers/physics_2d/body_2d_sw.h +++ b/servers/physics_2d/body_2d_sw.h @@ -29,16 +29,14 @@ #ifndef BODY_2D_SW_H #define BODY_2D_SW_H +#include "area_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; @@ -71,7 +69,6 @@ class Body2DSW : public CollisionObject2DSW { Vector2 one_way_collision_direction; real_t one_way_collision_max_depth; - SelfList<Body2DSW> active_list; SelfList<Body2DSW> inertia_update_list; SelfList<Body2DSW> direct_state_query_list; @@ -88,25 +85,25 @@ class Body2DSW : public CollisionObject2DSW { virtual void _shapes_changed(); Transform2D new_transform; - - Map<Constraint2DSW*,int> constraint_map; + Map<Constraint2DSW *, int> constraint_map; struct AreaCMP { Area2DSW *area; int refCount; - _FORCE_INLINE_ bool operator==(const AreaCMP& p_cmp) const { return area->get_self() == p_cmp.area->get_self();} - _FORCE_INLINE_ bool operator<(const AreaCMP& p_cmp) const { return area->get_priority() < p_cmp.area->get_priority();} + _FORCE_INLINE_ bool operator==(const AreaCMP &p_cmp) const { return area->get_self() == p_cmp.area->get_self(); } + _FORCE_INLINE_ bool operator<(const AreaCMP &p_cmp) const { return area->get_priority() < p_cmp.area->get_priority(); } _FORCE_INLINE_ AreaCMP() {} - _FORCE_INLINE_ AreaCMP(Area2DSW *p_area) { area=p_area; refCount=1;} + _FORCE_INLINE_ AreaCMP(Area2DSW *p_area) { + area = p_area; + refCount = 1; + } }; - Vector<AreaCMP> areas; struct Contact { - Vector2 local_pos; Vector2 local_normal; real_t depth; @@ -130,24 +127,20 @@ class Body2DSW : public CollisionObject2DSW { ForceIntegrationCallback *fi_callback; - uint64_t island_step; Body2DSW *island_next; Body2DSW *island_list_next; _FORCE_INLINE_ void _compute_area_gravity_and_dampenings(const Area2DSW *p_area); -friend class Physics2DDirectBodyStateSW; // i give up, too many functions to expose + 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()); - + 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) { int index = areas.find(AreaCMP(p_area)); - if( index > -1 ) { + if (index > -1) { areas[index].refCount += 1; } else { areas.ordered_insert(AreaCMP(p_area)); @@ -156,62 +149,64 @@ public: _FORCE_INLINE_ void remove_area(Area2DSW *p_area) { int index = areas.find(AreaCMP(p_area)); - if( index > -1 ) { + if (index > -1) { areas[index].refCount -= 1; - if( areas[index].refCount < 1 ) + if (areas[index].refCount < 1) areas.remove(index); } } - _FORCE_INLINE_ void set_max_contacts_reported(int p_size) { contacts.resize(p_size); contact_count=0; if (mode==Physics2DServer::BODY_MODE_KINEMATIC && p_size) set_active(true);} + _FORCE_INLINE_ void set_max_contacts_reported(int p_size) { + contacts.resize(p_size); + contact_count = 0; + if (mode == Physics2DServer::BODY_MODE_KINEMATIC && p_size) set_active(true); + } _FORCE_INLINE_ int get_max_contacts_reported() const { return contacts.size(); } _FORCE_INLINE_ bool can_report_contacts() const { return !contacts.empty(); } - _FORCE_INLINE_ void add_contact(const Vector2& p_local_pos,const Vector2& p_local_normal, real_t p_depth, int p_local_shape, const Vector2& p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID& p_collider,const Vector2& p_collider_velocity_at_pos); + _FORCE_INLINE_ void add_contact(const Vector2 &p_local_pos, const Vector2 &p_local_normal, real_t p_depth, int p_local_shape, const Vector2 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector2 &p_collider_velocity_at_pos); - - _FORCE_INLINE_ void add_exception(const RID& p_exception) { exceptions.insert(p_exception);} - _FORCE_INLINE_ void remove_exception(const RID& p_exception) { exceptions.erase(p_exception);} - _FORCE_INLINE_ bool has_exception(const RID& p_exception) const { return exceptions.has(p_exception);} - _FORCE_INLINE_ const VSet<RID>& get_exceptions() const { return exceptions;} + _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_ 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_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_ 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 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_ 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_ 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_ 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_ 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_ 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_offset, const Vector2& p_impulse) { + _FORCE_INLINE_ void apply_impulse(const Vector2 &p_offset, const Vector2 &p_impulse) { linear_velocity += p_impulse * _inv_mass; angular_velocity += _inv_inertia * p_offset.cross(p_impulse); } - _FORCE_INLINE_ void apply_bias_impulse(const Vector2& p_pos, const Vector2& 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); @@ -221,45 +216,42 @@ public: _FORCE_INLINE_ bool is_active() const { return active; } _FORCE_INLINE_ void wakeup() { - if ((!get_space()) || mode==Physics2DServer::BODY_MODE_STATIC || mode==Physics2DServer::BODY_MODE_KINEMATIC) + if ((!get_space()) || mode == Physics2DServer::BODY_MODE_STATIC || mode == Physics2DServer::BODY_MODE_KINEMATIC) return; set_active(true); } - - - void set_param(Physics2DServer::BodyParameter p_param, real_t); real_t 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); + 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; } + 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; } + void set_applied_torque(real_t p_torque) { applied_torque = p_torque; } real_t get_applied_torque() const { return applied_torque; } - _FORCE_INLINE_ void add_force(const Vector2& p_force, const Vector2& p_offset) { + _FORCE_INLINE_ void add_force(const Vector2 &p_force, const Vector2 &p_offset) { applied_force += p_force; applied_torque += p_offset.cross(p_force); } - _FORCE_INLINE_ void set_continuous_collision_detection_mode(Physics2DServer::CCDMode p_mode) { continuous_cd_mode=p_mode; } + _FORCE_INLINE_ void set_continuous_collision_detection_mode(Physics2DServer::CCDMode p_mode) { continuous_cd_mode = p_mode; } _FORCE_INLINE_ Physics2DServer::CCDMode get_continuous_collision_detection_mode() const { return continuous_cd_mode; } - void set_one_way_collision_direction(const Vector2& p_dir) { - one_way_collision_direction=p_dir; - using_one_way_cache=one_way_collision_direction!=Vector2(); + void set_one_way_collision_direction(const Vector2 &p_dir) { + one_way_collision_direction = p_dir; + using_one_way_cache = one_way_collision_direction != Vector2(); } Vector2 get_one_way_collision_direction() const { return one_way_collision_direction; } - void set_one_way_collision_max_depth(real_t p_depth) { one_way_collision_max_depth=p_depth; } + void set_one_way_collision_max_depth(real_t p_depth) { one_way_collision_max_depth = p_depth; } real_t get_one_way_collision_max_depth() const { return one_way_collision_max_depth; } _FORCE_INLINE_ bool is_using_one_way_collision() const { return using_one_way_cache; } @@ -276,16 +268,15 @@ public: _FORCE_INLINE_ real_t get_linear_damp() const { return linear_damp; } _FORCE_INLINE_ real_t get_angular_damp() const { return angular_damp; } - void integrate_forces(real_t p_step); void integrate_velocities(real_t p_step); _FORCE_INLINE_ Vector2 get_motion() const { - if (mode>Physics2DServer::BODY_MODE_KINEMATIC) { + if (mode > Physics2DServer::BODY_MODE_KINEMATIC) { return new_transform.get_origin() - get_transform().get_origin(); - } else if (mode==Physics2DServer::BODY_MODE_KINEMATIC) { - return get_transform().get_origin() -new_transform.get_origin(); //kinematic simulates forward + } else if (mode == Physics2DServer::BODY_MODE_KINEMATIC) { + return get_transform().get_origin() - new_transform.get_origin(); //kinematic simulates forward } return Vector2(); } @@ -297,111 +288,127 @@ public: Body2DSW(); ~Body2DSW(); - }; - //add contact inline -void Body2DSW::add_contact(const Vector2& p_local_pos,const Vector2& p_local_normal, real_t p_depth, int p_local_shape, const Vector2& p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID& p_collider,const Vector2& p_collider_velocity_at_pos) { +void Body2DSW::add_contact(const Vector2 &p_local_pos, const Vector2 &p_local_normal, real_t p_depth, int p_local_shape, const Vector2 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector2 &p_collider_velocity_at_pos) { - int c_max=contacts.size(); + int c_max = contacts.size(); - if (c_max==0) + if (c_max == 0) return; Contact *c = &contacts[0]; + int idx = -1; - int idx=-1; - - if (contact_count<c_max) { - idx=contact_count++; + if (contact_count < c_max) { + idx = contact_count++; } else { - real_t least_depth=1e20; - int least_deep=-1; - for(int i=0;i<c_max;i++) { + real_t least_depth = 1e20; + int least_deep = -1; + for (int i = 0; i < c_max; i++) { - if (i==0 || c[i].depth<least_depth) { - least_deep=i; - least_depth=c[i].depth; + if (i == 0 || c[i].depth < least_depth) { + least_deep = i; + least_depth = c[i].depth; } } - if (least_deep>=0 && least_depth<p_depth) { + if (least_deep >= 0 && least_depth < p_depth) { - idx=least_deep; + idx = least_deep; } - if (idx==-1) + 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; - + 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 { - GDCLASS( Physics2DDirectBodyStateSW, Physics2DDirectBodyState ); + GDCLASS(Physics2DDirectBodyStateSW, Physics2DDirectBodyState); public: - static Physics2DDirectBodyStateSW *singleton; Body2DSW *body; real_t step; - virtual Vector2 get_total_gravity() const { return body->gravity; } // get gravity vector working on this body space/area - virtual real_t get_total_angular_damp() const { return body->area_angular_damp; } // get density of this body space/area - virtual real_t get_total_linear_damp() const { return body->area_linear_damp; } // get density of this body space/area + virtual Vector2 get_total_gravity() const { return body->gravity; } // get gravity vector working on this body space/area + virtual real_t get_total_angular_damp() const { return body->area_angular_damp; } // get density of this body space/area + virtual real_t get_total_linear_damp() const { return body->area_linear_damp; } // get density of this body space/area - virtual real_t 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 real_t 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_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_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 Transform2D& p_transform) { body->set_state(Physics2DServer::BODY_STATE_TRANSFORM,p_transform); } - virtual Transform2D get_transform() const { return body->get_transform(); } + virtual void set_transform(const Transform2D &p_transform) { body->set_state(Physics2DServer::BODY_STATE_TRANSFORM, p_transform); } + virtual Transform2D 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 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 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()); + 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 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 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 Variant get_contact_collider_shape_metadata(int p_contact_idx) const; - 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 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; } + 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 index c05d61b658..ee94a7acec 100644 --- a/servers/physics_2d/body_pair_2d_sw.cpp +++ b/servers/physics_2d/body_pair_2d_sw.cpp @@ -30,54 +30,52 @@ #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) { +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); - + 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) { +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); + 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) ); + ERR_FAIL_COND(new_index >= (MAX_CONTACTS + 1)); Contact contact; - contact.acc_normal_impulse=0; - contact.acc_bias_impulse=0; - contact.acc_tangent_impulse=0; - contact.local_A=local_A; - contact.local_B=local_B; - contact.reused=true; - contact.normal=(p_point_A-p_point_B).normalized(); + contact.acc_normal_impulse = 0; + contact.acc_bias_impulse = 0; + contact.acc_tangent_impulse = 0; + contact.local_A = local_A; + contact.local_B = local_B; + contact.reused = true; + contact.normal = (p_point_A - p_point_B).normalized(); // 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++) { + for (int i = 0; i < contact_count; i++) { - Contact& c = contacts[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) ) { + 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; + 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; } } @@ -88,44 +86,41 @@ void BodyPair2DSW::_contact_added_callback(const Vector2& p_point_A,const Vector // remove the contact with the minimum depth - int least_deep=-1; - real_t min_depth=1e10; + int least_deep = -1; + real_t min_depth = 1e10; + for (int i = 0; i <= contact_count; i++) { - for (int i=0;i<=contact_count;i++) { - - Contact& c = (i==contact_count)?contact:contacts[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 global_B = B->get_transform().basis_xform(c.local_B) + offset_B; Vector2 axis = global_A - global_B; - real_t depth = axis.dot( c.normal ); - + real_t depth = axis.dot(c.normal); - if (depth<min_depth) { + if (depth < min_depth) { - min_depth=depth; - least_deep=i; + min_depth = depth; + least_deep = i; } } - ERR_FAIL_COND(least_deep==-1); + ERR_FAIL_COND(least_deep == -1); if (least_deep < contact_count) { //replace the last deep contact by the new one - contacts[least_deep]=contact; + contacts[least_deep] = contact; } return; } - contacts[new_index]=contact; + contacts[new_index] = contact; - if (new_index==contact_count) { + if (new_index == contact_count) { contact_count++; } - } void BodyPair2DSW::_validate_contacts() { @@ -133,39 +128,35 @@ 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; + real_t max_separation2 = max_separation * max_separation; - for (int i=0;i<contact_count;i++) { + for (int i = 0; i < contact_count; i++) { - Contact& c = contacts[i]; + Contact &c = contacts[i]; - bool erase=false; - if (c.reused==false) { + bool erase = false; + if (c.reused == false) { //was left behind in previous frame - erase=true; + erase = true; } else { - c.reused=false; + c.reused = false; 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 global_B = B->get_transform().basis_xform(c.local_B) + offset_B; Vector2 axis = global_A - global_B; - real_t depth = axis.dot( c.normal ); - - + real_t depth = axis.dot(c.normal); if (depth < -max_separation || (global_B + c.normal * depth - global_A).length_squared() > max_separation2) { - erase=true; + erase = true; } } if (erase) { // contact no longer needed, remove - - if ((i+1) < contact_count) { + if ((i + 1) < contact_count) { // swap with the last one - SWAP( contacts[i], contacts[ contact_count-1 ] ); - + SWAP(contacts[i], contacts[contact_count - 1]); } i--; @@ -174,21 +165,18 @@ void BodyPair2DSW::_validate_contacts() { } } +bool BodyPair2DSW::_test_ccd(real_t p_step, Body2DSW *p_A, int p_shape_A, const Transform2D &p_xform_A, Body2DSW *p_B, int p_shape_B, const Transform2D &p_xform_B, bool p_swap_result) { -bool BodyPair2DSW::_test_ccd(real_t p_step,Body2DSW *p_A, int p_shape_A,const Transform2D& p_xform_A,Body2DSW *p_B, int p_shape_B,const Transform2D& p_xform_B,bool p_swap_result) { - - - - Vector2 motion = p_A->get_linear_velocity()*p_step; + Vector2 motion = p_A->get_linear_velocity() * p_step; real_t mlen = motion.length(); - if (mlen<CMP_EPSILON) + if (mlen < CMP_EPSILON) return false; Vector2 mnormal = motion / mlen; - real_t min,max; - p_A->get_shape(p_shape_A)->project_rangev(mnormal,p_xform_A,min,max); - bool fast_object = mlen > (max-min)*0.3; //going too fast in that direction + real_t min, max; + p_A->get_shape(p_shape_A)->project_rangev(mnormal, p_xform_A, min, max); + bool fast_object = mlen > (max - min) * 0.3; //going too fast in that direction if (!fast_object) { //did it move enough in this direction to even attempt raycast? let's say it should move more than 1/3 the size of the object in that axis return false; @@ -198,22 +186,21 @@ bool BodyPair2DSW::_test_ccd(real_t p_step,Body2DSW *p_A, int p_shape_A,const Tr //support is the worst case collision point, so real collision happened before int a; Vector2 s[2]; - p_A->get_shape(p_shape_A)->get_supports(p_xform_A.basis_xform(mnormal).normalized(),s,a); + p_A->get_shape(p_shape_A)->get_supports(p_xform_A.basis_xform(mnormal).normalized(), s, a); Vector2 from = p_xform_A.xform(s[0]); Vector2 to = from + motion; Transform2D from_inv = p_xform_B.affine_inverse(); - Vector2 local_from = from_inv.xform(from-mnormal*mlen*0.1); //start from a little inside the bounding box + Vector2 local_from = from_inv.xform(from - mnormal * mlen * 0.1); //start from a little inside the bounding box Vector2 local_to = from_inv.xform(to); - Vector2 rpos,rnorm; - if (!p_B->get_shape(p_shape_B)->intersect_segment(local_from,local_to,rpos,rnorm)) + Vector2 rpos, rnorm; + if (!p_B->get_shape(p_shape_B)->intersect_segment(local_from, local_to, rpos, rnorm)) return false; //ray hit something - Vector2 hitpos = p_xform_B.xform(rpos); Vector2 contact_A = to; @@ -222,20 +209,18 @@ bool BodyPair2DSW::_test_ccd(real_t p_step,Body2DSW *p_A, int p_shape_A,const Tr //create a contact if (p_swap_result) - _contact_added_callback(contact_B,contact_A); + _contact_added_callback(contact_B, contact_A); else - _contact_added_callback(contact_A,contact_B); - + _contact_added_callback(contact_A, contact_B); return true; } bool BodyPair2DSW::setup(real_t p_step) { - //cannot collide - if (!A->test_collision_mask(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self()) || (A->get_mode()<=Physics2DServer::BODY_MODE_KINEMATIC && B->get_mode()<=Physics2DServer::BODY_MODE_KINEMATIC && A->get_max_contacts_reported()==0 && B->get_max_contacts_reported()==0)) { - collided=false; + if (!A->test_collision_mask(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self()) || (A->get_mode() <= Physics2DServer::BODY_MODE_KINEMATIC && B->get_mode() <= Physics2DServer::BODY_MODE_KINEMATIC && A->get_max_contacts_reported() == 0 && B->get_max_contacts_reported() == 0)) { + collided = false; return false; } @@ -249,44 +234,43 @@ bool BodyPair2DSW::setup(real_t p_step) { Transform2D xform_A = xform_Au * A->get_shape_transform(shape_A); Transform2D xform_Bu = B->get_transform(); - xform_Bu.elements[2]-=A->get_transform().get_origin(); + xform_Bu.elements[2] -= A->get_transform().get_origin(); Transform2D xform_B = xform_Bu * B->get_shape_transform(shape_B); - Shape2DSW *shape_A_ptr=A->get_shape(shape_A); - Shape2DSW *shape_B_ptr=B->get_shape(shape_B); + Shape2DSW *shape_A_ptr = A->get_shape(shape_A); + Shape2DSW *shape_B_ptr = B->get_shape(shape_B); - Vector2 motion_A,motion_B; + Vector2 motion_A, motion_B; - if (A->get_continuous_collision_detection_mode()==Physics2DServer::CCD_MODE_CAST_SHAPE) { - motion_A=A->get_motion(); + if (A->get_continuous_collision_detection_mode() == Physics2DServer::CCD_MODE_CAST_SHAPE) { + motion_A = A->get_motion(); } - if (B->get_continuous_collision_detection_mode()==Physics2DServer::CCD_MODE_CAST_SHAPE) { - motion_B=B->get_motion(); + if (B->get_continuous_collision_detection_mode() == Physics2DServer::CCD_MODE_CAST_SHAPE) { + motion_B = B->get_motion(); } //faster to set than to check.. //bool prev_collided=collided; - collided = CollisionSolver2DSW::solve(shape_A_ptr,xform_A,motion_A,shape_B_ptr,xform_B,motion_B,_add_contact,this,&sep_axis); + collided = CollisionSolver2DSW::solve(shape_A_ptr, xform_A, motion_A, shape_B_ptr, xform_B, motion_B, _add_contact, this, &sep_axis); if (!collided) { //test ccd (currently just a raycast) - if (A->get_continuous_collision_detection_mode()==Physics2DServer::CCD_MODE_CAST_RAY && A->get_mode()>Physics2DServer::BODY_MODE_KINEMATIC) { - if (_test_ccd(p_step,A,shape_A,xform_A,B,shape_B,xform_B)) - collided=true; + if (A->get_continuous_collision_detection_mode() == Physics2DServer::CCD_MODE_CAST_RAY && A->get_mode() > Physics2DServer::BODY_MODE_KINEMATIC) { + if (_test_ccd(p_step, A, shape_A, xform_A, B, shape_B, xform_B)) + collided = true; } - if (B->get_continuous_collision_detection_mode()==Physics2DServer::CCD_MODE_CAST_RAY && B->get_mode()>Physics2DServer::BODY_MODE_KINEMATIC) { - if (_test_ccd(p_step,B,shape_B,xform_B,A,shape_A,xform_A,true)) - collided=true; + if (B->get_continuous_collision_detection_mode() == Physics2DServer::CCD_MODE_CAST_RAY && B->get_mode() > Physics2DServer::BODY_MODE_KINEMATIC) { + if (_test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A, true)) + collided = true; } if (!collided) { - oneway_disabled=false; + oneway_disabled = false; return false; } - } if (oneway_disabled) @@ -297,45 +281,45 @@ bool BodyPair2DSW::setup(real_t p_step) { if (A->is_using_one_way_collision()) { Vector2 direction = A->get_one_way_collision_direction(); - bool valid=false; - if (B->get_linear_velocity().dot(direction)>=0){ - for(int i=0;i<contact_count;i++) { - Contact& c = contacts[i]; + bool valid = false; + if (B->get_linear_velocity().dot(direction) >= 0) { + for (int i = 0; i < contact_count; i++) { + Contact &c = contacts[i]; if (!c.reused) continue; - if (c.normal.dot(direction)<0) + if (c.normal.dot(direction) < 0) continue; - valid=true; + valid = true; break; } } if (!valid) { - collided=false; - oneway_disabled=true; + collided = false; + oneway_disabled = true; return false; } } if (B->is_using_one_way_collision()) { Vector2 direction = B->get_one_way_collision_direction(); - bool valid=false; - if (A->get_linear_velocity().dot(direction)>=0){ - for(int i=0;i<contact_count;i++) { - Contact& c = contacts[i]; + bool valid = false; + if (A->get_linear_velocity().dot(direction) >= 0) { + for (int i = 0; i < contact_count; i++) { + Contact &c = contacts[i]; if (!c.reused) continue; - if (c.normal.dot(direction)<0) + if (c.normal.dot(direction) < 0) continue; - valid=true; + valid = true; break; } } if (!valid) { - collided=false; - oneway_disabled=true; + collided = false; + oneway_disabled = true; return false; } } @@ -346,72 +330,69 @@ bool BodyPair2DSW::setup(real_t p_step) { real_t bias = 0.3; if (shape_A_ptr->get_custom_bias() || shape_B_ptr->get_custom_bias()) { - if (shape_A_ptr->get_custom_bias()==0) - bias=shape_B_ptr->get_custom_bias(); - else if (shape_B_ptr->get_custom_bias()==0) - bias=shape_A_ptr->get_custom_bias(); + 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; + bias = (shape_B_ptr->get_custom_bias() + shape_A_ptr->get_custom_bias()) * 0.5; } + cc = 0; - cc=0; - - - real_t inv_dt = 1.0/p_step; + real_t inv_dt = 1.0 / p_step; - bool do_process=false; + bool do_process = false; for (int i = 0; i < contact_count; i++) { - Contact& c = contacts[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.reused) { - c.active=false; + if (depth <= 0 || !c.reused) { + c.active = false; continue; } - c.active=true; + c.active = true; #ifdef DEBUG_ENABLED if (space->is_debugging_contacts()) { - space->add_debug_contact(global_A+offset_A); - space->add_debug_contact(global_B+offset_A); + space->add_debug_contact(global_A + offset_A); + space->add_debug_contact(global_B + offset_A); } #endif int gather_A = A->can_report_contacts(); int gather_B = B->can_report_contacts(); c.rA = global_A; - c.rB = global_B-offset_B; + 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; + 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()); + 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()); + 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()); } } - if (A->is_shape_set_as_trigger(shape_A) || B->is_shape_set_as_trigger(shape_B) || (A->get_mode()<=Physics2DServer::BODY_MODE_KINEMATIC && B->get_mode()<=Physics2DServer::BODY_MODE_KINEMATIC)) { - c.active=false; - collided=false; + if (A->is_shape_set_as_trigger(shape_A) || B->is_shape_set_as_trigger(shape_B) || (A->get_mode() <= Physics2DServer::BODY_MODE_KINEMATIC && B->get_mode() <= Physics2DServer::BODY_MODE_KINEMATIC)) { + c.active = false; + collided = false; continue; - } // Precompute normal mass, tangent mass, and bias. @@ -426,39 +407,33 @@ bool BodyPair2DSW::setup(real_t p_step) { 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.mass_tangent = 1.0f / kTangent; c.bias = -bias * inv_dt * MIN(0.0f, -depth + max_penetration); - c.depth=depth; - //c.acc_bias_impulse=0; - + 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); + A->apply_impulse(c.rA, -P); B->apply_impulse(c.rB, P); } #endif - - c.bounce=MAX(A->get_bounce(),B->get_bounce()); + c.bounce = MAX(A->get_bounce(), B->get_bounce()); if (c.bounce) { - Vector2 crA( -A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x ); - Vector2 crB( -B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x ); + Vector2 crA(-A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x); + Vector2 crB(-B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x); Vector2 dv = B->get_linear_velocity() + crB - A->get_linear_velocity() - crA; c.bounce = c.bounce * dv.dot(c.normal); } - do_process=true; - + do_process = true; } return do_process; @@ -471,81 +446,71 @@ void BodyPair2DSW::solve(real_t p_step) { for (int i = 0; i < contact_count; ++i) { - Contact& c = contacts[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 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 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 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); + A->apply_bias_impulse(c.rA, -jb); B->apply_bias_impulse(c.rB, jb); - real_t jn = -(c.bounce + vn)*c.mass_normal; + real_t jn = -(c.bounce + vn) * c.mass_normal; real_t jnOld = c.acc_normal_impulse; c.acc_normal_impulse = MAX(jnOld + jn, 0.0f); - real_t friction = A->get_friction() * B->get_friction(); - real_t jtMax = friction*c.acc_normal_impulse; - real_t jt = -vt*c.mass_tangent; + 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 ); + Vector2 j = c.normal * (c.acc_normal_impulse - jnOld) + tangent * (c.acc_tangent_impulse - jtOld); - A->apply_impulse(c.rA,-j); + 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; - oneway_disabled=false; - +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; + oneway_disabled = 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 index 7a4771782a..023c182c31 100644 --- a/servers/physics_2d/body_pair_2d_sw.h +++ b/servers/physics_2d/body_pair_2d_sw.h @@ -35,7 +35,7 @@ class BodyPair2DSW : public Constraint2DSW { enum { - MAX_CONTACTS=2 + MAX_CONTACTS = 2 }; union { struct { @@ -56,18 +56,17 @@ class BodyPair2DSW : public Constraint2DSW { 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 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 rA, rB; bool reused; real_t bounce; - }; Vector2 offset_B; //use local A coordinates to avoid numerical issues on collision detection @@ -79,20 +78,17 @@ class BodyPair2DSW : public Constraint2DSW { bool oneway_disabled; int cc; - - bool _test_ccd(real_t p_step,Body2DSW *p_A, int p_shape_A,const Transform2D& p_xform_A,Body2DSW *p_B, int p_shape_B,const Transform2D& p_xform_B,bool p_swap_result=false); + bool _test_ccd(real_t p_step, Body2DSW *p_A, int p_shape_A, const Transform2D &p_xform_A, Body2DSW *p_B, int p_shape_B, const Transform2D &p_xform_B, bool p_swap_result = false); void _validate_contacts(); - static void _add_contact(const Vector2& p_point_A,const Vector2& p_point_B,void *p_self); - _FORCE_INLINE_ void _contact_added_callback(const Vector2& p_point_A,const Vector2& p_point_B); + 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(real_t p_step); void solve(real_t p_step); - BodyPair2DSW(Body2DSW *p_A, int p_shape_A,Body2DSW *p_B, int p_shape_B); + 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 index e1f6f4f92b..c282c0364e 100644 --- a/servers/physics_2d/broad_phase_2d_basic.cpp +++ b/servers/physics_2d/broad_phase_2d_basic.cpp @@ -30,94 +30,87 @@ BroadPhase2DBasic::ID BroadPhase2DBasic::create(CollisionObject2DSW *p_object_, int p_subindex) { - current++; Element e; - e.owner=p_object_; - e._static=false; - e.subindex=p_subindex; + e.owner = p_object_; + e._static = false; + e.subindex = p_subindex; - element_map[current]=e; + element_map[current] = e; return current; } -void BroadPhase2DBasic::move(ID p_id, const Rect2& p_aabb) { +void BroadPhase2DBasic::move(ID p_id, const Rect2 &p_aabb) { - Map<ID,Element>::Element *E=element_map.find(p_id); + Map<ID, Element>::Element *E = element_map.find(p_id); ERR_FAIL_COND(!E); - E->get().aabb=p_aabb; - + 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); + Map<ID, Element>::Element *E = element_map.find(p_id); ERR_FAIL_COND(!E); - E->get()._static=p_static; - + E->get()._static = p_static; } void BroadPhase2DBasic::remove(ID p_id) { - Map<ID,Element>::Element *E=element_map.find(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); + 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); + 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); + 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 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; + int rc = 0; - for (Map<ID,Element>::Element *E=element_map.front();E;E=E->next()) { + 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)) { + 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; + p_results[rc] = E->get().owner; + p_result_indices[rc] = E->get().subindex; rc++; - if (rc>=p_max_results) + 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 BroadPhase2DBasic::cull_aabb(const Rect2 &p_aabb, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices) { - int rc=0; + int rc = 0; - for (Map<ID,Element>::Element *E=element_map.front();E;E=E->next()) { + for (Map<ID, Element>::Element *E = element_map.front(); E; E = E->next()) { - const Rect2 aabb=E->get().aabb; + const Rect2 aabb = E->get().aabb; if (aabb.intersects(p_aabb)) { - p_results[rc]=E->get().owner; - p_result_indices[rc]=E->get().subindex; + p_results[rc] = E->get().owner; + p_result_indices[rc] = E->get().subindex; rc++; - if (rc>=p_max_results) + if (rc >= p_max_results) break; } } @@ -125,68 +118,63 @@ int BroadPhase2DBasic::cull_aabb(const Rect2& p_aabb,CollisionObject2DSW** p_res 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_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::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 *I = element_map.front(); I; I = I->next()) { - for(Map<ID,Element>::Element *J=I->next();J;J=J->next()) { + for (Map<ID, Element>::Element *J = I->next(); J; J = J->next()) { - Element *elem_A=&I->get(); - Element *elem_B=&J->get(); + 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); - bool pair_ok=elem_A->aabb.intersects( elem_B->aabb ) && (!elem_A->_static || !elem_B->_static ); + PairKey key(I->key(), J->key()); - PairKey key(I->key(),J->key()); - - Map<PairKey,void*>::Element *E=pair_map.find(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); + 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; + 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); + 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 ); + return memnew(BroadPhase2DBasic); } BroadPhase2DBasic::BroadPhase2DBasic() { - current=1; - unpair_callback=NULL; - unpair_userdata=NULL; - pair_callback=NULL; - pair_userdata=NULL; - + 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 index 82e91118ce..7c5d799f9a 100644 --- a/servers/physics_2d/broad_phase_2d_basic.h +++ b/servers/physics_2d/broad_phase_2d_basic.h @@ -29,8 +29,8 @@ #ifndef BROAD_PHASE_2D_BASIC_H #define BROAD_PHASE_2D_BASIC_H -#include "space_2d_sw.h" #include "map.h" +#include "space_2d_sw.h" class BroadPhase2DBasic : public BroadPhase2DSW { struct Element { @@ -41,8 +41,7 @@ class BroadPhase2DBasic : public BroadPhase2DSW { int subindex; }; - - Map<ID,Element> element_map; + Map<ID, Element> element_map; ID current; @@ -56,17 +55,23 @@ class BroadPhase2DBasic : public BroadPhase2DSW { uint64_t key; }; - _FORCE_INLINE_ bool operator<(const PairKey& p_key) const { + _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; }} - + 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; - + Map<PairKey, void *> pair_map; PairCallback pair_callback; void *pair_userdata; @@ -74,10 +79,9 @@ class BroadPhase2DBasic : public BroadPhase2DSW { 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 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); @@ -85,11 +89,11 @@ public: 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 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 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(); diff --git a/servers/physics_2d/broad_phase_2d_hash_grid.cpp b/servers/physics_2d/broad_phase_2d_hash_grid.cpp index 06eead64cd..74c01e1220 100644 --- a/servers/physics_2d/broad_phase_2d_hash_grid.cpp +++ b/servers/physics_2d/broad_phase_2d_hash_grid.cpp @@ -31,110 +31,100 @@ #define LARGE_ELEMENT_FI 1.01239812 -void BroadPhase2DHashGrid::_pair_attempt(Element *p_elem, Element* p_with) { +void BroadPhase2DHashGrid::_pair_attempt(Element *p_elem, Element *p_with) { - Map<Element*,PairData*>::Element *E=p_elem->paired.find(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; + 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) { +void BroadPhase2DHashGrid::_unpair_attempt(Element *p_elem, Element *p_with) { - Map<Element*,PairData*>::Element *E=p_elem->paired.find(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()->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); + 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()) { + for (Map<Element *, PairData *>::Element *E = p_elem->paired.front(); E; E = E->next()) { - bool pairing = p_elem->aabb.intersects( E->key()->aabb ); + bool pairing = p_elem->aabb.intersects(E->key()->aabb); - if (pairing!=E->get()->colliding) { + 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); + 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); + unpair_callback(p_elem->owner, p_elem->subindex, E->key()->owner, E->key()->subindex, E->get()->ud, unpair_userdata); } - } - E->get()->colliding=pairing; + E->get()->colliding = pairing; } } } -void BroadPhase2DHashGrid::_enter_grid( Element* p_elem, const Rect2& p_rect,bool p_static) { - - +void BroadPhase2DHashGrid::_enter_grid(Element *p_elem, const Rect2 &p_rect, bool p_static) { - Vector2 sz = (p_rect.size/cell_size*LARGE_ELEMENT_FI); //use magic number to avoid floating point issues - if (sz.width*sz.height > large_object_min_surface) { + Vector2 sz = (p_rect.size / cell_size * LARGE_ELEMENT_FI); //use magic number to avoid floating point issues + if (sz.width * sz.height > large_object_min_surface) { //large object, do not use grid, must check against all elements - for (Map<ID,Element>::Element *E=element_map.front();E;E=E->next()) { - if (E->key()==p_elem->self) + for (Map<ID, Element>::Element *E = element_map.front(); E; E = E->next()) { + if (E->key() == p_elem->self) continue; // do not pair against itself if (E->get().owner == p_elem->owner) continue; if (E->get()._static && p_static) continue; - _pair_attempt(p_elem,&E->get()); + _pair_attempt(p_elem, &E->get()); } - large_elements[p_elem].inc(); return; } - 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++) { + 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++) { + for (int j = from.y; j <= to.y; j++) { PosKey pk; - pk.x=i; - pk.y=j; + pk.x = i; + pk.y = j; uint32_t idx = pk.hash() % hash_table_size; PosBin *pb = hash_table[idx]; @@ -145,102 +135,94 @@ void BroadPhase2DHashGrid::_enter_grid( Element* p_elem, const Rect2& p_rect,boo break; } - pb=pb->next; + pb = pb->next; } - - bool entered=false; + bool entered = false; if (!pb) { //does not exist, create! - pb = memnew( PosBin ); - pb->key=pk; - pb->next=hash_table[idx]; - hash_table[idx]=pb; + 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; + if (pb->static_object_set[p_elem].inc() == 1) { + entered = true; } } else { - if (pb->object_set[p_elem].inc()==1) { + if (pb->object_set[p_elem].inc() == 1) { - entered=true; + entered = true; } } if (entered) { - for(Map<Element*,RC>::Element *E=pb->object_set.front();E;E=E->next()) { + for (Map<Element *, RC>::Element *E = pb->object_set.front(); E; E = E->next()) { - if (E->key()->owner==p_elem->owner) + if (E->key()->owner == p_elem->owner) continue; - _pair_attempt(p_elem,E->key()); + _pair_attempt(p_elem, E->key()); } if (!p_static) { - for(Map<Element*,RC>::Element *E=pb->static_object_set.front();E;E=E->next()) { + for (Map<Element *, RC>::Element *E = pb->static_object_set.front(); E; E = E->next()) { - if (E->key()->owner==p_elem->owner) + if (E->key()->owner == p_elem->owner) continue; - _pair_attempt(p_elem,E->key()); + _pair_attempt(p_elem, E->key()); } } } - } - } //pair separatedly with large elements - for (Map<Element*,RC>::Element *E=large_elements.front();E;E=E->next()) { + for (Map<Element *, RC>::Element *E = large_elements.front(); E; E = E->next()) { - if (E->key()==p_elem) + if (E->key() == p_elem) continue; // do not pair against itself if (E->key()->owner == p_elem->owner) continue; if (E->key()->_static && p_static) continue; - _pair_attempt(E->key(),p_elem); + _pair_attempt(E->key(), p_elem); } - } +void BroadPhase2DHashGrid::_exit_grid(Element *p_elem, const Rect2 &p_rect, bool p_static) { -void BroadPhase2DHashGrid::_exit_grid( Element* p_elem, const Rect2& p_rect,bool p_static) { - - Vector2 sz = (p_rect.size/cell_size*LARGE_ELEMENT_FI); - if (sz.width*sz.height > large_object_min_surface) { + Vector2 sz = (p_rect.size / cell_size * LARGE_ELEMENT_FI); + if (sz.width * sz.height > large_object_min_surface) { //unpair all elements, instead of checking all, just check what is already paired, so we at least save from checking static vs static - for (Map<Element*,PairData*>::Element *E=p_elem->paired.front();E;E=E->next()) { + for (Map<Element *, PairData *>::Element *E = p_elem->paired.front(); E; E = E->next()) { - _unpair_attempt(p_elem,E->key()); + _unpair_attempt(p_elem, E->key()); } - if (large_elements[p_elem].dec()==0) { + if (large_elements[p_elem].dec() == 0) { large_elements.erase(p_elem); } return; } + Point2i from = (p_rect.pos / cell_size).floor(); + Point2i to = ((p_rect.pos + p_rect.size) / cell_size).floor(); - 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 i = from.x; i <= to.x; i++) { - for(int j=from.y;j<=to.y;j++) { + for (int j = from.y; j <= to.y; j++) { PosKey pk; - pk.x=i; - pk.y=j; + pk.x = i; + pk.y = j; uint32_t idx = pk.hash() % hash_table_size; PosBin *pb = hash_table[idx]; @@ -251,82 +233,75 @@ void BroadPhase2DHashGrid::_exit_grid( Element* p_elem, const Rect2& p_rect,bool break; } - pb=pb->next; + pb = pb->next; } ERR_CONTINUE(!pb); //should exist!! - bool exited=false; - + bool exited = false; if (p_static) { - if (pb->static_object_set[p_elem].dec()==0) { + if (pb->static_object_set[p_elem].dec() == 0) { pb->static_object_set.erase(p_elem); - exited=true; - + exited = true; } } else { - if (pb->object_set[p_elem].dec()==0) { + if (pb->object_set[p_elem].dec() == 0) { pb->object_set.erase(p_elem); - exited=true; - + exited = true; } } if (exited) { - for(Map<Element*,RC>::Element *E=pb->object_set.front();E;E=E->next()) { + for (Map<Element *, RC>::Element *E = pb->object_set.front(); E; E = E->next()) { - if (E->key()->owner==p_elem->owner) + if (E->key()->owner == p_elem->owner) continue; - _unpair_attempt(p_elem,E->key()); - + _unpair_attempt(p_elem, E->key()); } if (!p_static) { - for(Map<Element*,RC>::Element *E=pb->static_object_set.front();E;E=E->next()) { + for (Map<Element *, RC>::Element *E = pb->static_object_set.front(); E; E = E->next()) { - if (E->key()->owner==p_elem->owner) + if (E->key()->owner == p_elem->owner) continue; - _unpair_attempt(p_elem,E->key()); + _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; + 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; + if (px->next == pb) { + px->next = pb->next; break; } - px=px->next; + px = px->next; } ERR_CONTINUE(!px); } memdelete(pb); - } } - } - - for (Map<Element*,RC>::Element *E=large_elements.front();E;E=E->next()) { - if (E->key()==p_elem) + for (Map<Element *, RC>::Element *E = large_elements.front(); E; E = E->next()) { + if (E->key() == p_elem) continue; // do not pair against itself if (E->key()->owner == p_elem->owner) continue; @@ -334,121 +309,109 @@ void BroadPhase2DHashGrid::_exit_grid( Element* p_elem, const Rect2& p_rect,bool continue; //unpair from large elements - _unpair_attempt(p_elem,E->key()); + _unpair_attempt(p_elem, E->key()); } - - } - 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; + e.owner = p_object; + e._static = false; + e.subindex = p_subindex; + e.self = current; + e.pass = 0; - element_map[current]=e; + element_map[current] = e; return current; - } -void BroadPhase2DHashGrid::move(ID p_id, const Rect2& p_aabb) { - +void BroadPhase2DHashGrid::move(ID p_id, const Rect2 &p_aabb) { - Map<ID,Element>::Element *E=element_map.find(p_id); + Map<ID, Element>::Element *E = element_map.find(p_id); ERR_FAIL_COND(!E); - Element &e=E->get(); + Element &e = E->get(); - if (p_aabb==e.aabb) + if (p_aabb == e.aabb) return; + if (p_aabb != Rect2()) { - if (p_aabb!=Rect2()) { - - _enter_grid(&e,p_aabb,e._static); + _enter_grid(&e, p_aabb, e._static); } - if (e.aabb!=Rect2()) { + if (e.aabb != Rect2()) { - _exit_grid(&e,e.aabb,e._static); + _exit_grid(&e, e.aabb, e._static); } - e.aabb=p_aabb; + e.aabb = p_aabb; _check_motion(&e); - e.aabb=p_aabb; - + e.aabb = p_aabb; } void BroadPhase2DHashGrid::set_static(ID p_id, bool p_static) { - Map<ID,Element>::Element *E=element_map.find(p_id); + Map<ID, Element>::Element *E = element_map.find(p_id); ERR_FAIL_COND(!E); - Element &e=E->get(); + Element &e = E->get(); - if (e._static==p_static) + if (e._static == p_static) return; - if (e.aabb!=Rect2()) - _exit_grid(&e,e.aabb,e._static); + if (e.aabb != Rect2()) + _exit_grid(&e, e.aabb, e._static); - e._static=p_static; + e._static = p_static; - if (e.aabb!=Rect2()) { - _enter_grid(&e,e.aabb,e._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); + Map<ID, Element>::Element *E = element_map.find(p_id); ERR_FAIL_COND(!E); - Element &e=E->get(); + Element &e = E->get(); - if (e.aabb!=Rect2()) - _exit_grid(&e,e.aabb,e._static); + 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); + 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); + 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); + const Map<ID, Element>::Element *E = element_map.find(p_id); + ERR_FAIL_COND_V(!E, -1); return E->get().subindex; } -template<bool use_aabb,bool use_segment> -void BroadPhase2DHashGrid::_cull(const Point2i p_cell,const Rect2& p_aabb,const Point2& p_from, const Point2& p_to,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices,int &index) { - +template <bool use_aabb, bool use_segment> +void BroadPhase2DHashGrid::_cull(const Point2i p_cell, const Rect2 &p_aabb, const Point2 &p_from, const Point2 &p_to, 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; + pk.x = p_cell.x; + pk.y = p_cell.y; uint32_t idx = pk.hash() % hash_table_size; PosBin *pb = hash_table[idx]; @@ -459,190 +422,177 @@ void BroadPhase2DHashGrid::_cull(const Point2i p_cell,const Rect2& p_aabb,const break; } - pb=pb->next; + pb = pb->next; } if (!pb) return; + for (Map<Element *, RC>::Element *E = pb->object_set.front(); E; E = E->next()) { - - for(Map<Element*,RC>::Element *E=pb->object_set.front();E;E=E->next()) { - - - if (index>=p_max_results) + if (index >= p_max_results) break; - if (E->key()->pass==pass) + if (E->key()->pass == pass) continue; - E->key()->pass=pass; + E->key()->pass = pass; if (use_aabb && !p_aabb.intersects(E->key()->aabb)) continue; - if (use_segment && !E->key()->aabb.intersects_segment(p_from,p_to)) + if (use_segment && !E->key()->aabb.intersects_segment(p_from, p_to)) continue; - p_results[index]=E->key()->owner; - p_result_indices[index]=E->key()->subindex; + 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()) { - + for (Map<Element *, RC>::Element *E = pb->static_object_set.front(); E; E = E->next()) { - if (index>=p_max_results) + if (index >= p_max_results) break; - if (E->key()->pass==pass) + if (E->key()->pass == pass) continue; if (use_aabb && !p_aabb.intersects(E->key()->aabb)) { continue; } - if (use_segment && !E->key()->aabb.intersects_segment(p_from,p_to)) + if (use_segment && !E->key()->aabb.intersects_segment(p_from, p_to)) continue; - E->key()->pass=pass; - p_results[index]=E->key()->owner; - p_result_indices[index]=E->key()->subindex; + 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) { +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()) + 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; + 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; + delta.x = cell_size / delta.x; + delta.y = cell_size / delta.y; - Point2i pos = (p_from/cell_size).floor(); - Point2i end = (p_to/cell_size).floor(); + Point2i pos = (p_from / cell_size).floor(); + Point2i end = (p_to / cell_size).floor(); - Point2i step = Vector2( SGN(dir.x), SGN(dir.y) ); + Point2i step = Vector2(SGN(dir.x), SGN(dir.y)); Vector2 max; - if (dir.x<0) - max.x= (Math::floor((double)pos.x)*cell_size - p_from.x) / dir.x; + if (dir.x < 0) + max.x = (Math::floor((double)pos.x) * cell_size - p_from.x) / dir.x; else - max.x= (Math::floor((double)pos.x + 1)*cell_size - p_from.x) / dir.x; + max.x = (Math::floor((double)pos.x + 1) * cell_size - p_from.x) / dir.x; - if (dir.y<0) - max.y= (Math::floor((double)pos.y)*cell_size - p_from.y) / dir.y; + if (dir.y < 0) + max.y = (Math::floor((double)pos.y) * cell_size - p_from.y) / dir.y; else - max.y= (Math::floor((double)pos.y + 1)*cell_size - p_from.y) / dir.y; + max.y = (Math::floor((double)pos.y + 1) * cell_size - p_from.y) / dir.y; - int cullcount=0; - _cull<false,true>(pos,Rect2(),p_from,p_to,p_results,p_max_results,p_result_indices,cullcount); + int cullcount = 0; + _cull<false, true>(pos, Rect2(), p_from, p_to, p_results, p_max_results, p_result_indices, cullcount); - bool reached_x=false; - bool reached_y=false; + bool reached_x = false; + bool reached_y = false; - while(true) { + while (true) { if (max.x < max.y) { - max.x+=delta.x; - pos.x+=step.x; + max.x += delta.x; + pos.x += step.x; } else { - max.y+=delta.y; - pos.y+=step.y; - + 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) { + if (step.x > 0) { + if (pos.x >= end.x) + reached_x = true; + } else if (pos.x <= end.x) { - reached_x=true; + reached_x = true; } - if (step.y>0) { - if (pos.y>=end.y) - reached_y=true; - } else if (pos.y<=end.y) { + if (step.y > 0) { + if (pos.y >= end.y) + reached_y = true; + } else if (pos.y <= end.y) { - reached_y=true; + reached_y = true; } - _cull<false,true>(pos,Rect2(),p_from,p_to,p_results,p_max_results,p_result_indices,cullcount); + _cull<false, true>(pos, Rect2(), p_from, p_to, p_results, p_max_results, p_result_indices, cullcount); if (reached_x && reached_y) break; - } - for (Map<Element*,RC>::Element *E=large_elements.front();E;E=E->next()) { + for (Map<Element *, RC>::Element *E = large_elements.front(); E; E = E->next()) { - if (cullcount>=p_max_results) + if (cullcount >= p_max_results) break; - if (E->key()->pass==pass) + if (E->key()->pass == pass) continue; - E->key()->pass=pass; + E->key()->pass = pass; /* if (use_aabb && !p_aabb.intersects(E->key()->aabb)) continue; */ - if (!E->key()->aabb.intersects_segment(p_from,p_to)) + if (!E->key()->aabb.intersects_segment(p_from, p_to)) continue; - p_results[cullcount]=E->key()->owner; - p_result_indices[cullcount]=E->key()->subindex; + p_results[cullcount] = E->key()->owner; + p_result_indices[cullcount] = E->key()->subindex; cullcount++; - - } return cullcount; } - -int BroadPhase2DHashGrid::cull_aabb(const Rect2& p_aabb,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices) { +int BroadPhase2DHashGrid::cull_aabb(const Rect2 &p_aabb, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices) { pass++; - Point2i from = (p_aabb.pos/cell_size).floor(); - Point2i to = ((p_aabb.pos+p_aabb.size)/cell_size).floor(); - int cullcount=0; + Point2i from = (p_aabb.pos / cell_size).floor(); + Point2i to = ((p_aabb.pos + p_aabb.size) / cell_size).floor(); + int cullcount = 0; - for(int i=from.x;i<=to.x;i++) { + for (int i = from.x; i <= to.x; i++) { - for(int j=from.y;j<=to.y;j++) { + for (int j = from.y; j <= to.y; j++) { - _cull<true,false>(Point2i(i,j),p_aabb,Point2(),Point2(),p_results,p_max_results,p_result_indices,cullcount); + _cull<true, false>(Point2i(i, j), p_aabb, Point2(), Point2(), p_results, p_max_results, p_result_indices, cullcount); } - } - for (Map<Element*,RC>::Element *E=large_elements.front();E;E=E->next()) { + for (Map<Element *, RC>::Element *E = large_elements.front(); E; E = E->next()) { - if (cullcount>=p_max_results) + if (cullcount >= p_max_results) break; - if (E->key()->pass==pass) + if (E->key()->pass == pass) continue; - E->key()->pass=pass; + E->key()->pass = pass; if (!p_aabb.intersects(E->key()->aabb)) continue; @@ -652,72 +602,61 @@ int BroadPhase2DHashGrid::cull_aabb(const Rect2& p_aabb,CollisionObject2DSW** p_ continue; */ - p_results[cullcount]=E->key()->owner; - p_result_indices[cullcount]=E->key()->subindex; + p_results[cullcount] = E->key()->owner; + p_result_indices[cullcount] = E->key()->subindex; cullcount++; - - } return cullcount; } -void BroadPhase2DHashGrid::set_pair_callback(PairCallback p_pair_callback,void *p_userdata) { - - pair_callback=p_pair_callback; - pair_userdata=p_userdata; +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::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 ); + return memnew(BroadPhase2DHashGrid); } - BroadPhase2DHashGrid::BroadPhase2DHashGrid() { - hash_table_size = GLOBAL_DEF("physics/2d/bp_hash_table_size",4096); + 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); + hash_table = memnew_arr(PosBin *, hash_table_size); - cell_size = GLOBAL_DEF("physics/2d/cell_size",128); - large_object_min_surface = GLOBAL_DEF("physics/2d/large_object_surface_treshold_in_cells",512); + cell_size = GLOBAL_DEF("physics/2d/cell_size", 128); + large_object_min_surface = GLOBAL_DEF("physics/2d/large_object_surface_treshold_in_cells", 512); - for(int i=0;i<hash_table_size;i++) - hash_table[i]=NULL; - pass=1; + for (int i = 0; i < hash_table_size; i++) + hash_table[i] = NULL; + pass = 1; - current=0; + 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; + 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 ); - - + memdelete_arr(hash_table); } - - /* 3D version of voxel traversal: public IEnumerable<Point3D> GetCellsOnRay(Ray ray, int maxDepth) diff --git a/servers/physics_2d/broad_phase_2d_hash_grid.h b/servers/physics_2d/broad_phase_2d_hash_grid.h index 857053ccf0..f30f72d6db 100644 --- a/servers/physics_2d/broad_phase_2d_hash_grid.h +++ b/servers/physics_2d/broad_phase_2d_hash_grid.h @@ -34,13 +34,16 @@ class BroadPhase2DHashGrid : public BroadPhase2DSW { - struct PairData { bool colliding; int rc; void *ud; - PairData() { colliding=false; rc=1; ud=NULL; } + PairData() { + colliding = false; + rc = 1; + ud = NULL; + } }; struct Element { @@ -51,8 +54,7 @@ class BroadPhase2DHashGrid : public BroadPhase2DSW { Rect2 aabb; int subindex; uint64_t pass; - Map<Element*,PairData*> paired; - + Map<Element *, PairData *> paired; }; struct RC { @@ -69,18 +71,17 @@ class BroadPhase2DHashGrid : public BroadPhase2DSW { } _FORCE_INLINE_ RC() { - ref=0; + ref = 0; } }; - Map<ID,Element> element_map; - Map<Element*,RC> large_elements; + Map<ID, Element> element_map; + Map<Element *, RC> large_elements; ID current; uint64_t pass; - struct PairKey { union { @@ -91,17 +92,23 @@ class BroadPhase2DHashGrid : public BroadPhase2DSW { uint64_t key; }; - _FORCE_INLINE_ bool operator<(const PairKey& p_key) const { + _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; }} - + 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; + Map<PairKey, PairData> pair_map; int cell_size; int large_object_min_surface; @@ -111,11 +118,10 @@ class BroadPhase2DHashGrid : public BroadPhase2DSW { 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); - template<bool use_aabb,bool use_segment> - _FORCE_INLINE_ void _cull(const Point2i p_cell,const Rect2& p_aabb,const Point2& p_from, const Point2& p_to,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices,int &index); - + 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); + template <bool use_aabb, bool use_segment> + _FORCE_INLINE_ void _cull(const Point2i p_cell, const Rect2 &p_aabb, const Point2 &p_from, const Point2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices, int &index); struct PosKey { @@ -127,9 +133,8 @@ class BroadPhase2DHashGrid : public BroadPhase2DSW { uint64_t key; }; - _FORCE_INLINE_ uint32_t hash() const { - uint64_t k=key; + 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); @@ -139,36 +144,30 @@ class BroadPhase2DHashGrid : public BroadPhase2DSW { return k; } - bool operator==(const PosKey& p_key) const { return key==p_key.key; } - _FORCE_INLINE_ bool operator<(const PosKey& p_key) const { + 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 PosBin { PosKey key; - Map<Element*,RC> object_set; - Map<Element*,RC> static_object_set; + 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 _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 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); @@ -176,21 +175,18 @@ public: 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 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 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 index 4347155c2c..8cde46217b 100644 --- a/servers/physics_2d/broad_phase_2d_sw.cpp +++ b/servers/physics_2d/broad_phase_2d_sw.cpp @@ -28,8 +28,7 @@ /*************************************************************************/ #include "broad_phase_2d_sw.h" -BroadPhase2DSW::CreateFunction BroadPhase2DSW::create_func=NULL; +BroadPhase2DSW::CreateFunction BroadPhase2DSW::create_func = NULL; -BroadPhase2DSW::~BroadPhase2DSW() -{ +BroadPhase2DSW::~BroadPhase2DSW() { } diff --git a/servers/physics_2d/broad_phase_2d_sw.h b/servers/physics_2d/broad_phase_2d_sw.h index b9ec434ae9..a255cc6ba7 100644 --- a/servers/physics_2d/broad_phase_2d_sw.h +++ b/servers/physics_2d/broad_phase_2d_sw.h @@ -29,45 +29,42 @@ #ifndef BROAD_PHASE_2D_SW_H #define BROAD_PHASE_2D_SW_H -#include "math_funcs.h" #include "math_2d.h" +#include "math_funcs.h" class CollisionObject2DSW; - class BroadPhase2DSW { public: - typedef BroadPhase2DSW* (*CreateFunction)(); + 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); + 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 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 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 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 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 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 index 9ae0e40417..30bb9f6b52 100644 --- a/servers/physics_2d/collision_object_2d_sw.cpp +++ b/servers/physics_2d/collision_object_2d_sw.cpp @@ -29,46 +29,43 @@ #include "collision_object_2d_sw.h" #include "space_2d_sw.h" -void CollisionObject2DSW::add_shape(Shape2DSW *p_shape,const Transform2D& p_transform) { +void CollisionObject2DSW::add_shape(Shape2DSW *p_shape, const Transform2D &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; + 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){ +void CollisionObject2DSW::set_shape(int p_index, Shape2DSW *p_shape) { - ERR_FAIL_INDEX(p_index,shapes.size()); + ERR_FAIL_INDEX(p_index, shapes.size()); shapes[p_index].shape->remove_owner(this); - shapes[p_index].shape=p_shape; + shapes[p_index].shape = p_shape; p_shape->add_owner(this); _update_shapes(); _shapes_changed(); - } -void CollisionObject2DSW::set_shape_metadata(int p_index,const Variant& p_metadata) { - - ERR_FAIL_INDEX(p_index,shapes.size()); - shapes[p_index].metadata=p_metadata; +void CollisionObject2DSW::set_shape_metadata(int p_index, const Variant &p_metadata) { + ERR_FAIL_INDEX(p_index, shapes.size()); + shapes[p_index].metadata = p_metadata; } -void CollisionObject2DSW::set_shape_transform(int p_index,const Transform2D& p_transform){ +void CollisionObject2DSW::set_shape_transform(int p_index, const Transform2D &p_transform) { - ERR_FAIL_INDEX(p_index,shapes.size()); + ERR_FAIL_INDEX(p_index, shapes.size()); - shapes[p_index].xform=p_transform; - shapes[p_index].xform_inv=p_transform.affine_inverse(); + shapes[p_index].xform = p_transform; + shapes[p_index].xform_inv = p_transform.affine_inverse(); _update_shapes(); _shapes_changed(); } @@ -76,61 +73,58 @@ void CollisionObject2DSW::set_shape_transform(int p_index,const Transform2D& p_t void CollisionObject2DSW::remove_shape(Shape2DSW *p_shape) { //remove a shape, all the times it appears - for(int i=0;i<shapes.size();i++) { + for (int i = 0; i < shapes.size(); i++) { - if (shapes[i].shape==p_shape) { + if (shapes[i].shape == p_shape) { remove_shape(i); i--; } } } -void CollisionObject2DSW::remove_shape(int p_index){ +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++) { + ERR_FAIL_INDEX(p_index, shapes.size()); + for (int i = p_index; i < shapes.size(); i++) { - if (shapes[i].bpid==0) + 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[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) + if (_static == p_static) return; - _static=p_static; + _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); + 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++) { + for (int i = 0; i < shapes.size(); i++) { - Shape &s=shapes[i]; - if (s.bpid>0) { + Shape &s = shapes[i]; + if (s.bpid > 0) { space->get_broadphase()->remove(s.bpid); - s.bpid=0; + s.bpid = 0; } } - } void CollisionObject2DSW::_update_shapes() { @@ -138,53 +132,47 @@ void CollisionObject2DSW::_update_shapes() { if (!space) return; + for (int i = 0; i < shapes.size(); i++) { - 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); + 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(); + Rect2 shape_aabb = s.shape->get_aabb(); Transform2D 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 ); + 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); + space->get_broadphase()->move(s.bpid, s.aabb_cache); } - } -void CollisionObject2DSW::_update_shapes_with_motion(const Vector2& p_motion) { - +void CollisionObject2DSW::_update_shapes_with_motion(const Vector2 &p_motion) { if (!space) return; - for(int i=0;i<shapes.size();i++) { + 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); + 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(); + Rect2 shape_aabb = s.shape->get_aabb(); Transform2D xform = transform * s.xform; - shape_aabb=xform.xform(shape_aabb); - shape_aabb=shape_aabb.merge(Rect2( shape_aabb.pos+p_motion,shape_aabb.size)); //use motion - s.aabb_cache=shape_aabb; + 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); + space->get_broadphase()->move(s.bpid, shape_aabb); } - - } void CollisionObject2DSW::_set_space(Space2DSW *p_space) { @@ -193,25 +181,23 @@ void CollisionObject2DSW::_set_space(Space2DSW *p_space) { space->remove_object(this); - for(int i=0;i<shapes.size();i++) { + for (int i = 0; i < shapes.size(); i++) { - Shape &s=shapes[i]; + Shape &s = shapes[i]; if (s.bpid) { space->get_broadphase()->remove(s.bpid); - s.bpid=0; + s.bpid = 0; } } - } - space=p_space; + space = p_space; if (space) { space->add_object(this); _update_shapes(); } - } void CollisionObject2DSW::_shape_changed() { @@ -222,11 +208,11 @@ void CollisionObject2DSW::_shape_changed() { CollisionObject2DSW::CollisionObject2DSW(Type p_type) { - _static=true; - type=p_type; - space=NULL; - instance_id=0; - collision_mask=1; - layer_mask=1; - pickable=true; + _static = true; + type = p_type; + space = NULL; + instance_id = 0; + collision_mask = 1; + layer_mask = 1; + pickable = true; } diff --git a/servers/physics_2d/collision_object_2d_sw.h b/servers/physics_2d/collision_object_2d_sw.h index 0f77e9b426..e6eec05f3d 100644 --- a/servers/physics_2d/collision_object_2d_sw.h +++ b/servers/physics_2d/collision_object_2d_sw.h @@ -29,10 +29,10 @@ #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" +#include "self_list.h" +#include "servers/physics_2d_server.h" +#include "shape_2d_sw.h" class Space2DSW; @@ -42,8 +42,8 @@ public: TYPE_AREA, TYPE_BODY }; -private: +private: Type type; RID self; ObjectID instance_id; @@ -58,7 +58,7 @@ private: Shape2DSW *shape; Variant metadata; bool trigger; - Shape() { trigger=false; } + Shape() { trigger = false; } }; Vector<Shape> shapes; @@ -72,74 +72,74 @@ private: void _update_shapes(); protected: - - - void _update_shapes_with_motion(const Vector2& p_motion); + void _update_shapes_with_motion(const Vector2 &p_motion); void _unregister_shapes(); - _FORCE_INLINE_ void _set_transform(const Transform2D& p_transform, bool p_update_shapes=true) { transform=p_transform; if (p_update_shapes) {_update_shapes();} } - _FORCE_INLINE_ void _set_inv_transform(const Transform2D& p_transform) { inv_transform=p_transform; } + _FORCE_INLINE_ void _set_transform(const Transform2D &p_transform, bool p_update_shapes = true) { + transform = p_transform; + if (p_update_shapes) { + _update_shapes(); + } + } + _FORCE_INLINE_ void _set_inv_transform(const Transform2D &p_transform) { inv_transform = p_transform; } void _set_static(bool p_static); - virtual void _shapes_changed()=0; + 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; } +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_ 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 Transform2D& p_transform=Transform2D()); - void set_shape(int p_index,Shape2DSW *p_shape); - void set_shape_transform(int p_index,const Transform2D& p_transform); - void set_shape_metadata(int p_index,const Variant& p_metadata); - + void add_shape(Shape2DSW *p_shape, const Transform2D &p_transform = Transform2D()); + void set_shape(int p_index, Shape2DSW *p_shape); + void set_shape_transform(int p_index, const Transform2D &p_transform); + void set_shape_metadata(int p_index, const Variant &p_metadata); _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 Transform2D& get_shape_transform(int p_index) const { return shapes[p_index].xform; } - _FORCE_INLINE_ const Transform2D& 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_ const Variant& get_shape_metadata(int p_index) const { return shapes[p_index].metadata; } + _FORCE_INLINE_ const Transform2D &get_shape_transform(int p_index) const { return shapes[p_index].xform; } + _FORCE_INLINE_ const Transform2D &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_ const Variant &get_shape_metadata(int p_index) const { return shapes[p_index].metadata; } _FORCE_INLINE_ Transform2D get_transform() const { return transform; } _FORCE_INLINE_ Transform2D get_inv_transform() const { return inv_transform; } - _FORCE_INLINE_ Space2DSW* get_space() const { return space; } + _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_ 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 set_collision_mask(uint32_t p_mask) {collision_mask=p_mask;} + void set_collision_mask(uint32_t p_mask) { collision_mask = p_mask; } _FORCE_INLINE_ uint32_t get_collision_mask() const { return collision_mask; } - void set_layer_mask(uint32_t p_mask) {layer_mask=p_mask;} + void set_layer_mask(uint32_t p_mask) { layer_mask = p_mask; } _FORCE_INLINE_ uint32_t get_layer_mask() const { return layer_mask; } void remove_shape(Shape2DSW *p_shape); void remove_shape(int p_index); - virtual void set_space(Space2DSW *p_space)=0; + virtual void set_space(Space2DSW *p_space) = 0; - _FORCE_INLINE_ bool is_static() const { return _static; } + _FORCE_INLINE_ bool is_static() const { return _static; } - void set_pickable(bool p_pickable) { pickable=p_pickable; } + void set_pickable(bool p_pickable) { pickable = p_pickable; } _FORCE_INLINE_ bool is_pickable() const { return pickable; } - _FORCE_INLINE_ bool test_collision_mask(CollisionObject2DSW* p_other) const { + _FORCE_INLINE_ bool test_collision_mask(CollisionObject2DSW *p_other) const { - return layer_mask&p_other->collision_mask || p_other->layer_mask&collision_mask; + return layer_mask & p_other->collision_mask || p_other->layer_mask & collision_mask; } virtual ~CollisionObject2DSW() {} - }; #endif // COLLISION_OBJECT_2D_SW_H diff --git a/servers/physics_2d/collision_solver_2d_sat.cpp b/servers/physics_2d/collision_solver_2d_sat.cpp index 4c116cee3f..f72a7ef0e4 100644 --- a/servers/physics_2d/collision_solver_2d_sat.cpp +++ b/servers/physics_2d/collision_solver_2d_sat.cpp @@ -38,61 +38,57 @@ struct _CollectorCallback2D { Vector2 normal; Vector2 *sep_axis; - _FORCE_INLINE_ void call(const Vector2& p_point_A, const Vector2& p_point_B) { + _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); + callback(p_point_B, p_point_A, userdata); else - callback(p_point_A,p_point_B,userdata); + callback(p_point_A, p_point_B, userdata); } - }; -typedef void (*GenerateContactsFunc)(const Vector2 *,int, const Vector2 *,int ,_CollectorCallback2D *); - +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) { +_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 ); + 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); + 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) { +_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 ); + 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); - + 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 { bool a; int idx; real_t d; - _FORCE_INLINE_ bool operator <(const _generate_contacts_Pair& l) const { return d< l.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) { +_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 + ERR_FAIL_COND(p_point_count_A != 2); + ERR_FAIL_COND(p_point_count_B != 2); // circle is actually a 4x3 matrix #endif -# if 0 +#if 0 Vector2 rel_A=p_points_A[1]-p_points_A[0]; Vector2 rel_B=p_points_B[1]-p_points_B[0]; @@ -210,9 +206,6 @@ _FORCE_INLINE_ static void _generate_contacts_edge_edge(const Vector2 * p_points #if 1 - - - Vector2 n = p_collector->normal; Vector2 t = n.tangent(); real_t dA = n.dot(p_points_A[0]); @@ -220,90 +213,87 @@ _FORCE_INLINE_ static void _generate_contacts_edge_edge(const Vector2 * p_points _generate_contacts_Pair dvec[4]; - dvec[0].d=t.dot(p_points_A[0]); - dvec[0].a=true; - dvec[0].idx=0; - dvec[1].d=t.dot(p_points_A[1]); - dvec[1].a=true; - dvec[1].idx=1; - dvec[2].d=t.dot(p_points_B[0]); - dvec[2].a=false; - dvec[2].idx=0; - dvec[3].d=t.dot(p_points_B[1]); - dvec[3].a=false; - dvec[3].idx=1; + dvec[0].d = t.dot(p_points_A[0]); + dvec[0].a = true; + dvec[0].idx = 0; + dvec[1].d = t.dot(p_points_A[1]); + dvec[1].a = true; + dvec[1].idx = 1; + dvec[2].d = t.dot(p_points_B[0]); + dvec[2].a = false; + dvec[2].idx = 0; + dvec[3].d = t.dot(p_points_B[1]); + dvec[3].a = false; + dvec[3].idx = 1; SortArray<_generate_contacts_Pair> sa; - sa.sort(dvec,4); + sa.sort(dvec, 4); - for(int i=1;i<=2;i++) { + for (int i = 1; i <= 2; i++) { if (dvec[i].a) { Vector2 a = p_points_A[dvec[i].idx]; - Vector2 b = n.plane_project(dB,a); - if (n.dot(a) > n.dot(b)-CMP_EPSILON) + Vector2 b = n.plane_project(dB, a); + if (n.dot(a) > n.dot(b) - CMP_EPSILON) continue; - p_collector->call(a,b); + p_collector->call(a, b); } else { Vector2 b = p_points_B[dvec[i].idx]; - Vector2 a = n.plane_project(dA,b); - if (n.dot(a) > n.dot(b)-CMP_EPSILON) + Vector2 a = n.plane_project(dA, b); + if (n.dot(a) > n.dot(b) - CMP_EPSILON) continue; - p_collector->call(a,b); + 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; + 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 - real_t dvec[4]={ axis.dot(p_points_A[0]), axis.dot(p_points_A[1]), axis.dot(p_points_B[0]), axis.dot(p_points_B[1]) }; + real_t dvec[4] = { axis.dot(p_points_A[0]), axis.dot(p_points_A[1]), axis.dot(p_points_B[0]), axis.dot(p_points_B[1]) }; //todo , find max/min and then use 2 central points SortArray<real_t> sa; - sa.sort(dvec,4); + sa.sort(dvec, 4); //use the middle ones as contacts - for (int i=1;i<=2;i++) { + 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); + 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); + 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) { - +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 ); + 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]={ + static const GenerateContactsFunc generate_contacts_func_table[2][2] = { { - _generate_contacts_point_point, - _generate_contacts_point_edge, - },{ - 0, - _generate_contacts_edge_edge, + _generate_contacts_point_point, + _generate_contacts_point_edge, + }, + { + 0, + _generate_contacts_edge_edge, } }; @@ -319,28 +309,25 @@ static void _generate_contacts_from_supports(const Vector2 * p_points_A,int p_po pointcount_B = p_point_count_A; pointcount_A = p_point_count_B; - points_A=p_points_B; - points_B=p_points_A; + 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; + 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; + 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); - + contacts_func(points_A, pointcount_A, points_B, pointcount_B, p_collector); } - - -template<class ShapeA, class ShapeB,bool castA=false,bool castB=false, bool withMargin=false> +template <class ShapeA, class ShapeB, bool castA = false, bool castB = false, bool withMargin = false> class SeparatorAxisTest2D { const ShapeA *shape_A; @@ -358,16 +345,14 @@ class SeparatorAxisTest2D { _CollectorCallback2D *callback; public: - _FORCE_INLINE_ bool test_previous_axis() { - if (callback && callback->sep_axis && *callback->sep_axis!=Vector2()) { + 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; } @@ -395,45 +380,44 @@ public: return true; } - _FORCE_INLINE_ bool test_axis(const Vector2& p_axis) { + _FORCE_INLINE_ bool test_axis(const Vector2 &p_axis) { - Vector2 axis=p_axis; + Vector2 axis = p_axis; - - if ( Math::abs(axis.x)<CMP_EPSILON && - Math::abs(axis.y)<CMP_EPSILON) { + 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); + axis = Vector2(0.0, 1.0); } - real_t min_A,max_A,min_B,max_B; + real_t min_A, max_A, min_B, max_B; if (castA) - shape_A->project_range_cast(motion_A,axis,*transform_A,min_A,max_A); + shape_A->project_range_cast(motion_A, axis, *transform_A, min_A, max_A); else - shape_A->project_range(axis,*transform_A,min_A,max_A); + shape_A->project_range(axis, *transform_A, min_A, max_A); if (castB) - shape_B->project_range_cast(motion_B,axis,*transform_B,min_B,max_B); + shape_B->project_range_cast(motion_B, axis, *transform_B, min_B, max_B); else - shape_B->project_range(axis,*transform_B,min_B,max_B); + shape_B->project_range(axis, *transform_B, min_B, max_B); if (withMargin) { - min_A-=margin_A; - max_A+=margin_A; - min_B-=margin_B; - max_B+=margin_B; + min_A -= margin_A; + max_A += margin_A; + min_B -= margin_B; + max_B += margin_B; } - min_B -= ( max_A - min_A ) * 0.5; - max_B += ( max_A - min_A ) * 0.5; + min_B -= (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; + 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; + *callback->sep_axis = axis; #ifdef DEBUG_ENABLED best_axis_count++; #endif @@ -445,26 +429,25 @@ public: dmin = Math::abs(dmin); - if ( dmax < dmin ) { - if ( dmax < best_depth ) { - best_depth=dmax; - best_axis=axis; + if (dmax < dmin) { + if (dmax < best_depth) { + best_depth = dmax; + best_axis = axis; #ifdef DEBUG_ENABLED - best_axis_index=best_axis_count; + best_axis_index = best_axis_count; #endif - } } else { - if ( dmin < best_depth ) { - best_depth=dmin; - best_axis=-axis; // keep it as A axis + if (dmin < best_depth) { + best_depth = dmin; + best_axis = -axis; // keep it as A axis #ifdef DEBUG_ENABLED - best_axis_index=best_axis_count; + best_axis_index = best_axis_count; #endif } } - //print_line("test axis: "+p_axis+" depth: "+rtos(best_depth)); +//print_line("test axis: "+p_axis+" depth: "+rtos(best_depth)); #ifdef DEBUG_ENABLED best_axis_count++; #endif @@ -472,60 +455,55 @@ public: return true; } - _FORCE_INLINE_ void generate_contacts() { // nothing to do, don't generate - if (best_axis==Vector2(0.0,0.0)) + if (best_axis == Vector2(0.0, 0.0)) return; - callback->collided=true; + callback->collided = true; if (!callback->callback) return; //only collide, no callback - static const int max_supports=2; + static const int max_supports = 2; Vector2 supports_A[max_supports]; int support_count_A; if (castA) { - shape_A->get_supports_transformed_cast(motion_A,-best_axis,*transform_A,supports_A,support_count_A); + shape_A->get_supports_transformed_cast(motion_A, -best_axis, *transform_A, supports_A, support_count_A); } else { - shape_A->get_supports(transform_A->basis_xform_inv(-best_axis).normalized(),supports_A,support_count_A); - for(int i=0;i<support_count_A;i++) { + shape_A->get_supports(transform_A->basis_xform_inv(-best_axis).normalized(), supports_A, support_count_A); + for (int i = 0; i < support_count_A; i++) { supports_A[i] = transform_A->xform(supports_A[i]); } } if (withMargin) { - for(int i=0;i<support_count_A;i++) { - supports_A[i]+=-best_axis*margin_A; + for (int i = 0; i < support_count_A; i++) { + supports_A[i] += -best_axis * margin_A; } - } - - Vector2 supports_B[max_supports]; int support_count_B; if (castB) { - shape_B->get_supports_transformed_cast(motion_B,best_axis,*transform_B,supports_B,support_count_B); + shape_B->get_supports_transformed_cast(motion_B, best_axis, *transform_B, supports_B, support_count_B); } else { - shape_B->get_supports(transform_B->basis_xform_inv(best_axis).normalized(),supports_B,support_count_B); - for(int i=0;i<support_count_B;i++) { + shape_B->get_supports(transform_B->basis_xform_inv(best_axis).normalized(), supports_B, support_count_B); + for (int i = 0; i < support_count_B; i++) { supports_B[i] = transform_B->xform(supports_B[i]); } } if (withMargin) { - for(int i=0;i<support_count_B;i++) { - supports_B[i]+=best_axis*margin_B; + for (int i = 0; i < support_count_B; i++) { + supports_B[i] += best_axis * margin_B; } - } -/* + /* print_line("**************************"); @@ -549,38 +527,33 @@ public: } //*/ + callback->normal = best_axis; + _generate_contacts_from_supports(supports_A, support_count_A, supports_B, support_count_B, callback); - - - 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) + 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 Transform2D& p_transform_a, const ShapeB *p_shape_B,const Transform2D& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_A=Vector2(), const Vector2& p_motion_B=Vector2(),real_t p_margin_A=0,real_t p_margin_B=0) { + _FORCE_INLINE_ SeparatorAxisTest2D(const ShapeA *p_shape_A, const Transform2D &p_transform_a, const ShapeB *p_shape_B, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_A = Vector2(), const Vector2 &p_motion_B = Vector2(), real_t p_margin_A = 0, real_t p_margin_B = 0) { - margin_A=p_margin_A; - margin_B=p_margin_B; - best_depth=1e15; - shape_A=p_shape_A; - shape_B=p_shape_B; - transform_A=&p_transform_a; - transform_B=&p_transform_b; - motion_A=p_motion_A; - motion_B=p_motion_B; + margin_A = p_margin_A; + margin_B = p_margin_B; + best_depth = 1e15; + shape_A = p_shape_A; + shape_B = p_shape_B; + transform_A = &p_transform_a; + transform_B = &p_transform_b; + motion_A = p_motion_A; + motion_B = p_motion_B; - callback=p_collector; + callback = p_collector; #ifdef DEBUG_ENABLED - best_axis_count=0; - best_axis_index=-1; + best_axis_count = 0; + best_axis_index = -1; #endif } - }; /****** SAT TESTS *******/ @@ -588,30 +561,26 @@ public: /****** SAT TESTS *******/ /****** SAT TESTS *******/ +#define TEST_POINT(m_a, m_b) \ + ((!separator.test_axis(((m_a) - (m_b)).normalized())) || \ + (castA && !separator.test_axis(((m_a) + p_motion_a - (m_b)).normalized())) || \ + (castB && !separator.test_axis(((m_a) - ((m_b) + p_motion_b)).normalized())) || \ + (castA && castB && !separator.test_axis(((m_a) + p_motion_a - ((m_b) + p_motion_b)).normalized()))) -#define TEST_POINT(m_a,m_b) \ - ( (!separator.test_axis(((m_a)-(m_b)).normalized())) ||\ - (castA && !separator.test_axis(((m_a)+p_motion_a-(m_b)).normalized())) ||\ - (castB && !separator.test_axis(((m_a)-((m_b)+p_motion_b)).normalized())) ||\ - (castA && castB && !separator.test_axis(((m_a)+p_motion_a-((m_b)+p_motion_b)).normalized())) ) - - -typedef void (*CollisionFunc)(const Shape2DSW*,const Transform2D&,const Shape2DSW*,const Transform2D&,_CollectorCallback2D *p_collector,const Vector2&,const Vector2&,real_t,real_t); - +typedef void (*CollisionFunc)(const Shape2DSW *, const Transform2D &, const Shape2DSW *, const Transform2D &, _CollectorCallback2D *p_collector, const Vector2 &, const Vector2 &, real_t, real_t); -template<bool castA, bool castB,bool withMargin> -static void _collision_segment_segment(const Shape2DSW* p_a,const Transform2D& p_transform_a,const Shape2DSW* p_b,const Transform2D& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,real_t p_margin_A,real_t p_margin_B) { +template <bool castA, bool castB, bool withMargin> +static void _collision_segment_segment(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a); - const SegmentShape2DSW *segment_B = static_cast<const SegmentShape2DSW*>(p_b); + const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW *>(p_a); + const SegmentShape2DSW *segment_B = static_cast<const SegmentShape2DSW *>(p_b); - SeparatorAxisTest2D<SegmentShape2DSW,SegmentShape2DSW,castA,castB,withMargin> separator(segment_A,p_transform_a,segment_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B); + SeparatorAxisTest2D<SegmentShape2DSW, SegmentShape2DSW, castA, castB, withMargin> separator(segment_A, p_transform_a, segment_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); if (!separator.test_previous_axis()) return; //this collision is kind of pointless - /* if (!separator.test_previous_axis()) return; @@ -620,7 +589,6 @@ static void _collision_segment_segment(const Shape2DSW* p_a,const Transform2D& p if (!separator.test_cast()) return; - if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a))) return; if (!separator.test_axis(segment_B->get_xformed_normal(p_transform_b))) @@ -629,30 +597,26 @@ static void _collision_segment_segment(const Shape2DSW* p_a,const Transform2D& p if (withMargin) { //points grow to circles - - if (TEST_POINT( p_transform_a.xform(segment_A->get_a()),p_transform_b.xform(segment_B->get_a())) ) + if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), p_transform_b.xform(segment_B->get_a()))) return; - if (TEST_POINT( p_transform_a.xform(segment_A->get_a()),p_transform_b.xform(segment_B->get_b())) ) + if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), p_transform_b.xform(segment_B->get_b()))) return; - if (TEST_POINT( p_transform_a.xform(segment_A->get_b()),p_transform_b.xform(segment_B->get_a())) ) + if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), p_transform_b.xform(segment_B->get_a()))) return; - if (TEST_POINT( p_transform_a.xform(segment_A->get_b()),p_transform_b.xform(segment_B->get_b())) ) + if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), p_transform_b.xform(segment_B->get_b()))) return; } separator.generate_contacts(); - } -template<bool castA, bool castB,bool withMargin> -static void _collision_segment_circle(const Shape2DSW* p_a,const Transform2D& p_transform_a,const Shape2DSW* p_b,const Transform2D& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,real_t p_margin_A,real_t p_margin_B) { - - - const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a); - const CircleShape2DSW *circle_B = static_cast<const CircleShape2DSW*>(p_b); +template <bool castA, bool castB, bool withMargin> +static void _collision_segment_circle(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW *>(p_a); + const CircleShape2DSW *circle_B = static_cast<const CircleShape2DSW *>(p_b); - SeparatorAxisTest2D<SegmentShape2DSW,CircleShape2DSW,castA,castB,withMargin> separator(segment_A,p_transform_a,circle_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B); + SeparatorAxisTest2D<SegmentShape2DSW, CircleShape2DSW, castA, castB, withMargin> separator(segment_A, p_transform_a, circle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); if (!separator.test_previous_axis()) return; @@ -662,28 +626,26 @@ static void _collision_segment_circle(const Shape2DSW* p_a,const Transform2D& p_ //segment normal if (!separator.test_axis( - (p_transform_a.xform(segment_A->get_b())-p_transform_a.xform(segment_A->get_a())).normalized().tangent() - )) + (p_transform_a.xform(segment_A->get_b()) - p_transform_a.xform(segment_A->get_a())).normalized().tangent())) return; //endpoint a vs circle - if (TEST_POINT(p_transform_a.xform(segment_A->get_a()),p_transform_b.get_origin())) + if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), p_transform_b.get_origin())) return; //endpoint b vs circle - if (TEST_POINT(p_transform_a.xform(segment_A->get_b()),p_transform_b.get_origin())) + if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), p_transform_b.get_origin())) return; - separator.generate_contacts(); } -template<bool castA, bool castB,bool withMargin> -static void _collision_segment_rectangle(const Shape2DSW* p_a,const Transform2D& p_transform_a,const Shape2DSW* p_b,const Transform2D& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,real_t p_margin_A,real_t p_margin_B) { +template <bool castA, bool castB, bool withMargin> +static void _collision_segment_rectangle(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a); - const RectangleShape2DSW *rectangle_B = static_cast<const RectangleShape2DSW*>(p_b); + const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW *>(p_a); + const RectangleShape2DSW *rectangle_B = static_cast<const RectangleShape2DSW *>(p_b); - SeparatorAxisTest2D<SegmentShape2DSW,RectangleShape2DSW,castA,castB,withMargin> separator(segment_A,p_transform_a,rectangle_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B); + SeparatorAxisTest2D<SegmentShape2DSW, RectangleShape2DSW, castA, castB, withMargin> separator(segment_A, p_transform_a, rectangle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); if (!separator.test_previous_axis()) return; @@ -707,48 +669,46 @@ static void _collision_segment_rectangle(const Shape2DSW* p_a,const Transform2D& Vector2 a = p_transform_a.xform(segment_A->get_a()); Vector2 b = p_transform_a.xform(segment_A->get_b()); - if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,inv,a))) + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a))) return; - if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,inv,b))) + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, b))) return; if (castA) { - if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,inv,a+p_motion_a))) + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a + p_motion_a))) return; - if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,inv,b+p_motion_a))) + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, b + p_motion_a))) return; } if (castB) { - if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,inv,a-p_motion_b))) + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a - p_motion_b))) return; - if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,inv,b-p_motion_b))) + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, b - p_motion_b))) return; } if (castA && castB) { - if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,inv,a-p_motion_b+p_motion_a))) + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a - p_motion_b + p_motion_a))) return; - if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,inv,b-p_motion_b+p_motion_a))) + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, b - p_motion_b + p_motion_a))) return; } - } separator.generate_contacts(); - } -template<bool castA, bool castB,bool withMargin> -static void _collision_segment_capsule(const Shape2DSW* p_a,const Transform2D& p_transform_a,const Shape2DSW* p_b,const Transform2D& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,real_t p_margin_A,real_t p_margin_B) { +template <bool castA, bool castB, bool withMargin> +static void _collision_segment_capsule(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a); - const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b); + const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW *>(p_a); + const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW *>(p_b); - SeparatorAxisTest2D<SegmentShape2DSW,CapsuleShape2DSW,castA,castB,withMargin> separator(segment_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B); + SeparatorAxisTest2D<SegmentShape2DSW, CapsuleShape2DSW, castA, castB, withMargin> separator(segment_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); if (!separator.test_previous_axis()) return; @@ -762,25 +722,25 @@ static void _collision_segment_capsule(const Shape2DSW* p_a,const Transform2D& p if (!separator.test_axis(p_transform_b.elements[0].normalized())) return; - if (TEST_POINT(p_transform_a.xform(segment_A->get_a()),(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*0.5))) + if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * 0.5))) return; - if (TEST_POINT(p_transform_a.xform(segment_A->get_a()),(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*-0.5))) + if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * -0.5))) return; - if (TEST_POINT(p_transform_a.xform(segment_A->get_b()),(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*0.5))) + if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * 0.5))) return; - if (TEST_POINT(p_transform_a.xform(segment_A->get_b()),(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*-0.5))) + if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * -0.5))) return; separator.generate_contacts(); } -template<bool castA, bool castB,bool withMargin> -static void _collision_segment_convex_polygon(const Shape2DSW* p_a,const Transform2D& p_transform_a,const Shape2DSW* p_b,const Transform2D& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,real_t p_margin_A,real_t p_margin_B) { +template <bool castA, bool castB, bool withMargin> +static void _collision_segment_convex_polygon(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a); - const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b); + const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW *>(p_a); + const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW *>(p_b); - SeparatorAxisTest2D<SegmentShape2DSW,ConvexPolygonShape2DSW,castA,castB,withMargin> separator(segment_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B); + SeparatorAxisTest2D<SegmentShape2DSW, ConvexPolygonShape2DSW, castA, castB, withMargin> separator(segment_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); if (!separator.test_previous_axis()) return; @@ -791,37 +751,32 @@ static void _collision_segment_convex_polygon(const Shape2DSW* p_a,const Transfo if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a))) return; - for(int i=0;i<convex_B->get_point_count();i++) { + for (int i = 0; i < convex_B->get_point_count(); i++) { - if (!separator.test_axis( convex_B->get_xformed_segment_normal(p_transform_b,i))) + if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) return; if (withMargin) { - if (TEST_POINT(p_transform_a.xform(segment_A->get_a()),p_transform_b.xform(convex_B->get_point(i) ))) + if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), p_transform_b.xform(convex_B->get_point(i)))) return; - if (TEST_POINT(p_transform_a.xform(segment_A->get_b()),p_transform_b.xform(convex_B->get_point(i) ))) + if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), p_transform_b.xform(convex_B->get_point(i)))) return; - } - } separator.generate_contacts(); - } - ///////// -template<bool castA, bool castB,bool withMargin> -static void _collision_circle_circle(const Shape2DSW* p_a,const Transform2D& p_transform_a,const Shape2DSW* p_b,const Transform2D& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,real_t p_margin_A,real_t p_margin_B) { - - const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW*>(p_a); - const CircleShape2DSW *circle_B = static_cast<const CircleShape2DSW*>(p_b); +template <bool castA, bool castB, bool withMargin> +static void _collision_circle_circle(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW *>(p_a); + const CircleShape2DSW *circle_B = static_cast<const CircleShape2DSW *>(p_b); - SeparatorAxisTest2D<CircleShape2DSW,CircleShape2DSW,castA,castB,withMargin> separator(circle_A,p_transform_a,circle_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B); + SeparatorAxisTest2D<CircleShape2DSW, CircleShape2DSW, castA, castB, withMargin> separator(circle_A, p_transform_a, circle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); if (!separator.test_previous_axis()) return; @@ -829,22 +784,19 @@ static void _collision_circle_circle(const Shape2DSW* p_a,const Transform2D& p_t if (!separator.test_cast()) return; - if (TEST_POINT(p_transform_a.get_origin(),p_transform_b.get_origin())) + if (TEST_POINT(p_transform_a.get_origin(), p_transform_b.get_origin())) return; - separator.generate_contacts(); - } -template<bool castA, bool castB,bool withMargin> -static void _collision_circle_rectangle(const Shape2DSW* p_a,const Transform2D& p_transform_a,const Shape2DSW* p_b,const Transform2D& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,real_t p_margin_A,real_t p_margin_B) { - - const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW*>(p_a); - const RectangleShape2DSW *rectangle_B = static_cast<const RectangleShape2DSW*>(p_b); +template <bool castA, bool castB, bool withMargin> +static void _collision_circle_rectangle(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW *>(p_a); + const RectangleShape2DSW *rectangle_B = static_cast<const RectangleShape2DSW *>(p_b); - SeparatorAxisTest2D<CircleShape2DSW,RectangleShape2DSW,castA,castB,withMargin> separator(circle_A,p_transform_a,rectangle_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B); + SeparatorAxisTest2D<CircleShape2DSW, RectangleShape2DSW, castA, castB, withMargin> separator(circle_A, p_transform_a, rectangle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); if (!separator.test_previous_axis()) return; @@ -852,8 +804,8 @@ static void _collision_circle_rectangle(const Shape2DSW* p_a,const Transform2D& if (!separator.test_cast()) return; - const Vector2 &sphere=p_transform_a.elements[2]; - const Vector2 *axis=&p_transform_b.elements[0]; + 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())) @@ -865,42 +817,41 @@ static void _collision_circle_rectangle(const Shape2DSW* p_a,const Transform2D& Transform2D binv = p_transform_b.affine_inverse(); { - if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,binv,sphere ) ) ) + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphere))) return; } if (castA) { Vector2 sphereofs = sphere + p_motion_a; - if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,binv, sphereofs) ) ) + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphereofs))) return; } if (castB) { Vector2 sphereofs = sphere - p_motion_b; - if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,binv, sphereofs) ) ) + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphereofs))) return; } if (castA && castB) { Vector2 sphereofs = sphere - p_motion_b + p_motion_a; - if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,binv, sphereofs) ) ) + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphereofs))) return; } separator.generate_contacts(); } -template<bool castA, bool castB,bool withMargin> -static void _collision_circle_capsule(const Shape2DSW* p_a,const Transform2D& p_transform_a,const Shape2DSW* p_b,const Transform2D& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,real_t p_margin_A,real_t p_margin_B) { +template <bool castA, bool castB, bool withMargin> +static void _collision_circle_capsule(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW*>(p_a); - const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b); + const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW *>(p_a); + const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW *>(p_b); - - SeparatorAxisTest2D<CircleShape2DSW,CapsuleShape2DSW,castA,castB,withMargin> separator(circle_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B); + SeparatorAxisTest2D<CircleShape2DSW, CapsuleShape2DSW, castA, castB, withMargin> separator(circle_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); if (!separator.test_previous_axis()) return; @@ -913,24 +864,21 @@ static void _collision_circle_capsule(const Shape2DSW* p_a,const Transform2D& p_ return; //capsule endpoints - if (TEST_POINT(p_transform_a.get_origin(),(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*0.5))) + if (TEST_POINT(p_transform_a.get_origin(), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * 0.5))) return; - if (TEST_POINT(p_transform_a.get_origin(),(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*-0.5))) + if (TEST_POINT(p_transform_a.get_origin(), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * -0.5))) return; separator.generate_contacts(); - - } -template<bool castA, bool castB,bool withMargin> -static void _collision_circle_convex_polygon(const Shape2DSW* p_a,const Transform2D& p_transform_a,const Shape2DSW* p_b,const Transform2D& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,real_t p_margin_A,real_t p_margin_B) { +template <bool castA, bool castB, bool withMargin> +static void _collision_circle_convex_polygon(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW*>(p_a); - const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b); + const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW *>(p_a); + const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW *>(p_b); - - SeparatorAxisTest2D<CircleShape2DSW,ConvexPolygonShape2DSW,castA,castB,withMargin> separator(circle_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B); + SeparatorAxisTest2D<CircleShape2DSW, ConvexPolygonShape2DSW, castA, castB, withMargin> separator(circle_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); if (!separator.test_previous_axis()) return; @@ -938,31 +886,28 @@ static void _collision_circle_convex_polygon(const Shape2DSW* p_a,const Transfor if (!separator.test_cast()) return; - //poly faces and poly points vs circle - for(int i=0;i<convex_B->get_point_count();i++) { + for (int i = 0; i < convex_B->get_point_count(); i++) { - if (TEST_POINT( p_transform_a.get_origin(),p_transform_b.xform(convex_B->get_point(i)) )) + if (TEST_POINT(p_transform_a.get_origin(), p_transform_b.xform(convex_B->get_point(i)))) return; - if (!separator.test_axis( convex_B->get_xformed_segment_normal(p_transform_b,i))) + if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) return; } separator.generate_contacts(); } - ///////// -template<bool castA, bool castB,bool withMargin> -static void _collision_rectangle_rectangle(const Shape2DSW* p_a,const Transform2D& p_transform_a,const Shape2DSW* p_b,const Transform2D& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,real_t p_margin_A,real_t p_margin_B) { - - const RectangleShape2DSW *rectangle_A = static_cast<const RectangleShape2DSW*>(p_a); - const RectangleShape2DSW *rectangle_B = static_cast<const RectangleShape2DSW*>(p_b); +template <bool castA, bool castB, bool withMargin> +static void _collision_rectangle_rectangle(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const RectangleShape2DSW *rectangle_A = static_cast<const RectangleShape2DSW *>(p_a); + const RectangleShape2DSW *rectangle_B = static_cast<const RectangleShape2DSW *>(p_b); - SeparatorAxisTest2D<RectangleShape2DSW,RectangleShape2DSW,castA,castB,withMargin> separator(rectangle_A,p_transform_a,rectangle_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B); + SeparatorAxisTest2D<RectangleShape2DSW, RectangleShape2DSW, castA, castB, withMargin> separator(rectangle_A, p_transform_a, rectangle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); if (!separator.test_previous_axis()) return; @@ -986,38 +931,38 @@ static void _collision_rectangle_rectangle(const Shape2DSW* p_a,const Transform2 if (withMargin) { - Transform2D invA=p_transform_a.affine_inverse(); - Transform2D invB=p_transform_b.affine_inverse(); + Transform2D invA = p_transform_a.affine_inverse(); + Transform2D invB = p_transform_b.affine_inverse(); - if (!separator.test_axis( rectangle_A->get_box_axis(p_transform_a,invA,rectangle_B,p_transform_b,invB) ) ) + if (!separator.test_axis(rectangle_A->get_box_axis(p_transform_a, invA, rectangle_B, p_transform_b, invB))) return; if (castA || castB) { Transform2D aofs = p_transform_a; - aofs.elements[2]+=p_motion_a; + aofs.elements[2] += p_motion_a; Transform2D bofs = p_transform_b; - bofs.elements[2]+=p_motion_b; + bofs.elements[2] += p_motion_b; Transform2D aofsinv = aofs.affine_inverse(); Transform2D bofsinv = bofs.affine_inverse(); if (castA) { - if (!separator.test_axis( rectangle_A->get_box_axis(aofs,aofsinv,rectangle_B,p_transform_b,invB) ) ) + if (!separator.test_axis(rectangle_A->get_box_axis(aofs, aofsinv, rectangle_B, p_transform_b, invB))) return; } if (castB) { - if (!separator.test_axis( rectangle_A->get_box_axis(p_transform_a,invA,rectangle_B,bofs,bofsinv) ) ) + if (!separator.test_axis(rectangle_A->get_box_axis(p_transform_a, invA, rectangle_B, bofs, bofsinv))) return; } if (castA && castB) { - if (!separator.test_axis( rectangle_A->get_box_axis(aofs,aofsinv,rectangle_B,bofs,bofsinv) ) ) + if (!separator.test_axis(rectangle_A->get_box_axis(aofs, aofsinv, rectangle_B, bofs, bofsinv))) return; } } @@ -1026,14 +971,13 @@ static void _collision_rectangle_rectangle(const Shape2DSW* p_a,const Transform2 separator.generate_contacts(); } -template<bool castA, bool castB,bool withMargin> -static void _collision_rectangle_capsule(const Shape2DSW* p_a,const Transform2D& p_transform_a,const Shape2DSW* p_b,const Transform2D& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,real_t p_margin_A,real_t p_margin_B) { - - const RectangleShape2DSW *rectangle_A = static_cast<const RectangleShape2DSW*>(p_a); - const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b); +template <bool castA, bool castB, bool withMargin> +static void _collision_rectangle_capsule(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const RectangleShape2DSW *rectangle_A = static_cast<const RectangleShape2DSW *>(p_a); + const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW *>(p_b); - SeparatorAxisTest2D<RectangleShape2DSW,CapsuleShape2DSW,castA,castB,withMargin> separator(rectangle_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B); + SeparatorAxisTest2D<RectangleShape2DSW, CapsuleShape2DSW, castA, castB, withMargin> separator(rectangle_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); if (!separator.test_previous_axis()) return; @@ -1052,61 +996,55 @@ static void _collision_rectangle_capsule(const Shape2DSW* p_a,const Transform2D& if (!separator.test_axis(p_transform_b.elements[0].normalized())) return; - //box endpoints to capsule circles Transform2D boxinv = p_transform_a.affine_inverse(); - for(int i=0;i<2;i++) { + 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); + Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * (i == 0 ? 0.5 : -0.5); - if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,capsule_endpoint))) + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) return; } - if (castA) { - Vector2 capsule_endpoint = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(i==0?0.5:-0.5); - capsule_endpoint-=p_motion_a; + Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * (i == 0 ? 0.5 : -0.5); + capsule_endpoint -= p_motion_a; - if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,capsule_endpoint))) + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) return; } if (castB) { - Vector2 capsule_endpoint = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(i==0?0.5:-0.5); - capsule_endpoint+=p_motion_b; + Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * (i == 0 ? 0.5 : -0.5); + capsule_endpoint += p_motion_b; - if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,capsule_endpoint))) + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) return; } if (castA && castB) { - Vector2 capsule_endpoint = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(i==0?0.5:-0.5); - capsule_endpoint-=p_motion_a; - capsule_endpoint+=p_motion_b; - + Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * (i == 0 ? 0.5 : -0.5); + capsule_endpoint -= p_motion_a; + capsule_endpoint += p_motion_b; - if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,capsule_endpoint))) + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) return; } - } - separator.generate_contacts(); } -template<bool castA, bool castB,bool withMargin> -static void _collision_rectangle_convex_polygon(const Shape2DSW* p_a,const Transform2D& p_transform_a,const Shape2DSW* p_b,const Transform2D& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,real_t p_margin_A,real_t p_margin_B) { - - const RectangleShape2DSW *rectangle_A = static_cast<const RectangleShape2DSW*>(p_a); - const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b); +template <bool castA, bool castB, bool withMargin> +static void _collision_rectangle_convex_polygon(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - SeparatorAxisTest2D<RectangleShape2DSW,ConvexPolygonShape2DSW,castA,castB,withMargin> separator(rectangle_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B); + const RectangleShape2DSW *rectangle_A = static_cast<const RectangleShape2DSW *>(p_a); + const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW *>(p_b); + SeparatorAxisTest2D<RectangleShape2DSW, ConvexPolygonShape2DSW, castA, castB, withMargin> separator(rectangle_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); if (!separator.test_previous_axis()) return; @@ -1124,51 +1062,47 @@ static void _collision_rectangle_convex_polygon(const Shape2DSW* p_a,const Trans //convex faces Transform2D boxinv; if (withMargin) { - boxinv=p_transform_a.affine_inverse(); + boxinv = p_transform_a.affine_inverse(); } - for(int i=0;i<convex_B->get_point_count();i++) { + for (int i = 0; i < convex_B->get_point_count(); i++) { - if (!separator.test_axis( convex_B->get_xformed_segment_normal(p_transform_b,i))) + if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) return; if (withMargin) { //all points vs all points need to be tested if margin exist - if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,p_transform_b.xform(convex_B->get_point(i))))) + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, p_transform_b.xform(convex_B->get_point(i))))) return; if (castA) { - if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,p_transform_b.xform(convex_B->get_point(i))-p_motion_a))) + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, p_transform_b.xform(convex_B->get_point(i)) - p_motion_a))) return; } if (castB) { - if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,p_transform_b.xform(convex_B->get_point(i))+p_motion_b))) + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, p_transform_b.xform(convex_B->get_point(i)) + p_motion_b))) return; } if (castA && castB) { - if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,p_transform_b.xform(convex_B->get_point(i))+p_motion_b-p_motion_a))) + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, p_transform_b.xform(convex_B->get_point(i)) + p_motion_b - p_motion_a))) return; } - } } separator.generate_contacts(); - } - ///////// -template<bool castA, bool castB,bool withMargin> -static void _collision_capsule_capsule(const Shape2DSW* p_a,const Transform2D& p_transform_a,const Shape2DSW* p_b,const Transform2D& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,real_t p_margin_A,real_t p_margin_B) { - - const CapsuleShape2DSW *capsule_A = static_cast<const CapsuleShape2DSW*>(p_a); - const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b); +template <bool castA, bool castB, bool withMargin> +static void _collision_capsule_capsule(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const CapsuleShape2DSW *capsule_A = static_cast<const CapsuleShape2DSW *>(p_a); + const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW *>(p_b); - SeparatorAxisTest2D<CapsuleShape2DSW,CapsuleShape2DSW,castA,castB,withMargin> separator(capsule_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B); + SeparatorAxisTest2D<CapsuleShape2DSW, CapsuleShape2DSW, castA, castB, withMargin> separator(capsule_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); if (!separator.test_previous_axis()) return; @@ -1186,32 +1120,29 @@ static void _collision_capsule_capsule(const Shape2DSW* p_a,const Transform2D& p //capsule endpoints - for(int i=0;i<2;i++) { + 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); + 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++) { + 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); + 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 (TEST_POINT(capsule_endpoint_A,capsule_endpoint_B) ) + if (TEST_POINT(capsule_endpoint_A, capsule_endpoint_B)) return; - } } separator.generate_contacts(); - } -template<bool castA, bool castB,bool withMargin> -static void _collision_capsule_convex_polygon(const Shape2DSW* p_a,const Transform2D& p_transform_a,const Shape2DSW* p_b,const Transform2D& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,real_t p_margin_A,real_t p_margin_B) { - - const CapsuleShape2DSW *capsule_A = static_cast<const CapsuleShape2DSW*>(p_a); - const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b); +template <bool castA, bool castB, bool withMargin> +static void _collision_capsule_convex_polygon(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const CapsuleShape2DSW *capsule_A = static_cast<const CapsuleShape2DSW *>(p_a); + const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW *>(p_b); - SeparatorAxisTest2D<CapsuleShape2DSW,ConvexPolygonShape2DSW,castA,castB,withMargin> separator(capsule_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B); + SeparatorAxisTest2D<CapsuleShape2DSW, ConvexPolygonShape2DSW, castA, castB, withMargin> separator(capsule_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); if (!separator.test_previous_axis()) return; @@ -1224,40 +1155,35 @@ static void _collision_capsule_convex_polygon(const Shape2DSW* p_a,const Transfo 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++) { + 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++) { + 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); + 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 (TEST_POINT(capsule_endpoint_A,cpoint )) + if (TEST_POINT(capsule_endpoint_A, cpoint)) return; - } - if (!separator.test_axis( convex_B->get_xformed_segment_normal(p_transform_b,i))) + if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) return; } separator.generate_contacts(); } - ///////// +template <bool castA, bool castB, bool withMargin> +static void _collision_convex_polygon_convex_polygon(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { -template<bool castA, bool castB,bool withMargin> -static void _collision_convex_polygon_convex_polygon(const Shape2DSW* p_a,const Transform2D& p_transform_a,const Shape2DSW* p_b,const Transform2D& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,real_t p_margin_A,real_t p_margin_B) { - + const ConvexPolygonShape2DSW *convex_A = static_cast<const ConvexPolygonShape2DSW *>(p_a); + const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW *>(p_b); - const ConvexPolygonShape2DSW *convex_A = static_cast<const ConvexPolygonShape2DSW*>(p_a); - const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b); - - SeparatorAxisTest2D<ConvexPolygonShape2DSW,ConvexPolygonShape2DSW,castA,castB,withMargin> separator(convex_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B); + SeparatorAxisTest2D<ConvexPolygonShape2DSW, ConvexPolygonShape2DSW, castA, castB, withMargin> separator(convex_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); if (!separator.test_previous_axis()) return; @@ -1265,345 +1191,332 @@ static void _collision_convex_polygon_convex_polygon(const Shape2DSW* p_a,const if (!separator.test_cast()) return; + for (int i = 0; i < convex_A->get_point_count(); i++) { - for(int i=0;i<convex_A->get_point_count();i++) { - - if (!separator.test_axis( convex_A->get_xformed_segment_normal(p_transform_a,i))) + if (!separator.test_axis(convex_A->get_xformed_segment_normal(p_transform_a, i))) return; } - for(int i=0;i<convex_B->get_point_count();i++) { + for (int i = 0; i < convex_B->get_point_count(); i++) { - if (!separator.test_axis( convex_B->get_xformed_segment_normal(p_transform_b,i))) + if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) return; - } if (withMargin) { - for(int i=0;i<convex_A->get_point_count();i++) { - for(int j=0;j<convex_B->get_point_count();j++) { + for (int i = 0; i < convex_A->get_point_count(); i++) { + for (int j = 0; j < convex_B->get_point_count(); j++) { - if (TEST_POINT(p_transform_a.xform(convex_A->get_point(i)) , p_transform_b.xform(convex_B->get_point(j)))) + if (TEST_POINT(p_transform_a.xform(convex_A->get_point(i)), p_transform_b.xform(convex_B->get_point(j)))) return; } } - } separator.generate_contacts(); - } - //////// -bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Transform2D& p_transform_A, const Vector2& p_motion_A, const Shape2DSW *p_shape_B, const Transform2D& p_transform_B,const Vector2& p_motion_B, CollisionSolver2DSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap,Vector2 *sep_axis,real_t p_margin_A,real_t p_margin_B) { +bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CollisionSolver2DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap, Vector2 *sep_axis, real_t p_margin_A, real_t p_margin_B) { - Physics2DServer::ShapeType type_A=p_shape_A->get_type(); + 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_LINE, false); //ERR_FAIL_COND_V(type_A==Physics2DServer::SHAPE_RAY,false); - ERR_FAIL_COND_V(p_shape_A->is_concave(),false); + ERR_FAIL_COND_V(p_shape_A->is_concave(), false); - Physics2DServer::ShapeType type_B=p_shape_B->get_type(); + 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_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<false,false,false>, - _collision_segment_circle<false,false,false>, - _collision_segment_rectangle<false,false,false>, - _collision_segment_capsule<false,false,false>, - _collision_segment_convex_polygon<false,false,false>}, - {0, - _collision_circle_circle<false,false,false>, - _collision_circle_rectangle<false,false,false>, - _collision_circle_capsule<false,false,false>, - _collision_circle_convex_polygon<false,false,false>}, - {0, - 0, - _collision_rectangle_rectangle<false,false,false>, - _collision_rectangle_capsule<false,false,false>, - _collision_rectangle_convex_polygon<false,false,false>}, - {0, - 0, - 0, - _collision_capsule_capsule<false,false,false>, - _collision_capsule_convex_polygon<false,false,false>}, - {0, - 0, - 0, - 0, - _collision_convex_polygon_convex_polygon<false,false,false>} + ERR_FAIL_COND_V(p_shape_B->is_concave(), false); + + static const CollisionFunc collision_table[5][5] = { + { _collision_segment_segment<false, false, false>, + _collision_segment_circle<false, false, false>, + _collision_segment_rectangle<false, false, false>, + _collision_segment_capsule<false, false, false>, + _collision_segment_convex_polygon<false, false, false> }, + { 0, + _collision_circle_circle<false, false, false>, + _collision_circle_rectangle<false, false, false>, + _collision_circle_capsule<false, false, false>, + _collision_circle_convex_polygon<false, false, false> }, + { 0, + 0, + _collision_rectangle_rectangle<false, false, false>, + _collision_rectangle_capsule<false, false, false>, + _collision_rectangle_convex_polygon<false, false, false> }, + { 0, + 0, + 0, + _collision_capsule_capsule<false, false, false>, + _collision_capsule_convex_polygon<false, false, false> }, + { 0, + 0, + 0, + 0, + _collision_convex_polygon_convex_polygon<false, false, false> } }; - static const CollisionFunc collision_table_castA[5][5]={ - {_collision_segment_segment<true,false,false>, - _collision_segment_circle<true,false,false>, - _collision_segment_rectangle<true,false,false>, - _collision_segment_capsule<true,false,false>, - _collision_segment_convex_polygon<true,false,false>}, - {0, - _collision_circle_circle<true,false,false>, - _collision_circle_rectangle<true,false,false>, - _collision_circle_capsule<true,false,false>, - _collision_circle_convex_polygon<true,false,false>}, - {0, - 0, - _collision_rectangle_rectangle<true,false,false>, - _collision_rectangle_capsule<true,false,false>, - _collision_rectangle_convex_polygon<true,false,false>}, - {0, - 0, - 0, - _collision_capsule_capsule<true,false,false>, - _collision_capsule_convex_polygon<true,false,false>}, - {0, - 0, - 0, - 0, - _collision_convex_polygon_convex_polygon<true,false,false>} + static const CollisionFunc collision_table_castA[5][5] = { + { _collision_segment_segment<true, false, false>, + _collision_segment_circle<true, false, false>, + _collision_segment_rectangle<true, false, false>, + _collision_segment_capsule<true, false, false>, + _collision_segment_convex_polygon<true, false, false> }, + { 0, + _collision_circle_circle<true, false, false>, + _collision_circle_rectangle<true, false, false>, + _collision_circle_capsule<true, false, false>, + _collision_circle_convex_polygon<true, false, false> }, + { 0, + 0, + _collision_rectangle_rectangle<true, false, false>, + _collision_rectangle_capsule<true, false, false>, + _collision_rectangle_convex_polygon<true, false, false> }, + { 0, + 0, + 0, + _collision_capsule_capsule<true, false, false>, + _collision_capsule_convex_polygon<true, false, false> }, + { 0, + 0, + 0, + 0, + _collision_convex_polygon_convex_polygon<true, false, false> } }; - static const CollisionFunc collision_table_castB[5][5]={ - {_collision_segment_segment<false,true,false>, - _collision_segment_circle<false,true,false>, - _collision_segment_rectangle<false,true,false>, - _collision_segment_capsule<false,true,false>, - _collision_segment_convex_polygon<false,true,false>}, - {0, - _collision_circle_circle<false,true,false>, - _collision_circle_rectangle<false,true,false>, - _collision_circle_capsule<false,true,false>, - _collision_circle_convex_polygon<false,true,false>}, - {0, - 0, - _collision_rectangle_rectangle<false,true,false>, - _collision_rectangle_capsule<false,true,false>, - _collision_rectangle_convex_polygon<false,true,false>}, - {0, - 0, - 0, - _collision_capsule_capsule<false,true,false>, - _collision_capsule_convex_polygon<false,true,false>}, - {0, - 0, - 0, - 0, - _collision_convex_polygon_convex_polygon<false,true,false>} + static const CollisionFunc collision_table_castB[5][5] = { + { _collision_segment_segment<false, true, false>, + _collision_segment_circle<false, true, false>, + _collision_segment_rectangle<false, true, false>, + _collision_segment_capsule<false, true, false>, + _collision_segment_convex_polygon<false, true, false> }, + { 0, + _collision_circle_circle<false, true, false>, + _collision_circle_rectangle<false, true, false>, + _collision_circle_capsule<false, true, false>, + _collision_circle_convex_polygon<false, true, false> }, + { 0, + 0, + _collision_rectangle_rectangle<false, true, false>, + _collision_rectangle_capsule<false, true, false>, + _collision_rectangle_convex_polygon<false, true, false> }, + { 0, + 0, + 0, + _collision_capsule_capsule<false, true, false>, + _collision_capsule_convex_polygon<false, true, false> }, + { 0, + 0, + 0, + 0, + _collision_convex_polygon_convex_polygon<false, true, false> } }; - static const CollisionFunc collision_table_castA_castB[5][5]={ - {_collision_segment_segment<true,true,false>, - _collision_segment_circle<true,true,false>, - _collision_segment_rectangle<true,true,false>, - _collision_segment_capsule<true,true,false>, - _collision_segment_convex_polygon<true,true,false>}, - {0, - _collision_circle_circle<true,true,false>, - _collision_circle_rectangle<true,true,false>, - _collision_circle_capsule<true,true,false>, - _collision_circle_convex_polygon<true,true,false>}, - {0, - 0, - _collision_rectangle_rectangle<true,true,false>, - _collision_rectangle_capsule<true,true,false>, - _collision_rectangle_convex_polygon<true,true,false>}, - {0, - 0, - 0, - _collision_capsule_capsule<true,true,false>, - _collision_capsule_convex_polygon<true,true,false>}, - {0, - 0, - 0, - 0, - _collision_convex_polygon_convex_polygon<true,true,false>} + static const CollisionFunc collision_table_castA_castB[5][5] = { + { _collision_segment_segment<true, true, false>, + _collision_segment_circle<true, true, false>, + _collision_segment_rectangle<true, true, false>, + _collision_segment_capsule<true, true, false>, + _collision_segment_convex_polygon<true, true, false> }, + { 0, + _collision_circle_circle<true, true, false>, + _collision_circle_rectangle<true, true, false>, + _collision_circle_capsule<true, true, false>, + _collision_circle_convex_polygon<true, true, false> }, + { 0, + 0, + _collision_rectangle_rectangle<true, true, false>, + _collision_rectangle_capsule<true, true, false>, + _collision_rectangle_convex_polygon<true, true, false> }, + { 0, + 0, + 0, + _collision_capsule_capsule<true, true, false>, + _collision_capsule_convex_polygon<true, true, false> }, + { 0, + 0, + 0, + 0, + _collision_convex_polygon_convex_polygon<true, true, false> } }; - static const CollisionFunc collision_table_margin[5][5]={ - {_collision_segment_segment<false,false,true>, - _collision_segment_circle<false,false,true>, - _collision_segment_rectangle<false,false,true>, - _collision_segment_capsule<false,false,true>, - _collision_segment_convex_polygon<false,false,true>}, - {0, - _collision_circle_circle<false,false,true>, - _collision_circle_rectangle<false,false,true>, - _collision_circle_capsule<false,false,true>, - _collision_circle_convex_polygon<false,false,true>}, - {0, - 0, - _collision_rectangle_rectangle<false,false,true>, - _collision_rectangle_capsule<false,false,true>, - _collision_rectangle_convex_polygon<false,false,true>}, - {0, - 0, - 0, - _collision_capsule_capsule<false,false,true>, - _collision_capsule_convex_polygon<false,false,true>}, - {0, - 0, - 0, - 0, - _collision_convex_polygon_convex_polygon<false,false,true>} + static const CollisionFunc collision_table_margin[5][5] = { + { _collision_segment_segment<false, false, true>, + _collision_segment_circle<false, false, true>, + _collision_segment_rectangle<false, false, true>, + _collision_segment_capsule<false, false, true>, + _collision_segment_convex_polygon<false, false, true> }, + { 0, + _collision_circle_circle<false, false, true>, + _collision_circle_rectangle<false, false, true>, + _collision_circle_capsule<false, false, true>, + _collision_circle_convex_polygon<false, false, true> }, + { 0, + 0, + _collision_rectangle_rectangle<false, false, true>, + _collision_rectangle_capsule<false, false, true>, + _collision_rectangle_convex_polygon<false, false, true> }, + { 0, + 0, + 0, + _collision_capsule_capsule<false, false, true>, + _collision_capsule_convex_polygon<false, false, true> }, + { 0, + 0, + 0, + 0, + _collision_convex_polygon_convex_polygon<false, false, true> } }; - static const CollisionFunc collision_table_castA_margin[5][5]={ - {_collision_segment_segment<true,false,true>, - _collision_segment_circle<true,false,true>, - _collision_segment_rectangle<true,false,true>, - _collision_segment_capsule<true,false,true>, - _collision_segment_convex_polygon<true,false,true>}, - {0, - _collision_circle_circle<true,false,true>, - _collision_circle_rectangle<true,false,true>, - _collision_circle_capsule<true,false,true>, - _collision_circle_convex_polygon<true,false,true>}, - {0, - 0, - _collision_rectangle_rectangle<true,false,true>, - _collision_rectangle_capsule<true,false,true>, - _collision_rectangle_convex_polygon<true,false,true>}, - {0, - 0, - 0, - _collision_capsule_capsule<true,false,true>, - _collision_capsule_convex_polygon<true,false,true>}, - {0, - 0, - 0, - 0, - _collision_convex_polygon_convex_polygon<true,false,true>} + static const CollisionFunc collision_table_castA_margin[5][5] = { + { _collision_segment_segment<true, false, true>, + _collision_segment_circle<true, false, true>, + _collision_segment_rectangle<true, false, true>, + _collision_segment_capsule<true, false, true>, + _collision_segment_convex_polygon<true, false, true> }, + { 0, + _collision_circle_circle<true, false, true>, + _collision_circle_rectangle<true, false, true>, + _collision_circle_capsule<true, false, true>, + _collision_circle_convex_polygon<true, false, true> }, + { 0, + 0, + _collision_rectangle_rectangle<true, false, true>, + _collision_rectangle_capsule<true, false, true>, + _collision_rectangle_convex_polygon<true, false, true> }, + { 0, + 0, + 0, + _collision_capsule_capsule<true, false, true>, + _collision_capsule_convex_polygon<true, false, true> }, + { 0, + 0, + 0, + 0, + _collision_convex_polygon_convex_polygon<true, false, true> } }; - static const CollisionFunc collision_table_castB_margin[5][5]={ - {_collision_segment_segment<false,true,true>, - _collision_segment_circle<false,true,true>, - _collision_segment_rectangle<false,true,true>, - _collision_segment_capsule<false,true,true>, - _collision_segment_convex_polygon<false,true,true>}, - {0, - _collision_circle_circle<false,true,true>, - _collision_circle_rectangle<false,true,true>, - _collision_circle_capsule<false,true,true>, - _collision_circle_convex_polygon<false,true,true>}, - {0, - 0, - _collision_rectangle_rectangle<false,true,true>, - _collision_rectangle_capsule<false,true,true>, - _collision_rectangle_convex_polygon<false,true,true>}, - {0, - 0, - 0, - _collision_capsule_capsule<false,true,true>, - _collision_capsule_convex_polygon<false,true,true>}, - {0, - 0, - 0, - 0, - _collision_convex_polygon_convex_polygon<false,true,true>} + static const CollisionFunc collision_table_castB_margin[5][5] = { + { _collision_segment_segment<false, true, true>, + _collision_segment_circle<false, true, true>, + _collision_segment_rectangle<false, true, true>, + _collision_segment_capsule<false, true, true>, + _collision_segment_convex_polygon<false, true, true> }, + { 0, + _collision_circle_circle<false, true, true>, + _collision_circle_rectangle<false, true, true>, + _collision_circle_capsule<false, true, true>, + _collision_circle_convex_polygon<false, true, true> }, + { 0, + 0, + _collision_rectangle_rectangle<false, true, true>, + _collision_rectangle_capsule<false, true, true>, + _collision_rectangle_convex_polygon<false, true, true> }, + { 0, + 0, + 0, + _collision_capsule_capsule<false, true, true>, + _collision_capsule_convex_polygon<false, true, true> }, + { 0, + 0, + 0, + 0, + _collision_convex_polygon_convex_polygon<false, true, true> } }; - static const CollisionFunc collision_table_castA_castB_margin[5][5]={ - {_collision_segment_segment<true,true,true>, - _collision_segment_circle<true,true,true>, - _collision_segment_rectangle<true,true,true>, - _collision_segment_capsule<true,true,true>, - _collision_segment_convex_polygon<true,true,true>}, - {0, - _collision_circle_circle<true,true,true>, - _collision_circle_rectangle<true,true,true>, - _collision_circle_capsule<true,true,true>, - _collision_circle_convex_polygon<true,true,true>}, - {0, - 0, - _collision_rectangle_rectangle<true,true,true>, - _collision_rectangle_capsule<true,true,true>, - _collision_rectangle_convex_polygon<true,true,true>}, - {0, - 0, - 0, - _collision_capsule_capsule<true,true,true>, - _collision_capsule_convex_polygon<true,true,true>}, - {0, - 0, - 0, - 0, - _collision_convex_polygon_convex_polygon<true,true,true>} + static const CollisionFunc collision_table_castA_castB_margin[5][5] = { + { _collision_segment_segment<true, true, true>, + _collision_segment_circle<true, true, true>, + _collision_segment_rectangle<true, true, true>, + _collision_segment_capsule<true, true, true>, + _collision_segment_convex_polygon<true, true, true> }, + { 0, + _collision_circle_circle<true, true, true>, + _collision_circle_rectangle<true, true, true>, + _collision_circle_capsule<true, true, true>, + _collision_circle_convex_polygon<true, true, true> }, + { 0, + 0, + _collision_rectangle_rectangle<true, true, true>, + _collision_rectangle_capsule<true, true, true>, + _collision_rectangle_convex_polygon<true, true, true> }, + { 0, + 0, + 0, + _collision_capsule_capsule<true, true, true>, + _collision_capsule_convex_polygon<true, true, true> }, + { 0, + 0, + 0, + 0, + _collision_convex_polygon_convex_polygon<true, true, true> } }; - _CollectorCallback2D callback; - callback.callback=p_result_callback; - callback.swap=p_swap; - callback.userdata=p_userdata; - callback.collided=false; - callback.sep_axis=sep_axis; - - const Shape2DSW *A=p_shape_A; - const Shape2DSW *B=p_shape_B; - const Transform2D *transform_A=&p_transform_A; - const Transform2D *transform_B=&p_transform_B; - const Vector2 *motion_A=&p_motion_A; - const Vector2 *motion_B=&p_motion_B; - real_t margin_A=p_margin_A,margin_B=p_margin_B; + callback.callback = p_result_callback; + callback.swap = p_swap; + callback.userdata = p_userdata; + callback.collided = false; + callback.sep_axis = sep_axis; + + const Shape2DSW *A = p_shape_A; + const Shape2DSW *B = p_shape_B; + const Transform2D *transform_A = &p_transform_A; + const Transform2D *transform_B = &p_transform_B; + const Vector2 *motion_A = &p_motion_A; + const Vector2 *motion_B = &p_motion_B; + real_t margin_A = p_margin_A, margin_B = p_margin_B; if (type_A > type_B) { - SWAP(A,B); - SWAP(transform_A,transform_B); - SWAP(type_A,type_B); - SWAP(motion_A,motion_B); - SWAP(margin_A,margin_B); + SWAP(A, B); + SWAP(transform_A, transform_B); + SWAP(type_A, type_B); + SWAP(motion_A, motion_B); + SWAP(margin_A, margin_B); callback.swap = !callback.swap; } - CollisionFunc collision_func; if (p_margin_A || p_margin_B) { - if (*motion_A==Vector2() && *motion_B==Vector2()) { - collision_func = collision_table_margin[type_A-2][type_B-2]; - } else if (*motion_A!=Vector2() && *motion_B==Vector2()) { - collision_func = collision_table_castA_margin[type_A-2][type_B-2]; - } else if (*motion_A==Vector2() && *motion_B!=Vector2()) { - collision_func = collision_table_castB_margin[type_A-2][type_B-2]; + if (*motion_A == Vector2() && *motion_B == Vector2()) { + collision_func = collision_table_margin[type_A - 2][type_B - 2]; + } else if (*motion_A != Vector2() && *motion_B == Vector2()) { + collision_func = collision_table_castA_margin[type_A - 2][type_B - 2]; + } else if (*motion_A == Vector2() && *motion_B != Vector2()) { + collision_func = collision_table_castB_margin[type_A - 2][type_B - 2]; } else { - collision_func = collision_table_castA_castB_margin[type_A-2][type_B-2]; + collision_func = collision_table_castA_castB_margin[type_A - 2][type_B - 2]; } } else { - if (*motion_A==Vector2() && *motion_B==Vector2()) { - collision_func = collision_table[type_A-2][type_B-2]; - } else if (*motion_A!=Vector2() && *motion_B==Vector2()) { - collision_func = collision_table_castA[type_A-2][type_B-2]; - } else if (*motion_A==Vector2() && *motion_B!=Vector2()) { - collision_func = collision_table_castB[type_A-2][type_B-2]; + if (*motion_A == Vector2() && *motion_B == Vector2()) { + collision_func = collision_table[type_A - 2][type_B - 2]; + } else if (*motion_A != Vector2() && *motion_B == Vector2()) { + collision_func = collision_table_castA[type_A - 2][type_B - 2]; + } else if (*motion_A == Vector2() && *motion_B != Vector2()) { + collision_func = collision_table_castB[type_A - 2][type_B - 2]; } else { - collision_func = collision_table_castA_castB[type_A-2][type_B-2]; + collision_func = collision_table_castA_castB[type_A - 2][type_B - 2]; } - } + ERR_FAIL_COND_V(!collision_func, false); - - ERR_FAIL_COND_V(!collision_func,false); - - collision_func(A,*transform_A,B,*transform_B,&callback,*motion_A,*motion_B,margin_A,margin_B); + collision_func(A, *transform_A, B, *transform_B, &callback, *motion_A, *motion_B, margin_A, margin_B); return callback.collided; - - } diff --git a/servers/physics_2d/collision_solver_2d_sat.h b/servers/physics_2d/collision_solver_2d_sat.h index 6b698a09f2..a13c3dd14d 100644 --- a/servers/physics_2d/collision_solver_2d_sat.h +++ b/servers/physics_2d/collision_solver_2d_sat.h @@ -31,7 +31,6 @@ #include "collision_solver_2d_sw.h" - -bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Transform2D& p_transform_A, const Vector2& p_motion_A,const Shape2DSW *p_shape_B, const Transform2D& p_transform_B,const Vector2& p_motion_B, CollisionSolver2DSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap=false,Vector2 *sep_axis=NULL,real_t p_margin_A=0,real_t p_margin_B=0); +bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CollisionSolver2DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap = false, Vector2 *sep_axis = NULL, real_t p_margin_A = 0, real_t p_margin_B = 0); #endif // COLLISION_SOLVER_2D_SAT_H diff --git a/servers/physics_2d/collision_solver_2d_sw.cpp b/servers/physics_2d/collision_solver_2d_sw.cpp index 3fdecdf413..6218af72a1 100644 --- a/servers/physics_2d/collision_solver_2d_sw.cpp +++ b/servers/physics_2d/collision_solver_2d_sw.cpp @@ -29,89 +29,78 @@ #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 Transform2D &p_transform_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) { -bool CollisionSolver2DSW::solve_static_line(const Shape2DSW *p_shape_A,const Transform2D& p_transform_A,const Shape2DSW *p_shape_B,const Transform2D& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result) { - - - const LineShape2DSW *line = static_cast<const LineShape2DSW*>(p_shape_A); - if (p_shape_B->get_type()==Physics2DServer::SHAPE_LINE) + 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()); + 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_A.affine_inverse().basis_xform(-n).normalized(),supports,support_count); - - bool found=false; + p_shape_B->get_supports(p_transform_A.affine_inverse().basis_xform(-n).normalized(), supports, support_count); + bool found = false; - for(int i=0;i<support_count;i++) { + for (int i = 0; i < support_count; i++) { - supports[i] = p_transform_B.xform( supports[i] ); + supports[i] = p_transform_B.xform(supports[i]); real_t pd = n.dot(supports[i]); - if (pd>=d) + if (pd >= d) continue; - found=true; + found = true; - Vector2 support_A = supports[i] - n*(pd-d); + 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); + p_result_callback(supports[i], support_A, p_userdata); else - p_result_callback(support_A,supports[i],p_userdata); + p_result_callback(support_A, supports[i], p_userdata); } - } - return found; } -bool CollisionSolver2DSW::solve_raycast(const Shape2DSW *p_shape_A,const Transform2D& p_transform_A,const Shape2DSW *p_shape_B,const Transform2D& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis) { - +bool CollisionSolver2DSW::solve_raycast(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *sep_axis) { - - const RayShape2DSW *ray = static_cast<const RayShape2DSW*>(p_shape_A); - if (p_shape_B->get_type()==Physics2DServer::SHAPE_RAY) + 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; + Vector2 to = from + p_transform_A[1] * ray->get_length(); + Vector2 support_A = to; Transform2D invb = p_transform_B.affine_inverse(); from = invb.xform(from); to = invb.xform(to); - Vector2 p,n; - if (!p_shape_B->intersect_segment(from,to,p,n)) { + Vector2 p, n; + if (!p_shape_B->intersect_segment(from, to, p, n)) { if (sep_axis) - *sep_axis=p_transform_A[1].normalized(); + *sep_axis = p_transform_A[1].normalized(); return false; } - - Vector2 support_B=p_transform_B.xform(p); + 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); + p_result_callback(support_B, support_A, p_userdata); else - p_result_callback(support_A,support_B,p_userdata); + p_result_callback(support_A, support_B, p_userdata); } return true; - } /* @@ -159,101 +148,90 @@ struct _ConcaveCollisionInfo2D { int aabb_tests; int collisions; Vector2 *sep_axis; - }; void CollisionSolver2DSW::concave_callback(void *p_userdata, Shape2DSW *p_convex) { - - - _ConcaveCollisionInfo2D &cinfo = *(_ConcaveCollisionInfo2D*)(p_userdata); + _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.motion_A, p_convex,*cinfo.transform_B, cinfo.motion_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result,cinfo.sep_axis,cinfo.margin_A,cinfo.margin_B ); + bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, cinfo.motion_A, p_convex, *cinfo.transform_B, cinfo.motion_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result, cinfo.sep_axis, cinfo.margin_A, cinfo.margin_B); if (!collided) return; - - cinfo.collided=true; + cinfo.collided = true; cinfo.collisions++; - } -bool CollisionSolver2DSW::solve_concave(const Shape2DSW *p_shape_A,const Transform2D& p_transform_A,const Vector2& p_motion_A,const Shape2DSW *p_shape_B,const Transform2D& p_transform_B,const Vector2& p_motion_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis,real_t p_margin_A,real_t p_margin_B) { +bool CollisionSolver2DSW::solve_concave(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *sep_axis, real_t p_margin_A, real_t p_margin_B) { - - const ConcaveShape2DSW *concave_B=static_cast<const ConcaveShape2DSW*>(p_shape_B); + const ConcaveShape2DSW *concave_B = static_cast<const ConcaveShape2DSW *>(p_shape_B); _ConcaveCollisionInfo2D cinfo; - cinfo.transform_A=&p_transform_A; - cinfo.shape_A=p_shape_A; - cinfo.transform_B=&p_transform_B; - cinfo.motion_A=p_motion_A; - cinfo.result_callback=p_result_callback; - cinfo.userdata=p_userdata; - cinfo.swap_result=p_swap_result; - cinfo.collided=false; - cinfo.collisions=0; - cinfo.sep_axis=sep_axis; - cinfo.margin_A=p_margin_A; - cinfo.margin_B=p_margin_B; - - cinfo.aabb_tests=0; + cinfo.transform_A = &p_transform_A; + cinfo.shape_A = p_shape_A; + cinfo.transform_B = &p_transform_B; + cinfo.motion_A = p_motion_A; + cinfo.result_callback = p_result_callback; + cinfo.userdata = p_userdata; + cinfo.swap_result = p_swap_result; + cinfo.collided = false; + cinfo.collisions = 0; + cinfo.sep_axis = sep_axis; + cinfo.margin_A = p_margin_A; + cinfo.margin_B = p_margin_B; + + cinfo.aabb_tests = 0; Transform2D rel_transform = p_transform_A; - rel_transform.elements[2]-=p_transform_B.get_origin(); + rel_transform.elements[2] -= p_transform_B.get_origin(); //quickly compute a local Rect2 Rect2 local_aabb; - for(int i=0;i<2;i++) { + for (int i = 0; i < 2; i++) { - Vector2 axis( p_transform_B.elements[i] ); - real_t axis_scale = 1.0/axis.length(); - axis*=axis_scale; + Vector2 axis(p_transform_B.elements[i]); + real_t axis_scale = 1.0 / axis.length(); + axis *= axis_scale; - real_t smin,smax; - p_shape_A->project_rangev(axis,rel_transform,smin,smax); - smin*=axis_scale; - smax*=axis_scale; + real_t 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; + local_aabb.pos[i] = smin; + local_aabb.size[i] = smax - smin; } - concave_B->cull(local_aabb,concave_callback,&cinfo); - + concave_B->cull(local_aabb, concave_callback, &cinfo); //print_line("Rect2 TESTS: "+itos(cinfo.aabb_tests)); return cinfo.collided; } +bool CollisionSolver2DSW::solve(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, Vector2 *sep_axis, real_t p_margin_A, real_t p_margin_B) { -bool CollisionSolver2DSW::solve(const Shape2DSW *p_shape_A,const Transform2D& p_transform_A,const Vector2& p_motion_A,const Shape2DSW *p_shape_B,const Transform2D& p_transform_B,const Vector2& p_motion_B,CallbackResult p_result_callback,void *p_userdata,Vector2 *sep_axis,real_t p_margin_A,real_t p_margin_B) { - - - - - 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(); - real_t margin_A=p_margin_A,margin_B=p_margin_B; + 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(); + real_t margin_A = p_margin_A, margin_B = p_margin_B; bool swap = false; - if (type_A>type_B) { - SWAP(type_A,type_B); - SWAP(concave_A,concave_B); - SWAP(margin_A,margin_B); - swap=true; + if (type_A > type_B) { + SWAP(type_A, type_B); + SWAP(concave_A, concave_B); + SWAP(margin_A, margin_B); + swap = true; } - if (type_A==Physics2DServer::SHAPE_LINE) { + if (type_A == Physics2DServer::SHAPE_LINE) { - if (type_B==Physics2DServer::SHAPE_LINE || type_B==Physics2DServer::SHAPE_RAY) { + if (type_B == Physics2DServer::SHAPE_LINE || type_B == Physics2DServer::SHAPE_RAY) { return false; } /* @@ -262,12 +240,12 @@ bool CollisionSolver2DSW::solve(const Shape2DSW *p_shape_A,const Transform2D& p_ */ if (swap) { - return solve_static_line(p_shape_B,p_transform_B,p_shape_A,p_transform_A,p_result_callback,p_userdata,true); + return solve_static_line(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true); } else { - return solve_static_line(p_shape_A,p_transform_A,p_shape_B,p_transform_B,p_result_callback,p_userdata,false); + return solve_static_line(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false); } - /*} else if (type_A==Physics2DServer::SHAPE_RAY) { + /*} else if (type_A==Physics2DServer::SHAPE_RAY) { if (type_B==Physics2DServer::SHAPE_RAY) return false; @@ -278,41 +256,33 @@ bool CollisionSolver2DSW::solve(const Shape2DSW *p_shape_A,const Transform2D& p_ 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) { + } else if (type_A == Physics2DServer::SHAPE_RAY) { - if (type_B==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_shape_A,p_transform_A,p_result_callback,p_userdata,true,sep_axis); + return solve_raycast(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true, sep_axis); } else { - return solve_raycast(p_shape_A,p_transform_A,p_shape_B,p_transform_B,p_result_callback,p_userdata,false,sep_axis); + return solve_raycast(p_shape_A, p_transform_A, p_shape_B, p_transform_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_motion_A,p_shape_B,p_transform_B,p_motion_B,p_result_callback,p_userdata,false,sep_axis,margin_A,margin_B); + return solve_concave(p_shape_A, p_transform_A, p_motion_A, p_shape_B, p_transform_B, p_motion_B, p_result_callback, p_userdata, false, sep_axis, margin_A, margin_B); else - return solve_concave(p_shape_B,p_transform_B,p_motion_B,p_shape_A,p_transform_A,p_motion_A,p_result_callback,p_userdata,true,sep_axis,margin_A,margin_B); - - + return solve_concave(p_shape_B, p_transform_B, p_motion_B, p_shape_A, p_transform_A, p_motion_A, p_result_callback, p_userdata, true, sep_axis, margin_A, margin_B); } else { - - return collision_solver(p_shape_A, p_transform_A,p_motion_A, p_shape_B, p_transform_B, p_motion_B,p_result_callback,p_userdata,false,sep_axis,margin_A,margin_B); + return collision_solver(p_shape_A, p_transform_A, p_motion_A, p_shape_B, p_transform_B, p_motion_B, p_result_callback, p_userdata, false, sep_axis, margin_A, margin_B); } - return false; } - diff --git a/servers/physics_2d/collision_solver_2d_sw.h b/servers/physics_2d/collision_solver_2d_sw.h index 2a5fc9fe1d..886cb90ace 100644 --- a/servers/physics_2d/collision_solver_2d_sw.h +++ b/servers/physics_2d/collision_solver_2d_sw.h @@ -33,20 +33,16 @@ class CollisionSolver2DSW { public: - typedef void (*CallbackResult)(const Vector2& p_point_A,const Vector2& p_point_B,void *p_userdata); + 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 Transform2D& p_transform_A,const Shape2DSW *p_shape_B,const Transform2D& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result); + static bool solve_static_line(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); static void concave_callback(void *p_userdata, Shape2DSW *p_convex); - static bool solve_concave(const Shape2DSW *p_shape_A,const Transform2D& p_transform_A,const Vector2& p_motion_A,const Shape2DSW *p_shape_B,const Transform2D& p_transform_B,const Vector2& p_motion_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis=NULL,real_t p_margin_A=0,real_t p_margin_B=0); - static bool solve_raycast(const Shape2DSW *p_shape_A,const Transform2D& p_transform_A,const Shape2DSW *p_shape_B,const Transform2D& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis=NULL); - - + static bool solve_concave(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *sep_axis = NULL, real_t p_margin_A = 0, real_t p_margin_B = 0); + static bool solve_raycast(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *sep_axis = NULL); public: - - static bool solve(const Shape2DSW *p_shape_A,const Transform2D& p_transform_A,const Vector2& p_motion_A,const Shape2DSW *p_shape_B,const Transform2D& p_transform_B,const Vector2& p_motion_B,CallbackResult p_result_callback,void *p_userdata,Vector2 *sep_axis=NULL,real_t p_margin_A=0,real_t p_margin_B=0); - - + static bool solve(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, Vector2 *sep_axis = NULL, real_t p_margin_A = 0, real_t p_margin_B = 0); }; #endif // COLLISION_SOLVER_2D_SW_H diff --git a/servers/physics_2d/constraint_2d_sw.h b/servers/physics_2d/constraint_2d_sw.h index cce668405c..659b5b3219 100644 --- a/servers/physics_2d/constraint_2d_sw.h +++ b/servers/physics_2d/constraint_2d_sw.h @@ -39,32 +39,33 @@ class Constraint2DSW : public RID_Data { 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: + Constraint2DSW(Body2DSW **p_body_ptr = NULL, int p_body_count = 0) { + _body_ptr = p_body_ptr; + _body_count = p_body_count; + island_step = 0; + } - _FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; } +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_ 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_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_ 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(real_t p_step)=0; - virtual void solve(real_t p_step)=0; + virtual bool setup(real_t p_step) = 0; + virtual void solve(real_t p_step) = 0; virtual ~Constraint2DSW() {} }; diff --git a/servers/physics_2d/joints_2d_sw.cpp b/servers/physics_2d/joints_2d_sw.cpp index 76adf06429..0277586495 100644 --- a/servers/physics_2d/joints_2d_sw.cpp +++ b/servers/physics_2d/joints_2d_sw.cpp @@ -52,40 +52,37 @@ * SOFTWARE. */ -static inline real_t k_scalar(Body2DSW *a,Body2DSW *b,const Vector2& rA, const Vector2& rB, const Vector2& n) { - - - real_t value=0; +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(); + value += a->get_inv_mass(); real_t rcn = rA.cross(n); - value+=a->get_inv_inertia() * rcn * rcn; + value += a->get_inv_inertia() * rcn * rcn; } if (b) { - value+=b->get_inv_mass(); + value += b->get_inv_mass(); real_t rcn = rB.cross(n); - value+=b->get_inv_inertia() * rcn * rcn; + 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(); +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; + 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){ +normal_relative_velocity(Body2DSW *a, Body2DSW *b, Vector2 rA, Vector2 rB, Vector2 n) { return relative_velocity(a, b, rA, rB).dot(n); } @@ -188,13 +185,12 @@ PinJoint2DSW::~PinJoint2DSW() { #else - bool PinJoint2DSW::setup(real_t p_step) { Space2DSW *space = A->get_space(); - ERR_FAIL_COND_V(!space,false;) + 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; + 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; @@ -212,29 +208,34 @@ bool PinJoint2DSW::setup(real_t p_step) { // = [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; - + real_t B_inv_mass = B ? B->get_inv_mass() : 0.0; Transform2D K1; - K1[0].x = A->get_inv_mass() + B_inv_mass; K1[1].x = 0.0f; - K1[0].y = 0.0f; K1[1].y = A->get_inv_mass() + B_inv_mass; + K1[0].x = A->get_inv_mass() + B_inv_mass; + K1[1].x = 0.0f; + K1[0].y = 0.0f; + K1[1].y = A->get_inv_mass() + B_inv_mass; Transform2D K2; - K2[0].x = A->get_inv_inertia() * rA.y * rA.y; K2[1].x = -A->get_inv_inertia() * rA.x * rA.y; - K2[0].y = -A->get_inv_inertia() * rA.x * rA.y; K2[1].y = A->get_inv_inertia() * rA.x * rA.x; + K2[0].x = A->get_inv_inertia() * rA.y * rA.y; + K2[1].x = -A->get_inv_inertia() * rA.x * rA.y; + K2[0].y = -A->get_inv_inertia() * rA.x * rA.y; + K2[1].y = A->get_inv_inertia() * rA.x * rA.x; Transform2D K; - K[0]= K1[0] + K2[0]; - K[1]= K1[1] + K2[1]; + K[0] = K1[0] + K2[0]; + K[1] = K1[1] + K2[1]; if (B) { Transform2D K3; - K3[0].x = B->get_inv_inertia() * rB.y * rB.y; K3[1].x = -B->get_inv_inertia() * rB.x * rB.y; - K3[0].y = -B->get_inv_inertia() * rB.x * rB.y; K3[1].y = B->get_inv_inertia() * rB.x * rB.x; + 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] += K3[0]; + K[1] += K3[1]; } K[0].x += softness; @@ -242,23 +243,22 @@ bool PinJoint2DSW::setup(real_t p_step) { M = K.affine_inverse(); - Vector2 gA = rA+A->get_transform().get_origin(); - Vector2 gB = B?rB+B->get_transform().get_origin():rB; + 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); + bias = delta * -(get_bias() == 0 ? space->get_constraint_bias() : get_bias()) * (1.0 / p_step); // apply accumulated impulse - A->apply_impulse(rA,-P); + A->apply_impulse(rA, -P); if (B) - B->apply_impulse(rB,P); + B->apply_impulse(rB, P); return true; } -void PinJoint2DSW::solve(real_t p_step){ - +void PinJoint2DSW::solve(real_t p_step) { // compute relative velocity Vector2 vA = A->get_linear_velocity() - rA.cross(A->get_angular_velocity()); @@ -269,114 +269,110 @@ void PinJoint2DSW::solve(real_t p_step){ else rel_vel = -vA; - Vector2 impulse = M.basis_xform(bias - rel_vel - Vector2(softness,softness) * P); + Vector2 impulse = M.basis_xform(bias - rel_vel - Vector2(softness, softness) * P); - A->apply_impulse(rA,-impulse); + A->apply_impulse(rA, -impulse); if (B) - B->apply_impulse(rB,impulse); - + B->apply_impulse(rB, impulse); P += impulse; } void PinJoint2DSW::set_param(Physics2DServer::PinJointParam p_param, real_t p_value) { - if(p_param == Physics2DServer::PIN_JOINT_SOFTNESS) + if (p_param == Physics2DServer::PIN_JOINT_SOFTNESS) softness = p_value; } real_t PinJoint2DSW::get_param(Physics2DServer::PinJointParam p_param) const { - if(p_param == Physics2DServer::PIN_JOINT_SOFTNESS) + if (p_param == Physics2DServer::PIN_JOINT_SOFTNESS) return softness; ERR_FAIL_V(0); } -PinJoint2DSW::PinJoint2DSW(const Vector2& p_pos,Body2DSW* p_body_a,Body2DSW* p_body_b) : Joint2DSW(_arr,p_body_b?2:1) { +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; + 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; + anchor_B = p_body_b ? p_body_b->get_inv_transform().xform(p_pos) : p_pos; - softness=0; + softness = 0; - p_body_a->add_constraint(this,0); + p_body_a->add_constraint(this, 0); if (p_body_b) - p_body_b->add_constraint(this,1); - + 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) -{ +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; + 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 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; + 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 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; + k11 += r2ysq; + k12 += r2nxy; + k21 += r2nxy; + k22 += r2xsq; // invert - real_t determinant = k11*k22 - k12*k21; - ERR_FAIL_COND(determinant== 0.0); + 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); + 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) -{ +mult_k(const Vector2 &vr, const Vector2 &k1, const Vector2 &k2) { return Vector2(vr.dot(k1), vr.dot(k2)); } bool GrooveJoint2DSW::setup(real_t p_step) { - // 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(); + Space2DSW *space = A->get_space(); // calculate axis Vector2 n = -(tb - ta).tangent().normalized(); @@ -388,16 +384,16 @@ bool GrooveJoint2DSW::setup(real_t p_step) { // 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)){ + if (td <= ta.cross(n)) { clamp = 1.0f; rA = ta - A->get_transform().get_origin(); - } else if(td >= tb.cross(n)){ + } 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(); + rA = ((-n.tangent() * -td) + n * d) - A->get_transform().get_origin(); } // Calculate mass tensor @@ -410,52 +406,49 @@ bool GrooveJoint2DSW::setup(real_t p_step) { //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); + Vector2 delta = (B->get_transform().get_origin() + rB) - (A->get_transform().get_origin() + rA); real_t _b = get_bias(); - _b=0.001; - gbias=(delta*-(_b==0?space->get_constraint_bias():_b)*(1.0/p_step)).clamped(get_max_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); + A->apply_impulse(rA, -jn_acc); + B->apply_impulse(rB, jn_acc); - correct=true; + correct = true; return true; } -void GrooveJoint2DSW::solve(real_t p_step){ - +void GrooveJoint2DSW::solve(real_t p_step) { // compute impulse - Vector2 vr = relative_velocity(A, B, rA,rB); + Vector2 vr = relative_velocity(A, B, rA, rB); - Vector2 j = mult_k(gbias-vr, k1, k2); + Vector2 j = mult_k(gbias - vr, k1, k2); Vector2 jOld = jn_acc; - j+=jOld; + 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); + 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) { -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 = 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); + 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); - + A->add_constraint(this, 0); + B->add_constraint(this, 1); } GrooveJoint2DSW::~GrooveJoint2DSW() { @@ -464,38 +457,35 @@ GrooveJoint2DSW::~GrooveJoint2DSW() { B->remove_constraint(this); } - ////////////////////////////////////////////// ////////////////////////////////////////////// ////////////////////////////////////////////// - bool DampedSpringJoint2DSW::setup(real_t 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) ; + Vector2 delta = (B->get_transform().get_origin() + rB) - (A->get_transform().get_origin() + rA); real_t dist = delta.length(); if (dist) - n=delta/dist; + n = delta / dist; else - n=Vector2(); + n = Vector2(); real_t k = k_scalar(A, B, rA, rB, n); - n_mass = 1.0f/k; + n_mass = 1.0f / k; target_vrn = 0.0f; - v_coef = 1.0f - Math::exp(-damping*(p_step)*k); + 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); + Vector2 j = n * f_spring * (p_step); + A->apply_impulse(rA, -j); + B->apply_impulse(rB, j); return true; } @@ -507,38 +497,36 @@ void DampedSpringJoint2DSW::solve(real_t p_step) { // compute velocity loss from drag // not 100% certain this is derived correctly, though it makes sense - real_t v_damp = -vrn*v_coef; + 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); + 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) { + switch (p_param) { case Physics2DServer::DAMPED_STRING_REST_LENGTH: { - rest_length=p_value; + rest_length = p_value; } break; case Physics2DServer::DAMPED_STRING_DAMPING: { - damping=p_value; + damping = p_value; } break; case Physics2DServer::DAMPED_STRING_STIFFNESS: { - stiffness=p_value; + stiffness = p_value; } break; } - } -real_t DampedSpringJoint2DSW::get_param(Physics2DServer::DampedStringParam p_param) const{ +real_t DampedSpringJoint2DSW::get_param(Physics2DServer::DampedStringParam p_param) const { - switch(p_param) { + switch (p_param) { case Physics2DServer::DAMPED_STRING_REST_LENGTH: { @@ -557,30 +545,24 @@ real_t DampedSpringJoint2DSW::get_param(Physics2DServer::DampedStringParam p_par 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) { -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; + 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); + 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 index 1be15e4edc..8fe38f365a 100644 --- a/servers/physics_2d/joints_2d_sw.h +++ b/servers/physics_2d/joints_2d_sw.h @@ -29,30 +29,31 @@ #ifndef JOINTS_2D_SW_H #define JOINTS_2D_SW_H -#include "constraint_2d_sw.h" #include "body_2d_sw.h" - - +#include "constraint_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; } +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_ 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_ 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; }; - + 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 @@ -107,7 +108,7 @@ class PinJoint2DSW : public Joint2DSW { }; Transform2D M; - Vector2 rA,rB; + Vector2 rA, rB; Vector2 anchor_A; Vector2 anchor_B; Vector2 bias; @@ -115,7 +116,6 @@ class PinJoint2DSW : public Joint2DSW { real_t softness; public: - virtual Physics2DServer::JointType get_type() const { return Physics2DServer::JOINT_PIN; } virtual bool setup(real_t p_step); @@ -124,11 +124,10 @@ public: void set_param(Physics2DServer::PinJointParam p_param, real_t p_value); real_t get_param(Physics2DServer::PinJointParam p_param) const; - PinJoint2DSW(const Vector2& p_pos,Body2DSW* p_body_a,Body2DSW* p_body_b=NULL); + PinJoint2DSW(const Vector2 &p_pos, Body2DSW *p_body_a, Body2DSW *p_body_b = NULL); ~PinJoint2DSW(); }; - #endif class GrooveJoint2DSW : public Joint2DSW { @@ -150,25 +149,21 @@ class GrooveJoint2DSW : public Joint2DSW { real_t jn_max; real_t clamp; Vector2 xf_normal; - Vector2 rA,rB; - Vector2 k1,k2; - + Vector2 rA, rB; + Vector2 k1, k2; bool correct; public: - virtual Physics2DServer::JointType get_type() const { return Physics2DServer::JOINT_GROOVE; } virtual bool setup(real_t p_step); virtual void solve(real_t 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(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 { @@ -180,7 +175,6 @@ class DampedSpringJoint2DSW : public Joint2DSW { Body2DSW *_arr[2]; }; - Vector2 anchor_A; Vector2 anchor_B; @@ -188,14 +182,13 @@ class DampedSpringJoint2DSW : public Joint2DSW { real_t damping; real_t stiffness; - Vector2 rA,rB; + 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(real_t p_step); @@ -204,9 +197,8 @@ public: 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(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 index 920742ea0b..0a56683c5d 100644 --- a/servers/physics_2d/physics_2d_server_sw.cpp +++ b/servers/physics_2d/physics_2d_server_sw.cpp @@ -31,52 +31,51 @@ #include "broad_phase_2d_hash_grid.h" #include "collision_solver_2d_sw.h" #include "global_config.h" -#include "script_language.h" #include "os/os.h" +#include "script_language.h" RID Physics2DServerSW::shape_create(ShapeType p_shape) { - Shape2DSW *shape=NULL; - switch(p_shape) { + Shape2DSW *shape = NULL; + switch (p_shape) { case SHAPE_LINE: { - shape=memnew( LineShape2DSW ); + shape = memnew(LineShape2DSW); } break; case SHAPE_RAY: { - shape=memnew( RayShape2DSW ); + shape = memnew(RayShape2DSW); } break; case SHAPE_SEGMENT: { - shape=memnew( SegmentShape2DSW); + shape = memnew(SegmentShape2DSW); } break; case SHAPE_CIRCLE: { - shape=memnew( CircleShape2DSW); + shape = memnew(CircleShape2DSW); } break; case SHAPE_RECTANGLE: { - shape=memnew( RectangleShape2DSW); + shape = memnew(RectangleShape2DSW); } break; case SHAPE_CAPSULE: { - shape=memnew( CapsuleShape2DSW ); + shape = memnew(CapsuleShape2DSW); } break; case SHAPE_CONVEX_POLYGON: { - shape=memnew( ConvexPolygonShape2DSW ); + shape = memnew(ConvexPolygonShape2DSW); } break; case SHAPE_CONCAVE_POLYGON: { - shape=memnew( ConcavePolygonShape2DSW ); + shape = memnew(ConcavePolygonShape2DSW); } break; case SHAPE_CUSTOM: { ERR_FAIL_V(RID()); } break; - } RID id = shape_owner.make_rid(shape); @@ -85,68 +84,55 @@ RID Physics2DServerSW::shape_create(ShapeType p_shape) { return id; }; - - - -void Physics2DServerSW::shape_set_data(RID p_shape, const Variant& p_data) { +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); + 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()); + 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); + ERR_FAIL_COND_V(!shape, 0); return shape->get_custom_bias(); - } +void Physics2DServerSW::_shape_col_cbk(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_userdata) { -void Physics2DServerSW::_shape_col_cbk(const Vector2& p_point_A,const Vector2& p_point_B,void *p_userdata) { - + CollCbkData *cbk = (CollCbkData *)p_userdata; - CollCbkData *cbk=(CollCbkData *)p_userdata; - - if (cbk->max==0) + if (cbk->max == 0) return; - if (cbk->valid_dir!=Vector2()) { - if (p_point_A.distance_squared_to(p_point_B)>cbk->valid_depth*cbk->valid_depth) { + if (cbk->valid_dir != Vector2()) { + if (p_point_A.distance_squared_to(p_point_B) > cbk->valid_depth * cbk->valid_depth) { return; } - if (cbk->valid_dir.dot((p_point_A-p_point_B).normalized())<0.7071) { -/* print_line("A: "+p_point_A); + if (cbk->valid_dir.dot((p_point_A - p_point_B).normalized()) < 0.7071) { + /* print_line("A: "+p_point_A); print_line("B: "+p_point_B); print_line("discard too angled "+rtos(cbk->valid_dir.dot((p_point_A-p_point_B)))); print_line("resnorm: "+(p_point_A-p_point_B).normalized()); @@ -158,66 +144,61 @@ void Physics2DServerSW::_shape_col_cbk(const Vector2& p_point_A,const Vector2& p if (cbk->amount == cbk->max) { //find least deep - real_t min_depth=1e20; - int min_depth_idx=0; - for(int i=0;i<cbk->amount;i++) { - - real_t d = cbk->ptr[i*2+0].distance_squared_to(cbk->ptr[i*2+1]); - if (d<min_depth) { - min_depth=d; - min_depth_idx=i; + real_t min_depth = 1e20; + int min_depth_idx = 0; + for (int i = 0; i < cbk->amount; i++) { + + real_t d = cbk->ptr[i * 2 + 0].distance_squared_to(cbk->ptr[i * 2 + 1]); + if (d < min_depth) { + min_depth = d; + min_depth_idx = i; } - } real_t d = p_point_A.distance_squared_to(p_point_B); - if (d<min_depth) + if (d < min_depth) return; - cbk->ptr[min_depth_idx*2+0]=p_point_A; - cbk->ptr[min_depth_idx*2+1]=p_point_B; - + cbk->ptr[min_depth_idx * 2 + 0] = p_point_A; + cbk->ptr[min_depth_idx * 2 + 1] = p_point_B; } else { - cbk->ptr[cbk->amount*2+0]=p_point_A; - cbk->ptr[cbk->amount*2+1]=p_point_B; + cbk->ptr[cbk->amount * 2 + 0] = p_point_A; + cbk->ptr[cbk->amount * 2 + 1] = p_point_B; cbk->amount++; } } -bool Physics2DServerSW::shape_collide(RID p_shape_A, const Transform2D& p_xform_A,const Vector2& p_motion_A,RID p_shape_B, const Transform2D& p_xform_B, const Vector2& p_motion_B,Vector2 *r_results,int p_result_max,int &r_result_count) { - +bool Physics2DServerSW::shape_collide(RID p_shape_A, const Transform2D &p_xform_A, const Vector2 &p_motion_A, RID p_shape_B, const Transform2D &p_xform_B, const Vector2 &p_motion_B, Vector2 *r_results, int p_result_max, int &r_result_count) { Shape2DSW *shape_A = shape_owner.get(p_shape_A); - ERR_FAIL_COND_V(!shape_A,false); + ERR_FAIL_COND_V(!shape_A, false); Shape2DSW *shape_B = shape_owner.get(p_shape_B); - ERR_FAIL_COND_V(!shape_B,false); + ERR_FAIL_COND_V(!shape_B, false); - if (p_result_max==0) { + if (p_result_max == 0) { - return CollisionSolver2DSW::solve(shape_A,p_xform_A,p_motion_A,shape_B,p_xform_B,p_motion_B,NULL,NULL); + return CollisionSolver2DSW::solve(shape_A, p_xform_A, p_motion_A, shape_B, p_xform_B, p_motion_B, NULL, NULL); } CollCbkData cbk; - cbk.max=p_result_max; - cbk.amount=0; - cbk.ptr=r_results; + cbk.max = p_result_max; + cbk.amount = 0; + cbk.ptr = r_results; - bool res= CollisionSolver2DSW::solve(shape_A,p_xform_A,p_motion_A,shape_B,p_xform_B,p_motion_B,_shape_col_cbk,&cbk); - r_result_count=cbk.amount; + bool res = CollisionSolver2DSW::solve(shape_A, p_xform_A, p_motion_A, shape_B, p_xform_B, p_motion_B, _shape_col_cbk, &cbk); + r_result_count = cbk.amount; return res; - } - RID Physics2DServerSW::space_create() { - Space2DSW *space = memnew( Space2DSW ); + 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()); + ERR_FAIL_COND_V(!area, RID()); space->set_default_area(area); area->set_space(space); area->set_priority(-1); @@ -225,7 +206,7 @@ RID Physics2DServerSW::space_create() { return id; }; -void Physics2DServerSW::space_set_active(RID p_space,bool p_active) { +void Physics2DServerSW::space_set_active(RID p_space, bool p_active) { Space2DSW *space = space_owner.get(p_space); ERR_FAIL_COND(!space); @@ -238,56 +219,51 @@ void Physics2DServerSW::space_set_active(RID p_space,bool p_active) { bool Physics2DServerSW::space_is_active(RID p_space) const { const Space2DSW *space = space_owner.get(p_space); - ERR_FAIL_COND_V(!space,false); + 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) { +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); - + space->set_param(p_param, p_value); } -real_t Physics2DServerSW::space_get_param(RID p_space,SpaceParameter p_param) const { +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); + ERR_FAIL_COND_V(!space, 0); return space->get_param(p_param); } -void Physics2DServerSW::space_set_debug_contacts(RID p_space,int p_max_contacts) { +void Physics2DServerSW::space_set_debug_contacts(RID p_space, int p_max_contacts) { Space2DSW *space = space_owner.get(p_space); ERR_FAIL_COND(!space); space->set_debug_contacts(p_max_contacts); - } Vector<Vector2> Physics2DServerSW::space_get_contacts(RID p_space) const { Space2DSW *space = space_owner.get(p_space); - ERR_FAIL_COND_V(!space,Vector<Vector2>()); + ERR_FAIL_COND_V(!space, Vector<Vector2>()); return space->get_debug_contacts(); - } int Physics2DServerSW::space_get_contact_count(RID p_space) const { Space2DSW *space = space_owner.get(p_space); - ERR_FAIL_COND_V(!space,0); + ERR_FAIL_COND_V(!space, 0); return space->get_debug_contact_count(); - } -Physics2DDirectSpaceState* Physics2DServerSW::space_get_direct_state(RID p_space) { +Physics2DDirectSpaceState *Physics2DServerSW::space_get_direct_state(RID p_space) { Space2DSW *space = space_owner.get(p_space); - ERR_FAIL_COND_V(!space,NULL); + ERR_FAIL_COND_V(!space, NULL); if ((using_threads && !doing_sync) || space->is_locked()) { ERR_EXPLAIN("Space state is inaccesible right now, wait for iteration or fixed process notification."); @@ -299,7 +275,7 @@ Physics2DDirectSpaceState* Physics2DServerSW::space_get_direct_state(RID p_space RID Physics2DServerSW::area_create() { - Area2DSW *area = memnew( Area2DSW ); + Area2DSW *area = memnew(Area2DSW); RID rid = area_owner.make_rid(area); area->set_self(rid); return rid; @@ -309,20 +285,19 @@ 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; + 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()); + ERR_FAIL_COND_V(!area, RID()); Space2DSW *space = area->get_space(); if (!space) @@ -332,7 +307,6 @@ RID Physics2DServerSW::area_get_space(RID p_area) const { void Physics2DServerSW::area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode) { - Area2DSW *area = area_owner.get(p_area); ERR_FAIL_COND(!area); @@ -342,13 +316,12 @@ void Physics2DServerSW::area_set_space_override_mode(RID p_area, AreaSpaceOverri 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); + 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 Transform2D& p_transform) { +void Physics2DServerSW::area_add_shape(RID p_area, RID p_shape, const Transform2D &p_transform) { Area2DSW *area = area_owner.get(p_area); ERR_FAIL_COND(!area); @@ -356,11 +329,10 @@ void Physics2DServerSW::area_add_shape(RID p_area, RID p_shape, const Transform2 Shape2DSW *shape = shape_owner.get(p_shape); ERR_FAIL_COND(!shape); - area->add_shape(shape,p_transform); - + area->add_shape(shape, p_transform); } -void Physics2DServerSW::area_set_shape(RID p_area, int p_shape_idx,RID p_shape) { +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); @@ -369,39 +341,37 @@ void Physics2DServerSW::area_set_shape(RID p_area, int p_shape_idx,RID p_shape) ERR_FAIL_COND(!shape); ERR_FAIL_COND(!shape->is_configured()); - area->set_shape(p_shape_idx,shape); - + area->set_shape(p_shape_idx, shape); } -void Physics2DServerSW::area_set_shape_transform(RID p_area, int p_shape_idx, const Transform2D& p_transform) { +void Physics2DServerSW::area_set_shape_transform(RID p_area, int p_shape_idx, const Transform2D &p_transform) { Area2DSW *area = area_owner.get(p_area); ERR_FAIL_COND(!area); - area->set_shape_transform(p_shape_idx,p_transform); + 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); + 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()); + ERR_FAIL_COND_V(!area, RID()); Shape2DSW *shape = area->get_shape(p_shape_idx); - ERR_FAIL_COND_V(!shape,RID()); + ERR_FAIL_COND_V(!shape, RID()); return shape->get_self(); } Transform2D 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,Transform2D()); + ERR_FAIL_COND_V(!area, Transform2D()); return area->get_shape_transform(p_shape_idx); } @@ -419,65 +389,57 @@ void Physics2DServerSW::area_clear_shapes(RID p_area) { Area2DSW *area = area_owner.get(p_area); ERR_FAIL_COND(!area); - while(area->get_shape_count()) + while (area->get_shape_count()) area->remove_shape(0); - } -void Physics2DServerSW::area_attach_object_instance_ID(RID p_area,ObjectID p_ID) { +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(); + 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(); + 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); + 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) { +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(); + 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); - + area->set_param(p_param, p_value); }; - -void Physics2DServerSW::area_set_transform(RID p_area, const Transform2D& p_transform) { +void Physics2DServerSW::area_set_transform(RID p_area, const Transform2D &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 { +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(); + 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()); + ERR_FAIL_COND_V(!area, Variant()); return area->get_param(p_param); }; @@ -485,29 +447,27 @@ Variant Physics2DServerSW::area_get_param(RID p_area,AreaParameter p_param) cons Transform2D Physics2DServerSW::area_get_transform(RID p_area) const { Area2DSW *area = area_owner.get(p_area); - ERR_FAIL_COND_V(!area,Transform2D()); + ERR_FAIL_COND_V(!area, Transform2D()); return area->get_transform(); }; -void Physics2DServerSW::area_set_pickable(RID p_area,bool p_pickable) { +void Physics2DServerSW::area_set_pickable(RID p_area, bool p_pickable) { Area2DSW *area = area_owner.get(p_area); ERR_FAIL_COND(!area); area->set_pickable(p_pickable); - } -void Physics2DServerSW::area_set_monitorable(RID p_area,bool p_monitorable) { +void Physics2DServerSW::area_set_monitorable(RID p_area, bool p_monitorable) { Area2DSW *area = area_owner.get(p_area); ERR_FAIL_COND(!area); area->set_monitorable(p_monitorable); - } -void Physics2DServerSW::area_set_collision_mask(RID p_area,uint32_t p_mask) { +void Physics2DServerSW::area_set_collision_mask(RID p_area, uint32_t p_mask) { Area2DSW *area = area_owner.get(p_area); ERR_FAIL_COND(!area); @@ -515,7 +475,7 @@ void Physics2DServerSW::area_set_collision_mask(RID p_area,uint32_t p_mask) { area->set_collision_mask(p_mask); } -void Physics2DServerSW::area_set_layer_mask(RID p_area,uint32_t p_mask) { +void Physics2DServerSW::area_set_layer_mask(RID p_area, uint32_t p_mask) { Area2DSW *area = area_owner.get(p_area); ERR_FAIL_COND(!area); @@ -523,59 +483,53 @@ void Physics2DServerSW::area_set_layer_mask(RID p_area,uint32_t p_mask) { area->set_layer_mask(p_mask); } - -void Physics2DServerSW::area_set_monitor_callback(RID p_area,Object *p_receiver,const StringName& p_method) { +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); - - + area->set_monitor_callback(p_receiver ? p_receiver->get_instance_ID() : 0, p_method); } -void Physics2DServerSW::area_set_area_monitor_callback(RID p_area,Object *p_receiver,const StringName& p_method) { - +void Physics2DServerSW::area_set_area_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_area_monitor_callback(p_receiver?p_receiver->get_instance_ID():0,p_method); + area->set_area_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) { +RID Physics2DServerSW::body_create(BodyMode p_mode, bool p_init_sleeping) { - Body2DSW *body = memnew( Body2DSW ); - if (p_mode!=BODY_MODE_RIGID) + 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); + 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; + 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()); + ERR_FAIL_COND_V(!body, RID()); Space2DSW *space = body->get_space(); if (!space) @@ -583,7 +537,6 @@ RID Physics2DServerSW::body_get_space(RID p_body) const { return space->get_self(); }; - void Physics2DServerSW::body_set_mode(RID p_body, BodyMode p_mode) { Body2DSW *body = body_owner.get(p_body); @@ -595,12 +548,12 @@ void Physics2DServerSW::body_set_mode(RID p_body, BodyMode p_mode) { Physics2DServer::BodyMode Physics2DServerSW::body_get_mode(RID p_body) const { Body2DSW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body,BODY_MODE_STATIC); + ERR_FAIL_COND_V(!body, BODY_MODE_STATIC); return body->get_mode(); }; -void Physics2DServerSW::body_add_shape(RID p_body, RID p_shape, const Transform2D& p_transform) { +void Physics2DServerSW::body_add_shape(RID p_body, RID p_shape, const Transform2D &p_transform) { Body2DSW *body = body_owner.get(p_body); ERR_FAIL_COND(!body); @@ -608,11 +561,10 @@ void Physics2DServerSW::body_add_shape(RID p_body, RID p_shape, const Transform2 Shape2DSW *shape = shape_owner.get(p_shape); ERR_FAIL_COND(!shape); - body->add_shape(shape,p_transform); - + body->add_shape(shape, p_transform); } -void Physics2DServerSW::body_set_shape(RID p_body, int p_shape_idx,RID p_shape) { +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); @@ -621,55 +573,51 @@ void Physics2DServerSW::body_set_shape(RID p_body, int p_shape_idx,RID p_shape) ERR_FAIL_COND(!shape); ERR_FAIL_COND(!shape->is_configured()); - body->set_shape(p_shape_idx,shape); - + body->set_shape(p_shape_idx, shape); } -void Physics2DServerSW::body_set_shape_transform(RID p_body, int p_shape_idx, const Transform2D& p_transform) { +void Physics2DServerSW::body_set_shape_transform(RID p_body, int p_shape_idx, const Transform2D &p_transform) { Body2DSW *body = body_owner.get(p_body); ERR_FAIL_COND(!body); - body->set_shape_transform(p_shape_idx,p_transform); + body->set_shape_transform(p_shape_idx, p_transform); } -void Physics2DServerSW::body_set_shape_metadata(RID p_body, int p_shape_idx, const Variant& p_metadata) { +void Physics2DServerSW::body_set_shape_metadata(RID p_body, int p_shape_idx, const Variant &p_metadata) { Body2DSW *body = body_owner.get(p_body); ERR_FAIL_COND(!body); - body->set_shape_metadata(p_shape_idx,p_metadata); + body->set_shape_metadata(p_shape_idx, p_metadata); } - Variant Physics2DServerSW::body_get_shape_metadata(RID p_body, int p_shape_idx) const { Body2DSW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body,Variant()); + ERR_FAIL_COND_V(!body, Variant()); return body->get_shape_metadata(p_shape_idx); } - int Physics2DServerSW::body_get_shape_count(RID p_body) const { Body2DSW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body,-1); + 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()); + ERR_FAIL_COND_V(!body, RID()); Shape2DSW *shape = body->get_shape(p_shape_idx); - ERR_FAIL_COND_V(!shape,RID()); + ERR_FAIL_COND_V(!shape, RID()); return shape->get_self(); } Transform2D 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,Transform2D()); + ERR_FAIL_COND_V(!body, Transform2D()); return body->get_shape_transform(p_shape_idx); } @@ -687,67 +635,57 @@ void Physics2DServerSW::body_clear_shapes(RID p_body) { Body2DSW *body = body_owner.get(p_body); ERR_FAIL_COND(!body); - while(body->get_shape_count()) + 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) { +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); + 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_COND_V(!body, false); - ERR_FAIL_INDEX_V(p_shape_idx,body->get_shape_count(),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_continuous_collision_detection_mode(RID p_body,CCDMode p_mode) { +void Physics2DServerSW::body_set_continuous_collision_detection_mode(RID p_body, CCDMode p_mode) { Body2DSW *body = body_owner.get(p_body); ERR_FAIL_COND(!body); body->set_continuous_collision_detection_mode(p_mode); - } -Physics2DServerSW::CCDMode Physics2DServerSW::body_get_continuous_collision_detection_mode(RID p_body) const{ +Physics2DServerSW::CCDMode Physics2DServerSW::body_get_continuous_collision_detection_mode(RID p_body) const { const Body2DSW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body,CCD_MODE_DISABLED); + ERR_FAIL_COND_V(!body, CCD_MODE_DISABLED); return body->get_continuous_collision_detection_mode(); - } - - -void Physics2DServerSW::body_attach_object_instance_ID(RID p_body,uint32_t p_ID) { +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); + ERR_FAIL_COND_V(!body, 0); return body->get_instance_id(); }; @@ -757,30 +695,27 @@ void Physics2DServerSW::body_set_layer_mask(RID p_body, uint32_t p_flags) { Body2DSW *body = body_owner.get(p_body); ERR_FAIL_COND(!body); body->set_layer_mask(p_flags); - }; uint32_t Physics2DServerSW::body_get_layer_mask(RID p_body) const { Body2DSW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body,0); + ERR_FAIL_COND_V(!body, 0); return body->get_layer_mask(); }; - void Physics2DServerSW::body_set_collision_mask(RID p_body, uint32_t p_flags) { Body2DSW *body = body_owner.get(p_body); ERR_FAIL_COND(!body); body->set_collision_mask(p_flags); - }; uint32_t Physics2DServerSW::body_get_collision_mask(RID p_body) const { Body2DSW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body,0); + ERR_FAIL_COND_V(!body, 0); return body->get_collision_mask(); }; @@ -790,50 +725,46 @@ void Physics2DServerSW::body_set_param(RID p_body, BodyParameter p_param, real_t Body2DSW *body = body_owner.get(p_body); ERR_FAIL_COND(!body); - body->set_param(p_param,p_value); + body->set_param(p_param, p_value); }; real_t Physics2DServerSW::body_get_param(RID p_body, BodyParameter p_param) const { Body2DSW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body,0); + ERR_FAIL_COND_V(!body, 0); return body->get_param(p_param); }; - - -void Physics2DServerSW::body_set_state(RID p_body, BodyState p_state, const Variant& p_variant) { +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); + 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()); + 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) { +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); body->wakeup(); - }; Vector2 Physics2DServerSW::body_get_applied_force(RID p_body) const { Body2DSW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body,Vector2()); + ERR_FAIL_COND_V(!body, Vector2()); return body->get_applied_force(); }; @@ -849,38 +780,38 @@ void Physics2DServerSW::body_set_applied_torque(RID p_body, real_t p_torque) { real_t Physics2DServerSW::body_get_applied_torque(RID p_body) const { Body2DSW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body,0); + 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) { +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); + body->apply_impulse(p_pos, p_impulse); body->wakeup(); }; -void Physics2DServerSW::body_add_force(RID p_body, const Vector2& p_offset, const Vector2& p_force) { +void Physics2DServerSW::body_add_force(RID p_body, const Vector2 &p_offset, const Vector2 &p_force) { Body2DSW *body = body_owner.get(p_body); ERR_FAIL_COND(!body); - body->add_force(p_force,p_offset); + body->add_force(p_force, p_offset); body->wakeup(); }; -void Physics2DServerSW::body_set_axis_velocity(RID p_body, const Vector2& p_axis_velocity) { +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; + v -= axis * axis.dot(v); + v += p_axis_velocity; body->set_linear_velocity(v); body->wakeup(); }; @@ -901,7 +832,6 @@ void Physics2DServerSW::body_remove_collision_exception(RID p_body, RID p_body_b body->remove_exception(p_body_b); body->wakeup(); - }; void Physics2DServerSW::body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) { @@ -909,27 +839,25 @@ void Physics2DServerSW::body_get_collision_exceptions(RID p_body, List<RID> *p_e Body2DSW *body = body_owner.get(p_body); ERR_FAIL_COND(!body); - for(int i=0;i<body->get_exceptions().size();i++) { + 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, real_t p_treshold) { Body2DSW *body = body_owner.get(p_body); ERR_FAIL_COND(!body); - }; real_t Physics2DServerSW::body_get_contacts_reported_depth_treshold(RID p_body) const { Body2DSW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body,0); + ERR_FAIL_COND_V(!body, 0); return 0; }; -void Physics2DServerSW::body_set_omit_force_integration(RID p_body,bool p_omit) { +void Physics2DServerSW::body_set_omit_force_integration(RID p_body, bool p_omit) { Body2DSW *body = body_owner.get(p_body); ERR_FAIL_COND(!body); @@ -940,7 +868,7 @@ void Physics2DServerSW::body_set_omit_force_integration(RID p_body,bool 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); + ERR_FAIL_COND_V(!body, false); return body->get_omit_force_integration(); }; @@ -954,80 +882,71 @@ void Physics2DServerSW::body_set_max_contacts_reported(RID p_body, int p_contact int Physics2DServerSW::body_get_max_contacts_reported(RID p_body) const { Body2DSW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body,-1); + ERR_FAIL_COND_V(!body, -1); return body->get_max_contacts_reported(); } -void Physics2DServerSW::body_set_one_way_collision_direction(RID p_body,const Vector2& p_direction) { +void Physics2DServerSW::body_set_one_way_collision_direction(RID p_body, const Vector2 &p_direction) { Body2DSW *body = body_owner.get(p_body); ERR_FAIL_COND(!body); body->set_one_way_collision_direction(p_direction); } -Vector2 Physics2DServerSW::body_get_one_way_collision_direction(RID p_body) const{ +Vector2 Physics2DServerSW::body_get_one_way_collision_direction(RID p_body) const { Body2DSW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body,Vector2()); + ERR_FAIL_COND_V(!body, Vector2()); return body->get_one_way_collision_direction(); - } -void Physics2DServerSW::body_set_one_way_collision_max_depth(RID p_body,real_t p_max_depth) { +void Physics2DServerSW::body_set_one_way_collision_max_depth(RID p_body, real_t p_max_depth) { Body2DSW *body = body_owner.get(p_body); ERR_FAIL_COND(!body); body->set_one_way_collision_max_depth(p_max_depth); - } real_t Physics2DServerSW::body_get_one_way_collision_max_depth(RID p_body) const { Body2DSW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body,0); + ERR_FAIL_COND_V(!body, 0); return body->get_one_way_collision_max_depth(); - } -void Physics2DServerSW::body_set_force_integration_callback(RID p_body,Object *p_receiver,const StringName& p_method,const Variant& p_udata) { - +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); - + body->set_force_integration_callback(p_receiver ? p_receiver->get_instance_ID() : ObjectID(0), p_method, p_udata); } -bool Physics2DServerSW::body_collide_shape(RID p_body, int p_body_shape, RID p_shape, const Transform2D& p_shape_xform,const Vector2& p_motion,Vector2 *r_results,int p_result_max,int &r_result_count) { +bool Physics2DServerSW::body_collide_shape(RID p_body, int p_body_shape, RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, Vector2 *r_results, int p_result_max, int &r_result_count) { Body2DSW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body,false); - ERR_FAIL_INDEX_V(p_body_shape,body->get_shape_count(),false); - - return shape_collide(body->get_shape(p_body_shape)->get_self(),body->get_transform() * body->get_shape_transform(p_body_shape),Vector2(),p_shape,p_shape_xform,p_motion,r_results,p_result_max,r_result_count); + ERR_FAIL_COND_V(!body, false); + ERR_FAIL_INDEX_V(p_body_shape, body->get_shape_count(), false); + return shape_collide(body->get_shape(p_body_shape)->get_self(), body->get_transform() * body->get_shape_transform(p_body_shape), Vector2(), p_shape, p_shape_xform, p_motion, r_results, p_result_max, r_result_count); } -void Physics2DServerSW::body_set_pickable(RID p_body,bool p_pickable) { +void Physics2DServerSW::body_set_pickable(RID p_body, bool p_pickable) { Body2DSW *body = body_owner.get(p_body); ERR_FAIL_COND(!body); body->set_pickable(p_pickable); - } -bool Physics2DServerSW::body_test_motion(RID p_body, const Transform2D &p_from, const Vector2& p_motion, real_t p_margin, MotionResult *r_result) { +bool Physics2DServerSW::body_test_motion(RID p_body, const Transform2D &p_from, const Vector2 &p_motion, real_t p_margin, MotionResult *r_result) { Body2DSW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body,false); - ERR_FAIL_COND_V(!body->get_space(),false); - ERR_FAIL_COND_V(body->get_space()->is_locked(),false); - - return body->get_space()->test_body_motion(body,p_from,p_motion,p_margin,r_result); + ERR_FAIL_COND_V(!body, false); + ERR_FAIL_COND_V(!body->get_space(), false); + ERR_FAIL_COND_V(body->get_space()->is_locked(), false); + return body->get_space()->test_body_motion(body, p_from, p_motion, p_margin, r_result); } - /* JOINT API */ void Physics2DServerSW::joint_set_param(RID p_joint, JointParam p_param, real_t p_value) { @@ -1035,21 +954,19 @@ void Physics2DServerSW::joint_set_param(RID p_joint, JointParam p_param, real_t Joint2DSW *joint = joint_owner.get(p_joint); ERR_FAIL_COND(!joint); - switch(p_param) { + 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 { +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); + ERR_FAIL_COND_V(!joint, -1); - switch(p_param) { + 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; @@ -1058,115 +975,106 @@ real_t Physics2DServerSW::joint_get_param(RID p_joint,JointParam p_param) const return 0; } +RID Physics2DServerSW::pin_joint_create(const Vector2 &p_pos, RID p_body_a, RID p_body_b) { -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; + 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()); + B = body_owner.get(p_body_b); + ERR_FAIL_COND_V(!B, RID()); } - Joint2DSW *joint = memnew( PinJoint2DSW(p_pos,A,B) ); + 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) { - +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 *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()); + 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) ); + 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) { +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 *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()); + 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) ); + 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::pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value) { Joint2DSW *j = joint_owner.get(p_joint); ERR_FAIL_COND(!j); - ERR_FAIL_COND(j->get_type()!=JOINT_PIN); + ERR_FAIL_COND(j->get_type() != JOINT_PIN); - PinJoint2DSW *pin_joint = static_cast<PinJoint2DSW*>(j); + PinJoint2DSW *pin_joint = static_cast<PinJoint2DSW *>(j); pin_joint->set_param(p_param, p_value); } real_t Physics2DServerSW::pin_joint_get_param(RID p_joint, PinJointParam p_param) const { Joint2DSW *j = joint_owner.get(p_joint); - ERR_FAIL_COND_V(!j,0); - ERR_FAIL_COND_V(j->get_type()!=JOINT_PIN,0); + ERR_FAIL_COND_V(!j, 0); + ERR_FAIL_COND_V(j->get_type() != JOINT_PIN, 0); - PinJoint2DSW *pin_joint = static_cast<PinJoint2DSW*>(j); + PinJoint2DSW *pin_joint = static_cast<PinJoint2DSW *>(j); return pin_joint->get_param(p_param); } 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); + ERR_FAIL_COND(j->get_type() != JOINT_DAMPED_SPRING); - DampedSpringJoint2DSW *dsj = static_cast<DampedSpringJoint2DSW*>(j); - dsj->set_param(p_param,p_value); + 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); + ERR_FAIL_COND_V(!j, 0); + ERR_FAIL_COND_V(j->get_type() != JOINT_DAMPED_SPRING, 0); - DampedSpringJoint2DSW *dsj = static_cast<DampedSpringJoint2DSW*>(j); + 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); + 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(); + while (shape->get_owners().size()) { + ShapeOwner2DSW *so = shape->get_owners().front()->key(); so->remove_shape(shape); } @@ -1186,8 +1094,7 @@ void Physics2DServerSW::free(RID p_rid) { body->set_space(NULL); - - while( body->get_shape_count() ) { + while (body->get_shape_count()) { body->remove_shape(0); } @@ -1212,7 +1119,7 @@ void Physics2DServerSW::free(RID p_rid) { area->set_space(NULL); - while( area->get_shape_count() ) { + while (area->get_shape_count()) { area->remove_shape(0); } @@ -1223,7 +1130,7 @@ void Physics2DServerSW::free(RID p_rid) { Space2DSW *space = space_owner.get(p_rid); - while(space->get_objects().size()) { + while (space->get_objects().size()) { CollisionObject2DSW *co = (CollisionObject2DSW *)space->get_objects().front()->get(); co->set_space(NULL); } @@ -1244,52 +1151,46 @@ void Physics2DServerSW::free(RID p_rid) { ERR_EXPLAIN("Invalid ID"); ERR_FAIL(); } - - }; void Physics2DServerSW::set_active(bool p_active) { - active=p_active; + active = p_active; }; void Physics2DServerSW::init() { - doing_sync=false; - last_step=0.001; - iterations=8;// 8? - stepper = memnew( Step2DSW ); - direct_state = memnew( Physics2DDirectBodyStateSW ); + doing_sync = false; + last_step = 0.001; + iterations = 8; // 8? + stepper = memnew(Step2DSW); + direct_state = memnew(Physics2DDirectBodyStateSW); }; - void Physics2DServerSW::step(real_t p_step) { - if (!active) return; - doing_sync=false; + doing_sync = false; - last_step=p_step; - Physics2DDirectBodyStateSW::singleton->step=p_step; - island_count=0; - active_objects=0; - collision_pairs=0; - for( Set<const Space2DSW*>::Element *E=active_spaces.front();E;E=E->next()) { + last_step = p_step; + Physics2DDirectBodyStateSW::singleton->step = p_step; + island_count = 0; + active_objects = 0; + collision_pairs = 0; + for (Set<const Space2DSW *>::Element *E = active_spaces.front(); E; E = E->next()) { - stepper->step((Space2DSW*)E->get(),p_step,iterations); - island_count+=E->get()->get_island_count(); - active_objects+=E->get()->get_active_objects(); - collision_pairs+=E->get()->get_collision_pairs(); + stepper->step((Space2DSW *)E->get(), p_step, iterations); + island_count += E->get()->get_island_count(); + active_objects += E->get()->get_active_objects(); + collision_pairs += E->get()->get_collision_pairs(); } - - }; void Physics2DServerSW::sync() { - doing_sync=true; + doing_sync = true; }; void Physics2DServerSW::flush_queries() { @@ -1299,17 +1200,16 @@ void Physics2DServerSW::flush_queries() { uint64_t time_beg = OS::get_singleton()->get_ticks_usec(); - for( Set<const Space2DSW*>::Element *E=active_spaces.front();E;E=E->next()) { + for (Set<const Space2DSW *>::Element *E = active_spaces.front(); E; E = E->next()) { - Space2DSW *space=(Space2DSW *)E->get(); + Space2DSW *space = (Space2DSW *)E->get(); space->call_queries(); } - if (ScriptDebugger::get_singleton() && ScriptDebugger::get_singleton()->is_profiling()) { uint64_t total_time[Space2DSW::ELAPSED_TIME_MAX]; - static const char* time_name[Space2DSW::ELAPSED_TIME_MAX]={ + static const char *time_name[Space2DSW::ELAPSED_TIME_MAX] = { "integrate_forces", "generate_islands", "setup_constraints", @@ -1317,39 +1217,34 @@ void Physics2DServerSW::flush_queries() { "integrate_velocities" }; - for(int i=0;i<Space2DSW::ELAPSED_TIME_MAX;i++) { - total_time[i]=0; + for (int i = 0; i < Space2DSW::ELAPSED_TIME_MAX; i++) { + total_time[i] = 0; } - for( Set<const Space2DSW*>::Element *E=active_spaces.front();E;E=E->next()) { + for (Set<const Space2DSW *>::Element *E = active_spaces.front(); E; E = E->next()) { - for(int i=0;i<Space2DSW::ELAPSED_TIME_MAX;i++) { - total_time[i]+=E->get()->get_elapsed_time(Space2DSW::ElapsedTime(i)); + for (int i = 0; i < Space2DSW::ELAPSED_TIME_MAX; i++) { + total_time[i] += E->get()->get_elapsed_time(Space2DSW::ElapsedTime(i)); } - } Array values; - values.resize(Space2DSW::ELAPSED_TIME_MAX*2); - for(int i=0;i<Space2DSW::ELAPSED_TIME_MAX;i++) { - values[i*2+0]=time_name[i]; - values[i*2+1]=USEC_TO_SEC(total_time[i]); + values.resize(Space2DSW::ELAPSED_TIME_MAX * 2); + for (int i = 0; i < Space2DSW::ELAPSED_TIME_MAX; i++) { + values[i * 2 + 0] = time_name[i]; + values[i * 2 + 1] = USEC_TO_SEC(total_time[i]); } values.push_back("flush_queries"); - values.push_back(USEC_TO_SEC(OS::get_singleton()->get_ticks_usec()-time_beg)); - - ScriptDebugger::get_singleton()->add_profiling_frame_data("physics_2d",values); + values.push_back(USEC_TO_SEC(OS::get_singleton()->get_ticks_usec() - time_beg)); + ScriptDebugger::get_singleton()->add_profiling_frame_data("physics_2d", values); } - } void Physics2DServerSW::end_sync() { - doing_sync=false; + doing_sync = false; } - - void Physics2DServerSW::finish() { memdelete(stepper); @@ -1358,7 +1253,7 @@ void Physics2DServerSW::finish() { int Physics2DServerSW::get_process_info(ProcessInfo p_info) { - switch(p_info) { + switch (p_info) { case INFO_ACTIVE_OBJECTS: { @@ -1371,32 +1266,26 @@ int Physics2DServerSW::get_process_info(ProcessInfo p_info) { return island_count; } break; - } return 0; } - -Physics2DServerSW *Physics2DServerSW::singletonsw=NULL; +Physics2DServerSW *Physics2DServerSW::singletonsw = NULL; Physics2DServerSW::Physics2DServerSW() { - singletonsw=this; - BroadPhase2DSW::create_func=BroadPhase2DHashGrid::_create; + singletonsw = this; + BroadPhase2DSW::create_func = BroadPhase2DHashGrid::_create; //BroadPhase2DSW::create_func=BroadPhase2DBasic::_create; - active=true; - island_count=0; - active_objects=0; - collision_pairs=0; - using_threads=int(GlobalConfig::get_singleton()->get("physics/2d/thread_model"))==2; - + active = true; + island_count = 0; + active_objects = 0; + collision_pairs = 0; + using_threads = int(GlobalConfig::get_singleton()->get("physics/2d/thread_model")) == 2; }; - -Physics2DServerSW::~Physics2DServerSW() { +Physics2DServerSW::~Physics2DServerSW(){ }; - - diff --git a/servers/physics_2d/physics_2d_server_sw.h b/servers/physics_2d/physics_2d_server_sw.h index e33e2d78c7..b49c37ab21 100644 --- a/servers/physics_2d/physics_2d_server_sw.h +++ b/servers/physics_2d/physics_2d_server_sw.h @@ -29,20 +29,18 @@ #ifndef PHYSICS_2D_SERVER_SW #define PHYSICS_2D_SERVER_SW - +#include "joints_2d_sw.h" #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 { - GDCLASS( Physics2DServerSW, Physics2DServer ); + GDCLASS(Physics2DServerSW, Physics2DServer); -friend class Physics2DDirectSpaceStateSW; -friend class Physics2DDirectBodyStateSW; + friend class Physics2DDirectSpaceStateSW; + friend class Physics2DDirectBodyStateSW; bool active; int iterations; bool doing_sync; @@ -54,10 +52,8 @@ friend class Physics2DDirectBodyStateSW; bool using_threads; - - Step2DSW *stepper; - Set<const Space2DSW*> active_spaces; + Set<const Space2DSW *> active_spaces; Physics2DDirectBodyStateSW *direct_state; @@ -69,10 +65,8 @@ friend class Physics2DDirectBodyStateSW; static Physics2DServerSW *singletonsw; - //void _clear_query(Query2DSW *p_query); public: - struct CollCbkData { Vector2 valid_dir; @@ -82,36 +76,33 @@ public: Vector2 *ptr; }; - static void _shape_col_cbk(const Vector2& p_point_A,const Vector2& p_point_B,void *p_userdata); - + static void _shape_col_cbk(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_userdata); virtual RID shape_create(ShapeType p_shape); - virtual void shape_set_data(RID p_shape, const Variant& p_data); + virtual void shape_set_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; - virtual bool shape_collide(RID p_shape_A, const Transform2D& p_xform_A,const Vector2& p_motion_A,RID p_shape_B, const Transform2D& p_xform_B, const Vector2& p_motion_B,Vector2 *r_results,int p_result_max,int &r_result_count); + virtual bool shape_collide(RID p_shape_A, const Transform2D &p_xform_A, const Vector2 &p_motion_A, RID p_shape_B, const Transform2D &p_xform_B, const Vector2 &p_motion_B, Vector2 *r_results, int p_result_max, int &r_result_count); /* SPACE API */ virtual RID space_create(); - virtual void space_set_active(RID p_space,bool p_active); + 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; + virtual void space_set_param(RID p_space, SpaceParameter p_param, real_t p_value); + virtual real_t space_get_param(RID p_space, SpaceParameter p_param) const; - virtual void space_set_debug_contacts(RID p_space,int p_max_contacts); + virtual void space_set_debug_contacts(RID p_space, int p_max_contacts); virtual Vector<Vector2> space_get_contacts(RID p_space) const; virtual int space_get_contact_count(RID p_space) const; - // this function only works on fixed process, errors and returns null otherwise - virtual Physics2DDirectSpaceState* space_get_direct_state(RID p_space); - + virtual Physics2DDirectSpaceState *space_get_direct_state(RID p_space); /* AREA API */ @@ -123,9 +114,9 @@ public: 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 Transform2D& p_transform=Transform2D()); - 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 Transform2D& p_transform); + virtual void area_add_shape(RID p_area, RID p_shape, const Transform2D &p_transform = Transform2D()); + 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 Transform2D &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; @@ -134,28 +125,27 @@ public: 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 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 Transform2D& p_transform); + virtual void area_set_param(RID p_area, AreaParameter p_param, const Variant &p_value); + virtual void area_set_transform(RID p_area, const Transform2D &p_transform); - virtual Variant area_get_param(RID p_parea,AreaParameter p_param) const; + virtual Variant area_get_param(RID p_parea, AreaParameter p_param) const; virtual Transform2D area_get_transform(RID p_area) const; - virtual void area_set_monitorable(RID p_area,bool p_monitorable); - virtual void area_set_collision_mask(RID p_area,uint32_t p_mask); - virtual void area_set_layer_mask(RID p_area,uint32_t p_mask); - - virtual void area_set_monitor_callback(RID p_area,Object *p_receiver,const StringName& p_method); - virtual void area_set_area_monitor_callback(RID p_area,Object *p_receiver,const StringName& p_method); + virtual void area_set_monitorable(RID p_area, bool p_monitorable); + virtual void area_set_collision_mask(RID p_area, uint32_t p_mask); + virtual void area_set_layer_mask(RID p_area, uint32_t p_mask); - virtual void area_set_pickable(RID p_area,bool p_pickable); + virtual void area_set_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method); + virtual void area_set_area_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method); + virtual void area_set_pickable(RID p_area, bool p_pickable); /* BODY API */ // create a body of a given type - virtual RID body_create(BodyMode p_mode=BODY_MODE_RIGID,bool p_init_sleeping=false); + virtual RID body_create(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; @@ -163,28 +153,26 @@ public: virtual void body_set_mode(RID p_body, BodyMode p_mode); virtual BodyMode body_get_mode(RID p_body) const; - virtual void body_add_shape(RID p_body, RID p_shape, const Transform2D& p_transform=Transform2D()); - 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 Transform2D& p_transform); - virtual void body_set_shape_metadata(RID p_body, int p_shape_idx, const Variant& p_metadata); - + virtual void body_add_shape(RID p_body, RID p_shape, const Transform2D &p_transform = Transform2D()); + 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 Transform2D &p_transform); + virtual void body_set_shape_metadata(RID p_body, int p_shape_idx, const Variant &p_metadata); virtual int body_get_shape_count(RID p_body) const; virtual RID body_get_shape(RID p_body, int p_shape_idx) const; virtual Transform2D body_get_shape_transform(RID p_body, int p_shape_idx) const; virtual Variant body_get_shape_metadata(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 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 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_continuous_collision_detection_mode(RID p_body,CCDMode p_mode); + virtual void body_set_continuous_collision_detection_mode(RID p_body, CCDMode p_mode); virtual CCDMode body_get_continuous_collision_detection_mode(RID p_body) const; virtual void body_set_layer_mask(RID p_body, uint32_t p_mask); @@ -196,20 +184,19 @@ public: virtual void body_set_param(RID p_body, BodyParameter p_param, real_t p_value); virtual real_t body_get_param(RID p_body, BodyParameter p_param) const; - - virtual void body_set_state(RID p_body, BodyState p_state, const Variant& p_variant); + 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 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, real_t p_torque); virtual real_t body_get_applied_torque(RID p_body) const; - virtual void body_add_force(RID p_body, const Vector2& p_offset, const Vector2& p_force); + virtual void body_add_force(RID p_body, const Vector2 &p_offset, const Vector2 &p_force); - 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_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); @@ -218,35 +205,33 @@ public: virtual void body_set_contacts_reported_depth_treshold(RID p_body, real_t p_treshold); virtual real_t body_get_contacts_reported_depth_treshold(RID p_body) const; - virtual void body_set_omit_force_integration(RID p_body,bool p_omit); + 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_one_way_collision_direction(RID p_body,const Vector2& p_direction); + virtual void body_set_one_way_collision_direction(RID p_body, const Vector2 &p_direction); virtual Vector2 body_get_one_way_collision_direction(RID p_body) const; - virtual void body_set_one_way_collision_max_depth(RID p_body,real_t p_max_depth); + virtual void body_set_one_way_collision_max_depth(RID p_body, real_t p_max_depth); virtual real_t body_get_one_way_collision_max_depth(RID p_body) const; + virtual void body_set_force_integration_callback(RID p_body, Object *p_receiver, const StringName &p_method, const Variant &p_udata = Variant()); + virtual bool body_collide_shape(RID p_body, int p_body_shape, RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, Vector2 *r_results, int p_result_max, int &r_result_count); - virtual void body_set_force_integration_callback(RID p_body,Object *p_receiver,const StringName& p_method,const Variant& p_udata=Variant()); - virtual bool body_collide_shape(RID p_body, int p_body_shape,RID p_shape, const Transform2D& p_shape_xform,const Vector2& p_motion,Vector2 *r_results,int p_result_max,int &r_result_count); - - virtual void body_set_pickable(RID p_body,bool p_pickable); - - virtual bool body_test_motion(RID p_body,const Transform2D& p_from,const Vector2& p_motion,real_t p_margin=0.001,MotionResult *r_result=NULL); + virtual void body_set_pickable(RID p_body, bool p_pickable); + virtual bool body_test_motion(RID p_body, const Transform2D &p_from, const Vector2 &p_motion, real_t p_margin = 0.001, MotionResult *r_result = NULL); /* 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 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 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 pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value); virtual real_t pin_joint_get_param(RID p_joint, PinJointParam p_param) const; virtual void damped_string_joint_set_param(RID p_joint, DampedStringParam p_param, real_t p_value); @@ -270,8 +255,6 @@ public: Physics2DServerSW(); ~Physics2DServerSW(); - }; #endif - diff --git a/servers/physics_2d/physics_2d_server_wrap_mt.cpp b/servers/physics_2d/physics_2d_server_wrap_mt.cpp index 34ba149d23..ef1a5b333c 100644 --- a/servers/physics_2d/physics_2d_server_wrap_mt.cpp +++ b/servers/physics_2d/physics_2d_server_wrap_mt.cpp @@ -32,35 +32,33 @@ void Physics2DServerWrapMT::thread_exit() { - exit=true; + exit = true; } void Physics2DServerWrapMT::thread_step(real_t p_delta) { physics_2d_server->step(p_delta); step_sem->post(); - } void Physics2DServerWrapMT::_thread_callback(void *_instance) { - Physics2DServerWrapMT *vsmt = reinterpret_cast<Physics2DServerWrapMT*>(_instance); - + Physics2DServerWrapMT *vsmt = reinterpret_cast<Physics2DServerWrapMT *>(_instance); vsmt->thread_loop(); } void Physics2DServerWrapMT::thread_loop() { - server_thread=Thread::get_caller_ID(); + server_thread = Thread::get_caller_ID(); OS::get_singleton()->make_rendering_thread(); physics_2d_server->init(); - exit=false; - step_thread_up=true; - while(!exit) { + exit = false; + step_thread_up = true; + while (!exit) { // flush commands one by one, until exit is requested command_queue.wait_and_flush_one(); } @@ -68,18 +66,15 @@ void Physics2DServerWrapMT::thread_loop() { command_queue.flush_all(); // flush all physics_2d_server->finish(); - } - /* EVENT QUEUING */ - void Physics2DServerWrapMT::step(real_t p_step) { if (create_thread) { - command_queue.push( this, &Physics2DServerWrapMT::thread_step,p_step); + command_queue.push(this, &Physics2DServerWrapMT::thread_step, p_step); } else { command_queue.flush_all(); //flush all pending from other threads @@ -91,14 +86,14 @@ void Physics2DServerWrapMT::sync() { if (step_sem) { if (first_frame) - first_frame=false; + first_frame = false; else step_sem->wait(); //must not wait if a step was not issued } physics_2d_server->sync(); } -void Physics2DServerWrapMT::flush_queries(){ +void Physics2DServerWrapMT::flush_queries() { physics_2d_server->flush_queries(); } @@ -116,10 +111,10 @@ void Physics2DServerWrapMT::init() { print_line("CREATING PHYSICS 2D THREAD"); //OS::get_singleton()->release_rendering_thread(); if (create_thread) { - thread = Thread::create( _thread_callback, this ); + thread = Thread::create(_thread_callback, this); print_line("STARTING PHYISICS 2D THREAD"); } - while(!step_thread_up) { + while (!step_thread_up) { OS::get_singleton()->delay_usec(1000); } print_line("DONE PHYSICS 2D THREAD"); @@ -127,19 +122,17 @@ void Physics2DServerWrapMT::init() { physics_2d_server->init(); } - } void Physics2DServerWrapMT::finish() { - if (thread) { - command_queue.push( this, &Physics2DServerWrapMT::thread_exit); - Thread::wait_to_finish( thread ); + command_queue.push(this, &Physics2DServerWrapMT::thread_exit); + Thread::wait_to_finish(thread); memdelete(thread); -/* + /* shape_free_cached_ids(); area_free_cached_ids(); body_free_cached_ids(); @@ -147,51 +140,46 @@ void Physics2DServerWrapMT::finish() { groove_joint_free_cached_ids(); damped_string_free_cached_ids(); */ - thread=NULL; + thread = NULL; } else { physics_2d_server->finish(); } if (step_sem) memdelete(step_sem); - } +Physics2DServerWrapMT::Physics2DServerWrapMT(Physics2DServer *p_contained, bool p_create_thread) + : command_queue(p_create_thread) { -Physics2DServerWrapMT::Physics2DServerWrapMT(Physics2DServer* p_contained,bool p_create_thread) : command_queue(p_create_thread) { - - physics_2d_server=p_contained; - create_thread=p_create_thread; - thread=NULL; - step_sem=NULL; - step_pending=0; - step_thread_up=false; - alloc_mutex=Mutex::create(); + physics_2d_server = p_contained; + create_thread = p_create_thread; + thread = NULL; + step_sem = NULL; + step_pending = 0; + step_thread_up = false; + alloc_mutex = Mutex::create(); - shape_pool_max_size=GLOBAL_GET("memory/multithread/thread_rid_pool_prealloc"); - area_pool_max_size=GLOBAL_GET("memory/multithread/thread_rid_pool_prealloc"); - body_pool_max_size=GLOBAL_GET("memory/multithread/thread_rid_pool_prealloc"); - pin_joint_pool_max_size=GLOBAL_GET("memory/multithread/thread_rid_pool_prealloc"); - groove_joint_pool_max_size=GLOBAL_GET("memory/multithread/thread_rid_pool_prealloc"); - damped_spring_joint_pool_max_size=GLOBAL_GET("memory/multithread/thread_rid_pool_prealloc"); + shape_pool_max_size = GLOBAL_GET("memory/multithread/thread_rid_pool_prealloc"); + area_pool_max_size = GLOBAL_GET("memory/multithread/thread_rid_pool_prealloc"); + body_pool_max_size = GLOBAL_GET("memory/multithread/thread_rid_pool_prealloc"); + pin_joint_pool_max_size = GLOBAL_GET("memory/multithread/thread_rid_pool_prealloc"); + groove_joint_pool_max_size = GLOBAL_GET("memory/multithread/thread_rid_pool_prealloc"); + damped_spring_joint_pool_max_size = GLOBAL_GET("memory/multithread/thread_rid_pool_prealloc"); if (!p_create_thread) { - server_thread=Thread::get_caller_ID(); + server_thread = Thread::get_caller_ID(); } else { - server_thread=0; + server_thread = 0; } main_thread = Thread::get_caller_ID(); - first_frame=true; + first_frame = true; } - Physics2DServerWrapMT::~Physics2DServerWrapMT() { memdelete(physics_2d_server); memdelete(alloc_mutex); //finish(); - } - - diff --git a/servers/physics_2d/physics_2d_server_wrap_mt.h b/servers/physics_2d/physics_2d_server_wrap_mt.h index f8b533080e..ff30b2df09 100644 --- a/servers/physics_2d/physics_2d_server_wrap_mt.h +++ b/servers/physics_2d/physics_2d_server_wrap_mt.h @@ -29,19 +29,17 @@ #ifndef PHYSICS2DSERVERWRAPMT_H #define PHYSICS2DSERVERWRAPMT_H - -#include "servers/physics_2d_server.h" #include "command_queue_mt.h" -#include "os/thread.h" #include "global_config.h" +#include "os/thread.h" +#include "servers/physics_2d_server.h" #ifdef DEBUG_SYNC -#define SYNC_DEBUG print_line("sync on: "+String(__FUNCTION__)); +#define SYNC_DEBUG print_line("sync on: " + String(__FUNCTION__)); #else #define SYNC_DEBUG #endif - class Physics2DServerWrapMT : public Physics2DServer { mutable Physics2DServer *physics_2d_server; @@ -65,7 +63,7 @@ class Physics2DServerWrapMT : public Physics2DServer { void thread_exit(); - Mutex*alloc_mutex; + Mutex *alloc_mutex; bool first_frame; int shape_pool_max_size; @@ -81,226 +79,209 @@ class Physics2DServerWrapMT : public Physics2DServer { int damped_spring_joint_pool_max_size; List<RID> damped_spring_joint_id_pool; - public: - #define ServerName Physics2DServer #define ServerNameWrapMT Physics2DServerWrapMT #define server_name physics_2d_server #include "servers/server_wrap_mt_common.h" //FUNC1RID(shape,ShapeType); todo fix - FUNC1R(RID,shape_create,ShapeType); - FUNC2(shape_set_data,RID,const Variant& ); - FUNC2(shape_set_custom_solver_bias,RID,real_t ); - - FUNC1RC(ShapeType,shape_get_type,RID ); - FUNC1RC(Variant,shape_get_data,RID); - FUNC1RC(real_t,shape_get_custom_solver_bias,RID); + FUNC1R(RID, shape_create, ShapeType); + FUNC2(shape_set_data, RID, const Variant &); + FUNC2(shape_set_custom_solver_bias, RID, real_t); + FUNC1RC(ShapeType, shape_get_type, RID); + FUNC1RC(Variant, shape_get_data, RID); + FUNC1RC(real_t, shape_get_custom_solver_bias, RID); //these work well, but should be used from the main thread only - bool shape_collide(RID p_shape_A, const Transform2D& p_xform_A,const Vector2& p_motion_A,RID p_shape_B, const Transform2D& p_xform_B, const Vector2& p_motion_B,Vector2 *r_results,int p_result_max,int &r_result_count) { + bool shape_collide(RID p_shape_A, const Transform2D &p_xform_A, const Vector2 &p_motion_A, RID p_shape_B, const Transform2D &p_xform_B, const Vector2 &p_motion_B, Vector2 *r_results, int p_result_max, int &r_result_count) { - ERR_FAIL_COND_V(main_thread!=Thread::get_caller_ID(),false); - return physics_2d_server->shape_collide(p_shape_A,p_xform_A,p_motion_A,p_shape_B,p_xform_B,p_motion_B,r_results,p_result_max,r_result_count); + ERR_FAIL_COND_V(main_thread != Thread::get_caller_ID(), false); + return physics_2d_server->shape_collide(p_shape_A, p_xform_A, p_motion_A, p_shape_B, p_xform_B, p_motion_B, r_results, p_result_max, r_result_count); } /* SPACE API */ - FUNC0R(RID,space_create); - FUNC2(space_set_active,RID,bool); - FUNC1RC(bool,space_is_active,RID); + FUNC0R(RID, space_create); + FUNC2(space_set_active, RID, bool); + FUNC1RC(bool, space_is_active, RID); - FUNC3(space_set_param,RID,SpaceParameter,real_t); - FUNC2RC(real_t,space_get_param,RID,SpaceParameter); + FUNC3(space_set_param, RID, SpaceParameter, real_t); + FUNC2RC(real_t, space_get_param, RID, SpaceParameter); // this function only works on fixed process, errors and returns null otherwise - Physics2DDirectSpaceState* space_get_direct_state(RID p_space) { + Physics2DDirectSpaceState *space_get_direct_state(RID p_space) { - ERR_FAIL_COND_V(main_thread!=Thread::get_caller_ID(),NULL); + ERR_FAIL_COND_V(main_thread != Thread::get_caller_ID(), NULL); return physics_2d_server->space_get_direct_state(p_space); } - FUNC2(space_set_debug_contacts,RID,int); + FUNC2(space_set_debug_contacts, RID, int); virtual Vector<Vector2> space_get_contacts(RID p_space) const { - ERR_FAIL_COND_V(main_thread!=Thread::get_caller_ID(),Vector<Vector2>()); + ERR_FAIL_COND_V(main_thread != Thread::get_caller_ID(), Vector<Vector2>()); return physics_2d_server->space_get_contacts(p_space); - } virtual int space_get_contact_count(RID p_space) const { - ERR_FAIL_COND_V(main_thread!=Thread::get_caller_ID(),0); + ERR_FAIL_COND_V(main_thread != Thread::get_caller_ID(), 0); return physics_2d_server->space_get_contact_count(p_space); - } - - /* AREA API */ //FUNC0RID(area); - FUNC0R(RID,area_create); - - FUNC2(area_set_space,RID,RID); - FUNC1RC(RID,area_get_space,RID); + FUNC0R(RID, area_create); - FUNC2(area_set_space_override_mode,RID,AreaSpaceOverrideMode); - FUNC1RC(AreaSpaceOverrideMode,area_get_space_override_mode,RID); + FUNC2(area_set_space, RID, RID); + FUNC1RC(RID, area_get_space, RID); - FUNC3(area_add_shape,RID,RID,const Transform2D&); - FUNC3(area_set_shape,RID,int,RID); - FUNC3(area_set_shape_transform,RID,int,const Transform2D&); + FUNC2(area_set_space_override_mode, RID, AreaSpaceOverrideMode); + FUNC1RC(AreaSpaceOverrideMode, area_get_space_override_mode, RID); - FUNC1RC(int,area_get_shape_count,RID); - FUNC2RC(RID,area_get_shape,RID,int); - FUNC2RC(Transform2D,area_get_shape_transform,RID,int); - FUNC2(area_remove_shape,RID,int); - FUNC1(area_clear_shapes,RID); + FUNC3(area_add_shape, RID, RID, const Transform2D &); + FUNC3(area_set_shape, RID, int, RID); + FUNC3(area_set_shape_transform, RID, int, const Transform2D &); - FUNC2(area_attach_object_instance_ID,RID,ObjectID); - FUNC1RC(ObjectID,area_get_object_instance_ID,RID); + FUNC1RC(int, area_get_shape_count, RID); + FUNC2RC(RID, area_get_shape, RID, int); + FUNC2RC(Transform2D, area_get_shape_transform, RID, int); + FUNC2(area_remove_shape, RID, int); + FUNC1(area_clear_shapes, RID); - FUNC3(area_set_param,RID,AreaParameter,const Variant&); - FUNC2(area_set_transform,RID,const Transform2D&); + FUNC2(area_attach_object_instance_ID, RID, ObjectID); + FUNC1RC(ObjectID, area_get_object_instance_ID, RID); - FUNC2RC(Variant,area_get_param,RID,AreaParameter); - FUNC1RC(Transform2D,area_get_transform,RID); + FUNC3(area_set_param, RID, AreaParameter, const Variant &); + FUNC2(area_set_transform, RID, const Transform2D &); - FUNC2(area_set_collision_mask,RID,uint32_t); - FUNC2(area_set_layer_mask,RID,uint32_t); + FUNC2RC(Variant, area_get_param, RID, AreaParameter); + FUNC1RC(Transform2D, area_get_transform, RID); - FUNC2(area_set_monitorable,RID,bool); - FUNC2(area_set_pickable,RID,bool); + FUNC2(area_set_collision_mask, RID, uint32_t); + FUNC2(area_set_layer_mask, RID, uint32_t); - FUNC3(area_set_monitor_callback,RID,Object*,const StringName&); - FUNC3(area_set_area_monitor_callback,RID,Object*,const StringName&); + FUNC2(area_set_monitorable, RID, bool); + FUNC2(area_set_pickable, RID, bool); + FUNC3(area_set_monitor_callback, RID, Object *, const StringName &); + FUNC3(area_set_area_monitor_callback, RID, Object *, const StringName &); /* BODY API */ //FUNC2RID(body,BodyMode,bool); - FUNC2R(RID,body_create,BodyMode,bool) + FUNC2R(RID, body_create, BodyMode, bool) - FUNC2(body_set_space,RID,RID); - FUNC1RC(RID,body_get_space,RID); + FUNC2(body_set_space, RID, RID); + FUNC1RC(RID, body_get_space, RID); - FUNC2(body_set_mode,RID,BodyMode); - FUNC1RC(BodyMode,body_get_mode,RID); + FUNC2(body_set_mode, RID, BodyMode); + FUNC1RC(BodyMode, body_get_mode, RID); + FUNC3(body_add_shape, RID, RID, const Transform2D &); + FUNC3(body_set_shape, RID, int, RID); + FUNC3(body_set_shape_transform, RID, int, const Transform2D &); + FUNC3(body_set_shape_metadata, RID, int, const Variant &); - FUNC3(body_add_shape,RID,RID,const Transform2D&); - FUNC3(body_set_shape,RID,int,RID); - FUNC3(body_set_shape_transform,RID,int,const Transform2D&); - FUNC3(body_set_shape_metadata,RID,int,const Variant&); + FUNC1RC(int, body_get_shape_count, RID); + FUNC2RC(Transform2D, body_get_shape_transform, RID, int); + FUNC2RC(Variant, body_get_shape_metadata, RID, int); + FUNC2RC(RID, body_get_shape, RID, int); - FUNC1RC(int,body_get_shape_count,RID); - FUNC2RC(Transform2D,body_get_shape_transform,RID,int); - FUNC2RC(Variant,body_get_shape_metadata,RID,int); - FUNC2RC(RID,body_get_shape,RID,int); + FUNC3(body_set_shape_as_trigger, RID, int, bool); + FUNC2RC(bool, body_is_shape_set_as_trigger, RID, int); - FUNC3(body_set_shape_as_trigger,RID,int,bool); - FUNC2RC(bool,body_is_shape_set_as_trigger,RID,int); + FUNC2(body_remove_shape, RID, int); + FUNC1(body_clear_shapes, RID); - FUNC2(body_remove_shape,RID,int); - FUNC1(body_clear_shapes,RID); + FUNC2(body_attach_object_instance_ID, RID, uint32_t); + FUNC1RC(uint32_t, body_get_object_instance_ID, RID); - FUNC2(body_attach_object_instance_ID,RID,uint32_t); - FUNC1RC(uint32_t,body_get_object_instance_ID,RID); + FUNC2(body_set_continuous_collision_detection_mode, RID, CCDMode); + FUNC1RC(CCDMode, body_get_continuous_collision_detection_mode, RID); - FUNC2(body_set_continuous_collision_detection_mode,RID,CCDMode); - FUNC1RC(CCDMode,body_get_continuous_collision_detection_mode,RID); + FUNC2(body_set_layer_mask, RID, uint32_t); + FUNC1RC(uint32_t, body_get_layer_mask, RID); - FUNC2(body_set_layer_mask,RID,uint32_t); - FUNC1RC(uint32_t,body_get_layer_mask,RID); + FUNC2(body_set_collision_mask, RID, uint32_t); + FUNC1RC(uint32_t, body_get_collision_mask, RID); - FUNC2(body_set_collision_mask,RID,uint32_t); - FUNC1RC(uint32_t,body_get_collision_mask,RID); + FUNC3(body_set_param, RID, BodyParameter, real_t); + FUNC2RC(real_t, body_get_param, RID, BodyParameter); + FUNC3(body_set_state, RID, BodyState, const Variant &); + FUNC2RC(Variant, body_get_state, RID, BodyState); - FUNC3(body_set_param,RID,BodyParameter,real_t); - FUNC2RC(real_t,body_get_param,RID,BodyParameter); + FUNC2(body_set_applied_force, RID, const Vector2 &); + FUNC1RC(Vector2, body_get_applied_force, RID); + FUNC2(body_set_applied_torque, RID, real_t); + FUNC1RC(real_t, body_get_applied_torque, RID); - FUNC3(body_set_state,RID,BodyState,const Variant&); - FUNC2RC(Variant,body_get_state,RID,BodyState); + FUNC3(body_add_force, RID, const Vector2 &, const Vector2 &); + FUNC3(body_apply_impulse, RID, const Vector2 &, const Vector2 &); + FUNC2(body_set_axis_velocity, RID, const Vector2 &); - FUNC2(body_set_applied_force,RID,const Vector2&); - FUNC1RC(Vector2,body_get_applied_force,RID); + FUNC2(body_add_collision_exception, RID, RID); + FUNC2(body_remove_collision_exception, RID, RID); + FUNC2S(body_get_collision_exceptions, RID, List<RID> *); - FUNC2(body_set_applied_torque,RID,real_t); - FUNC1RC(real_t,body_get_applied_torque,RID); + FUNC2(body_set_max_contacts_reported, RID, int); + FUNC1RC(int, body_get_max_contacts_reported, RID); - FUNC3(body_add_force,RID,const Vector2&,const Vector2&); - FUNC3(body_apply_impulse,RID,const Vector2&,const Vector2&); - FUNC2(body_set_axis_velocity,RID,const Vector2&); + FUNC2(body_set_one_way_collision_direction, RID, const Vector2 &); + FUNC1RC(Vector2, body_get_one_way_collision_direction, RID); - FUNC2(body_add_collision_exception,RID,RID); - FUNC2(body_remove_collision_exception,RID,RID); - FUNC2S(body_get_collision_exceptions,RID,List<RID>*); + FUNC2(body_set_one_way_collision_max_depth, RID, real_t); + FUNC1RC(real_t, body_get_one_way_collision_max_depth, RID); - FUNC2(body_set_max_contacts_reported,RID,int); - FUNC1RC(int,body_get_max_contacts_reported,RID); + FUNC2(body_set_contacts_reported_depth_treshold, RID, real_t); + FUNC1RC(real_t, body_get_contacts_reported_depth_treshold, RID); - FUNC2(body_set_one_way_collision_direction,RID,const Vector2&); - FUNC1RC(Vector2,body_get_one_way_collision_direction,RID); + FUNC2(body_set_omit_force_integration, RID, bool); + FUNC1RC(bool, body_is_omitting_force_integration, RID); - FUNC2(body_set_one_way_collision_max_depth,RID,real_t); - FUNC1RC(real_t,body_get_one_way_collision_max_depth,RID); + FUNC4(body_set_force_integration_callback, RID, Object *, const StringName &, const Variant &); - - FUNC2(body_set_contacts_reported_depth_treshold,RID,real_t); - FUNC1RC(real_t,body_get_contacts_reported_depth_treshold,RID); - - FUNC2(body_set_omit_force_integration,RID,bool); - FUNC1RC(bool,body_is_omitting_force_integration,RID); - - FUNC4(body_set_force_integration_callback,RID ,Object *,const StringName& ,const Variant& ); - - - bool body_collide_shape(RID p_body, int p_body_shape,RID p_shape, const Transform2D& p_shape_xform,const Vector2& p_motion,Vector2 *r_results,int p_result_max,int &r_result_count) { - return physics_2d_server->body_collide_shape(p_body,p_body_shape,p_shape,p_shape_xform,p_motion,r_results,p_result_max,r_result_count); + bool body_collide_shape(RID p_body, int p_body_shape, RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, Vector2 *r_results, int p_result_max, int &r_result_count) { + return physics_2d_server->body_collide_shape(p_body, p_body_shape, p_shape, p_shape_xform, p_motion, r_results, p_result_max, r_result_count); } - FUNC2(body_set_pickable,RID,bool); + FUNC2(body_set_pickable, RID, bool); - bool body_test_motion(RID p_body,const Transform2D& p_from,const Vector2& p_motion,real_t p_margin=0.001,MotionResult *r_result=NULL) { + bool body_test_motion(RID p_body, const Transform2D &p_from, const Vector2 &p_motion, real_t p_margin = 0.001, MotionResult *r_result = NULL) { - ERR_FAIL_COND_V(main_thread!=Thread::get_caller_ID(),false); - return physics_2d_server->body_test_motion(p_body,p_from,p_motion,p_margin,r_result); + ERR_FAIL_COND_V(main_thread != Thread::get_caller_ID(), false); + return physics_2d_server->body_test_motion(p_body, p_from, p_motion, p_margin, r_result); } /* JOINT API */ - - FUNC3(joint_set_param,RID,JointParam,real_t); - FUNC2RC(real_t,joint_get_param,RID,JointParam); - + FUNC3(joint_set_param, RID, JointParam, real_t); + FUNC2RC(real_t, joint_get_param, RID, JointParam); ///FUNC3RID(pin_joint,const Vector2&,RID,RID); ///FUNC5RID(groove_joint,const Vector2&,const Vector2&,const Vector2&,RID,RID); ///FUNC4RID(damped_spring_joint,const Vector2&,const Vector2&,RID,RID); - FUNC3R(RID,pin_joint_create,const Vector2&,RID,RID); - FUNC5R(RID,groove_joint_create,const Vector2&,const Vector2&,const Vector2&,RID,RID); - FUNC4R(RID,damped_spring_joint_create,const Vector2&,const Vector2&,RID,RID); + FUNC3R(RID, pin_joint_create, const Vector2 &, RID, RID); + FUNC5R(RID, groove_joint_create, const Vector2 &, const Vector2 &, const Vector2 &, RID, RID); + FUNC4R(RID, damped_spring_joint_create, const Vector2 &, const Vector2 &, RID, RID); - FUNC3(pin_joint_set_param,RID,PinJointParam,real_t); - FUNC2RC(real_t,pin_joint_get_param,RID,PinJointParam); + FUNC3(pin_joint_set_param, RID, PinJointParam, real_t); + FUNC2RC(real_t, pin_joint_get_param, RID, PinJointParam); - FUNC3(damped_string_joint_set_param,RID,DampedStringParam,real_t); - FUNC2RC(real_t,damped_string_joint_get_param,RID,DampedStringParam); - - FUNC1RC(JointType,joint_get_type,RID); + FUNC3(damped_string_joint_set_param, RID, DampedStringParam, real_t); + FUNC2RC(real_t, damped_string_joint_get_param, RID, DampedStringParam); + FUNC1RC(JointType, joint_get_type, RID); /* MISC */ - - FUNC1(free,RID); - FUNC1(set_active,bool); + FUNC1(free, RID); + FUNC1(set_active, bool); virtual void init(); virtual void step(real_t p_step); @@ -313,28 +294,24 @@ public: return physics_2d_server->get_process_info(p_info); } - Physics2DServerWrapMT(Physics2DServer* p_contained,bool p_create_thread); + Physics2DServerWrapMT(Physics2DServer *p_contained, bool p_create_thread); ~Physics2DServerWrapMT(); + template <class T> + static Physics2DServer *init_server() { - template<class T> - static Physics2DServer* init_server() { - - int tm = GLOBAL_DEF("physics/2d/thread_model",1); - if (tm==0) //single unsafe - return memnew( T ); - else if (tm==1) //single saef - return memnew( Physics2DServerWrapMT( memnew( T ), false )); + int tm = GLOBAL_DEF("physics/2d/thread_model", 1); + if (tm == 0) //single unsafe + return memnew(T); + else if (tm == 1) //single saef + return memnew(Physics2DServerWrapMT(memnew(T), false)); else //single unsafe - return memnew( Physics2DServerWrapMT( memnew( T ), true )); - - + return memnew(Physics2DServerWrapMT(memnew(T), true)); } #undef ServerNameWrapMT #undef ServerName #undef server_name - }; #ifdef DEBUG_SYNC diff --git a/servers/physics_2d/shape_2d_sw.cpp b/servers/physics_2d/shape_2d_sw.cpp index b6eb427131..32d632453a 100644 --- a/servers/physics_2d/shape_2d_sw.cpp +++ b/servers/physics_2d/shape_2d_sw.cpp @@ -30,108 +30,98 @@ #include "geometry.h" #include "sort.h" - - -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(); +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 Shape2DSW::get_support(const Vector2 &p_normal) const { Vector2 res[2]; int amnt; - get_supports(p_normal,res,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); + Map<ShapeOwner2DSW *, int>::Element *E = owners.find(p_owner); if (E) { E->get()++; } else { - owners[p_owner]=1; + owners[p_owner] = 1; } } -void Shape2DSW::remove_owner(ShapeOwner2DSW *p_owner){ +void Shape2DSW::remove_owner(ShapeOwner2DSW *p_owner) { - Map<ShapeOwner2DSW*,int>::Element *E=owners.find(p_owner); + Map<ShapeOwner2DSW *, int>::Element *E = owners.find(p_owner); ERR_FAIL_COND(!E); E->get()--; - if (E->get()==0) { + if (E->get() == 0) { owners.erase(E); } - } -bool Shape2DSW::is_owner(ShapeOwner2DSW *p_owner) const{ +bool Shape2DSW::is_owner(ShapeOwner2DSW *p_owner) const { return owners.has(p_owner); - } -const Map<ShapeOwner2DSW*,int>& Shape2DSW::get_owners() const{ +const Map<ShapeOwner2DSW *, int> &Shape2DSW::get_owners() const { return owners; } - Shape2DSW::Shape2DSW() { - custom_bias=0; - configured=false; + 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 { - -void LineShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const { - - r_amount=0; + r_amount = 0; } -bool LineShape2DSW::contains_point(const Vector2& p_point) const { +bool LineShape2DSW::contains_point(const Vector2 &p_point) const { return normal.dot(p_point) < d; } -bool LineShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const { +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 ); + Vector2 segment = p_begin - p_end; + real_t den = normal.dot(segment); //printf("den is %i\n",den); - if (Math::abs(den)<=CMP_EPSILON) { + if (Math::abs(den) <= CMP_EPSILON) { return false; } - real_t dist=(normal.dot( p_begin ) - d) / den; + real_t dist = (normal.dot(p_begin) - d) / den; //printf("dist is %i\n",dist); - if (dist<-CMP_EPSILON || dist > (1.0 +CMP_EPSILON)) { + if (dist < -CMP_EPSILON || dist > (1.0 + CMP_EPSILON)) { return false; } r_point = p_begin + segment * -dist; - r_normal=normal; + r_normal = normal; return true; } @@ -141,24 +131,22 @@ real_t LineShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) return 0; } +void LineShape2DSW::set_data(const Variant &p_data) { -void LineShape2DSW::set_data(const Variant& p_data) { - - ERR_FAIL_COND(p_data.get_type()!=Variant::ARRAY); + 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))); - + 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; + arr[0] = normal; + arr[1] = d; return arr; } @@ -166,29 +154,24 @@ Variant LineShape2DSW::get_data() const { /*********************************************************/ /*********************************************************/ +void RayShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { + r_amount = 1; -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); + if (p_normal.y > 0) + *r_supports = Vector2(0, length); else - *r_supports=Vector2(); - + *r_supports = Vector2(); } -bool RayShape2DSW::contains_point(const Vector2& p_point) const { +bool RayShape2DSW::contains_point(const Vector2 &p_point) const { return false; } -bool RayShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const { +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(real_t p_mass, const Size2 &p_scale) const { @@ -196,10 +179,10 @@ real_t RayShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) return 0; //rays are mass-less } -void RayShape2DSW::set_data(const Variant& p_data) { +void RayShape2DSW::set_data(const Variant &p_data) { - length=p_data; - configure(Rect2(0,0,0.001,length)); + length = p_data; + configure(Rect2(0, 0, 0.001, length)); } Variant RayShape2DSW::get_data() const { @@ -207,46 +190,41 @@ Variant RayShape2DSW::get_data() const { return length; } - /*********************************************************/ /*********************************************************/ /*********************************************************/ +void SegmentShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { - -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; + if (Math::abs(p_normal.dot(n)) > _SEGMENT_IS_VALID_SUPPORT_TRESHOLD) { + r_supports[0] = a; + r_supports[1] = b; + r_amount = 2; return; - } - real_t dp=p_normal.dot(b-a); - if (dp>0) - *r_supports=b; + real_t dp = p_normal.dot(b - a); + if (dp > 0) + *r_supports = b; else - *r_supports=a; - r_amount=1; - + *r_supports = a; + r_amount = 1; } -bool SegmentShape2DSW::contains_point(const Vector2& p_point) const { +bool SegmentShape2DSW::contains_point(const Vector2 &p_point) const { return false; } -bool SegmentShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const { +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)) + if (!Geometry::segment_intersects_segment_2d(p_begin, p_end, a, b, &r_point)) return false; if (n.dot(p_begin) > n.dot(a)) { - r_normal=n; + r_normal = n; } else { - r_normal=-n; + r_normal = -n; } return true; @@ -254,38 +232,38 @@ bool SegmentShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p real_t SegmentShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { - Vector2 s[2]={a*p_scale,b*p_scale}; + Vector2 s[2] = { a * p_scale, b * p_scale }; real_t l = s[1].distance_to(s[0]); - Vector2 ofs = (s[0]+s[1])*0.5; + Vector2 ofs = (s[0] + s[1]) * 0.5; - return p_mass*(l*l/12.0 + ofs.length_squared()); + return p_mass * (l * l / 12.0 + ofs.length_squared()); } -void SegmentShape2DSW::set_data(const Variant& p_data) { +void SegmentShape2DSW::set_data(const Variant &p_data) { - ERR_FAIL_COND(p_data.get_type()!=Variant::RECT2); + ERR_FAIL_COND(p_data.get_type() != Variant::RECT2); Rect2 r = p_data; - a=r.pos; - b=r.size; - n=(b-a).tangent(); + a = r.pos; + b = r.size; + n = (b - a).tangent(); Rect2 aabb; - aabb.pos=a; + 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; + 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; + r.pos = a; + r.size = b; return r; } @@ -293,24 +271,18 @@ Variant SegmentShape2DSW::get_data() const { /*********************************************************/ /*********************************************************/ +void CircleShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { - -void CircleShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const { - - r_amount=1; - *r_supports=p_normal*radius; - + r_amount = 1; + *r_supports = p_normal * radius; } +bool CircleShape2DSW::contains_point(const Vector2 &p_point) const { -bool CircleShape2DSW::contains_point(const Vector2& p_point) const { - - return p_point.length_squared() < radius*radius; + return p_point.length_squared() < radius * radius; } - -bool CircleShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const { - +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; @@ -320,33 +292,32 @@ bool CircleShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_ b = 2 * p_begin.dot(line_vec); c = p_begin.dot(p_begin) - radius * radius; - real_t sqrtterm = b*b - 4*a*c; + real_t sqrtterm = b * b - 4 * a * c; - if(sqrtterm < 0) + if (sqrtterm < 0) return false; sqrtterm = Math::sqrt(sqrtterm); - real_t res = ( -b - sqrtterm ) / (2 * a); + real_t res = (-b - sqrtterm) / (2 * a); - if (res <0 || res >1+CMP_EPSILON) { + if (res < 0 || res > 1 + CMP_EPSILON) { return false; } - r_point=p_begin+line_vec*res; - r_normal=r_point.normalized(); + r_point = p_begin + line_vec * res; + r_normal = r_point.normalized(); return true; } real_t CircleShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { - return (radius*radius)*(p_scale.x*0.5+p_scale.y*0.5); - + return (radius * radius) * (p_scale.x * 0.5 + p_scale.y * 0.5); } -void CircleShape2DSW::set_data(const Variant& p_data) { +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)); + radius = p_data; + configure(Rect2(-radius, -radius, radius * 2, radius * 2)); } Variant CircleShape2DSW::get_data() const { @@ -354,71 +325,63 @@ Variant CircleShape2DSW::get_data() const { return radius; } - /*********************************************************/ /*********************************************************/ /*********************************************************/ +void RectangleShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { - -void RectangleShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const { - - for(int i=0;i<2;i++) { + for (int i = 0; i < 2; i++) { Vector2 ag; - ag[i]=1.0; + ag[i] = 1.0; real_t dp = ag.dot(p_normal); - if (Math::abs(dp)<_SEGMENT_IS_VALID_SUPPORT_TRESHOLD) + if (Math::abs(dp) < _SEGMENT_IS_VALID_SUPPORT_TRESHOLD) continue; - real_t sgn = dp>0 ? 1.0 : -1.0; + real_t sgn = dp > 0 ? 1.0 : -1.0; - r_amount=2; + r_amount = 2; - r_supports[0][i]=half_extents[i]*sgn; - r_supports[0][i^1]=half_extents[i^1]; + 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]; + 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 - ); - + r_amount = 1; + r_supports[0] = Vector2( + (p_normal.x < 0) ? -half_extents.x : half_extents.x, + (p_normal.y < 0) ? -half_extents.y : half_extents.y); } -bool RectangleShape2DSW::contains_point(const Vector2& p_point) const { +bool RectangleShape2DSW::contains_point(const Vector2 &p_point) const { - return Math::abs(p_point.x)<half_extents.x && Math::abs(p_point.y)<half_extents.y; + return Math::abs(p_point.x) < half_extents.x && Math::abs(p_point.y) < half_extents.y; } -bool RectangleShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const { - +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); + return get_aabb().intersects_segment(p_begin, p_end, &r_point, &r_normal); } -real_t RectangleShape2DSW::get_moment_of_inertia(real_t p_mass,const Size2& p_scale) const { +real_t RectangleShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { - Vector2 he2=half_extents*2*p_scale; - return p_mass*he2.dot(he2)/12.0; + Vector2 he2 = half_extents * 2 * p_scale; + return p_mass * he2.dot(he2) / 12.0; } -void RectangleShape2DSW::set_data(const Variant& p_data) { +void RectangleShape2DSW::set_data(const Variant &p_data) { - ERR_FAIL_COND(p_data.get_type()!=Variant::VECTOR2); + ERR_FAIL_COND(p_data.get_type() != Variant::VECTOR2); - half_extents=p_data; - configure(Rect2(-half_extents,half_extents*2.0)); + half_extents = p_data; + configure(Rect2(-half_extents, half_extents * 2.0)); } Variant RectangleShape2DSW::get_data() const { @@ -426,71 +389,65 @@ Variant RectangleShape2DSW::get_data() const { return half_extents; } - - /*********************************************************/ /*********************************************************/ /*********************************************************/ +void CapsuleShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { - -void CapsuleShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const { - - Vector2 n=p_normal; + Vector2 n = p_normal; real_t d = n.y; - if (Math::abs( d )<(1.0-_SEGMENT_IS_VALID_SUPPORT_TRESHOLD) ) { + if (Math::abs(d) < (1.0 - _SEGMENT_IS_VALID_SUPPORT_TRESHOLD)) { // make it flat - n.y=0.0; + n.y = 0.0; n.normalize(); - n*=radius; + 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; + 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 { real_t h = (d > 0) ? height : -height; - n*=radius; - n.y += h*0.5; - r_amount=1; - *r_supports=n; - + n *= radius; + n.y += h * 0.5; + r_amount = 1; + *r_supports = n; } } -bool CapsuleShape2DSW::contains_point(const Vector2& p_point) const { +bool CapsuleShape2DSW::contains_point(const Vector2 &p_point) const { Vector2 p = p_point; - p.y=Math::abs(p.y); - p.y-=height*0.5; - if (p.y<0) - p.y=0; + p.y = Math::abs(p.y); + p.y -= height * 0.5; + if (p.y < 0) + p.y = 0; - return p.length_squared() < radius*radius; + return p.length_squared() < radius * radius; } -bool CapsuleShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const { - +bool CapsuleShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { real_t d = 1e10; - Vector2 n = (p_end-p_begin).normalized(); - bool collided=false; + Vector2 n = (p_end - p_begin).normalized(); + bool collided = false; //try spheres - for(int i=0;i<2;i++) { + for (int i = 0; i < 2; i++) { Vector2 begin = p_begin; Vector2 end = p_end; - real_t ofs = (i==0)?-height*0.5:height*0.5; - begin.y+=ofs; - end.y+=ofs; + real_t ofs = (i == 0) ? -height * 0.5 : height * 0.5; + begin.y += ofs; + end.y += ofs; Vector2 line_vec = end - begin; @@ -500,39 +457,38 @@ bool CapsuleShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p b = 2 * begin.dot(line_vec); c = begin.dot(begin) - radius * radius; - real_t sqrtterm = b*b - 4*a*c; + real_t sqrtterm = b * b - 4 * a * c; - if(sqrtterm < 0) + if (sqrtterm < 0) continue; sqrtterm = Math::sqrt(sqrtterm); - real_t res = ( -b - sqrtterm ) / (2 * a); + real_t res = (-b - sqrtterm) / (2 * a); - if (res <0 || res >1+CMP_EPSILON) { + if (res < 0 || res > 1 + CMP_EPSILON) { continue; } - Vector2 point = begin+line_vec*res; - Vector2 pointf(point.x,point.y-ofs); + 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; + 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)) { + 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; + if (pd < d) { + r_point = rpos; + r_normal = rnorm; + d = pd; + collided = true; } } @@ -542,101 +498,93 @@ bool CapsuleShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p real_t CapsuleShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { - Vector2 he2=Vector2(radius*2,height+radius*2)*p_scale; - return p_mass*he2.dot(he2)/12.0; + Vector2 he2 = Vector2(radius * 2, height + radius * 2) * p_scale; + return p_mass * he2.dot(he2) / 12.0; } -void CapsuleShape2DSW::set_data(const Variant& p_data) { +void CapsuleShape2DSW::set_data(const Variant &p_data) { - ERR_FAIL_COND(p_data.get_type()!=Variant::ARRAY && p_data.get_type()!=Variant::VECTOR2); + 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]; + 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; + radius = p.x; + height = p.y; } - Point2 he(radius,height*0.5+radius); - configure(Rect2(-he,he*2)); - + Point2 he(radius, height * 0.5 + radius); + configure(Rect2(-he, he * 2)); } Variant CapsuleShape2DSW::get_data() const { - return Point2(height,radius); + 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; -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++) { + 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; + if (ld > d) { + support_idx = i; + d = ld; } //test segment - if (points[i].normal.dot(p_normal)>_SEGMENT_IS_VALID_SUPPORT_TRESHOLD) { + 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; + 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; + ERR_FAIL_COND(support_idx == -1); + r_amount = 1; + r_supports[0] = points[support_idx].pos; } +bool ConvexPolygonShape2DSW::contains_point(const Vector2 &p_point) const { -bool ConvexPolygonShape2DSW::contains_point(const Vector2& p_point) const { - - bool out=false; - bool in=false; + bool out = false; + bool in = false; - for(int i=0;i<point_count;i++) { + for (int i = 0; i < point_count; i++) { real_t d = points[i].normal.dot(p_point) - points[i].normal.dot(points[i].pos); - if (d>0) - out=true; + if (d > 0) + out = true; else - in=true; + in = true; } return (in && !out) || (!in && out); } +bool ConvexPolygonShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { -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; + Vector2 n = (p_end - p_begin).normalized(); + real_t d = 1e10; + bool inters = false; - for(int i=0;i<point_count;i++) { + for (int i = 0; i < point_count; i++) { //hmm crap.. no can do.. /* @@ -644,98 +592,92 @@ bool ConvexPolygonShape2DSW::intersect_segment(const Vector2& p_begin,const Vect continue; */ - Vector2 res; - if (!Geometry::segment_intersects_segment_2d(p_begin,p_end,points[i].pos,points[(i+1)%point_count].pos,&res)) + if (!Geometry::segment_intersects_segment_2d(p_begin, p_end, points[i].pos, points[(i + 1) % point_count].pos, &res)) continue; real_t nd = n.dot(res); - if (nd<d) { + if (nd < d) { - d=nd; - r_point=res; - r_normal=points[i].normal; - inters=true; + d = nd; + r_point = res; + r_normal = points[i].normal; + inters = true; } - } - if (inters) { - if (n.dot(r_normal)>0) - r_normal=-r_normal; + 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(real_t p_mass,const Size2& p_scale) const { +real_t ConvexPolygonShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { Rect2 aabb; - aabb.pos=points[0].pos*p_scale; - for(int i=0;i<point_count;i++) { + aabb.pos = points[0].pos * p_scale; + for (int i = 0; i < point_count; i++) { - aabb.expand_to(points[i].pos*p_scale); + aabb.expand_to(points[i].pos * p_scale); } - return p_mass*aabb.size.dot(aabb.size)/12.0 + p_mass * (aabb.pos+aabb.size*0.5).length_squared(); + return p_mass * aabb.size.dot(aabb.size) / 12.0 + p_mass * (aabb.pos + aabb.size * 0.5).length_squared(); } -void ConvexPolygonShape2DSW::set_data(const Variant& p_data) { - - ERR_FAIL_COND(p_data.get_type()!=Variant::POOL_VECTOR2_ARRAY && p_data.get_type()!=Variant::POOL_REAL_ARRAY); +void ConvexPolygonShape2DSW::set_data(const Variant &p_data) { + ERR_FAIL_COND(p_data.get_type() != Variant::POOL_VECTOR2_ARRAY && p_data.get_type() != Variant::POOL_REAL_ARRAY); if (points) memdelete_arr(points); - points=NULL; - point_count=0; - - if (p_data.get_type()==Variant::POOL_VECTOR2_ARRAY) { - PoolVector<Vector2> arr=p_data; - ERR_FAIL_COND(arr.size()==0); - point_count=arr.size(); - points = memnew_arr(Point,point_count); + points = NULL; + point_count = 0; + + if (p_data.get_type() == Variant::POOL_VECTOR2_ARRAY) { + PoolVector<Vector2> arr = p_data; + ERR_FAIL_COND(arr.size() == 0); + point_count = arr.size(); + points = memnew_arr(Point, point_count); PoolVector<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++) { + points[i].pos = r[i]; } - for(int i=0;i<point_count;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(); + Vector2 pn = points[(i + 1) % point_count].pos; + points[i].normal = (pn - p).tangent().normalized(); } } else { PoolVector<real_t> dvr = p_data; - point_count=dvr.size()/4; - ERR_FAIL_COND(point_count==0); + point_count = dvr.size() / 4; + ERR_FAIL_COND(point_count == 0); - points = memnew_arr(Point,point_count); + points = memnew_arr(Point, point_count); PoolVector<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]; + 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); + ERR_FAIL_COND(point_count == 0); Rect2 aabb; - aabb.pos=points[0].pos; - for(int i=1;i<point_count;i++) + aabb.pos = points[0].pos; + for (int i = 1; i < point_count; i++) aabb.expand_to(points[i].pos); configure(aabb); @@ -747,337 +689,312 @@ Variant ConvexPolygonShape2DSW::get_data() const { dvr.resize(point_count); - for(int i=0;i<point_count;i++) { - dvr.set(i,points[i].pos); + for (int i = 0; i < point_count; i++) { + dvr.set(i, points[i].pos); } return dvr; } - ConvexPolygonShape2DSW::ConvexPolygonShape2DSW() { - points=NULL; - point_count=0; - + points = NULL; + point_count = 0; } -ConvexPolygonShape2DSW::~ConvexPolygonShape2DSW(){ +ConvexPolygonShape2DSW::~ConvexPolygonShape2DSW() { if (points) memdelete_arr(points); - } ////////////////////////////////////////////////// +void ConcavePolygonShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { -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 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; + if (ld > d) { + d = ld; + idx = i; } } - - r_amount=1; - ERR_FAIL_COND(idx==-1); - *r_supports=points[idx]; - + r_amount = 1; + ERR_FAIL_COND(idx == -1); + *r_supports = points[idx]; } -bool ConcavePolygonShape2DSW::contains_point(const Vector2& p_point) const { +bool ConcavePolygonShape2DSW::contains_point(const Vector2 &p_point) const { return false; //sorry } +bool ConcavePolygonShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { -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); + 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, - + 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; + 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; + int level = 0; - const Segment *segmentptr=&segments[0]; - const Vector2 *pointptr=&points[0]; + const Segment *segmentptr = &segments[0]; + const Vector2 *pointptr = &points[0]; const BVH *bvhptr = &bvh[0]; + stack[0] = 0; + while (true) { - stack[0]=0; - while(true) { - - uint32_t node = stack[level]&NODE_IDX_MASK; - const BVH &b = bvhptr[ node ]; - bool done=false; + uint32_t node = stack[level] & NODE_IDX_MASK; + const BVH &b = bvhptr[node]; + bool done = false; - switch(stack[level]>>VISITED_BIT_SHIFT) { + switch (stack[level] >> VISITED_BIT_SHIFT) { case TEST_AABB_BIT: { - - bool valid = b.aabb.intersects_segment(p_begin,p_end); + bool valid = b.aabb.intersects_segment(p_begin, p_end); if (!valid) { - stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node; + 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] ]; + 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)) { + if (Geometry::segment_intersects_segment_2d(p_begin, p_end, a, b, &res)) { real_t nd = n.dot(res); - if (nd<d) { + if (nd < d) { - d=nd; - r_point=res; - r_normal=(b-a).tangent().normalized(); - inters=true; + d = nd; + r_point = res; + r_normal = (b - a).tangent().normalized(); + inters = true; } - } - stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node; + stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; } else { - stack[level]=(VISIT_LEFT_BIT<<VISITED_BIT_SHIFT)|node; + stack[level] = (VISIT_LEFT_BIT << VISITED_BIT_SHIFT) | node; } } - - } continue; + } + continue; case VISIT_LEFT_BIT: { - stack[level]=(VISIT_RIGHT_BIT<<VISITED_BIT_SHIFT)|node; - stack[level+1]=b.left|TEST_AABB_BIT; + stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node; + stack[level + 1] = b.left | TEST_AABB_BIT; level++; - - } continue; + } + continue; case VISIT_RIGHT_BIT: { - stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node; - stack[level+1]=b.right|TEST_AABB_BIT; + stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; + stack[level + 1] = b.right | TEST_AABB_BIT; level++; - } continue; + } + continue; case VISIT_DONE_BIT: { - if (level==0) { - done=true; + if (level == 0) { + done = true; break; } else level--; - - } continue; + } + continue; } - if (done) break; } - if (inters) { - if (n.dot(r_normal)>0) - r_normal=-r_normal; + 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) { -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_depth = MAX(p_depth, bvh_depth); bvh.push_back(*p_bvh); - return bvh.size()-1; + 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); + 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); + SortArray<BVH, BVH_CompareX> sort; + sort.sort(p_bvh, p_len); } else { - SortArray<BVH,BVH_CompareY> sort; - sort.sort(p_bvh,p_len); - + SortArray<BVH, BVH_CompareY> sort; + sort.sort(p_bvh, p_len); } - int median = p_len/2; - + int median = p_len / 2; BVH node; - node.aabb=global_aabb; + 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; + 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) { +void ConcavePolygonShape2DSW::set_data(const Variant &p_data) { - ERR_FAIL_COND(p_data.get_type()!=Variant::POOL_VECTOR2_ARRAY && p_data.get_type()!=Variant::POOL_REAL_ARRAY); + ERR_FAIL_COND(p_data.get_type() != Variant::POOL_VECTOR2_ARRAY && p_data.get_type() != Variant::POOL_REAL_ARRAY); Rect2 aabb; - if (p_data.get_type()==Variant::POOL_VECTOR2_ARRAY) { + if (p_data.get_type() == Variant::POOL_VECTOR2_ARRAY) { PoolVector<Vector2> p2arr = p_data; int len = p2arr.size(); - ERR_FAIL_COND(len%2); + ERR_FAIL_COND(len % 2); segments.clear(); points.clear(); bvh.clear(); - bvh_depth=1; + bvh_depth = 1; - if (len==0) { + if (len == 0) { configure(aabb); return; } PoolVector<Vector2>::Read arr = p2arr.read(); - Map<Point2,int> pointmap; - for(int i=0;i<len;i+=2) { + 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; + Point2 p1 = arr[i]; + Point2 p2 = arr[i + 1]; + int idx_p1, idx_p2; if (pointmap.has(p1)) { - idx_p1=pointmap[p1]; + idx_p1 = pointmap[p1]; } else { - idx_p1=pointmap.size(); - pointmap[p1]=idx_p1; + idx_p1 = pointmap.size(); + pointmap[p1] = idx_p1; } if (pointmap.has(p2)) { - idx_p2=pointmap[p2]; + idx_p2 = pointmap[p2]; } else { - idx_p2=pointmap.size(); - pointmap[p2]=idx_p2; + idx_p2 = pointmap.size(); + pointmap[p2] = idx_p2; } Segment s; - s.points[0]=idx_p1; - s.points[1]=idx_p2; + 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.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(); + points[E->get()] = E->key(); } Vector<BVH> main_vbh; main_vbh.resize(segments.size()); - for(int i=0;i<main_vbh.size();i++) { + for (int i = 0; i < main_vbh.size(); i++) { - - main_vbh[i].aabb.pos=points[segments[i].points[0]]; + 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; + main_vbh[i].left = -1; + main_vbh[i].right = i; } - _generate_bvh(&main_vbh[0],main_vbh.size(),1); - + _generate_bvh(&main_vbh[0], main_vbh.size(), 1); } else { //dictionary with arrays - } - configure(aabb); } Variant ConcavePolygonShape2DSW::get_data() const { - PoolVector<Vector2> rsegments; int len = segments.size(); - rsegments.resize(len*2); + rsegments.resize(len * 2); PoolVector<Vector2>::Write w = rsegments.write(); - for(int i=0;i<len;i++) { + 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[(i << 1) + 0] = points[segments[i].points[0]]; + w[(i << 1) + 1] = points[segments[i].points[1]]; } - w=PoolVector<Vector2>::Write(); + w = PoolVector<Vector2>::Write(); return rsegments; } -void ConcavePolygonShape2DSW::cull(const Rect2& p_local_aabb,Callback p_callback,void* p_userdata) const { +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); + 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, - + 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, }; @@ -1086,73 +1003,68 @@ void ConcavePolygonShape2DSW::cull(const Rect2& p_local_aabb,Callback p_callback stack[i]=0; */ + int level = 0; - int level=0; - - const Segment *segmentptr=&segments[0]; - const Vector2 *pointptr=&points[0]; + const Segment *segmentptr = &segments[0]; + const Vector2 *pointptr = &points[0]; const BVH *bvhptr = &bvh[0]; + stack[0] = 0; + while (true) { - stack[0]=0; - while(true) { - - uint32_t node = stack[level]&NODE_IDX_MASK; - const BVH &b = bvhptr[ node ]; + uint32_t node = stack[level] & NODE_IDX_MASK; + const BVH &b = bvhptr[node]; - switch(stack[level]>>VISITED_BIT_SHIFT) { + 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; + stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; } else { - if (b.left<0) { + if (b.left < 0) { - const Segment &s=segmentptr[ b.right ]; - Vector2 a = pointptr[ s.points[0] ]; - Vector2 b = pointptr[ s.points[1] ]; + 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()); + SegmentShape2DSW ss(a, b, (b - a).tangent().normalized()); - p_callback(p_userdata,&ss); - stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node; + p_callback(p_userdata, &ss); + stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; } else { - stack[level]=(VISIT_LEFT_BIT<<VISITED_BIT_SHIFT)|node; + stack[level] = (VISIT_LEFT_BIT << VISITED_BIT_SHIFT) | node; } } - - } continue; + } + continue; case VISIT_LEFT_BIT: { - stack[level]=(VISIT_RIGHT_BIT<<VISITED_BIT_SHIFT)|node; - stack[level+1]=b.left|TEST_AABB_BIT; + stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node; + stack[level + 1] = b.left | TEST_AABB_BIT; level++; - - } continue; + } + continue; case VISIT_RIGHT_BIT: { - stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node; - stack[level+1]=b.right|TEST_AABB_BIT; + stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; + stack[level + 1] = b.right | TEST_AABB_BIT; level++; - } continue; + } + continue; case VISIT_DONE_BIT: { - if (level==0) + if (level == 0) return; else level--; - - } continue; + } + continue; } } - } - - diff --git a/servers/physics_2d/shape_2d_sw.h b/servers/physics_2d/shape_2d_sw.h index c04cdfa456..00d86da7fb 100644 --- a/servers/physics_2d/shape_2d_sw.h +++ b/servers/physics_2d/shape_2d_sw.h @@ -46,11 +46,10 @@ SHAPE_CUSTOM, ///< Server-Implementation based custom shape, calling shape_creat class Shape2DSW; -class ShapeOwner2DSW : public RID_Data{ +class ShapeOwner2DSW : public RID_Data { public: - - virtual void _shape_changed()=0; - virtual void remove_shape(Shape2DSW *p_shape)=0; + virtual void _shape_changed() = 0; + virtual void remove_shape(Shape2DSW *p_shape) = 0; virtual ~ShapeOwner2DSW() {} }; @@ -62,77 +61,75 @@ class Shape2DSW : public RID_Data { bool configured; real_t custom_bias; - Map<ShapeOwner2DSW*,int> owners; + Map<ShapeOwner2DSW *, int> owners; + protected: + void configure(const Rect2 &p_aabb); - 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; } - _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; + 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 bool contains_point(const Vector2& p_point) const=0; + virtual bool contains_point(const Vector2 &p_point) const = 0; - virtual void project_rangev(const Vector2& p_normal, const Transform2D& p_transform, real_t &r_min, real_t &r_max) const=0; - virtual void project_range_castv(const Vector2& p_cast, const Vector2& p_normal, const Transform2D& p_transform, real_t &r_min, real_t &r_max) const=0; - virtual Vector2 get_support(const Vector2& p_normal) const; - virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const=0; + virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const = 0; + virtual void project_range_castv(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const = 0; + virtual Vector2 get_support(const Vector2 &p_normal) const; + virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const = 0; - virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const=0; - virtual real_t get_moment_of_inertia(real_t p_mass,const Size2& p_scale) const=0; - virtual void set_data(const Variant& p_data)=0; - virtual Variant get_data() const=0; + virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const = 0; + virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const = 0; + virtual void set_data(const Variant &p_data) = 0; + virtual Variant get_data() const = 0; - _FORCE_INLINE_ void set_custom_bias(real_t p_bias) { custom_bias=p_bias; } + _FORCE_INLINE_ 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; - + const Map<ShapeOwner2DSW *, int> &get_owners() const; - _FORCE_INLINE_ void get_supports_transformed_cast(const Vector2& p_cast,const Vector2& p_normal,const Transform2D& p_xform,Vector2 *r_supports,int & r_amount) const { + _FORCE_INLINE_ void get_supports_transformed_cast(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_xform, Vector2 *r_supports, int &r_amount) const { - get_supports(p_xform.basis_xform_inv(p_normal).normalized(),r_supports,r_amount); - for(int i=0;i<r_amount;i++) - r_supports[i]=p_xform.xform(r_supports[i]); + get_supports(p_xform.basis_xform_inv(p_normal).normalized(), r_supports, r_amount); + for (int i = 0; i < r_amount; i++) + r_supports[i] = p_xform.xform(r_supports[i]); - if (r_amount==1) { + if (r_amount == 1) { - if (Math::abs( p_normal.dot(p_cast.normalized()) )<(1.0-_SEGMENT_IS_VALID_SUPPORT_TRESHOLD) ) { + if (Math::abs(p_normal.dot(p_cast.normalized())) < (1.0 - _SEGMENT_IS_VALID_SUPPORT_TRESHOLD)) { //make line because they are parallel - r_amount=2; - r_supports[1]=r_supports[0]+p_cast; - } else if (p_cast.dot(p_normal)>0) { + r_amount = 2; + r_supports[1] = r_supports[0] + p_cast; + } else if (p_cast.dot(p_normal) > 0) { //normal points towards cast, add cast - r_supports[0]+=p_cast; + r_supports[0] += p_cast; } } else { - if (Math::abs( p_normal.dot(p_cast.normalized()) )<(1.0-_SEGMENT_IS_VALID_SUPPORT_TRESHOLD) ) { + if (Math::abs(p_normal.dot(p_cast.normalized())) < (1.0 - _SEGMENT_IS_VALID_SUPPORT_TRESHOLD)) { //optimize line and make it larger because they are parallel - if ((r_supports[1]-r_supports[0]).dot(p_cast)>0) { + if ((r_supports[1] - r_supports[0]).dot(p_cast) > 0) { //larger towards 1 - r_supports[1]+=p_cast; + r_supports[1] += p_cast; } else { //larger towards 0 - r_supports[0]+=p_cast; + r_supports[0] += p_cast; } - } else if (p_cast.dot(p_normal)>0) { + } else if (p_cast.dot(p_normal) > 0) { //normal points towards cast, add cast - r_supports[0]+=p_cast; - r_supports[1]+=p_cast; + r_supports[0] += p_cast; + r_supports[1] += p_cast; } - } } @@ -141,305 +138,274 @@ public: }; //let the optimizer do the magic -#define DEFAULT_PROJECT_RANGE_CAST \ -virtual void project_range_castv(const Vector2& p_cast, const Vector2& p_normal, const Transform2D& p_transform, real_t &r_min, real_t &r_max) const {\ - project_range_cast(p_cast,p_normal,p_transform,r_min,r_max);\ -}\ -_FORCE_INLINE_ void project_range_cast(const Vector2& p_cast, const Vector2& p_normal, const Transform2D& p_transform, real_t &r_min, real_t &r_max) const {\ -\ - real_t mina,maxa;\ - real_t minb,maxb;\ - Transform2D ofsb=p_transform;\ - ofsb.elements[2]+=p_cast;\ - project_range(p_normal,p_transform,mina,maxa);\ - project_range(p_normal,ofsb,minb,maxb); \ - r_min=MIN(mina,minb);\ - r_max=MAX(maxa,maxb);\ -} +#define DEFAULT_PROJECT_RANGE_CAST \ + virtual void project_range_castv(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { \ + project_range_cast(p_cast, p_normal, p_transform, r_min, r_max); \ + } \ + _FORCE_INLINE_ void project_range_cast(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { \ + \ + real_t mina, maxa; \ + real_t minb, maxb; \ + Transform2D ofsb = p_transform; \ + ofsb.elements[2] += p_cast; \ + project_range(p_normal, p_transform, mina, maxa); \ + project_range(p_normal, ofsb, minb, maxb); \ + r_min = MIN(mina, minb); \ + r_max = MAX(maxa, maxb); \ + } class 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 Transform2D& 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 void project_rangev(const Vector2 &p_normal, const Transform2D &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 contains_point(const Vector2& p_point) 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(real_t p_mass,const Size2& p_scale) const; + virtual bool contains_point(const Vector2 &p_point) 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(real_t p_mass, const Size2 &p_scale) const; - virtual void set_data(const Variant& p_data); + virtual void set_data(const Variant &p_data); virtual Variant get_data() const; - _FORCE_INLINE_ void project_range(const Vector2& p_normal, const Transform2D& p_transform, real_t &r_min, real_t &r_max) const { + _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { //real large - r_min=-1e10; - r_max=1e10; + r_min = -1e10; + r_max = 1e10; } - virtual void project_range_castv(const Vector2& p_cast, const Vector2& p_normal, const Transform2D& p_transform, real_t &r_min, real_t &r_max) const { - project_range_cast(p_cast,p_normal,p_transform,r_min,r_max); + virtual void project_range_castv(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { + project_range_cast(p_cast, p_normal, p_transform, r_min, r_max); } - _FORCE_INLINE_ void project_range_cast(const Vector2& p_cast, const Vector2& p_normal, const Transform2D& p_transform, real_t &r_min, real_t &r_max) const { + _FORCE_INLINE_ void project_range_cast(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { //real large - r_min=-1e10; - r_max=1e10; + r_min = -1e10; + r_max = 1e10; } - - - }; - class RayShape2DSW : public Shape2DSW { - real_t length; public: - - - _FORCE_INLINE_ real_t get_length() const { return length; } + _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 Transform2D& 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 void project_rangev(const Vector2 &p_normal, const Transform2D &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 contains_point(const Vector2& p_point) 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(real_t p_mass,const Size2& p_scale) const; + virtual bool contains_point(const Vector2 &p_point) 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(real_t p_mass, const Size2 &p_scale) const; - virtual void set_data(const Variant& p_data); + virtual void set_data(const Variant &p_data); virtual Variant get_data() const; - _FORCE_INLINE_ void project_range(const Vector2& p_normal, const Transform2D& p_transform, real_t &r_min, real_t &r_max) const { + _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { //real large r_max = p_normal.dot(p_transform.get_origin()); - r_min = p_normal.dot(p_transform.xform(Vector2(0,length))); - if (r_max<r_min) { + r_min = p_normal.dot(p_transform.xform(Vector2(0, length))); + if (r_max < r_min) { - SWAP(r_max,r_min); + SWAP(r_max, r_min); } } DEFAULT_PROJECT_RANGE_CAST - _FORCE_INLINE_ RayShape2DSW() {} - _FORCE_INLINE_ RayShape2DSW(real_t p_length) { length=p_length; } + _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; } + _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; } - _FORCE_INLINE_ Vector2 get_xformed_normal(const Transform2D& p_xform) const { + _FORCE_INLINE_ Vector2 get_xformed_normal(const Transform2D &p_xform) const { return (p_xform.xform(b) - p_xform.xform(a)).normalized().tangent(); } - virtual void project_rangev(const Vector2& p_normal, const Transform2D& 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 void project_rangev(const Vector2 &p_normal, const Transform2D &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 contains_point(const Vector2& p_point) 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(real_t p_mass,const Size2& p_scale) const; + virtual bool contains_point(const Vector2 &p_point) 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(real_t p_mass, const Size2 &p_scale) const; - virtual void set_data(const Variant& p_data); + virtual void set_data(const Variant &p_data); virtual Variant get_data() const; - _FORCE_INLINE_ void project_range(const Vector2& p_normal, const Transform2D& p_transform, real_t &r_min, real_t &r_max) const { + _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { //real large r_max = p_normal.dot(p_transform.xform(a)); r_min = p_normal.dot(p_transform.xform(b)); - if (r_max<r_min) { + if (r_max < r_min) { - SWAP(r_max,r_min); + SWAP(r_max, r_min); } } DEFAULT_PROJECT_RANGE_CAST _FORCE_INLINE_ SegmentShape2DSW() {} - _FORCE_INLINE_ SegmentShape2DSW(const Vector2& p_a,const Vector2& p_b,const Vector2& p_n) { a=p_a; b=p_b; n=p_n; } + _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; } + _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 Transform2D& 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 void project_rangev(const Vector2 &p_normal, const Transform2D &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 contains_point(const Vector2& p_point) 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(real_t p_mass,const Size2& p_scale) const; + virtual bool contains_point(const Vector2 &p_point) 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(real_t p_mass, const Size2 &p_scale) const; - virtual void set_data(const Variant& p_data); + virtual void set_data(const Variant &p_data); virtual Variant get_data() const; - _FORCE_INLINE_ void project_range(const Vector2& p_normal, const Transform2D& p_transform, real_t &r_min, real_t &r_max) const { + _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { //real large - real_t d = p_normal.dot( p_transform.get_origin() ); + 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; + r_min = d - (radius)*scale; + r_max = d + (radius)*scale; } - DEFAULT_PROJECT_RANGE_CAST - }; - - class RectangleShape2DSW : public Shape2DSW { - Vector2 half_extents; public: - - _FORCE_INLINE_ const Vector2& get_half_extents() const { return half_extents; } + _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 Transform2D& 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 void project_rangev(const Vector2 &p_normal, const Transform2D &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 contains_point(const Vector2& p_point) 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(real_t p_mass,const Size2& p_scale) const; + virtual bool contains_point(const Vector2 &p_point) 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(real_t p_mass, const Size2 &p_scale) const; - virtual void set_data(const Variant& p_data); + virtual void set_data(const Variant &p_data); virtual Variant get_data() const; - _FORCE_INLINE_ void project_range(const Vector2& p_normal, const Transform2D& p_transform, real_t &r_min, real_t &r_max) const { + _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { // no matter the angle, the box is mirrored anyway - r_max=-1e20; - r_min=1e20; - for(int i=0;i<4;i++) { - - real_t d=p_normal.dot(p_transform.xform(Vector2( ((i&1)*2-1)*half_extents.x, ((i>>1)*2-1)*half_extents.y ))); + r_max = -1e20; + r_min = 1e20; + for (int i = 0; i < 4; i++) { - if (d>r_max) - r_max=d; - if (d<r_min) - r_min=d; + real_t d = p_normal.dot(p_transform.xform(Vector2(((i & 1) * 2 - 1) * half_extents.x, ((i >> 1) * 2 - 1) * half_extents.y))); + if (d > r_max) + r_max = d; + if (d < r_min) + r_min = d; } } - - - _FORCE_INLINE_ Vector2 get_circle_axis(const Transform2D& p_xform, const Transform2D& p_xform_inv,const Vector2& p_circle) const { + _FORCE_INLINE_ Vector2 get_circle_axis(const Transform2D &p_xform, const Transform2D &p_xform_inv, const Vector2 &p_circle) const { Vector2 local_v = p_xform_inv.xform(p_circle); Vector2 he( - (local_v.x<0) ? -half_extents.x : half_extents.x, - (local_v.y<0) ? -half_extents.y : half_extents.y - ); + (local_v.x < 0) ? -half_extents.x : half_extents.x, + (local_v.y < 0) ? -half_extents.y : half_extents.y); - return (p_xform.xform(he)-p_circle).normalized(); + return (p_xform.xform(he) - p_circle).normalized(); } - _FORCE_INLINE_ Vector2 get_box_axis(const Transform2D& p_xform, const Transform2D& p_xform_inv,const RectangleShape2DSW *p_B,const Transform2D& p_B_xform, const Transform2D& p_B_xform_inv) const { + _FORCE_INLINE_ Vector2 get_box_axis(const Transform2D &p_xform, const Transform2D &p_xform_inv, const RectangleShape2DSW *p_B, const Transform2D &p_B_xform, const Transform2D &p_B_xform_inv) const { - Vector2 a,b; + Vector2 a, b; { Vector2 local_v = p_xform_inv.xform(p_B_xform.get_origin()); Vector2 he( - (local_v.x<0) ? -half_extents.x : half_extents.x, - (local_v.y<0) ? -half_extents.y : half_extents.y - ); - - a=p_xform.xform(he); + (local_v.x < 0) ? -half_extents.x : half_extents.x, + (local_v.y < 0) ? -half_extents.y : half_extents.y); + a = p_xform.xform(he); } { Vector2 local_v = p_B_xform_inv.xform(p_xform.get_origin()); Vector2 he( - (local_v.x<0) ? -p_B->half_extents.x : p_B->half_extents.x, - (local_v.y<0) ? -p_B->half_extents.y : p_B->half_extents.y - ); - - b=p_B_xform.xform(he); + (local_v.x < 0) ? -p_B->half_extents.x : p_B->half_extents.x, + (local_v.y < 0) ? -p_B->half_extents.y : p_B->half_extents.y); + b = p_B_xform.xform(he); } - return (a-b).normalized(); + return (a - b).normalized(); } - DEFAULT_PROJECT_RANGE_CAST - }; 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; } + _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 Transform2D& 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 void project_rangev(const Vector2 &p_normal, const Transform2D &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 contains_point(const Vector2& p_point) 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(real_t p_mass,const Size2& p_scale) const; + virtual bool contains_point(const Vector2 &p_point) 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(real_t p_mass, const Size2 &p_scale) const; - virtual void set_data(const Variant& p_data); + virtual void set_data(const Variant &p_data); virtual Variant get_data() const; - _FORCE_INLINE_ void project_range(const Vector2& p_normal, const Transform2D& p_transform, real_t &r_min, real_t &r_max) const { + _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { // no matter the angle, the box is mirrored anyway - Vector2 n=p_transform.basis_xform_inv(p_normal).normalized(); + Vector2 n = p_transform.basis_xform_inv(p_normal).normalized(); real_t h = (n.y > 0) ? height : -height; n *= radius; @@ -448,24 +414,19 @@ public: r_max = p_normal.dot(p_transform.xform(n)); r_min = p_normal.dot(p_transform.xform(-n)); - if (r_max<r_min) { + if (r_max < r_min) { - SWAP(r_max,r_min); + SWAP(r_max, r_min); } //ERR_FAIL_COND( r_max < r_min ); } DEFAULT_PROJECT_RANGE_CAST - }; - - - class ConvexPolygonShape2DSW : public Shape2DSW { - struct Point { Vector2 pos; @@ -476,44 +437,41 @@ class ConvexPolygonShape2DSW : public Shape2DSW { 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; } - _FORCE_INLINE_ Vector2 get_xformed_segment_normal(const Transform2D& p_xform, int p_idx) const { + _FORCE_INLINE_ const Vector2 &get_point(int p_idx) const { return points[p_idx].pos; } + _FORCE_INLINE_ const Vector2 &get_segment_normal(int p_idx) const { return points[p_idx].normal; } + _FORCE_INLINE_ Vector2 get_xformed_segment_normal(const Transform2D &p_xform, int p_idx) const { Vector2 a = points[p_idx].pos; p_idx++; - Vector2 b = points[p_idx==point_count?0:p_idx].pos; - return (p_xform.xform(b)-p_xform.xform(a)).normalized().tangent(); + Vector2 b = points[p_idx == point_count ? 0 : p_idx].pos; + return (p_xform.xform(b) - p_xform.xform(a)).normalized().tangent(); } virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_CONVEX_POLYGON; } - virtual void project_rangev(const Vector2& p_normal, const Transform2D& 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 void project_rangev(const Vector2 &p_normal, const Transform2D &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 contains_point(const Vector2& p_point) 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(real_t p_mass,const Size2& p_scale) const; + virtual bool contains_point(const Vector2 &p_point) 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(real_t p_mass, const Size2 &p_scale) const; - virtual void set_data(const Variant& p_data); + virtual void set_data(const Variant &p_data); virtual Variant get_data() const; - _FORCE_INLINE_ void project_range(const Vector2& p_normal, const Transform2D& p_transform, real_t &r_min, real_t &r_max) const { + _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { // no matter the angle, the box is mirrored anyway r_min = r_max = p_normal.dot(p_transform.xform(points[0].pos)); - for(int i=1;i<point_count;i++) { + for (int i = 1; i < point_count; i++) { real_t d = p_normal.dot(p_transform.xform(points[i].pos)); - if (d>r_max) - r_max=d; - if (d<r_min) - r_min=d; - + if (d > r_max) + r_max = d; + if (d < r_min) + r_min = d; } - } DEFAULT_PROJECT_RANGE_CAST @@ -522,16 +480,13 @@ public: ~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; + 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 { @@ -547,53 +502,50 @@ class ConcavePolygonShape2DSW : public ConcaveShape2DSW { struct BVH { Rect2 aabb; - int left,right; + int left, right; }; - Vector<BVH> bvh; int bvh_depth; - struct BVH_CompareX { - _FORCE_INLINE_ bool operator ()(const BVH& a, const BVH& b) const { + _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); + 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 { + _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); + 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); + 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 Transform2D& p_transform, real_t &r_min, real_t &r_max) const { /*project_range(p_normal,p_transform,r_min,r_max);*/ } - virtual void project_range(const Vector2& p_normal, const Transform2D& 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 void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { /*project_range(p_normal,p_transform,r_min,r_max);*/ + } + virtual void project_range(const Vector2 &p_normal, const Transform2D &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 contains_point(const Vector2& p_point) const; - virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const; + virtual bool contains_point(const Vector2 &p_point) 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(real_t p_mass,const Size2& p_scale) const { return 0; } + virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { return 0; } - virtual void set_data(const Variant& p_data); + 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; - + virtual void cull(const Rect2 &p_local_aabb, Callback p_callback, void *p_userdata) const; DEFAULT_PROJECT_RANGE_CAST - }; #undef DEFAULT_PROJECT_RANGE_CAST diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp index 9bced50061..93df0a00f6 100644 --- a/servers/physics_2d/space_2d_sw.cpp +++ b/servers/physics_2d/space_2d_sw.cpp @@ -30,109 +30,101 @@ #include "collision_solver_2d_sw.h" #include "physics_2d_server_sw.h" - _FORCE_INLINE_ static bool _match_object_type_query(CollisionObject2DSW *p_object, uint32_t p_layer_mask, uint32_t p_type_mask) { - if ((p_object->get_layer_mask()&p_layer_mask)==0) + if ((p_object->get_layer_mask() & p_layer_mask) == 0) return false; - if (p_object->get_type()==CollisionObject2DSW::TYPE_AREA) - return p_type_mask&Physics2DDirectSpaceState::TYPE_MASK_AREA; - - Body2DSW *body = static_cast<Body2DSW*>(p_object); + if (p_object->get_type() == CollisionObject2DSW::TYPE_AREA) + return p_type_mask & Physics2DDirectSpaceState::TYPE_MASK_AREA; - return (1<<body->get_mode())&p_type_mask; + Body2DSW *body = static_cast<Body2DSW *>(p_object); + return (1 << body->get_mode()) & p_type_mask; } +int Physics2DDirectSpaceStateSW::intersect_point(const Vector2 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_layer_mask, uint32_t p_object_type_mask, bool p_pick_point) { -int Physics2DDirectSpaceStateSW::intersect_point(const Vector2& p_point,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude,uint32_t p_layer_mask,uint32_t p_object_type_mask,bool p_pick_point) { - - if (p_result_max<=0) + if (p_result_max <= 0) return 0; Rect2 aabb; - aabb.pos=p_point-Vector2(0.00001,0.00001); - aabb.size=Vector2(0.00002,0.00002); + aabb.pos = p_point - Vector2(0.00001, 0.00001); + aabb.size = Vector2(0.00002, 0.00002); - int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,Space2DSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results); + int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, Space2DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); - int cc=0; + int cc = 0; - for(int i=0;i<amount;i++) { + for (int i = 0; i < amount; i++) { - if (!_match_object_type_query(space->intersection_query_results[i],p_layer_mask,p_object_type_mask)) + if (!_match_object_type_query(space->intersection_query_results[i], p_layer_mask, p_object_type_mask)) continue; - if (p_exclude.has( space->intersection_query_results[i]->get_self())) + if (p_exclude.has(space->intersection_query_results[i]->get_self())) continue; - const CollisionObject2DSW *col_obj=space->intersection_query_results[i]; + const CollisionObject2DSW *col_obj = space->intersection_query_results[i]; if (p_pick_point && !col_obj->is_pickable()) continue; - int shape_idx=space->intersection_query_subindex_results[i]; + int shape_idx = space->intersection_query_subindex_results[i]; - Shape2DSW * shape = col_obj->get_shape(shape_idx); + Shape2DSW *shape = col_obj->get_shape(shape_idx); Vector2 local_point = (col_obj->get_transform() * col_obj->get_shape_transform(shape_idx)).affine_inverse().xform(p_point); if (!shape->contains_point(local_point)) continue; - if (cc>=p_result_max) + if (cc >= p_result_max) 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; - r_results[cc].metadata=col_obj->get_shape_metadata(shape_idx); + 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; + r_results[cc].metadata = col_obj->get_shape_metadata(shape_idx); cc++; } return cc; - - } -bool Physics2DDirectSpaceStateSW::intersect_ray(const Vector2& p_from, const Vector2& p_to,RayResult &r_result,const Set<RID>& p_exclude,uint32_t p_layer_mask,uint32_t p_object_type_mask) { - +bool Physics2DDirectSpaceStateSW::intersect_ray(const Vector2 &p_from, const Vector2 &p_to, RayResult &r_result, const Set<RID> &p_exclude, uint32_t p_layer_mask, uint32_t p_object_type_mask) { + ERR_FAIL_COND_V(space->locked, false); - ERR_FAIL_COND_V(space->locked,false); - - Vector2 begin,end; + Vector2 begin, end; Vector2 normal; - begin=p_from; - end=p_to; - normal=(end-begin).normalized(); + 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); + 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; + bool collided = false; + Vector2 res_point, res_normal; int res_shape; const CollisionObject2DSW *res_obj; - real_t min_d=1e10; + real_t min_d = 1e10; + for (int i = 0; i < amount; i++) { - for(int i=0;i<amount;i++) { - - if (!_match_object_type_query(space->intersection_query_results[i],p_layer_mask,p_object_type_mask)) + if (!_match_object_type_query(space->intersection_query_results[i], p_layer_mask, p_object_type_mask)) continue; - if (p_exclude.has( space->intersection_query_results[i]->get_self())) + if (p_exclude.has(space->intersection_query_results[i]->get_self())) continue; - const CollisionObject2DSW *col_obj=space->intersection_query_results[i]; + const CollisionObject2DSW *col_obj = space->intersection_query_results[i]; - int shape_idx=space->intersection_query_subindex_results[i]; + int shape_idx = space->intersection_query_subindex_results[i]; Transform2D inv_xform = col_obj->get_shape_inv_transform(shape_idx) * col_obj->get_inv_transform(); Vector2 local_from = inv_xform.xform(begin); @@ -146,131 +138,113 @@ bool Physics2DDirectSpaceStateSW::intersect_ray(const Vector2& p_from, const Vec 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)) { - + Vector2 shape_point, shape_normal; + if (shape->intersect_segment(local_from, local_to, shape_point, shape_normal)) { Transform2D xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); - shape_point=xform.xform(shape_point); + shape_point = xform.xform(shape_point); real_t ld = normal.dot(shape_point); + if (ld < min_d) { - 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; + 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.metadata=res_obj->get_shape_metadata(res_shape); - r_result.position=res_point; - r_result.rid=res_obj->get_self(); - r_result.shape=res_shape; + 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.metadata = res_obj->get_shape_metadata(res_shape); + 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 Transform2D &p_xform, const Vector2 &p_motion, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_layer_mask, uint32_t p_object_type_mask) { -int Physics2DDirectSpaceStateSW::intersect_shape(const RID& p_shape, const Transform2D& p_xform,const Vector2& p_motion,real_t p_margin,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude,uint32_t p_layer_mask,uint32_t p_object_type_mask) { - - if (p_result_max<=0) + if (p_result_max <= 0) return 0; Shape2DSW *shape = Physics2DServerSW::singletonsw->shape_owner.get(p_shape); - ERR_FAIL_COND_V(!shape,0); + ERR_FAIL_COND_V(!shape, 0); Rect2 aabb = p_xform.xform(shape->get_aabb()); - aabb=aabb.grow(p_margin); + aabb = aabb.grow(p_margin); - int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,p_result_max,space->intersection_query_subindex_results); + int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, p_result_max, space->intersection_query_subindex_results); - int cc=0; + int cc = 0; - for(int i=0;i<amount;i++) { + for (int i = 0; i < amount; i++) { - if (!_match_object_type_query(space->intersection_query_results[i],p_layer_mask,p_object_type_mask)) + if (!_match_object_type_query(space->intersection_query_results[i], p_layer_mask, p_object_type_mask)) continue; - if (p_exclude.has( space->intersection_query_results[i]->get_self())) + 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]; - const CollisionObject2DSW *col_obj=space->intersection_query_results[i]; - int shape_idx=space->intersection_query_subindex_results[i]; - - if (!CollisionSolver2DSW::solve(shape,p_xform,p_motion,col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx),Vector2(),NULL,NULL,NULL,p_margin)) + if (!CollisionSolver2DSW::solve(shape, p_xform, p_motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), NULL, NULL, NULL, p_margin)) 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; - r_results[cc].metadata=col_obj->get_shape_metadata(shape_idx); + 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; + r_results[cc].metadata = col_obj->get_shape_metadata(shape_idx); cc++; - } return cc; - } - - -bool Physics2DDirectSpaceStateSW::cast_motion(const RID& p_shape, const Transform2D& p_xform,const Vector2& p_motion,real_t p_margin,real_t &p_closest_safe,real_t &p_closest_unsafe, const Set<RID>& p_exclude,uint32_t p_layer_mask,uint32_t p_object_type_mask) { - - +bool Physics2DDirectSpaceStateSW::cast_motion(const RID &p_shape, const Transform2D &p_xform, const Vector2 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set<RID> &p_exclude, uint32_t p_layer_mask, uint32_t p_object_type_mask) { Shape2DSW *shape = Physics2DServerSW::singletonsw->shape_owner.get(p_shape); - ERR_FAIL_COND_V(!shape,false); + ERR_FAIL_COND_V(!shape, false); Rect2 aabb = p_xform.xform(shape->get_aabb()); - aabb=aabb.merge(Rect2(aabb.pos+p_motion,aabb.size)); //motion - aabb=aabb.grow(p_margin); + aabb = aabb.merge(Rect2(aabb.pos + p_motion, aabb.size)); //motion + aabb = aabb.grow(p_margin); /* if (p_motion!=Vector2()) print_line(p_motion); */ - int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,Space2DSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results); - - real_t best_safe=1; - real_t best_unsafe=1; + int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, Space2DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); - for(int i=0;i<amount;i++) { + real_t best_safe = 1; + real_t best_unsafe = 1; + for (int i = 0; i < amount; i++) { - if (!_match_object_type_query(space->intersection_query_results[i],p_layer_mask,p_object_type_mask)) + if (!_match_object_type_query(space->intersection_query_results[i], p_layer_mask, p_object_type_mask)) continue; - if (p_exclude.has( space->intersection_query_results[i]->get_self())) + if (p_exclude.has(space->intersection_query_results[i]->get_self())) continue; //ignore excluded - - const CollisionObject2DSW *col_obj=space->intersection_query_results[i]; - int shape_idx=space->intersection_query_subindex_results[i]; - + const CollisionObject2DSW *col_obj = space->intersection_query_results[i]; + int shape_idx = space->intersection_query_subindex_results[i]; /*if (col_obj->get_type()==CollisionObject2DSW::TYPE_BODY) { @@ -281,21 +255,19 @@ bool Physics2DDirectSpaceStateSW::cast_motion(const RID& p_shape, const Transfor } }*/ - Transform2D col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); //test initial overlap, does it collide if going all the way? - if (!CollisionSolver2DSW::solve(shape,p_xform,p_motion,col_obj->get_shape(shape_idx),col_obj_xform,Vector2() ,NULL,NULL,NULL,p_margin)) { + if (!CollisionSolver2DSW::solve(shape, p_xform, p_motion, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), NULL, NULL, NULL, p_margin)) { continue; } - //test initial overlap - if (CollisionSolver2DSW::solve(shape,p_xform,Vector2(),col_obj->get_shape(shape_idx),col_obj_xform,Vector2() ,NULL,NULL,NULL,p_margin)) { + if (CollisionSolver2DSW::solve(shape, p_xform, Vector2(), col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), NULL, NULL, NULL, p_margin)) { - if (col_obj->get_type()==CollisionObject2DSW::TYPE_BODY) { + if (col_obj->get_type() == CollisionObject2DSW::TYPE_BODY) { //if one way collision direction ignore initial overlap - const Body2DSW *body=static_cast<const Body2DSW*>(col_obj); - if (body->get_one_way_collision_direction()!=Vector2()) { + const Body2DSW *body = static_cast<const Body2DSW *>(col_obj); + if (body->get_one_way_collision_direction() != Vector2()) { continue; } } @@ -303,131 +275,119 @@ bool Physics2DDirectSpaceStateSW::cast_motion(const RID& p_shape, const Transfor return false; } - //just do kinematic solving - real_t low=0; - real_t hi=1; - Vector2 mnormal=p_motion.normalized(); + real_t low = 0; + real_t hi = 1; + Vector2 mnormal = p_motion.normalized(); - for(int i=0;i<8;i++) { //steps should be customizable.. + for (int i = 0; i < 8; i++) { //steps should be customizable.. - real_t ofs = (low+hi)*0.5; + real_t ofs = (low + hi) * 0.5; - Vector2 sep=mnormal; //important optimization for this to work fast enough - bool collided = CollisionSolver2DSW::solve(shape,p_xform,p_motion*ofs,col_obj->get_shape(shape_idx),col_obj_xform,Vector2(),NULL,NULL,&sep,p_margin); + Vector2 sep = mnormal; //important optimization for this to work fast enough + bool collided = CollisionSolver2DSW::solve(shape, p_xform, p_motion * ofs, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), NULL, NULL, &sep, p_margin); if (collided) { - hi=ofs; + hi = ofs; } else { - low=ofs; + low = ofs; } } - if (col_obj->get_type()==CollisionObject2DSW::TYPE_BODY) { + if (col_obj->get_type() == CollisionObject2DSW::TYPE_BODY) { - const Body2DSW *body=static_cast<const Body2DSW*>(col_obj); - if (body->get_one_way_collision_direction()!=Vector2()) { + const Body2DSW *body = static_cast<const Body2DSW *>(col_obj); + if (body->get_one_way_collision_direction() != Vector2()) { Vector2 cd[2]; Physics2DServerSW::CollCbkData cbk; - cbk.max=1; - cbk.amount=0; - cbk.ptr=cd; - cbk.valid_dir=body->get_one_way_collision_direction(); - cbk.valid_depth=body->get_one_way_collision_max_depth(); - - Vector2 sep=mnormal; //important optimization for this to work fast enough - bool collided = CollisionSolver2DSW::solve(shape,p_xform,p_motion*(hi+space->contact_max_allowed_penetration),col_obj->get_shape(shape_idx),col_obj_xform,Vector2(),Physics2DServerSW::_shape_col_cbk,&cbk,&sep,p_margin); - if (!collided || cbk.amount==0) { + cbk.max = 1; + cbk.amount = 0; + cbk.ptr = cd; + cbk.valid_dir = body->get_one_way_collision_direction(); + cbk.valid_depth = body->get_one_way_collision_max_depth(); + + Vector2 sep = mnormal; //important optimization for this to work fast enough + bool collided = CollisionSolver2DSW::solve(shape, p_xform, p_motion * (hi + space->contact_max_allowed_penetration), col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), Physics2DServerSW::_shape_col_cbk, &cbk, &sep, p_margin); + if (!collided || cbk.amount == 0) { continue; } - } } - - if (low<best_safe) { - best_safe=low; - best_unsafe=hi; + if (low < best_safe) { + best_safe = low; + best_unsafe = hi; } - } - p_closest_safe=best_safe; - p_closest_unsafe=best_unsafe; + p_closest_safe = best_safe; + p_closest_unsafe = best_unsafe; return true; - - } +bool Physics2DDirectSpaceStateSW::collide_shape(RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, real_t p_margin, Vector2 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude, uint32_t p_layer_mask, uint32_t p_object_type_mask) { -bool Physics2DDirectSpaceStateSW::collide_shape(RID p_shape, const Transform2D& p_shape_xform,const Vector2& p_motion,real_t p_margin,Vector2 *r_results,int p_result_max,int &r_result_count, const Set<RID>& p_exclude,uint32_t p_layer_mask,uint32_t p_object_type_mask) { - - - if (p_result_max<=0) + if (p_result_max <= 0) return 0; Shape2DSW *shape = Physics2DServerSW::singletonsw->shape_owner.get(p_shape); - ERR_FAIL_COND_V(!shape,0); + ERR_FAIL_COND_V(!shape, 0); Rect2 aabb = p_shape_xform.xform(shape->get_aabb()); - aabb=aabb.merge(Rect2(aabb.pos+p_motion,aabb.size)); //motion - aabb=aabb.grow(p_margin); + aabb = aabb.merge(Rect2(aabb.pos + p_motion, aabb.size)); //motion + aabb = aabb.grow(p_margin); - int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,Space2DSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results); + int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, Space2DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); - bool collided=false; - r_result_count=0; + bool collided = false; + r_result_count = 0; Physics2DServerSW::CollCbkData cbk; - cbk.max=p_result_max; - cbk.amount=0; - cbk.ptr=r_results; - CollisionSolver2DSW::CallbackResult cbkres=NULL; - - Physics2DServerSW::CollCbkData *cbkptr=NULL; - if (p_result_max>0) { - cbkptr=&cbk; - cbkres=Physics2DServerSW::_shape_col_cbk; + cbk.max = p_result_max; + cbk.amount = 0; + cbk.ptr = r_results; + CollisionSolver2DSW::CallbackResult cbkres = NULL; + + Physics2DServerSW::CollCbkData *cbkptr = NULL; + if (p_result_max > 0) { + cbkptr = &cbk; + cbkres = Physics2DServerSW::_shape_col_cbk; } + for (int i = 0; i < amount; i++) { - for(int i=0;i<amount;i++) { - - if (!_match_object_type_query(space->intersection_query_results[i],p_layer_mask,p_object_type_mask)) + if (!_match_object_type_query(space->intersection_query_results[i], p_layer_mask, p_object_type_mask)) continue; - const CollisionObject2DSW *col_obj=space->intersection_query_results[i]; - int shape_idx=space->intersection_query_subindex_results[i]; + const CollisionObject2DSW *col_obj = space->intersection_query_results[i]; + int shape_idx = space->intersection_query_subindex_results[i]; - if (p_exclude.has( col_obj->get_self() )) + if (p_exclude.has(col_obj->get_self())) continue; - if (col_obj->get_type()==CollisionObject2DSW::TYPE_BODY) { + if (col_obj->get_type() == CollisionObject2DSW::TYPE_BODY) { - const Body2DSW *body=static_cast<const Body2DSW*>(col_obj); - cbk.valid_dir=body->get_one_way_collision_direction(); - cbk.valid_depth=body->get_one_way_collision_max_depth(); + const Body2DSW *body = static_cast<const Body2DSW *>(col_obj); + cbk.valid_dir = body->get_one_way_collision_direction(); + cbk.valid_depth = body->get_one_way_collision_max_depth(); } else { - cbk.valid_dir=Vector2(); - cbk.valid_depth=0; + cbk.valid_dir = Vector2(); + cbk.valid_depth = 0; } - if (CollisionSolver2DSW::solve(shape,p_shape_xform,p_motion,col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx),Vector2(),cbkres,cbkptr,NULL,p_margin)) { - collided=p_result_max==0 || cbk.amount>0; + if (CollisionSolver2DSW::solve(shape, p_shape_xform, p_motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), cbkres, cbkptr, NULL, p_margin)) { + collided = p_result_max == 0 || cbk.amount > 0; } - } - - r_result_count=cbk.amount; + r_result_count = cbk.amount; return collided; } - struct _RestCallbackData2D { const CollisionObject2DSW *object; @@ -441,20 +401,18 @@ struct _RestCallbackData2D { real_t valid_depth; }; -static void _rest_cbk_result(const Vector2& p_point_A,const Vector2& p_point_B,void *p_userdata) { - +static void _rest_cbk_result(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_userdata) { - _RestCallbackData2D *rd=(_RestCallbackData2D*)p_userdata; + _RestCallbackData2D *rd = (_RestCallbackData2D *)p_userdata; - if (rd->valid_dir!=Vector2()) { + if (rd->valid_dir != Vector2()) { - if (rd->valid_dir!=Vector2()) { - if (p_point_A.distance_squared_to(p_point_B)>rd->valid_depth*rd->valid_depth) + if (rd->valid_dir != Vector2()) { + if (p_point_A.distance_squared_to(p_point_B) > rd->valid_depth * rd->valid_depth) return; - if (rd->valid_dir.dot((p_point_A-p_point_B).normalized())<Math_PI*0.25) + if (rd->valid_dir.dot((p_point_A - p_point_B).normalized()) < Math_PI * 0.25) return; } - } Vector2 contact_rel = p_point_B - p_point_A; @@ -462,139 +420,121 @@ static void _rest_cbk_result(const Vector2& p_point_A,const Vector2& p_point_B,v if (len <= rd->best_len) return; - - rd->best_len=len; - rd->best_contact=p_point_B; - rd->best_normal=contact_rel/len; - rd->best_object=rd->object; - rd->best_shape=rd->shape; - - + rd->best_len = len; + rd->best_contact = p_point_B; + rd->best_normal = contact_rel / len; + rd->best_object = rd->object; + rd->best_shape = rd->shape; } - -bool Physics2DDirectSpaceStateSW::rest_info(RID p_shape, const Transform2D& p_shape_xform,const Vector2& p_motion,real_t p_margin,ShapeRestInfo *r_info, const Set<RID>& p_exclude,uint32_t p_layer_mask,uint32_t p_object_type_mask) { - +bool Physics2DDirectSpaceStateSW::rest_info(RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude, uint32_t p_layer_mask, uint32_t p_object_type_mask) { Shape2DSW *shape = Physics2DServerSW::singletonsw->shape_owner.get(p_shape); - ERR_FAIL_COND_V(!shape,0); + ERR_FAIL_COND_V(!shape, 0); Rect2 aabb = p_shape_xform.xform(shape->get_aabb()); - aabb=aabb.merge(Rect2(aabb.pos+p_motion,aabb.size)); //motion - aabb=aabb.grow(p_margin); + aabb = aabb.merge(Rect2(aabb.pos + p_motion, aabb.size)); //motion + aabb = aabb.grow(p_margin); - int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,Space2DSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results); + int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, Space2DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); _RestCallbackData2D rcd; - rcd.best_len=0; - rcd.best_object=NULL; - rcd.best_shape=0; - - for(int i=0;i<amount;i++) { + rcd.best_len = 0; + rcd.best_object = NULL; + rcd.best_shape = 0; + for (int i = 0; i < amount; i++) { - if (!_match_object_type_query(space->intersection_query_results[i],p_layer_mask,p_object_type_mask)) + if (!_match_object_type_query(space->intersection_query_results[i], p_layer_mask, p_object_type_mask)) continue; - const CollisionObject2DSW *col_obj=space->intersection_query_results[i]; - int shape_idx=space->intersection_query_subindex_results[i]; + const CollisionObject2DSW *col_obj = space->intersection_query_results[i]; + int shape_idx = space->intersection_query_subindex_results[i]; - if (p_exclude.has( col_obj->get_self() )) + if (p_exclude.has(col_obj->get_self())) continue; - if (col_obj->get_type()==CollisionObject2DSW::TYPE_BODY) { + if (col_obj->get_type() == CollisionObject2DSW::TYPE_BODY) { - const Body2DSW *body=static_cast<const Body2DSW*>(col_obj); - rcd.valid_dir=body->get_one_way_collision_direction(); - rcd.valid_depth=body->get_one_way_collision_max_depth(); + const Body2DSW *body = static_cast<const Body2DSW *>(col_obj); + rcd.valid_dir = body->get_one_way_collision_direction(); + rcd.valid_depth = body->get_one_way_collision_max_depth(); } else { - rcd.valid_dir=Vector2(); - rcd.valid_depth=0; + rcd.valid_dir = Vector2(); + rcd.valid_depth = 0; } - - rcd.object=col_obj; - rcd.shape=shape_idx; - bool sc = CollisionSolver2DSW::solve(shape,p_shape_xform,p_motion,col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx),Vector2() ,_rest_cbk_result,&rcd,NULL,p_margin); + rcd.object = col_obj; + rcd.shape = shape_idx; + bool sc = CollisionSolver2DSW::solve(shape, p_shape_xform, p_motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), _rest_cbk_result, &rcd, NULL, p_margin); if (!sc) continue; - - } - if (rcd.best_len==0) + if (rcd.best_len == 0) return false; - r_info->collider_id=rcd.best_object->get_instance_id(); - r_info->shape=rcd.best_shape; - r_info->normal=rcd.best_normal; - r_info->point=rcd.best_contact; - r_info->rid=rcd.best_object->get_self(); - r_info->metadata=rcd.best_object->get_shape_metadata(rcd.best_shape); - if (rcd.best_object->get_type()==CollisionObject2DSW::TYPE_BODY) { + r_info->collider_id = rcd.best_object->get_instance_id(); + r_info->shape = rcd.best_shape; + r_info->normal = rcd.best_normal; + r_info->point = rcd.best_contact; + r_info->rid = rcd.best_object->get_self(); + r_info->metadata = rcd.best_object->get_shape_metadata(rcd.best_shape); + if (rcd.best_object->get_type() == CollisionObject2DSW::TYPE_BODY) { - const Body2DSW *body = static_cast<const Body2DSW*>(rcd.best_object); - Vector2 rel_vec = r_info->point-body->get_transform().get_origin(); + const Body2DSW *body = static_cast<const Body2DSW *>(rcd.best_object); + Vector2 rel_vec = r_info->point - body->get_transform().get_origin(); r_info->linear_velocity = Vector2(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity(); } else { - r_info->linear_velocity=Vector2(); + r_info->linear_velocity = Vector2(); } return true; } - Physics2DDirectSpaceStateSW::Physics2DDirectSpaceStateSW() { - - space=NULL; + space = NULL; } - //////////////////////////////////////////////////////////////////////////////////////////////////////////// +int Space2DSW::_cull_aabb_for_body(Body2DSW *p_body, const Rect2 &p_aabb) { + int amount = broadphase->cull_aabb(p_aabb, intersection_query_results, INTERSECTION_QUERY_MAX, intersection_query_subindex_results); + for (int i = 0; i < amount; i++) { -int Space2DSW::_cull_aabb_for_body(Body2DSW *p_body,const Rect2& p_aabb) { - - - int amount = broadphase->cull_aabb(p_aabb,intersection_query_results,INTERSECTION_QUERY_MAX,intersection_query_subindex_results); + bool keep = true; - for(int i=0;i<amount;i++) { - - bool keep=true; - - if (intersection_query_results[i]==p_body) - keep=false; - else if (intersection_query_results[i]->get_type()==CollisionObject2DSW::TYPE_AREA) - keep=false; - else if ((static_cast<Body2DSW*>(intersection_query_results[i])->test_collision_mask(p_body))==0) - keep=false; - else if (static_cast<Body2DSW*>(intersection_query_results[i])->has_exception(p_body->get_self()) || p_body->has_exception(intersection_query_results[i]->get_self())) - keep=false; - else if (static_cast<Body2DSW*>(intersection_query_results[i])->is_shape_set_as_trigger(intersection_query_subindex_results[i])) - keep=false; + if (intersection_query_results[i] == p_body) + keep = false; + else if (intersection_query_results[i]->get_type() == CollisionObject2DSW::TYPE_AREA) + keep = false; + else if ((static_cast<Body2DSW *>(intersection_query_results[i])->test_collision_mask(p_body)) == 0) + keep = false; + else if (static_cast<Body2DSW *>(intersection_query_results[i])->has_exception(p_body->get_self()) || p_body->has_exception(intersection_query_results[i]->get_self())) + keep = false; + else if (static_cast<Body2DSW *>(intersection_query_results[i])->is_shape_set_as_trigger(intersection_query_subindex_results[i])) + keep = false; if (!keep) { - if (i<amount-1) { - SWAP(intersection_query_results[i],intersection_query_results[amount-1]); - SWAP(intersection_query_subindex_results[i],intersection_query_subindex_results[amount-1]); - + if (i < amount - 1) { + SWAP(intersection_query_results[i], intersection_query_results[amount - 1]); + SWAP(intersection_query_subindex_results[i], intersection_query_subindex_results[amount - 1]); } amount--; i--; - } } return amount; } -bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, const Vector2&p_motion, real_t p_margin, Physics2DServer::MotionResult *r_result) { +bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, const Vector2 &p_motion, real_t p_margin, Physics2DServer::MotionResult *r_result) { //give me back regular physics engine logic //this is madness @@ -604,65 +544,61 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co //but is it right? who knows at this point.. if (r_result) { - r_result->collider_id=0; - r_result->collider_shape=0; - + r_result->collider_id = 0; + r_result->collider_shape = 0; } Rect2 body_aabb; - for(int i=0;i<p_body->get_shape_count();i++) { + for (int i = 0; i < p_body->get_shape_count(); i++) { - if (i==0) - body_aabb=p_body->get_shape_aabb(i); + if (i == 0) + body_aabb = p_body->get_shape_aabb(i); else - body_aabb=body_aabb.merge(p_body->get_shape_aabb(i)); + body_aabb = body_aabb.merge(p_body->get_shape_aabb(i)); } - body_aabb=body_aabb.grow(p_margin); + body_aabb = body_aabb.grow(p_margin); Transform2D body_transform = p_from; - { //STEP 1, FREE BODY IF STUCK const int max_results = 32; - int recover_attempts=4; - Vector2 sr[max_results*2]; + int recover_attempts = 4; + Vector2 sr[max_results * 2]; do { Physics2DServerSW::CollCbkData cbk; - cbk.max=max_results; - cbk.amount=0; - cbk.ptr=sr; - - - CollisionSolver2DSW::CallbackResult cbkres=NULL; + cbk.max = max_results; + cbk.amount = 0; + cbk.ptr = sr; - Physics2DServerSW::CollCbkData *cbkptr=NULL; - cbkptr=&cbk; - cbkres=Physics2DServerSW::_shape_col_cbk; + CollisionSolver2DSW::CallbackResult cbkres = NULL; - bool collided=false; + Physics2DServerSW::CollCbkData *cbkptr = NULL; + cbkptr = &cbk; + cbkres = Physics2DServerSW::_shape_col_cbk; + bool collided = false; - int amount = _cull_aabb_for_body(p_body,body_aabb); + int amount = _cull_aabb_for_body(p_body, body_aabb); - for(int j=0;j<p_body->get_shape_count();j++) { + for (int j = 0; j < p_body->get_shape_count(); j++) { if (p_body->is_shape_set_as_trigger(j)) continue; Transform2D body_shape_xform = body_transform * p_body->get_shape_transform(j); Shape2DSW *body_shape = p_body->get_shape(j); - for(int i=0;i<amount;i++) { + for (int i = 0; i < amount; i++) { - const CollisionObject2DSW *col_obj=intersection_query_results[i]; - int shape_idx=intersection_query_subindex_results[i]; + const CollisionObject2DSW *col_obj = intersection_query_results[i]; + int shape_idx = intersection_query_subindex_results[i]; - if (col_obj->get_type()==CollisionObject2DSW::TYPE_BODY) { + if (col_obj->get_type() == CollisionObject2DSW::TYPE_BODY) { - const Body2DSW *body=static_cast<const Body2DSW*>(col_obj); + const Body2DSW *body = static_cast<const Body2DSW *>(col_obj); Vector2 cdir = body->get_one_way_collision_direction(); /* @@ -670,31 +606,29 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co continue; */ - cbk.valid_dir=cdir; - cbk.valid_depth=body->get_one_way_collision_max_depth(); + cbk.valid_dir = cdir; + cbk.valid_depth = body->get_one_way_collision_max_depth(); } else { - cbk.valid_dir=Vector2(); - cbk.valid_depth=0; + cbk.valid_dir = Vector2(); + cbk.valid_depth = 0; } - if (CollisionSolver2DSW::solve(body_shape,body_shape_xform,Vector2(),col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx),Vector2(),cbkres,cbkptr,NULL,p_margin)) { - collided=cbk.amount>0; + if (CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), cbkres, cbkptr, NULL, p_margin)) { + collided = cbk.amount > 0; } } } - if (!collided) { break; } Vector2 recover_motion; + for (int i = 0; i < cbk.amount; i++) { - for(int i=0;i<cbk.amount;i++) { - - Vector2 a = sr[i*2+0]; - Vector2 b = sr[i*2+1]; + Vector2 a = sr[i * 2 + 0]; + Vector2 b = sr[i * 2 + 1]; #if 0 Vector2 rel = b-a; @@ -710,38 +644,36 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co if (d<margin) continue; #endif - recover_motion+=(b-a)*0.4; + recover_motion += (b - a) * 0.4; } - if (recover_motion==Vector2()) { - collided=false; + if (recover_motion == Vector2()) { + collided = false; break; } - body_transform.elements[2]+=recover_motion; - body_aabb.pos+=recover_motion; + body_transform.elements[2] += recover_motion; + body_aabb.pos += recover_motion; recover_attempts--; } while (recover_attempts); } - - real_t safe = 1.0; real_t unsafe = 1.0; - int best_shape=-1; + int best_shape = -1; { // STEP 2 ATTEMPT MOTION - Rect2 motion_aabb=body_aabb; - motion_aabb.pos+=p_motion; - motion_aabb=motion_aabb.merge(body_aabb); + Rect2 motion_aabb = body_aabb; + motion_aabb.pos += p_motion; + motion_aabb = motion_aabb.merge(body_aabb); - int amount = _cull_aabb_for_body(p_body,motion_aabb); + int amount = _cull_aabb_for_body(p_body, motion_aabb); - for(int j=0;j<p_body->get_shape_count();j++) { + for (int j = 0; j < p_body->get_shape_count(); j++) { if (p_body->is_shape_set_as_trigger(j)) continue; @@ -749,201 +681,189 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co Transform2D body_shape_xform = body_transform * p_body->get_shape_transform(j); Shape2DSW *body_shape = p_body->get_shape(j); - bool stuck=false; + bool stuck = false; - real_t best_safe=1; - real_t best_unsafe=1; + real_t best_safe = 1; + real_t best_unsafe = 1; - for(int i=0;i<amount;i++) { - - const CollisionObject2DSW *col_obj=intersection_query_results[i]; - int shape_idx=intersection_query_subindex_results[i]; + for (int i = 0; i < amount; i++) { + const CollisionObject2DSW *col_obj = intersection_query_results[i]; + int shape_idx = intersection_query_subindex_results[i]; Transform2D col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); //test initial overlap, does it collide if going all the way? - if (!CollisionSolver2DSW::solve(body_shape,body_shape_xform,p_motion,col_obj->get_shape(shape_idx),col_obj_xform,Vector2() ,NULL,NULL,NULL,0)) { + if (!CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_motion, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), NULL, NULL, NULL, 0)) { continue; } - //test initial overlap - if (CollisionSolver2DSW::solve(body_shape,body_shape_xform,Vector2(),col_obj->get_shape(shape_idx),col_obj_xform,Vector2() ,NULL,NULL,NULL,0)) { + if (CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), NULL, NULL, NULL, 0)) { - if (col_obj->get_type()==CollisionObject2DSW::TYPE_BODY) { + if (col_obj->get_type() == CollisionObject2DSW::TYPE_BODY) { //if one way collision direction ignore initial overlap - const Body2DSW *body=static_cast<const Body2DSW*>(col_obj); - if (body->get_one_way_collision_direction()!=Vector2()) { + const Body2DSW *body = static_cast<const Body2DSW *>(col_obj); + if (body->get_one_way_collision_direction() != Vector2()) { continue; } } - stuck=true; + stuck = true; break; } - //just do kinematic solving - real_t low=0; - real_t hi=1; - Vector2 mnormal=p_motion.normalized(); + real_t low = 0; + real_t hi = 1; + Vector2 mnormal = p_motion.normalized(); - for(int i=0;i<8;i++) { //steps should be customizable.. + for (int i = 0; i < 8; i++) { //steps should be customizable.. - real_t ofs = (low+hi)*0.5; + real_t ofs = (low + hi) * 0.5; - Vector2 sep=mnormal; //important optimization for this to work fast enough - bool collided = CollisionSolver2DSW::solve(body_shape,body_shape_xform,p_motion*ofs,col_obj->get_shape(shape_idx),col_obj_xform,Vector2(),NULL,NULL,&sep,0); + Vector2 sep = mnormal; //important optimization for this to work fast enough + bool collided = CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_motion * ofs, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), NULL, NULL, &sep, 0); if (collided) { - hi=ofs; + hi = ofs; } else { - low=ofs; + low = ofs; } } - if (col_obj->get_type()==CollisionObject2DSW::TYPE_BODY) { + if (col_obj->get_type() == CollisionObject2DSW::TYPE_BODY) { - const Body2DSW *body=static_cast<const Body2DSW*>(col_obj); - if (body->get_one_way_collision_direction()!=Vector2()) { + const Body2DSW *body = static_cast<const Body2DSW *>(col_obj); + if (body->get_one_way_collision_direction() != Vector2()) { Vector2 cd[2]; Physics2DServerSW::CollCbkData cbk; - cbk.max=1; - cbk.amount=0; - cbk.ptr=cd; - cbk.valid_dir=body->get_one_way_collision_direction(); - cbk.valid_depth=body->get_one_way_collision_max_depth(); - - Vector2 sep=mnormal; //important optimization for this to work fast enough - bool collided = CollisionSolver2DSW::solve(body_shape,body_shape_xform,p_motion*(hi+contact_max_allowed_penetration),col_obj->get_shape(shape_idx),col_obj_xform,Vector2(),Physics2DServerSW::_shape_col_cbk,&cbk,&sep,0); - if (!collided || cbk.amount==0) { + cbk.max = 1; + cbk.amount = 0; + cbk.ptr = cd; + cbk.valid_dir = body->get_one_way_collision_direction(); + cbk.valid_depth = body->get_one_way_collision_max_depth(); + + Vector2 sep = mnormal; //important optimization for this to work fast enough + bool collided = CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_motion * (hi + contact_max_allowed_penetration), col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), Physics2DServerSW::_shape_col_cbk, &cbk, &sep, 0); + if (!collided || cbk.amount == 0) { continue; } - } } - - if (low<best_safe) { - best_safe=low; - best_unsafe=hi; + if (low < best_safe) { + best_safe = low; + best_unsafe = hi; } } if (stuck) { - safe=0; - unsafe=0; - best_shape=j; //sadly it's the best + safe = 0; + unsafe = 0; + best_shape = j; //sadly it's the best break; } - if (best_safe==1.0) { + if (best_safe == 1.0) { continue; } if (best_safe < safe) { - safe=best_safe; - unsafe=best_unsafe; - best_shape=j; + safe = best_safe; + unsafe = best_unsafe; + best_shape = j; } } } - bool collided=false; - if (safe>=1) { + bool collided = false; + if (safe >= 1) { //not collided - collided=false; + collided = false; if (r_result) { - r_result->motion=p_motion; - r_result->remainder=Vector2(); - r_result->motion+=(body_transform.get_origin()-p_from.get_origin()); + r_result->motion = p_motion; + r_result->remainder = Vector2(); + r_result->motion += (body_transform.get_origin() - p_from.get_origin()); } } else { //it collided, let's get the rest info in unsafe advance Transform2D ugt = body_transform; - ugt.elements[2]+=p_motion*unsafe; + ugt.elements[2] += p_motion * unsafe; _RestCallbackData2D rcd; - rcd.best_len=0; - rcd.best_object=NULL; - rcd.best_shape=0; + rcd.best_len = 0; + rcd.best_object = NULL; + rcd.best_shape = 0; Transform2D body_shape_xform = ugt * p_body->get_shape_transform(best_shape); Shape2DSW *body_shape = p_body->get_shape(best_shape); - body_aabb.pos+=p_motion*unsafe; - - int amount = _cull_aabb_for_body(p_body,body_aabb); + body_aabb.pos += p_motion * unsafe; + int amount = _cull_aabb_for_body(p_body, body_aabb); - for(int i=0;i<amount;i++) { + for (int i = 0; i < amount; i++) { + const CollisionObject2DSW *col_obj = intersection_query_results[i]; + int shape_idx = intersection_query_subindex_results[i]; - const CollisionObject2DSW *col_obj=intersection_query_results[i]; - int shape_idx=intersection_query_subindex_results[i]; + if (col_obj->get_type() == CollisionObject2DSW::TYPE_BODY) { - if (col_obj->get_type()==CollisionObject2DSW::TYPE_BODY) { - - const Body2DSW *body=static_cast<const Body2DSW*>(col_obj); - rcd.valid_dir=body->get_one_way_collision_direction(); - rcd.valid_depth=body->get_one_way_collision_max_depth(); + const Body2DSW *body = static_cast<const Body2DSW *>(col_obj); + rcd.valid_dir = body->get_one_way_collision_direction(); + rcd.valid_depth = body->get_one_way_collision_max_depth(); } else { - rcd.valid_dir=Vector2(); - rcd.valid_depth=0; + rcd.valid_dir = Vector2(); + rcd.valid_depth = 0; } - - rcd.object=col_obj; - rcd.shape=shape_idx; - bool sc = CollisionSolver2DSW::solve(body_shape,body_shape_xform,Vector2(),col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx),Vector2() ,_rest_cbk_result,&rcd,NULL,p_margin); + rcd.object = col_obj; + rcd.shape = shape_idx; + bool sc = CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), _rest_cbk_result, &rcd, NULL, p_margin); if (!sc) continue; - } - if (rcd.best_len!=0) { + if (rcd.best_len != 0) { if (r_result) { - r_result->collider=rcd.best_object->get_self(); - r_result->collider_id=rcd.best_object->get_instance_id(); - r_result->collider_shape=rcd.best_shape; - r_result->collision_normal=rcd.best_normal; - r_result->collision_point=rcd.best_contact; - r_result->collider_metadata=rcd.best_object->get_shape_metadata(rcd.best_shape); - - const Body2DSW *body = static_cast<const Body2DSW*>(rcd.best_object); - Vector2 rel_vec = r_result->collision_point-body->get_transform().get_origin(); + r_result->collider = rcd.best_object->get_self(); + r_result->collider_id = rcd.best_object->get_instance_id(); + r_result->collider_shape = rcd.best_shape; + r_result->collision_normal = rcd.best_normal; + r_result->collision_point = rcd.best_contact; + r_result->collider_metadata = rcd.best_object->get_shape_metadata(rcd.best_shape); + + const Body2DSW *body = static_cast<const Body2DSW *>(rcd.best_object); + Vector2 rel_vec = r_result->collision_point - body->get_transform().get_origin(); r_result->collider_velocity = Vector2(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity(); - r_result->motion=safe*p_motion; - r_result->remainder=p_motion - safe * p_motion; - r_result->motion+=(body_transform.get_origin()-p_from.get_origin()); - + r_result->motion = safe * p_motion; + r_result->remainder = p_motion - safe * p_motion; + r_result->motion += (body_transform.get_origin() - p_from.get_origin()); } - collided=true; + collided = true; } else { if (r_result) { - r_result->motion=p_motion; - r_result->remainder=Vector2(); - r_result->motion+=(body_transform.get_origin()-p_from.get_origin()); + r_result->motion = p_motion; + r_result->remainder = Vector2(); + r_result->motion += (body_transform.get_origin() - p_from.get_origin()); } - collided=false; - + collided = false; } } return collided; - #if 0 //give me back regular physics engine logic //this is madness @@ -1101,82 +1021,71 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co return false; } +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) { - -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); + SWAP(A, B); + SWAP(p_subindex_A, p_subindex_B); + SWAP(type_A, type_B); } - Space2DSW *self = (Space2DSW*)p_self; + Space2DSW *self = (Space2DSW *)p_self; self->collision_pairs++; - if (type_A==CollisionObject2DSW::TYPE_AREA) { + if (type_A == CollisionObject2DSW::TYPE_AREA) { - Area2DSW *area=static_cast<Area2DSW*>(A); - if (type_B==CollisionObject2DSW::TYPE_AREA) { + Area2DSW *area = static_cast<Area2DSW *>(A); + if (type_B == CollisionObject2DSW::TYPE_AREA) { - Area2DSW *area_b=static_cast<Area2DSW*>(B); - Area2Pair2DSW *area2_pair = memnew(Area2Pair2DSW(area_b,p_subindex_B,area,p_subindex_A) ); + Area2DSW *area_b = static_cast<Area2DSW *>(B); + Area2Pair2DSW *area2_pair = memnew(Area2Pair2DSW(area_b, p_subindex_B, area, p_subindex_A)); return area2_pair; } else { - Body2DSW *body=static_cast<Body2DSW*>(B); - AreaPair2DSW *area_pair = memnew(AreaPair2DSW(body,p_subindex_B,area,p_subindex_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) ); + 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) { +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; + Space2DSW *self = (Space2DSW *)p_self; self->collision_pairs--; - Constraint2DSW *c = (Constraint2DSW*)p_data; + Constraint2DSW *c = (Constraint2DSW *)p_data; memdelete(c); } - -const SelfList<Body2DSW>::List& Space2DSW::get_active_body_list() const { +const SelfList<Body2DSW>::List &Space2DSW::get_active_body_list() const { return active_list; } -void Space2DSW::body_add_to_active_list(SelfList<Body2DSW>* p_body) { +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) { +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) { - +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) { +void Space2DSW::body_remove_from_inertia_update_list(SelfList<Body2DSW> *p_body) { inertia_update_list.remove(p_body); } @@ -1188,109 +1097,103 @@ BroadPhase2DSW *Space2DSW::get_broadphase() { void Space2DSW::add_object(CollisionObject2DSW *p_object) { - ERR_FAIL_COND( objects.has(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) ); + ERR_FAIL_COND(!objects.has(p_object)); objects.erase(p_object); } -const Set<CollisionObject2DSW*> &Space2DSW::get_objects() const { +const Set<CollisionObject2DSW *> &Space2DSW::get_objects() const { return objects; } -void Space2DSW::body_add_to_state_query_list(SelfList<Body2DSW>* p_body) { +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) { +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) { +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) { +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) { +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) { +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 { +const SelfList<Area2DSW>::List &Space2DSW::get_moved_area_list() const { return area_moved_list; } - void Space2DSW::call_queries() { - while(state_query_list.first()) { + while (state_query_list.first()) { - Body2DSW * b = state_query_list.first()->self(); + Body2DSW *b = state_query_list.first()->self(); b->call_queries(); state_query_list.remove(state_query_list.first()); } - while(monitor_query_list.first()) { + while (monitor_query_list.first()) { - Area2DSW * a = monitor_query_list.first()->self(); + Area2DSW *a = monitor_query_list.first()->self(); a->call_queries(); monitor_query_list.remove(monitor_query_list.first()); } - } void Space2DSW::setup() { - contact_debug_count=0; + contact_debug_count = 0; - while(inertia_update_list.first()) { + 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) { + 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_CONSTRAINT_DEFAULT_BIAS: constraint_bias=p_value; break; + 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_CONSTRAINT_DEFAULT_BIAS: constraint_bias = p_value; break; } } real_t Space2DSW::get_param(Physics2DServer::SpaceParameter p_param) const { - switch(p_param) { + 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; @@ -1305,12 +1208,12 @@ real_t Space2DSW::get_param(Physics2DServer::SpaceParameter p_param) const { void Space2DSW::lock() { - locked=true; + locked = true; } void Space2DSW::unlock() { - locked=false; + locked = false; } bool Space2DSW::is_locked() const { @@ -1325,43 +1228,36 @@ Physics2DDirectSpaceStateSW *Space2DSW::get_direct_state() { Space2DSW::Space2DSW() { + collision_pairs = 0; + active_objects = 0; + island_count = 0; - collision_pairs=0; - active_objects=0; - island_count=0; + contact_debug_count = 0; - contact_debug_count=0; - - locked=false; - contact_recycle_radius=1.0; - contact_max_separation=1.5; - contact_max_allowed_penetration= 0.3; + locked = false; + contact_recycle_radius = 1.0; + contact_max_separation = 1.5; + contact_max_allowed_penetration = 0.3; constraint_bias = 0.2; - body_linear_velocity_sleep_treshold=GLOBAL_DEF("physics/2d/sleep_threashold_linear",2.0); - body_angular_velocity_sleep_treshold=GLOBAL_DEF("physics/2d/sleep_threshold_angular",(8.0 / 180.0 * Math_PI)); - body_time_to_sleep=GLOBAL_DEF("physics/2d/time_before_sleep",0.5); - + body_linear_velocity_sleep_treshold = GLOBAL_DEF("physics/2d/sleep_threashold_linear", 2.0); + body_angular_velocity_sleep_treshold = GLOBAL_DEF("physics/2d/sleep_threshold_angular", (8.0 / 180.0 * Math_PI)); + body_time_to_sleep = GLOBAL_DEF("physics/2d/time_before_sleep", 0.5); 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; - + broadphase->set_pair_callback(_broadphase_pair, this); + broadphase->set_unpair_callback(_broadphase_unpair, this); + area = NULL; - for(int i=0;i<ELAPSED_TIME_MAX;i++) - elapsed_time[i]=0; + direct_access = memnew(Physics2DDirectSpaceStateSW); + direct_access->space = this; + for (int i = 0; i < ELAPSED_TIME_MAX; i++) + elapsed_time[i] = 0; } Space2DSW::~Space2DSW() { memdelete(broadphase); - memdelete( direct_access ); + memdelete(direct_access); } - - - diff --git a/servers/physics_2d/space_2d_sw.h b/servers/physics_2d/space_2d_sw.h index 071aa7bdfb..103f328ed2 100644 --- a/servers/physics_2d/space_2d_sw.h +++ b/servers/physics_2d/space_2d_sw.h @@ -29,40 +29,36 @@ #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 "body_2d_sw.h" +#include "body_pair_2d_sw.h" #include "broad_phase_2d_sw.h" #include "collision_object_2d_sw.h" #include "global_config.h" - +#include "hash_map.h" +#include "typedefs.h" class Physics2DDirectSpaceStateSW : public Physics2DDirectSpaceState { - GDCLASS( Physics2DDirectSpaceStateSW, Physics2DDirectSpaceState ); -public: + GDCLASS(Physics2DDirectSpaceStateSW, Physics2DDirectSpaceState); +public: Space2DSW *space; - virtual int intersect_point(const Vector2& p_point,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION, bool p_pick_point=false); - virtual bool intersect_ray(const Vector2& p_from, const Vector2& p_to,RayResult &r_result,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION); - virtual int intersect_shape(const RID& p_shape, const Transform2D& p_xform,const Vector2& p_motion,real_t p_margin,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION); - virtual bool cast_motion(const RID& p_shape, const Transform2D& p_xform,const Vector2& p_motion,real_t p_margin,real_t &p_closest_safe,real_t &p_closest_unsafe, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION); - virtual bool collide_shape(RID p_shape, const Transform2D& p_shape_xform,const Vector2& p_motion,real_t p_margin,Vector2 *r_results,int p_result_max,int &r_result_count, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION); - virtual bool rest_info(RID p_shape, const Transform2D& p_shape_xform,const Vector2& p_motion,real_t p_margin,ShapeRestInfo *r_info, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION); + virtual int intersect_point(const Vector2 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_layer_mask = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION, bool p_pick_point = false); + virtual bool intersect_ray(const Vector2 &p_from, const Vector2 &p_to, RayResult &r_result, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_layer_mask = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION); + virtual int intersect_shape(const RID &p_shape, const Transform2D &p_xform, const Vector2 &p_motion, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_layer_mask = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION); + virtual bool cast_motion(const RID &p_shape, const Transform2D &p_xform, const Vector2 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_layer_mask = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION); + virtual bool collide_shape(RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, real_t p_margin, Vector2 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_layer_mask = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION); + virtual bool rest_info(RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_layer_mask = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION); Physics2DDirectSpaceStateSW(); }; - - class Space2DSW : public RID_Data { public: - enum ElapsedTime { ELAPSED_TIME_INTEGRATE_FORCES, ELAPSED_TIME_GENERATE_ISLANDS, @@ -72,8 +68,8 @@ public: ELAPSED_TIME_MAX }; -private: +private: uint64_t elapsed_time[ELAPSED_TIME_MAX]; Physics2DDirectSpaceStateSW *direct_access; @@ -86,10 +82,10 @@ private: 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); + 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; + Set<CollisionObject2DSW *> objects; Area2DSW *area; @@ -100,7 +96,7 @@ private: enum { - INTERSECTION_QUERY_MAX=2048 + INTERSECTION_QUERY_MAX = 2048 }; CollisionObject2DSW *intersection_query_results[INTERSECTION_QUERY_MAX]; @@ -116,44 +112,40 @@ private: int active_objects; int collision_pairs; - int _cull_aabb_for_body(Body2DSW *p_body,const Rect2& p_aabb); + int _cull_aabb_for_body(Body2DSW *p_body, const Rect2 &p_aabb); Vector<Vector2> contact_debug; int contact_debug_count; -friend class Physics2DDirectSpaceStateSW; + friend class Physics2DDirectSpaceStateSW; public: - - _FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; } + _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; } + 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; - - - + 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 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); + 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; + 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; } @@ -163,13 +155,10 @@ public: _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; } - - void update(); void setup(); void call_queries(); - bool is_locked() const; void lock(); void unlock(); @@ -177,32 +166,31 @@ public: void set_param(Physics2DServer::SpaceParameter p_param, real_t p_value); real_t get_param(Physics2DServer::SpaceParameter p_param) const; - void set_island_count(int p_island_count) { island_count=p_island_count; } + void set_island_count(int p_island_count) { island_count = p_island_count; } int get_island_count() const { return island_count; } - void set_active_objects(int p_active_objects) { active_objects=p_active_objects; } + void set_active_objects(int p_active_objects) { active_objects = p_active_objects; } int get_active_objects() const { return active_objects; } int get_collision_pairs() const { return collision_pairs; } - bool test_body_motion(Body2DSW *p_body, const Transform2D &p_from, const Vector2&p_motion, real_t p_margin, Physics2DServer::MotionResult *r_result); - + bool test_body_motion(Body2DSW *p_body, const Transform2D &p_from, const Vector2 &p_motion, real_t p_margin, Physics2DServer::MotionResult *r_result); void set_debug_contacts(int p_amount) { contact_debug.resize(p_amount); } _FORCE_INLINE_ bool is_debugging_contacts() const { return !contact_debug.empty(); } - _FORCE_INLINE_ void add_debug_contact(const Vector2& p_contact) { if (contact_debug_count<contact_debug.size()) contact_debug[contact_debug_count++]=p_contact; } + _FORCE_INLINE_ void add_debug_contact(const Vector2 &p_contact) { + if (contact_debug_count < contact_debug.size()) contact_debug[contact_debug_count++] = p_contact; + } _FORCE_INLINE_ Vector<Vector2> get_debug_contacts() { return contact_debug; } _FORCE_INLINE_ int get_debug_contact_count() { return contact_debug_count; } - Physics2DDirectSpaceStateSW *get_direct_state(); - void set_elapsed_time(ElapsedTime p_time,uint64_t p_msec) { elapsed_time[p_time]=p_msec; } + void set_elapsed_time(ElapsedTime p_time, uint64_t p_msec) { elapsed_time[p_time] = p_msec; } uint64_t get_elapsed_time(ElapsedTime p_time) const { return elapsed_time[p_time]; } Space2DSW(); ~Space2DSW(); }; - #endif // SPACE_2D_SW_H diff --git a/servers/physics_2d/step_2d_sw.cpp b/servers/physics_2d/step_2d_sw.cpp index 355cc25a69..55bc62975f 100644 --- a/servers/physics_2d/step_2d_sw.cpp +++ b/servers/physics_2d/step_2d_sw.cpp @@ -29,39 +29,38 @@ #include "step_2d_sw.h" #include "os/os.h" -void Step2DSW::_populate_island(Body2DSW* p_body,Body2DSW** p_island,Constraint2DSW **p_constraint_island) { +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; + *p_island = p_body; - for(Map<Constraint2DSW*,int>::Element *E=p_body->get_constraint_map().front();E;E=E->next()) { + 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) + 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; + *p_constraint_island = c; - - for(int i=0;i<c->get_body_count();i++) { - if (i==E->get()) + 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 || b->get_mode()==Physics2DServer::BODY_MODE_KINEMATIC) + if (b->get_island_step() == _step || b->get_mode() == Physics2DServer::BODY_MODE_STATIC || b->get_mode() == Physics2DServer::BODY_MODE_KINEMATIC) continue; //no go - _populate_island(c->get_body_ptr()[i],p_island,p_constraint_island); + _populate_island(c->get_body_ptr()[i], p_island, p_constraint_island); } } } -bool Step2DSW::_setup_island(Constraint2DSW *p_island,real_t p_delta) { +bool Step2DSW::_setup_island(Constraint2DSW *p_island, real_t p_delta) { - Constraint2DSW *ci=p_island; - Constraint2DSW *prev_ci=NULL; - bool removed_root=false; - while(ci) { + Constraint2DSW *ci = p_island; + Constraint2DSW *prev_ci = NULL; + bool removed_root = false; + while (ci) { bool process = ci->setup(p_delta); if (!process) { @@ -69,169 +68,161 @@ bool Step2DSW::_setup_island(Constraint2DSW *p_island,real_t p_delta) { if (prev_ci) { prev_ci->set_island_next(ci->get_island_next()); } else { - removed_root=true; - prev_ci=ci; + removed_root = true; + prev_ci = ci; } } else { - prev_ci=ci; + prev_ci = ci; } - ci=ci->get_island_next(); + ci = ci->get_island_next(); } return removed_root; } -void Step2DSW::_solve_island(Constraint2DSW *p_island,int p_iterations,real_t p_delta){ - +void Step2DSW::_solve_island(Constraint2DSW *p_island, int p_iterations, real_t p_delta) { - for(int i=0;i<p_iterations;i++) { + for (int i = 0; i < p_iterations; i++) { - Constraint2DSW *ci=p_island; - while(ci) { + Constraint2DSW *ci = p_island; + while (ci) { ci->solve(p_delta); - ci=ci->get_island_next(); + ci = ci->get_island_next(); } } } -void Step2DSW::_check_suspend(Body2DSW *p_island,real_t p_delta) { +void Step2DSW::_check_suspend(Body2DSW *p_island, real_t p_delta) { - - bool can_sleep=true; + bool can_sleep = true; Body2DSW *b = p_island; - while(b) { + while (b) { - if (b->get_mode()==Physics2DServer::BODY_MODE_STATIC || b->get_mode()==Physics2DServer::BODY_MODE_KINEMATIC) { - b=b->get_island_next(); + if (b->get_mode() == Physics2DServer::BODY_MODE_STATIC || b->get_mode() == Physics2DServer::BODY_MODE_KINEMATIC) { + b = b->get_island_next(); continue; //ignore for static } if (!b->sleep_test(p_delta)) - can_sleep=false; + can_sleep = false; - b=b->get_island_next(); + b = b->get_island_next(); } //put all to sleep or wake up everyoen b = p_island; - while(b) { + while (b) { - if (b->get_mode()==Physics2DServer::BODY_MODE_STATIC || b->get_mode()==Physics2DServer::BODY_MODE_KINEMATIC) { - b=b->get_island_next(); + if (b->get_mode() == Physics2DServer::BODY_MODE_STATIC || b->get_mode() == Physics2DServer::BODY_MODE_KINEMATIC) { + b = b->get_island_next(); continue; //ignore for static } bool active = b->is_active(); - if (active==can_sleep) + if (active == can_sleep) b->set_active(!can_sleep); - b=b->get_island_next(); + b = b->get_island_next(); } } -void Step2DSW::step(Space2DSW* p_space,real_t p_delta,int p_iterations) { - +void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) { p_space->lock(); // can't access space during this p_space->setup(); //update inertias, etc - const SelfList<Body2DSW>::List * body_list = &p_space->get_active_body_list(); + const SelfList<Body2DSW>::List *body_list = &p_space->get_active_body_list(); /* INTEGRATE FORCES */ uint64_t profile_begtime = OS::get_singleton()->get_ticks_usec(); - uint64_t profile_endtime=0; + uint64_t profile_endtime = 0; + int active_count = 0; - int active_count=0; - - const SelfList<Body2DSW>*b = body_list->first(); - while(b) { + const SelfList<Body2DSW> *b = body_list->first(); + while (b) { b->self()->integrate_forces(p_delta); - b=b->next(); + b = b->next(); active_count++; } p_space->set_active_objects(active_count); - { //profile - profile_endtime=OS::get_singleton()->get_ticks_usec(); - p_space->set_elapsed_time(Space2DSW::ELAPSED_TIME_INTEGRATE_FORCES,profile_endtime-profile_begtime); - profile_begtime=profile_endtime; + profile_endtime = OS::get_singleton()->get_ticks_usec(); + p_space->set_elapsed_time(Space2DSW::ELAPSED_TIME_INTEGRATE_FORCES, profile_endtime - profile_begtime); + profile_begtime = profile_endtime; } /* GENERATE CONSTRAINT ISLANDS */ - Body2DSW *island_list=NULL; - Constraint2DSW *constraint_island_list=NULL; + Body2DSW *island_list = NULL; + Constraint2DSW *constraint_island_list = NULL; b = body_list->first(); - int island_count=0; + int island_count = 0; - while(b) { + while (b) { Body2DSW *body = b->self(); + if (body->get_island_step() != _step) { - if (body->get_island_step()!=_step) { - - Body2DSW *island=NULL; - Constraint2DSW *constraint_island=NULL; - _populate_island(body,&island,&constraint_island); + Body2DSW *island = NULL; + Constraint2DSW *constraint_island = NULL; + _populate_island(body, &island, &constraint_island); island->set_island_list_next(island_list); - island_list=island; + island_list = island; if (constraint_island) { constraint_island->set_island_list_next(constraint_island_list); - constraint_island_list=constraint_island; + constraint_island_list = constraint_island; island_count++; } - } - b=b->next(); + b = b->next(); } p_space->set_island_count(island_count); 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()) { - 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) + 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; + constraint_island_list = c; } - p_space->area_remove_from_moved_list((SelfList<Area2DSW>*)aml.first()); //faster to remove here + 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)); { //profile - profile_endtime=OS::get_singleton()->get_ticks_usec(); - p_space->set_elapsed_time(Space2DSW::ELAPSED_TIME_GENERATE_ISLANDS,profile_endtime-profile_begtime); - profile_begtime=profile_endtime; + profile_endtime = OS::get_singleton()->get_ticks_usec(); + p_space->set_elapsed_time(Space2DSW::ELAPSED_TIME_GENERATE_ISLANDS, profile_endtime - profile_begtime); + profile_begtime = profile_endtime; } /* SETUP CONSTRAINT ISLANDS */ { - Constraint2DSW *ci=constraint_island_list; - Constraint2DSW *prev_ci=NULL; - while(ci) { + Constraint2DSW *ci = constraint_island_list; + Constraint2DSW *prev_ci = NULL; + while (ci) { - if (_setup_island(ci,p_delta)==true) { + if (_setup_island(ci, p_delta) == true) { //removed the root from the island graph because it is not to be processed @@ -243,10 +234,9 @@ void Step2DSW::step(Space2DSW* p_space,real_t p_delta,int p_iterations) { if (prev_ci) { prev_ci->set_island_list_next(next); } else { - constraint_island_list=next; - + constraint_island_list = next; } - prev_ci=next; + prev_ci = next; } else { //list is empty, just skip @@ -254,77 +244,73 @@ void Step2DSW::step(Space2DSW* p_space,real_t p_delta,int p_iterations) { prev_ci->set_island_list_next(ci->get_island_list_next()); } else { - constraint_island_list=ci->get_island_list_next(); + constraint_island_list = ci->get_island_list_next(); } - } } else { - prev_ci=ci; + prev_ci = ci; } - ci=ci->get_island_list_next(); + ci = ci->get_island_list_next(); } } { //profile - profile_endtime=OS::get_singleton()->get_ticks_usec(); - p_space->set_elapsed_time(Space2DSW::ELAPSED_TIME_SETUP_CONSTRAINTS,profile_endtime-profile_begtime); - profile_begtime=profile_endtime; + profile_endtime = OS::get_singleton()->get_ticks_usec(); + p_space->set_elapsed_time(Space2DSW::ELAPSED_TIME_SETUP_CONSTRAINTS, profile_endtime - profile_begtime); + profile_begtime = profile_endtime; } /* SOLVE CONSTRAINT ISLANDS */ { - Constraint2DSW *ci=constraint_island_list; - while(ci) { + 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(); + _solve_island(ci, p_iterations, p_delta); + ci = ci->get_island_list_next(); } } { //profile - profile_endtime=OS::get_singleton()->get_ticks_usec(); - p_space->set_elapsed_time(Space2DSW::ELAPSED_TIME_SOLVE_CONSTRAINTS,profile_endtime-profile_begtime); - profile_begtime=profile_endtime; + profile_endtime = OS::get_singleton()->get_ticks_usec(); + p_space->set_elapsed_time(Space2DSW::ELAPSED_TIME_SOLVE_CONSTRAINTS, profile_endtime - profile_begtime); + profile_begtime = profile_endtime; } /* INTEGRATE VELOCITIES */ b = body_list->first(); - while(b) { + while (b) { - const SelfList<Body2DSW>*n=b->next(); + const SelfList<Body2DSW> *n = b->next(); b->self()->integrate_velocities(p_delta); - b=n; // in case it shuts itself down + b = n; // in case it shuts itself down } /* SLEEP / WAKE UP ISLANDS */ { - Body2DSW *bi=island_list; - while(bi) { + Body2DSW *bi = island_list; + while (bi) { - _check_suspend(bi,p_delta); - bi=bi->get_island_list_next(); + _check_suspend(bi, p_delta); + bi = bi->get_island_list_next(); } } { //profile - profile_endtime=OS::get_singleton()->get_ticks_usec(); - p_space->set_elapsed_time(Space2DSW::ELAPSED_TIME_INTEGRATE_VELOCITIES,profile_endtime-profile_begtime); + profile_endtime = OS::get_singleton()->get_ticks_usec(); + p_space->set_elapsed_time(Space2DSW::ELAPSED_TIME_INTEGRATE_VELOCITIES, profile_endtime - profile_begtime); //profile_begtime=profile_endtime; } p_space->update(); p_space->unlock(); _step++; - - - } Step2DSW::Step2DSW() { - _step=1; + _step = 1; } diff --git a/servers/physics_2d/step_2d_sw.h b/servers/physics_2d/step_2d_sw.h index 0896e1016d..c743358b6a 100644 --- a/servers/physics_2d/step_2d_sw.h +++ b/servers/physics_2d/step_2d_sw.h @@ -35,13 +35,13 @@ class Step2DSW { uint64_t _step; - void _populate_island(Body2DSW* p_body,Body2DSW** p_island,Constraint2DSW **p_constraint_island); - bool _setup_island(Constraint2DSW *p_island,real_t p_delta); - void _solve_island(Constraint2DSW *p_island,int p_iterations,real_t p_delta); - void _check_suspend(Body2DSW *p_island,real_t p_delta); -public: + void _populate_island(Body2DSW *p_body, Body2DSW **p_island, Constraint2DSW **p_constraint_island); + bool _setup_island(Constraint2DSW *p_island, real_t p_delta); + void _solve_island(Constraint2DSW *p_island, int p_iterations, real_t p_delta); + void _check_suspend(Body2DSW *p_island, real_t p_delta); - void step(Space2DSW* p_space,real_t p_delta,int p_iterations); +public: + void step(Space2DSW *p_space, real_t p_delta, int p_iterations); Step2DSW(); }; |