diff options
Diffstat (limited to '')
80 files changed, 8452 insertions, 10405 deletions
diff --git a/servers/physics/area_pair_sw.cpp b/servers/physics/area_pair_sw.cpp index 3aa0816b0..d1040baa6 100644 --- a/servers/physics/area_pair_sw.cpp +++ b/servers/physics/area_pair_sw.cpp @@ -29,7 +29,6 @@ #include "area_pair_sw.h" #include "collision_solver_sw.h" - bool AreaPairSW::setup(real_t p_step) { if (!area->test_collision_mask(body)) { @@ -37,63 +36,55 @@ bool AreaPairSW::setup(real_t p_step) { return false; } - bool result = CollisionSolverSW::solve_static(body->get_shape(body_shape),body->get_transform() * body->get_shape_transform(body_shape),area->get_shape(area_shape),area->get_transform() * area->get_shape_transform(area_shape),NULL,this); + bool result = CollisionSolverSW::solve_static(body->get_shape(body_shape), body->get_transform() * body->get_shape_transform(body_shape), area->get_shape(area_shape), area->get_transform() * area->get_shape_transform(area_shape), NULL, this); - if (result!=colliding) { + if (result != colliding) { if (result) { - if (area->get_space_override_mode()!=PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED) + if (area->get_space_override_mode() != PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED) body->add_area(area); if (area->has_monitor_callback()) - area->add_body_to_query(body,body_shape,area_shape); + area->add_body_to_query(body, body_shape, area_shape); } else { - if (area->get_space_override_mode()!=PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED) + if (area->get_space_override_mode() != PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED) body->remove_area(area); if (area->has_monitor_callback()) - area->remove_body_from_query(body,body_shape,area_shape); - + area->remove_body_from_query(body, body_shape, area_shape); } - colliding=result; - + colliding = result; } return false; //never do any post solving } void AreaPairSW::solve(real_t p_step) { - - } +AreaPairSW::AreaPairSW(BodySW *p_body, int p_body_shape, AreaSW *p_area, int p_area_shape) { -AreaPairSW::AreaPairSW(BodySW *p_body,int p_body_shape, AreaSW *p_area,int p_area_shape) { - - body=p_body; - area=p_area; - body_shape=p_body_shape; - area_shape=p_area_shape; - colliding=false; - body->add_constraint(this,0); + body = p_body; + area = p_area; + body_shape = p_body_shape; + area_shape = p_area_shape; + colliding = false; + body->add_constraint(this, 0); area->add_constraint(this); - if (p_body->get_mode()==PhysicsServer::BODY_MODE_KINEMATIC) + if (p_body->get_mode() == PhysicsServer::BODY_MODE_KINEMATIC) p_body->set_active(true); - } AreaPairSW::~AreaPairSW() { if (colliding) { - if (area->get_space_override_mode()!=PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED) + if (area->get_space_override_mode() != PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED) body->remove_area(area); if (area->has_monitor_callback()) - area->remove_body_from_query(body,body_shape,area_shape); - - + area->remove_body_from_query(body, body_shape, area_shape); } body->remove_constraint(this); area->remove_constraint(this); @@ -101,8 +92,6 @@ AreaPairSW::~AreaPairSW() { //////////////////////////////////////////////////// - - bool Area2PairSW::setup(real_t p_step) { if (!area_a->test_collision_mask(area_b)) { @@ -111,51 +100,45 @@ bool Area2PairSW::setup(real_t p_step) { } //bool result = area_a->test_collision_mask(area_b) && CollisionSolverSW::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 = CollisionSolverSW::solve_static(area_a->get_shape(shape_a),area_a->get_transform() * area_a->get_shape_transform(shape_a),area_b->get_shape(shape_b),area_b->get_transform() * area_b->get_shape_transform(shape_b),NULL,this); + bool result = CollisionSolverSW::solve_static(area_a->get_shape(shape_a), area_a->get_transform() * area_a->get_shape_transform(shape_a), area_b->get_shape(shape_b), area_b->get_transform() * area_b->get_shape_transform(shape_b), NULL, this); - 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 Area2PairSW::solve(real_t p_step) { - - } +Area2PairSW::Area2PairSW(AreaSW *p_area_a, int p_shape_a, AreaSW *p_area_b, int p_shape_b) { -Area2PairSW::Area2PairSW(AreaSW *p_area_a,int p_shape_a, AreaSW *p_area_b,int p_shape_b) { - - - area_a=p_area_a; - area_b=p_area_b; - shape_a=p_shape_a; - shape_b=p_shape_b; - colliding=false; + area_a = p_area_a; + area_b = p_area_b; + shape_a = p_shape_a; + shape_b = p_shape_b; + colliding = false; area_a->add_constraint(this); area_b->add_constraint(this); - } Area2PairSW::~Area2PairSW() { @@ -163,10 +146,10 @@ Area2PairSW::~Area2PairSW() { 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/area_pair_sw.h b/servers/physics/area_pair_sw.h index 637976a09..8fc7e7efa 100644 --- a/servers/physics/area_pair_sw.h +++ b/servers/physics/area_pair_sw.h @@ -29,9 +29,9 @@ #ifndef AREA_PAIR_SW_H #define AREA_PAIR_SW_H -#include "constraint_sw.h" -#include "body_sw.h" #include "area_sw.h" +#include "body_sw.h" +#include "constraint_sw.h" class AreaPairSW : public ConstraintSW { @@ -40,16 +40,15 @@ class AreaPairSW : public ConstraintSW { int body_shape; int area_shape; bool colliding; -public: +public: bool setup(real_t p_step); void solve(real_t p_step); - AreaPairSW(BodySW *p_body,int p_body_shape, AreaSW *p_area,int p_area_shape); + AreaPairSW(BodySW *p_body, int p_body_shape, AreaSW *p_area, int p_area_shape); ~AreaPairSW(); }; - class Area2PairSW : public ConstraintSW { AreaSW *area_a; @@ -57,15 +56,13 @@ class Area2PairSW : public ConstraintSW { int shape_a; int shape_b; bool colliding; -public: +public: bool setup(real_t p_step); void solve(real_t p_step); - Area2PairSW(AreaSW *p_area_a,int p_shape_a, AreaSW *p_area_b,int p_shape_b); + Area2PairSW(AreaSW *p_area_a, int p_shape_a, AreaSW *p_area_b, int p_shape_b); ~Area2PairSW(); }; - #endif // AREA_PAIR__SW_H - diff --git a/servers/physics/area_sw.cpp b/servers/physics/area_sw.cpp index 8aed07d5e..dfb5d191b 100644 --- a/servers/physics/area_sw.cpp +++ b/servers/physics/area_sw.cpp @@ -27,18 +27,26 @@ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ #include "area_sw.h" -#include "space_sw.h" #include "body_sw.h" +#include "space_sw.h" -AreaSW::BodyKey::BodyKey(BodySW *p_body, uint32_t p_body_shape,uint32_t p_area_shape) { rid=p_body->get_self(); instance_id=p_body->get_instance_id(); body_shape=p_body_shape; area_shape=p_area_shape; } -AreaSW::BodyKey::BodyKey(AreaSW *p_body, uint32_t p_body_shape,uint32_t p_area_shape) { rid=p_body->get_self(); instance_id=p_body->get_instance_id(); body_shape=p_body_shape; area_shape=p_area_shape; } +AreaSW::BodyKey::BodyKey(BodySW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { + rid = p_body->get_self(); + instance_id = p_body->get_instance_id(); + body_shape = p_body_shape; + area_shape = p_area_shape; +} +AreaSW::BodyKey::BodyKey(AreaSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { + rid = p_body->get_self(); + instance_id = p_body->get_instance_id(); + body_shape = p_body_shape; + area_shape = p_area_shape; +} void AreaSW::_shapes_changed() { - - } -void AreaSW::set_transform(const Transform& p_transform) { +void AreaSW::set_transform(const Transform &p_transform) { if (!moved_list.in_list() && get_space()) get_space()->area_add_to_moved_list(&moved_list); @@ -54,7 +62,6 @@ void AreaSW::set_space(SpaceSW *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,44 +70,38 @@ void AreaSW::set_space(SpaceSW *p_space) { _set_space(p_space); } +void AreaSW::set_monitor_callback(ObjectID p_id, const StringName &p_method) { -void AreaSW::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(); - _shape_changed(); if (!moved_list.in_list() && get_space()) get_space()->area_add_to_moved_list(&moved_list); - - } -void AreaSW::set_area_monitor_callback(ObjectID p_id, const StringName& p_method) { +void AreaSW::set_area_monitor_callback(ObjectID p_id, const StringName &p_method) { - - if (p_id==area_monitor_callback_id) { - area_monitor_callback_method=p_method; + 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(); @@ -109,45 +110,39 @@ void AreaSW::set_area_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 AreaSW::set_space_override_mode(PhysicsServer::AreaSpaceOverrideMode p_mode) { - bool do_override=p_mode!=PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED; - if (do_override==(space_override_mode!=PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED)) + bool do_override = p_mode != PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED; + if (do_override == (space_override_mode != PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED)) return; _unregister_shapes(); - space_override_mode=p_mode; + space_override_mode = p_mode; _shape_changed(); } -void AreaSW::set_param(PhysicsServer::AreaParameter p_param, const Variant& p_value) { +void AreaSW::set_param(PhysicsServer::AreaParameter p_param, const Variant &p_value) { - switch(p_param) { - case PhysicsServer::AREA_PARAM_GRAVITY: gravity=p_value; break; - case PhysicsServer::AREA_PARAM_GRAVITY_VECTOR: gravity_vector=p_value; break; - case PhysicsServer::AREA_PARAM_GRAVITY_IS_POINT: gravity_is_point=p_value; break; - case PhysicsServer::AREA_PARAM_GRAVITY_DISTANCE_SCALE: gravity_distance_scale=p_value; break; - case PhysicsServer::AREA_PARAM_GRAVITY_POINT_ATTENUATION: point_attenuation=p_value; break; - case PhysicsServer::AREA_PARAM_LINEAR_DAMP: linear_damp=p_value; break; - case PhysicsServer::AREA_PARAM_ANGULAR_DAMP: angular_damp=p_value; break; - case PhysicsServer::AREA_PARAM_PRIORITY: priority=p_value; break; + switch (p_param) { + case PhysicsServer::AREA_PARAM_GRAVITY: gravity = p_value; break; + case PhysicsServer::AREA_PARAM_GRAVITY_VECTOR: gravity_vector = p_value; break; + case PhysicsServer::AREA_PARAM_GRAVITY_IS_POINT: gravity_is_point = p_value; break; + case PhysicsServer::AREA_PARAM_GRAVITY_DISTANCE_SCALE: gravity_distance_scale = p_value; break; + case PhysicsServer::AREA_PARAM_GRAVITY_POINT_ATTENUATION: point_attenuation = p_value; break; + case PhysicsServer::AREA_PARAM_LINEAR_DAMP: linear_damp = p_value; break; + case PhysicsServer::AREA_PARAM_ANGULAR_DAMP: angular_damp = p_value; break; + case PhysicsServer::AREA_PARAM_PRIORITY: priority = p_value; break; } - - } Variant AreaSW::get_param(PhysicsServer::AreaParameter p_param) const { - - switch(p_param) { + switch (p_param) { case PhysicsServer::AREA_PARAM_GRAVITY: return gravity; case PhysicsServer::AREA_PARAM_GRAVITY_VECTOR: return gravity_vector; case PhysicsServer::AREA_PARAM_GRAVITY_IS_POINT: return gravity_is_point; case PhysicsServer::AREA_PARAM_GRAVITY_DISTANCE_SCALE: return gravity_distance_scale; - case PhysicsServer::AREA_PARAM_GRAVITY_POINT_ATTENUATION: return point_attenuation; + case PhysicsServer::AREA_PARAM_GRAVITY_POINT_ATTENUATION: return point_attenuation; case PhysicsServer::AREA_PARAM_LINEAR_DAMP: return linear_damp; case PhysicsServer::AREA_PARAM_ANGULAR_DAMP: return angular_damp; case PhysicsServer::AREA_PARAM_PRIORITY: return priority; @@ -156,23 +151,20 @@ Variant AreaSW::get_param(PhysicsServer::AreaParameter p_param) const { return Variant(); } - void AreaSW::_queue_monitor_update() { ERR_FAIL_COND(!get_space()); if (!monitor_query_list.in_list()) get_space()->area_add_to_monitor_query_list(&monitor_query_list); - - } void AreaSW::set_monitorable(bool p_monitorable) { - if (monitorable==p_monitorable) + if (monitorable == p_monitorable) return; - monitorable=p_monitorable; + monitorable = p_monitorable; _set_static(!monitorable); } @@ -182,29 +174,29 @@ void AreaSW::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 ? PhysicsServer::AREA_BODY_ADDED : PhysicsServer::AREA_BODY_REMOVED; - res[1]=E->key().rid; - res[2]=E->key().instance_id; - res[3]=E->key().body_shape; - res[4]=E->key().area_shape; + res[0] = E->get().state > 0 ? PhysicsServer::AREA_BODY_ADDED : PhysicsServer::AREA_BODY_REMOVED; + res[1] = E->key().rid; + res[2] = E->key().instance_id; + res[3] = E->key().body_shape; + res[4] = E->key().area_shape; Variant::CallError ce; - obj->call(monitor_callback_method,(const Variant**)resptr,5,ce); + obj->call(monitor_callback_method, (const Variant **)resptr, 5, ce); } } @@ -212,64 +204,56 @@ void AreaSW::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 ? PhysicsServer::AREA_BODY_ADDED : PhysicsServer::AREA_BODY_REMOVED; - res[1]=E->key().rid; - res[2]=E->key().instance_id; - res[3]=E->key().body_shape; - res[4]=E->key().area_shape; - + res[0] = E->get().state > 0 ? PhysicsServer::AREA_BODY_ADDED : PhysicsServer::AREA_BODY_REMOVED; + res[1] = E->key().rid; + res[2] = E->key().instance_id; + res[3] = E->key().body_shape; + res[4] = E->key().area_shape; 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); - } -AreaSW::AreaSW() : CollisionObjectSW(TYPE_AREA), monitor_query_list(this), moved_list(this) { +AreaSW::AreaSW() + : CollisionObjectSW(TYPE_AREA), monitor_query_list(this), moved_list(this) { _set_static(true); //areas are never active - space_override_mode=PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED; - gravity=9.80665; - gravity_vector=Vector3(0,-1,0); - gravity_is_point=false; - gravity_distance_scale=0; - point_attenuation=1; - angular_damp=1.0; - linear_damp=0.1; - priority=0; + space_override_mode = PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED; + gravity = 9.80665; + gravity_vector = Vector3(0, -1, 0); + gravity_is_point = false; + gravity_distance_scale = 0; + point_attenuation = 1; + angular_damp = 1.0; + linear_damp = 0.1; + priority = 0; set_ray_pickable(false); - monitor_callback_id=0; - area_monitor_callback_id=0; - monitorable=false; - + monitor_callback_id = 0; + area_monitor_callback_id = 0; + monitorable = false; } AreaSW::~AreaSW() { - - } - diff --git a/servers/physics/area_sw.h b/servers/physics/area_sw.h index 4e6f1c5a5..2c0cd8dbc 100644 --- a/servers/physics/area_sw.h +++ b/servers/physics/area_sw.h @@ -29,17 +29,16 @@ #ifndef AREA_SW_H #define AREA_SW_H -#include "servers/physics_server.h" #include "collision_object_sw.h" #include "self_list.h" +#include "servers/physics_server.h" //#include "servers/physics/query_sw.h" class SpaceSW; class BodySW; class ConstraintSW; -class AreaSW : public CollisionObjectSW{ - +class AreaSW : public CollisionObjectSW { PhysicsServer::AreaSpaceOverrideMode space_override_mode; real_t gravity; @@ -68,24 +67,22 @@ class AreaSW : public CollisionObjectSW{ 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(BodySW *p_body, uint32_t p_body_shape,uint32_t p_area_shape); - BodyKey(AreaSW *p_body, uint32_t p_body_shape,uint32_t p_area_shape); - + BodyKey(BodySW *p_body, uint32_t p_body_shape, uint32_t p_area_shape); + BodyKey(AreaSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape); }; struct BodyState { @@ -93,125 +90,111 @@ class AreaSW : public CollisionObjectSW{ 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(ShapeSW *p_shape); //virtual void shape_deleted_notify(ShapeSW *p_shape); - Set<ConstraintSW*> constraints; - + Set<ConstraintSW *> constraints; virtual void _shapes_changed(); void _queue_monitor_update(); public: - //_FORCE_INLINE_ const Transform& get_inverse_transform() const { return inverse_transform; } //_FORCE_INLINE_ SpaceSW* get_owner() { return owner; } - void set_monitor_callback(ObjectID p_id, const StringName& p_method); + 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(BodySW *p_body, uint32_t p_body_shape,uint32_t p_area_shape); - _FORCE_INLINE_ void remove_body_from_query(BodySW *p_body, uint32_t p_body_shape,uint32_t p_area_shape); + _FORCE_INLINE_ void add_body_to_query(BodySW *p_body, uint32_t p_body_shape, uint32_t p_area_shape); + _FORCE_INLINE_ void remove_body_from_query(BodySW *p_body, uint32_t p_body_shape, uint32_t p_area_shape); - _FORCE_INLINE_ void add_area_to_query(AreaSW *p_area, uint32_t p_area_shape,uint32_t p_self_shape); - _FORCE_INLINE_ void remove_area_from_query(AreaSW *p_area, uint32_t p_area_shape,uint32_t p_self_shape); + _FORCE_INLINE_ void add_area_to_query(AreaSW *p_area, uint32_t p_area_shape, uint32_t p_self_shape); + _FORCE_INLINE_ void remove_area_from_query(AreaSW *p_area, uint32_t p_area_shape, uint32_t p_self_shape); - void set_param(PhysicsServer::AreaParameter p_param, const Variant& p_value); + void set_param(PhysicsServer::AreaParameter p_param, const Variant &p_value); Variant get_param(PhysicsServer::AreaParameter p_param) const; void set_space_override_mode(PhysicsServer::AreaSpaceOverrideMode p_mode); PhysicsServer::AreaSpaceOverrideMode get_space_override_mode() const { return space_override_mode; } - _FORCE_INLINE_ void set_gravity(real_t p_gravity) { gravity=p_gravity; } + _FORCE_INLINE_ void set_gravity(real_t p_gravity) { gravity = p_gravity; } _FORCE_INLINE_ real_t get_gravity() const { return gravity; } - _FORCE_INLINE_ void set_gravity_vector(const Vector3& p_gravity) { gravity_vector=p_gravity; } + _FORCE_INLINE_ void set_gravity_vector(const Vector3 &p_gravity) { gravity_vector = p_gravity; } _FORCE_INLINE_ Vector3 get_gravity_vector() const { return gravity_vector; } - _FORCE_INLINE_ void set_gravity_as_point(bool p_enable) { gravity_is_point=p_enable; } + _FORCE_INLINE_ 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( ConstraintSW* p_constraint) { constraints.insert(p_constraint); } - _FORCE_INLINE_ void remove_constraint( ConstraintSW* p_constraint) { constraints.erase(p_constraint); } - _FORCE_INLINE_ const Set<ConstraintSW*>& get_constraints() const { return constraints; } + _FORCE_INLINE_ void add_constraint(ConstraintSW *p_constraint) { constraints.insert(p_constraint); } + _FORCE_INLINE_ void remove_constraint(ConstraintSW *p_constraint) { constraints.erase(p_constraint); } + _FORCE_INLINE_ const Set<ConstraintSW *> &get_constraints() const { return constraints; } void set_monitorable(bool p_monitorable); _FORCE_INLINE_ bool is_monitorable() const { return monitorable; } - void set_transform(const Transform& p_transform); + void set_transform(const Transform &p_transform); void set_space(SpaceSW *p_space); - void call_queries(); AreaSW(); ~AreaSW(); }; -void AreaSW::add_body_to_query(BodySW *p_body, uint32_t p_body_shape,uint32_t p_area_shape) { +void AreaSW::add_body_to_query(BodySW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { - BodyKey bk(p_body,p_body_shape,p_area_shape); + BodyKey bk(p_body, p_body_shape, p_area_shape); monitored_bodies[bk].inc(); if (!monitor_query_list.in_list()) _queue_monitor_update(); } -void AreaSW::remove_body_from_query(BodySW *p_body, uint32_t p_body_shape,uint32_t p_area_shape) { +void AreaSW::remove_body_from_query(BodySW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { - BodyKey bk(p_body,p_body_shape,p_area_shape); + BodyKey bk(p_body, p_body_shape, p_area_shape); monitored_bodies[bk].dec(); if (!monitor_query_list.in_list()) _queue_monitor_update(); } +void AreaSW::add_area_to_query(AreaSW *p_area, uint32_t p_area_shape, uint32_t p_self_shape) { -void AreaSW::add_area_to_query(AreaSW *p_area, uint32_t p_area_shape,uint32_t p_self_shape) { - - - BodyKey bk(p_area,p_area_shape,p_self_shape); + BodyKey bk(p_area, p_area_shape, p_self_shape); monitored_areas[bk].inc(); if (!monitor_query_list.in_list()) _queue_monitor_update(); - - } -void AreaSW::remove_area_from_query(AreaSW *p_area, uint32_t p_area_shape,uint32_t p_self_shape) { - +void AreaSW::remove_area_from_query(AreaSW *p_area, uint32_t p_area_shape, uint32_t p_self_shape) { - BodyKey bk(p_area,p_area_shape,p_self_shape); + BodyKey bk(p_area, p_area_shape, p_self_shape); monitored_areas[bk].dec(); if (!monitor_query_list.in_list()) _queue_monitor_update(); - } - - - - - #endif // AREA__SW_H diff --git a/servers/physics/body_pair_sw.cpp b/servers/physics/body_pair_sw.cpp index 7fb3def38..555d5f15c 100644 --- a/servers/physics/body_pair_sw.cpp +++ b/servers/physics/body_pair_sw.cpp @@ -28,8 +28,8 @@ /*************************************************************************/ #include "body_pair_sw.h" #include "collision_solver_sw.h" -#include "space_sw.h" #include "os/os.h" +#include "space_sw.h" /* #define NO_ACCUMULATE_IMPULSES @@ -41,19 +41,17 @@ #define NO_TANGENTIALS /* BODY PAIR */ - //#define ALLOWED_PENETRATION 0.01 #define RELAXATION_TIMESTEPS 3 #define MIN_VELOCITY 0.0001 -void BodyPairSW::_contact_added_callback(const Vector3& p_point_A,const Vector3& p_point_B,void *p_userdata) { - - BodyPairSW* pair = (BodyPairSW*)p_userdata; - pair->contact_added_callback(p_point_A,p_point_B); +void BodyPairSW::_contact_added_callback(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata) { + BodyPairSW *pair = (BodyPairSW *)p_userdata; + pair->contact_added_callback(p_point_A, p_point_B); } -void BodyPairSW::contact_added_callback(const Vector3& p_point_A,const Vector3& p_point_B) { +void BodyPairSW::contact_added_callback(const Vector3 &p_point_A, const Vector3 &p_point_B) { // check if we already have the contact @@ -61,40 +59,36 @@ void BodyPairSW::contact_added_callback(const Vector3& p_point_A,const Vector3& //Vector3 local_B = B->get_inv_transform().xform(p_point_B); Vector3 local_A = A->get_inv_transform().basis.xform(p_point_A); - Vector3 local_B = B->get_inv_transform().basis.xform(p_point_B-offset_B); - - + Vector3 local_B = B->get_inv_transform().basis.xform(p_point_B - offset_B); int new_index = contact_count; - ERR_FAIL_COND( new_index >= (MAX_CONTACTS+1) ); + ERR_FAIL_COND(new_index >= (MAX_CONTACTS + 1)); Contact contact; - contact.acc_normal_impulse=0; - contact.acc_bias_impulse=0; - contact.acc_tangent_impulse=Vector3(); - contact.local_A=local_A; - contact.local_B=local_B; - contact.normal=(p_point_A-p_point_B).normalized(); - - + contact.acc_normal_impulse = 0; + contact.acc_bias_impulse = 0; + contact.acc_tangent_impulse = Vector3(); + contact.local_A = local_A; + contact.local_B = local_B; + contact.normal = (p_point_A - p_point_B).normalized(); // attempt to determine if the contact will be reused - real_t contact_recycle_radius=space->get_contact_recycle_radius(); + real_t 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 ) < (contact_recycle_radius*contact_recycle_radius) && - c.local_B.distance_squared_to( local_B ) < (contact_recycle_radius*contact_recycle_radius) ) { + c.local_A.distance_squared_to(local_A) < (contact_recycle_radius * contact_recycle_radius) && + c.local_B.distance_squared_to(local_B) < (contact_recycle_radius * contact_recycle_radius)) { - contact.acc_normal_impulse=c.acc_normal_impulse; - contact.acc_bias_impulse=c.acc_bias_impulse; - contact.acc_tangent_impulse=c.acc_tangent_impulse; - new_index=i; - break; + contact.acc_normal_impulse = c.acc_normal_impulse; + contact.acc_bias_impulse = c.acc_bias_impulse; + contact.acc_tangent_impulse = c.acc_tangent_impulse; + new_index = i; + break; } } @@ -104,66 +98,63 @@ void BodyPairSW::contact_added_callback(const Vector3& p_point_A,const Vector3& // 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]; Vector3 global_A = A->get_transform().basis.xform(c.local_A); - Vector3 global_B = B->get_transform().basis.xform(c.local_B)+offset_B; + Vector3 global_B = B->get_transform().basis.xform(c.local_B) + offset_B; Vector3 axis = global_A - global_B; - real_t depth = axis.dot( c.normal ); + 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 BodyPairSW::validate_contacts() { //make sure to erase contacts that are no longer valid - real_t contact_max_separation=space->get_contact_max_separation(); - for (int i=0;i<contact_count;i++) { + real_t contact_max_separation = space->get_contact_max_separation(); + for (int i = 0; i < contact_count; i++) { - Contact& c = contacts[i]; + Contact &c = contacts[i]; Vector3 global_A = A->get_transform().basis.xform(c.local_A); - Vector3 global_B = B->get_transform().basis.xform(c.local_B)+offset_B; + Vector3 global_B = B->get_transform().basis.xform(c.local_B) + offset_B; Vector3 axis = global_A - global_B; - real_t depth = axis.dot( c.normal ); + real_t depth = axis.dot(c.normal); if (depth < -contact_max_separation || (global_B + c.normal * depth - global_A).length() > contact_max_separation) { // contact no longer needed, remove - - if ((i+1) < contact_count) { + 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--; @@ -172,21 +163,18 @@ void BodyPairSW::validate_contacts() { } } +bool BodyPairSW::_test_ccd(real_t p_step, BodySW *p_A, int p_shape_A, const Transform &p_xform_A, BodySW *p_B, int p_shape_B, const Transform &p_xform_B) { -bool BodyPairSW::_test_ccd(real_t p_step,BodySW *p_A, int p_shape_A,const Transform& p_xform_A,BodySW *p_B, int p_shape_B,const Transform& p_xform_B) { - - - - Vector3 motion = p_A->get_linear_velocity()*p_step; + Vector3 motion = p_A->get_linear_velocity() * p_step; real_t mlen = motion.length(); - if (mlen<CMP_EPSILON) + if (mlen < CMP_EPSILON) return false; Vector3 mnormal = motion / mlen; - real_t min,max; - p_A->get_shape(p_shape_A)->project_range(mnormal,p_xform_A,min,max); - bool fast_object = mlen > (max-min)*0.3; //going too fast in that direction + real_t min, max; + p_A->get_shape(p_shape_A)->project_range(mnormal, p_xform_A, min, max); + bool fast_object = mlen > (max - min) * 0.3; //going too fast in that direction if (!fast_object) { //did it move enough in this direction to even attempt raycast? let's say it should move more than 1/3 the size of the object in that axis return false; @@ -194,35 +182,34 @@ bool BodyPairSW::_test_ccd(real_t p_step,BodySW *p_A, int p_shape_A,const Transf //cast a segment from support in motion normal, in the same direction of motion by motion length //support is the worst case collision point, so real collision happened before - Vector3 s=p_A->get_shape(p_shape_A)->get_support(p_xform_A.basis.xform(mnormal).normalized()); + Vector3 s = p_A->get_shape(p_shape_A)->get_support(p_xform_A.basis.xform(mnormal).normalized()); Vector3 from = p_xform_A.xform(s); Vector3 to = from + motion; Transform from_inv = p_xform_B.affine_inverse(); - Vector3 local_from = from_inv.xform(from-mnormal*mlen*0.1); //start from a little inside the bounding box + Vector3 local_from = from_inv.xform(from - mnormal * mlen * 0.1); //start from a little inside the bounding box Vector3 local_to = from_inv.xform(to); - Vector3 rpos,rnorm; - if (!p_B->get_shape(p_shape_B)->intersect_segment(local_from,local_to,rpos,rnorm)) { + Vector3 rpos, rnorm; + if (!p_B->get_shape(p_shape_B)->intersect_segment(local_from, local_to, rpos, rnorm)) { return false; } //shorten the linear velocity so it does not hit, but gets close enough, next frame will hit softly or soft enough Vector3 hitpos = p_xform_B.xform(rpos); - real_t newlen = hitpos.distance_to(from)-(max-min)*0.01; - p_A->set_linear_velocity((mnormal*newlen)/p_step); + real_t newlen = hitpos.distance_to(from) - (max - min) * 0.01; + p_A->set_linear_velocity((mnormal * newlen) / p_step); return true; } - bool BodyPairSW::setup(real_t p_step) { //cannot collide - if (!A->test_collision_mask(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self()) || (A->get_mode()<=PhysicsServer::BODY_MODE_KINEMATIC && B->get_mode()<=PhysicsServer::BODY_MODE_KINEMATIC && A->get_max_contacts_reported()==0 && B->get_max_contacts_reported()==0)) { - collided=false; + if (!A->test_collision_mask(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self()) || (A->get_mode() <= PhysicsServer::BODY_MODE_KINEMATIC && B->get_mode() <= PhysicsServer::BODY_MODE_KINEMATIC && A->get_max_contacts_reported() == 0 && B->get_max_contacts_reported() == 0)) { + collided = false; return false; } @@ -231,86 +218,79 @@ bool BodyPairSW::setup(real_t p_step) { validate_contacts(); Vector3 offset_A = A->get_transform().get_origin(); - Transform xform_Au = Transform(A->get_transform().basis,Vector3()); + Transform xform_Au = Transform(A->get_transform().basis, Vector3()); Transform xform_A = xform_Au * A->get_shape_transform(shape_A); Transform xform_Bu = B->get_transform(); - xform_Bu.origin-=offset_A; + xform_Bu.origin -= offset_A; Transform xform_B = xform_Bu * B->get_shape_transform(shape_B); - ShapeSW *shape_A_ptr=A->get_shape(shape_A); - ShapeSW *shape_B_ptr=B->get_shape(shape_B); - - bool collided = CollisionSolverSW::solve_static(shape_A_ptr,xform_A,shape_B_ptr,xform_B,_contact_added_callback,this,&sep_axis); - this->collided=collided; + ShapeSW *shape_A_ptr = A->get_shape(shape_A); + ShapeSW *shape_B_ptr = B->get_shape(shape_B); + bool collided = CollisionSolverSW::solve_static(shape_A_ptr, xform_A, shape_B_ptr, xform_B, _contact_added_callback, this, &sep_axis); + this->collided = collided; if (!collided) { //test ccd (currently just a raycast) - if (A->is_continuous_collision_detection_enabled() && A->get_mode()>PhysicsServer::BODY_MODE_KINEMATIC && B->get_mode()<=PhysicsServer::BODY_MODE_KINEMATIC) { - _test_ccd(p_step,A,shape_A,xform_A,B,shape_B,xform_B); + if (A->is_continuous_collision_detection_enabled() && A->get_mode() > PhysicsServer::BODY_MODE_KINEMATIC && B->get_mode() <= PhysicsServer::BODY_MODE_KINEMATIC) { + _test_ccd(p_step, A, shape_A, xform_A, B, shape_B, xform_B); } - if (B->is_continuous_collision_detection_enabled() && B->get_mode()>PhysicsServer::BODY_MODE_KINEMATIC && A->get_mode()<=PhysicsServer::BODY_MODE_KINEMATIC) { - _test_ccd(p_step,B,shape_B,xform_B,A,shape_A,xform_A); + if (B->is_continuous_collision_detection_enabled() && B->get_mode() > PhysicsServer::BODY_MODE_KINEMATIC && A->get_mode() <= PhysicsServer::BODY_MODE_KINEMATIC) { + _test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A); } return false; } - - real_t max_penetration = space->get_contact_max_allowed_penetration(); real_t bias = (real_t)0.3; if (shape_A_ptr->get_custom_bias() || shape_B_ptr->get_custom_bias()) { - if (shape_A_ptr->get_custom_bias()==0) - bias=shape_B_ptr->get_custom_bias(); - else if (shape_B_ptr->get_custom_bias()==0) - bias=shape_A_ptr->get_custom_bias(); + 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; } + real_t inv_dt = 1.0 / p_step; - - real_t inv_dt = 1.0/p_step; - - for(int i=0;i<contact_count;i++) { + for (int i = 0; i < contact_count; i++) { Contact &c = contacts[i]; - c.active=false; + c.active = false; Vector3 global_A = xform_Au.xform(c.local_A); Vector3 global_B = xform_Bu.xform(c.local_B); - real_t depth = c.normal.dot(global_A - global_B); - if (depth<=0) { - c.active=false; + if (depth <= 0) { + 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 - c.rA = global_A-A->get_center_of_mass(); - c.rB = global_B-B->get_center_of_mass()-offset_B; + c.rA = global_A - A->get_center_of_mass(); + c.rB = global_B - B->get_center_of_mass() - offset_B; - // contact query reporting... +// contact query reporting... #if 0 if (A->get_body_type() == PhysicsServer::BODY_CHARACTER) static_cast<CharacterBodySW*>(A)->report_character_contact( global_A, global_B, B ); @@ -323,30 +303,28 @@ bool BodyPairSW::setup(real_t p_step) { #endif if (A->can_report_contacts()) { - Vector3 crA = A->get_angular_velocity().cross( c.rA ) + A->get_linear_velocity(); - A->add_contact(global_A,-c.normal,depth,shape_A,global_B,shape_B,B->get_instance_id(),B->get_self(),crA); + Vector3 crA = A->get_angular_velocity().cross(c.rA) + A->get_linear_velocity(); + A->add_contact(global_A, -c.normal, depth, shape_A, global_B, shape_B, B->get_instance_id(), B->get_self(), crA); } if (B->can_report_contacts()) { - Vector3 crB = B->get_angular_velocity().cross( c.rB ) + B->get_linear_velocity(); - B->add_contact(global_B,c.normal,depth,shape_B,global_A,shape_A,A->get_instance_id(),A->get_self(),crB); + Vector3 crB = B->get_angular_velocity().cross(c.rB) + B->get_linear_velocity(); + B->add_contact(global_B, c.normal, depth, shape_B, global_A, shape_A, A->get_instance_id(), A->get_self(), crB); } - if (A->is_shape_set_as_trigger(shape_A) || B->is_shape_set_as_trigger(shape_B) || (A->get_mode()<=PhysicsServer::BODY_MODE_KINEMATIC && B->get_mode()<=PhysicsServer::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() <= PhysicsServer::BODY_MODE_KINEMATIC && B->get_mode() <= PhysicsServer::BODY_MODE_KINEMATIC)) { + c.active = false; + collided = false; continue; - } - - c.active=true; + c.active = true; // Precompute normal mass, tangent mass, and bias. - Vector3 inertia_A = A->get_inv_inertia_tensor().xform( c.rA.cross( c.normal ) ); - Vector3 inertia_B = B->get_inv_inertia_tensor().xform( c.rB.cross( c.normal ) ); + Vector3 inertia_A = A->get_inv_inertia_tensor().xform(c.rA.cross(c.normal)); + Vector3 inertia_B = B->get_inv_inertia_tensor().xform(c.rB.cross(c.normal)); real_t kNormal = A->get_inv_mass() + B->get_inv_mass(); - kNormal += c.normal.dot( inertia_A.cross(c.rA ) ) + c.normal.dot( inertia_B.cross( c.rB )); + kNormal += c.normal.dot(inertia_A.cross(c.rA)) + c.normal.dot(inertia_B.cross(c.rB)); c.mass_normal = 1.0f / kNormal; #if 1 @@ -354,34 +332,32 @@ bool BodyPairSW::setup(real_t p_step) { #else if (depth > max_penetration) { - c.bias = (depth - max_penetration) * (1.0/(p_step*(1.0/RELAXATION_TIMESTEPS))); + c.bias = (depth - max_penetration) * (1.0 / (p_step * (1.0 / RELAXATION_TIMESTEPS))); } else { real_t approach = -0.1 * (depth - max_penetration) / (CMP_EPSILON + max_penetration); - approach = CLAMP( approach, CMP_EPSILON, 1.0 ); - c.bias = approach * (depth - max_penetration) * (1.0/p_step); + approach = CLAMP(approach, CMP_EPSILON, 1.0); + c.bias = approach * (depth - max_penetration) * (1.0 / p_step); } #endif - c.depth=depth; + c.depth = depth; Vector3 j_vec = c.normal * c.acc_normal_impulse + c.acc_tangent_impulse; - A->apply_impulse( c.rA+A->get_center_of_mass(), -j_vec ); - B->apply_impulse( c.rB+B->get_center_of_mass(), j_vec ); - c.acc_bias_impulse=0; + A->apply_impulse(c.rA + A->get_center_of_mass(), -j_vec); + B->apply_impulse(c.rB + B->get_center_of_mass(), j_vec); + c.acc_bias_impulse = 0; Vector3 jb_vec = c.normal * c.acc_bias_impulse; - A->apply_bias_impulse( c.rA+A->get_center_of_mass(), -jb_vec ); - B->apply_bias_impulse( c.rB+B->get_center_of_mass(), jb_vec ); + A->apply_bias_impulse(c.rA + A->get_center_of_mass(), -jb_vec); + B->apply_bias_impulse(c.rB + B->get_center_of_mass(), jb_vec); - c.bounce = MAX(A->get_bounce(),B->get_bounce()); + c.bounce = MAX(A->get_bounce(), B->get_bounce()); if (c.bounce) { - Vector3 crA = A->get_angular_velocity().cross( c.rA ); - Vector3 crB = B->get_angular_velocity().cross( c.rB ); + Vector3 crA = A->get_angular_velocity().cross(c.rA); + Vector3 crB = B->get_angular_velocity().cross(c.rB); Vector3 dv = B->get_linear_velocity() + crB - A->get_linear_velocity() - crA; //normal impule c.bounce = c.bounce * dv.dot(c.normal); } - - } return true; @@ -392,68 +368,63 @@ void BodyPairSW::solve(real_t p_step) { if (!collided) return; - - for(int i=0;i<contact_count;i++) { + for (int i = 0; i < contact_count; i++) { Contact &c = contacts[i]; if (!c.active) continue; - c.active=false; //try to deactivate, will activate itself if still needed + c.active = false; //try to deactivate, will activate itself if still needed //bias impule - Vector3 crbA = A->get_biased_angular_velocity().cross( c.rA ); - Vector3 crbB = B->get_biased_angular_velocity().cross( c.rB ); + Vector3 crbA = A->get_biased_angular_velocity().cross(c.rA); + Vector3 crbB = B->get_biased_angular_velocity().cross(c.rB); Vector3 dbv = B->get_biased_linear_velocity() + crbB - A->get_biased_linear_velocity() - crbA; real_t vbn = dbv.dot(c.normal); - if (Math::abs(-vbn+c.bias)>MIN_VELOCITY) { + if (Math::abs(-vbn + c.bias) > MIN_VELOCITY) { - real_t jbn = (-vbn + c.bias)*c.mass_normal; + real_t jbn = (-vbn + c.bias) * c.mass_normal; real_t jbnOld = c.acc_bias_impulse; c.acc_bias_impulse = MAX(jbnOld + jbn, 0.0f); Vector3 jb = c.normal * (c.acc_bias_impulse - jbnOld); + A->apply_bias_impulse(c.rA + A->get_center_of_mass(), -jb); + B->apply_bias_impulse(c.rB + B->get_center_of_mass(), jb); - A->apply_bias_impulse(c.rA+A->get_center_of_mass(),-jb); - B->apply_bias_impulse(c.rB+B->get_center_of_mass(), jb); - - c.active=true; + c.active = true; } - - Vector3 crA = A->get_angular_velocity().cross( c.rA ); - Vector3 crB = B->get_angular_velocity().cross( c.rB ); + Vector3 crA = A->get_angular_velocity().cross(c.rA); + Vector3 crB = B->get_angular_velocity().cross(c.rB); Vector3 dv = B->get_linear_velocity() + crB - A->get_linear_velocity() - crA; //normal impule real_t vn = dv.dot(c.normal); - if (Math::abs(vn)>MIN_VELOCITY) { + if (Math::abs(vn) > MIN_VELOCITY) { - 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); + Vector3 j = c.normal * (c.acc_normal_impulse - jnOld); - Vector3 j =c.normal * (c.acc_normal_impulse - jnOld); - + A->apply_impulse(c.rA + A->get_center_of_mass(), -j); + B->apply_impulse(c.rB + B->get_center_of_mass(), j); - A->apply_impulse(c.rA+A->get_center_of_mass(),-j); - B->apply_impulse(c.rB+B->get_center_of_mass(), j); - - c.active=true; + c.active = true; } //friction impule real_t friction = A->get_friction() * B->get_friction(); - Vector3 lvA = A->get_linear_velocity() + A->get_angular_velocity().cross( c.rA ); - Vector3 lvB = B->get_linear_velocity() + B->get_angular_velocity().cross( c.rB ); + Vector3 lvA = A->get_linear_velocity() + A->get_angular_velocity().cross(c.rA); + Vector3 lvB = B->get_linear_velocity() + B->get_angular_velocity().cross(c.rB); Vector3 dtv = lvB - lvA; real_t tn = c.normal.dot(dtv); @@ -466,15 +437,14 @@ void BodyPairSW::solve(real_t p_step) { tv /= tvl; - Vector3 temp1 = A->get_inv_inertia_tensor().xform( c.rA.cross( tv ) ); - Vector3 temp2 = B->get_inv_inertia_tensor().xform( c.rB.cross( tv ) ); + Vector3 temp1 = A->get_inv_inertia_tensor().xform(c.rA.cross(tv)); + Vector3 temp2 = B->get_inv_inertia_tensor().xform(c.rB.cross(tv)); real_t t = -tvl / - (A->get_inv_mass() + B->get_inv_mass() + tv.dot(temp1.cross(c.rA) + temp2.cross(c.rB))); + (A->get_inv_mass() + B->get_inv_mass() + tv.dot(temp1.cross(c.rA) + temp2.cross(c.rB))); Vector3 jt = t * tv; - Vector3 jtOld = c.acc_tangent_impulse; c.acc_tangent_impulse += jt; @@ -483,46 +453,35 @@ void BodyPairSW::solve(real_t p_step) { if (fi_len > CMP_EPSILON && fi_len > jtMax) { - c.acc_tangent_impulse*=jtMax / fi_len; + c.acc_tangent_impulse *= jtMax / fi_len; } jt = c.acc_tangent_impulse - jtOld; + A->apply_impulse(c.rA + A->get_center_of_mass(), -jt); + B->apply_impulse(c.rB + B->get_center_of_mass(), jt); - A->apply_impulse( c.rA+A->get_center_of_mass(), -jt ); - B->apply_impulse( c.rB+B->get_center_of_mass(), jt ); - - c.active=true; - + c.active = true; } - - } - } +BodyPairSW::BodyPairSW(BodySW *p_A, int p_shape_A, BodySW *p_B, int p_shape_B) + : ConstraintSW(_arr, 2) { - - - -BodyPairSW::BodyPairSW(BodySW *p_A, int p_shape_A,BodySW *p_B, int p_shape_B) : ConstraintSW(_arr,2) { - - A=p_A; - B=p_B; - shape_A=p_shape_A; - shape_B=p_shape_B; - space=A->get_space(); - A->add_constraint(this,0); - B->add_constraint(this,1); - contact_count=0; - collided=false; - + A = p_A; + B = p_B; + shape_A = p_shape_A; + shape_B = p_shape_B; + space = A->get_space(); + A->add_constraint(this, 0); + B->add_constraint(this, 1); + contact_count = 0; + collided = false; } - BodyPairSW::~BodyPairSW() { A->remove_constraint(this); B->remove_constraint(this); - } diff --git a/servers/physics/body_pair_sw.h b/servers/physics/body_pair_sw.h index f282a56b9..fa426adaf 100644 --- a/servers/physics/body_pair_sw.h +++ b/servers/physics/body_pair_sw.h @@ -35,7 +35,7 @@ class BodyPairSW : public ConstraintSW { enum { - MAX_CONTACTS=4 + MAX_CONTACTS = 4 }; union { @@ -50,22 +50,21 @@ class BodyPairSW : public ConstraintSW { int shape_A; int shape_B; - struct Contact { Vector3 position; Vector3 normal; Vector3 local_A, local_B; - real_t acc_normal_impulse; // accumulated normal impulse (Pn) - Vector3 acc_tangent_impulse; // accumulated tangent impulse (Pt) - real_t acc_bias_impulse; // accumulated normal impulse for position bias (Pnb) + real_t acc_normal_impulse; // accumulated normal impulse (Pn) + Vector3 acc_tangent_impulse; // accumulated tangent impulse (Pt) + real_t acc_bias_impulse; // accumulated normal impulse for position bias (Pnb) real_t mass_normal; real_t bias; real_t bounce; real_t depth; bool active; - Vector3 rA,rB; // Offset in world orientation with respect to center of mass + Vector3 rA, rB; // Offset in world orientation with respect to center of mass }; Vector3 offset_B; //use local A coordinates to avoid numerical issues on collision detection @@ -76,24 +75,21 @@ class BodyPairSW : public ConstraintSW { bool collided; int cc; + static void _contact_added_callback(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata); - static void _contact_added_callback(const Vector3& p_point_A,const Vector3& p_point_B,void *p_userdata); - - void contact_added_callback(const Vector3& p_point_A,const Vector3& p_point_B); + void contact_added_callback(const Vector3 &p_point_A, const Vector3 &p_point_B); void validate_contacts(); - bool _test_ccd(real_t p_step,BodySW *p_A, int p_shape_A,const Transform& p_xform_A,BodySW *p_B, int p_shape_B,const Transform& p_xform_B); + bool _test_ccd(real_t p_step, BodySW *p_A, int p_shape_A, const Transform &p_xform_A, BodySW *p_B, int p_shape_B, const Transform &p_xform_B); SpaceSW *space; public: - bool setup(real_t p_step); void solve(real_t p_step); - BodyPairSW(BodySW *p_A, int p_shape_A,BodySW *p_B, int p_shape_B); + BodyPairSW(BodySW *p_A, int p_shape_A, BodySW *p_B, int p_shape_B); ~BodyPairSW(); - }; #endif // BODY_PAIR__SW_H diff --git a/servers/physics/body_sw.cpp b/servers/physics/body_sw.cpp index 7fcd76726..a4fc694f6 100644 --- a/servers/physics/body_sw.cpp +++ b/servers/physics/body_sw.cpp @@ -27,18 +27,17 @@ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ #include "body_sw.h" -#include "space_sw.h" #include "area_sw.h" +#include "space_sw.h" void BodySW::_update_inertia() { if (get_space() && !inertia_update_list.in_list()) get_space()->body_add_to_inertia_update_list(&inertia_update_list); - } void BodySW::_update_transform_dependant() { - + center_of_mass = get_transform().basis.xform(center_of_mass_local); principal_inertia_axes = get_transform().basis * principal_inertia_axes_local; @@ -47,30 +46,29 @@ void BodySW::_update_transform_dependant() { Basis tbt = tb.transposed(); tb.scale(_inv_inertia); _inv_inertia_tensor = tb * tbt; - } void BodySW::update_inertias() { //update shapes and motions - switch(mode) { + switch (mode) { case PhysicsServer::BODY_MODE_RIGID: { //update tensor for all shapes, not the best way but should be somehow OK. (inspired from bullet) - real_t total_area=0; + 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_area(i); + total_area += get_shape_area(i); } // We have to recompute the center of mass center_of_mass_local.zero(); - for (int i=0; i<get_shape_count(); i++) { - real_t area=get_shape_area(i); + for (int i = 0; i < get_shape_count(); i++) { + real_t area = get_shape_area(i); real_t mass = area * this->mass / total_area; @@ -84,25 +82,23 @@ void BodySW::update_inertias() { Basis inertia_tensor; inertia_tensor.set_zero(); - for (int i=0;i<get_shape_count();i++) { + for (int i = 0; i < get_shape_count(); i++) { - const ShapeSW* shape=get_shape(i); + const ShapeSW *shape = get_shape(i); - real_t area=get_shape_area(i); + real_t area = get_shape_area(i); real_t mass = area * this->mass / total_area; - Basis shape_inertia_tensor=shape->get_moment_of_inertia(mass).to_diagonal_matrix(); - Transform shape_transform=get_shape_transform(i); + Basis shape_inertia_tensor = shape->get_moment_of_inertia(mass).to_diagonal_matrix(); + Transform shape_transform = get_shape_transform(i); Basis shape_basis = shape_transform.basis.orthonormalized(); // NOTE: we don't take the scale of collision shapes into account when computing the inertia tensor! shape_inertia_tensor = shape_basis * shape_inertia_tensor * shape_basis.transposed(); Vector3 shape_origin = shape_transform.origin - center_of_mass_local; - inertia_tensor += shape_inertia_tensor + (Basis()*shape_origin.dot(shape_origin)-shape_origin.outer(shape_origin))*mass; - - + inertia_tensor += shape_inertia_tensor + (Basis() * shape_origin.dot(shape_origin) - shape_origin.outer(shape_origin)) * mass; } // Compute the principal axes of inertia @@ -110,9 +106,9 @@ void BodySW::update_inertias() { _inv_inertia = inertia_tensor.get_main_diagonal().inverse(); if (mass) - _inv_mass=1.0/mass; + _inv_mass = 1.0 / mass; else - _inv_mass=0; + _inv_mass = 0; } break; @@ -120,42 +116,39 @@ void BodySW::update_inertias() { case PhysicsServer::BODY_MODE_STATIC: { _inv_inertia_tensor.set_zero(); - _inv_mass=0; + _inv_mass = 0; } break; case PhysicsServer::BODY_MODE_CHARACTER: { _inv_inertia_tensor.set_zero(); - _inv_mass=1.0/mass; + _inv_mass = 1.0 / mass; } break; } - //_update_shapes(); _update_transform_dependant(); } - - void BodySW::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==PhysicsServer::BODY_MODE_STATIC) + if (mode == PhysicsServer::BODY_MODE_STATIC) return; //static bodies can't become active if (get_space()) get_space()->body_add_to_active_list(&active_list); //still_time=0; } -/* + /* if (!space) return; @@ -168,43 +161,41 @@ void BodySW::set_active(bool p_active) { */ } - - void BodySW::set_param(PhysicsServer::BodyParameter p_param, real_t p_value) { - switch(p_param) { + switch (p_param) { case PhysicsServer::BODY_PARAM_BOUNCE: { - bounce=p_value; + bounce = p_value; } break; case PhysicsServer::BODY_PARAM_FRICTION: { - friction=p_value; + friction = p_value; } break; case PhysicsServer::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 PhysicsServer::BODY_PARAM_GRAVITY_SCALE: { - gravity_scale=p_value; + gravity_scale = p_value; } break; case PhysicsServer::BODY_PARAM_LINEAR_DAMP: { - linear_damp=p_value; + linear_damp = p_value; } break; case PhysicsServer::BODY_PARAM_ANGULAR_DAMP: { - angular_damp=p_value; + angular_damp = p_value; } break; - default:{} + default: {} } } real_t BodySW::get_param(PhysicsServer::BodyParameter p_param) const { - switch(p_param) { + switch (p_param) { case PhysicsServer::BODY_PARAM_BOUNCE: { return bounce; @@ -228,7 +219,7 @@ real_t BodySW::get_param(PhysicsServer::BodyParameter p_param) const { return angular_damp; } break; - default:{} + default: {} } return 0; @@ -236,35 +227,35 @@ real_t BodySW::get_param(PhysicsServer::BodyParameter p_param) const { void BodySW::set_mode(PhysicsServer::BodyMode p_mode) { - PhysicsServer::BodyMode prev=mode; - mode=p_mode; + PhysicsServer::BodyMode prev = mode; + mode = p_mode; - switch(p_mode) { + switch (p_mode) { //CLEAR UP EVERYTHING IN CASE IT NOT WORKS! case PhysicsServer::BODY_MODE_STATIC: case PhysicsServer::BODY_MODE_KINEMATIC: { _set_inv_transform(get_transform().affine_inverse()); - _inv_mass=0; - _set_static(p_mode==PhysicsServer::BODY_MODE_STATIC); + _inv_mass = 0; + _set_static(p_mode == PhysicsServer::BODY_MODE_STATIC); //set_active(p_mode==PhysicsServer::BODY_MODE_KINEMATIC); - set_active(p_mode==PhysicsServer::BODY_MODE_KINEMATIC && contacts.size()); - linear_velocity=Vector3(); - angular_velocity=Vector3(); - if (mode==PhysicsServer::BODY_MODE_KINEMATIC && prev!=mode) { - first_time_kinematic=true; + set_active(p_mode == PhysicsServer::BODY_MODE_KINEMATIC && contacts.size()); + linear_velocity = Vector3(); + angular_velocity = Vector3(); + if (mode == PhysicsServer::BODY_MODE_KINEMATIC && prev != mode) { + first_time_kinematic = true; } } break; case PhysicsServer::BODY_MODE_RIGID: { - _inv_mass=mass>0?(1.0/mass):0; + _inv_mass = mass > 0 ? (1.0 / mass) : 0; _set_static(false); } break; case PhysicsServer::BODY_MODE_CHARACTER: { - _inv_mass=mass>0?(1.0/mass):0; + _inv_mass = mass > 0 ? (1.0 / mass) : 0; _set_static(false); } break; } @@ -274,7 +265,6 @@ void BodySW::set_mode(PhysicsServer::BodyMode p_mode) { if (get_space()) _update_queries(); */ - } PhysicsServer::BodyMode BodySW::get_mode() const { @@ -286,35 +276,33 @@ void BodySW::_shapes_changed() { _update_inertia(); } -void BodySW::set_state(PhysicsServer::BodyState p_state, const Variant& p_variant) { +void BodySW::set_state(PhysicsServer::BodyState p_state, const Variant &p_variant) { - switch(p_state) { + switch (p_state) { case PhysicsServer::BODY_STATE_TRANSFORM: { - - if (mode==PhysicsServer::BODY_MODE_KINEMATIC) { - new_transform=p_variant; + if (mode == PhysicsServer::BODY_MODE_KINEMATIC) { + new_transform = p_variant; //wakeup_neighbours(); set_active(true); if (first_time_kinematic) { _set_transform(p_variant); _set_inv_transform(get_transform().affine_inverse()); - first_time_kinematic=false; + first_time_kinematic = false; } - } else if (mode==PhysicsServer::BODY_MODE_STATIC) { + } else if (mode == PhysicsServer::BODY_MODE_STATIC) { _set_transform(p_variant); _set_inv_transform(get_transform().affine_inverse()); wakeup_neighbours(); } else { Transform t = p_variant; t.orthonormalize(); - new_transform=get_transform(); //used as old to compute motion - if (new_transform==t) + new_transform = get_transform(); //used as old to compute motion + if (new_transform == t) break; _set_transform(t); _set_inv_transform(get_transform().inverse()); - } wakeup(); @@ -325,7 +313,7 @@ void BodySW::set_state(PhysicsServer::BodyState p_state, const Variant& p_varian if (mode==PhysicsServer::BODY_MODE_STATIC) break; */ - linear_velocity=p_variant; + linear_velocity = p_variant; wakeup(); } break; case PhysicsServer::BODY_STATE_ANGULAR_VELOCITY: { @@ -333,38 +321,37 @@ void BodySW::set_state(PhysicsServer::BodyState p_state, const Variant& p_varian if (mode!=PhysicsServer::BODY_MODE_RIGID) break; */ - angular_velocity=p_variant; + angular_velocity = p_variant; wakeup(); } break; case PhysicsServer::BODY_STATE_SLEEPING: { //? - if (mode==PhysicsServer::BODY_MODE_STATIC || mode==PhysicsServer::BODY_MODE_KINEMATIC) + if (mode == PhysicsServer::BODY_MODE_STATIC || mode == PhysicsServer::BODY_MODE_KINEMATIC) break; - bool do_sleep=p_variant; + bool do_sleep = p_variant; if (do_sleep) { - linear_velocity=Vector3(); + linear_velocity = Vector3(); //biased_linear_velocity=Vector3(); - angular_velocity=Vector3(); + angular_velocity = Vector3(); //biased_angular_velocity=Vector3(); set_active(false); } else { - if (mode!=PhysicsServer::BODY_MODE_STATIC) + if (mode != PhysicsServer::BODY_MODE_STATIC) set_active(true); } } break; case PhysicsServer::BODY_STATE_CAN_SLEEP: { - can_sleep=p_variant; - if (mode==PhysicsServer::BODY_MODE_RIGID && !active && !can_sleep) + can_sleep = p_variant; + if (mode == PhysicsServer::BODY_MODE_RIGID && !active && !can_sleep) set_active(true); } break; } - } Variant BodySW::get_state(PhysicsServer::BodyState p_state) const { - switch(p_state) { + switch (p_state) { case PhysicsServer::BODY_STATE_TRANSFORM: { return get_transform(); } break; @@ -385,8 +372,7 @@ Variant BodySW::get_state(PhysicsServer::BodyState p_state) const { return Variant(); } - -void BodySW::set_space(SpaceSW *p_space){ +void BodySW::set_space(SpaceSW *p_space) { if (get_space()) { @@ -396,7 +382,6 @@ void BodySW::set_space(SpaceSW *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); @@ -413,19 +398,17 @@ void BodySW::set_space(SpaceSW *p_space){ set_active(true); } */ - } - first_integration=true; - + first_integration = true; } void BodySW::_compute_area_gravity_and_dampenings(const AreaSW *p_area) { if (p_area->is_gravity_point()) { - if(p_area->get_gravity_distance_scale() > 0) { + if (p_area->get_gravity_distance_scale() > 0) { Vector3 v = p_area->get_transform().xform(p_area->get_gravity_vector()) - get_transform().get_origin(); - gravity += v.normalized() * (p_area->get_gravity() / Math::pow(v.length() * p_area->get_gravity_distance_scale()+1, 2) ); + 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(); } @@ -439,8 +422,7 @@ void BodySW::_compute_area_gravity_and_dampenings(const AreaSW *p_area) { void BodySW::integrate_forces(real_t p_step) { - - if (mode==PhysicsServer::BODY_MODE_STATIC) + if (mode == PhysicsServer::BODY_MODE_STATIC) return; AreaSW *def_area = get_space()->get_default_area(); @@ -450,192 +432,179 @@ void BodySW::integrate_forces(real_t p_step) { int ac = areas.size(); bool stopped = false; - gravity = Vector3(0,0,0); + gravity = Vector3(0, 0, 0); area_linear_damp = 0; area_angular_damp = 0; if (ac) { areas.sort(); const AreaCMP *aa = &areas[0]; // damp_area = aa[ac-1].area; - for(int i=ac-1;i>=0 && !stopped;i--) { - PhysicsServer::AreaSpaceOverrideMode mode=aa[i].area->get_space_override_mode(); + for (int i = ac - 1; i >= 0 && !stopped; i--) { + PhysicsServer::AreaSpaceOverrideMode mode = aa[i].area->get_space_override_mode(); switch (mode) { case PhysicsServer::AREA_SPACE_OVERRIDE_COMBINE: case PhysicsServer::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: { _compute_area_gravity_and_dampenings(aa[i].area); - stopped = mode==PhysicsServer::AREA_SPACE_OVERRIDE_COMBINE_REPLACE; + stopped = mode == PhysicsServer::AREA_SPACE_OVERRIDE_COMBINE_REPLACE; } break; case PhysicsServer::AREA_SPACE_OVERRIDE_REPLACE: case PhysicsServer::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: { - gravity = Vector3(0,0,0); + gravity = Vector3(0, 0, 0); area_angular_damp = 0; area_linear_damp = 0; _compute_area_gravity_and_dampenings(aa[i].area); - stopped = mode==PhysicsServer::AREA_SPACE_OVERRIDE_REPLACE; + stopped = mode == PhysicsServer::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 Body - if (angular_damp>=0) - area_angular_damp=angular_damp; + if (angular_damp >= 0) + area_angular_damp = angular_damp; /* else area_angular_damp=damp_area->get_angular_damp(); */ - if (linear_damp>=0) - area_linear_damp=linear_damp; + if (linear_damp >= 0) + area_linear_damp = linear_damp; /* else area_linear_damp=damp_area->get_linear_damp(); */ - Vector3 motion; - bool do_motion=false; + bool do_motion = false; - if (mode==PhysicsServer::BODY_MODE_KINEMATIC) { + if (mode == PhysicsServer::BODY_MODE_KINEMATIC) { //compute motion, angular and etc. velocities from prev transform - linear_velocity = (new_transform.origin - get_transform().origin)/p_step; + linear_velocity = (new_transform.origin - get_transform().origin) / p_step; //compute a FAKE angular velocity, not so easy - Basis rot=new_transform.basis.orthonormalized().transposed() * get_transform().basis.orthonormalized(); + Basis rot = new_transform.basis.orthonormalized().transposed() * get_transform().basis.orthonormalized(); Vector3 axis; real_t angle; - rot.get_axis_and_angle(axis,angle); + rot.get_axis_and_angle(axis, angle); axis.normalize(); - angular_velocity=axis.normalized() * (angle/p_step); + angular_velocity = axis.normalized() * (angle / p_step); motion = new_transform.origin - get_transform().origin; - do_motion=true; + do_motion = true; } else { if (!omit_force_integration && !first_integration) { //overriden by direct state query - Vector3 force=gravity*mass; - force+=applied_force; - Vector3 torque=applied_torque; + Vector3 force = gravity * mass; + force += applied_force; + Vector3 torque = applied_torque; real_t damp = 1.0 - p_step * area_linear_damp; - if (damp<0) // reached zero in the given time - damp=0; + 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_tensor.xform(torque)*p_step; + linear_velocity += _inv_mass * force * p_step; + angular_velocity += _inv_inertia_tensor.xform(torque) * p_step; } if (continuous_cd) { - motion=linear_velocity*p_step; - do_motion=true; + motion = linear_velocity * p_step; + do_motion = true; } - } - applied_force=Vector3(); - applied_torque=Vector3(); - first_integration=false; + applied_force = Vector3(); + applied_torque = Vector3(); + first_integration = false; //motion=linear_velocity*p_step; - biased_angular_velocity=Vector3(); - biased_linear_velocity=Vector3(); - + biased_angular_velocity = Vector3(); + biased_linear_velocity = Vector3(); - if (do_motion) {//shapes temporarily extend for raycast + if (do_motion) { //shapes temporarily extend for raycast _update_shapes_with_motion(motion); } - - def_area=NULL; // clear the area, so it is set in the next frame - contact_count=0; - + def_area = NULL; // clear the area, so it is set in the next frame + contact_count = 0; } void BodySW::integrate_velocities(real_t p_step) { - if (mode==PhysicsServer::BODY_MODE_STATIC) + if (mode == PhysicsServer::BODY_MODE_STATIC) return; if (fi_callback) get_space()->body_add_to_state_query_list(&direct_state_query_list); - if (mode==PhysicsServer::BODY_MODE_KINEMATIC) { + if (mode == PhysicsServer::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==Vector3() && angular_velocity==Vector3()) + if (contacts.size() == 0 && linear_velocity == Vector3() && angular_velocity == Vector3()) set_active(false); //stopped moving, deactivate return; } - - //apply axis lock - if (axis_lock!=PhysicsServer::BODY_AXIS_LOCK_DISABLED) { + if (axis_lock != PhysicsServer::BODY_AXIS_LOCK_DISABLED) { - - int axis=axis_lock-1; - for(int i=0;i<3;i++) { - if (i==axis) { - linear_velocity[i]=0; - biased_linear_velocity[i]=0; + int axis = axis_lock - 1; + for (int i = 0; i < 3; i++) { + if (i == axis) { + linear_velocity[i] = 0; + biased_linear_velocity[i] = 0; } else { - angular_velocity[i]=0; - biased_angular_velocity[i]=0; + angular_velocity[i] = 0; + biased_angular_velocity[i] = 0; } } - } - - Vector3 total_angular_velocity = angular_velocity+biased_angular_velocity; - - + Vector3 total_angular_velocity = angular_velocity + biased_angular_velocity; real_t ang_vel = total_angular_velocity.length(); Transform transform = get_transform(); - - if (ang_vel!=0.0) { + if (ang_vel != 0.0) { Vector3 ang_vel_axis = total_angular_velocity / ang_vel; - Basis rot( ang_vel_axis, ang_vel*p_step ); + Basis rot(ang_vel_axis, ang_vel * p_step); Basis identity3(1, 0, 0, 0, 1, 0, 0, 0, 1); transform.origin += ((identity3 - rot) * transform.basis).xform(center_of_mass_local); transform.basis = rot * transform.basis; transform.orthonormalize(); } - Vector3 total_linear_velocity=linear_velocity+biased_linear_velocity; + Vector3 total_linear_velocity = linear_velocity + biased_linear_velocity; /*for(int i=0;i<3;i++) { if (axis_lock&(1<<i)) { transform.origin[i]=0.0; } }*/ - transform.origin+=total_linear_velocity * p_step; + transform.origin += total_linear_velocity * p_step; _set_transform(transform); _set_inv_transform(get_transform().inverse()); @@ -684,18 +653,18 @@ void BodySW::simulate_motion(const Transform& p_xform,real_t p_step) { void BodySW::wakeup_neighbours() { - for(Map<ConstraintSW*,int>::Element *E=constraint_map.front();E;E=E->next()) { + for (Map<ConstraintSW *, int>::Element *E = constraint_map.front(); E; E = E->next()) { - const ConstraintSW *c=E->key(); + const ConstraintSW *c = E->key(); BodySW **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; BodySW *b = n[i]; - if (b->mode!=PhysicsServer::BODY_MODE_RIGID) + if (b->mode != PhysicsServer::BODY_MODE_RIGID) continue; if (!b->is_active()) @@ -706,109 +675,96 @@ void BodySW::wakeup_neighbours() { void BodySW::call_queries() { - if (fi_callback) { PhysicsDirectBodyStateSW *dbs = PhysicsDirectBodyStateSW::singleton; - dbs->body=this; + dbs->body = this; - Variant v=dbs; + Variant v = dbs; Object *obj = ObjectDB::get_instance(fi_callback->id); if (!obj) { - set_force_integration_callback(0,StringName()); + set_force_integration_callback(0, StringName()); } else { - const Variant *vp[2]={&v,&fi_callback->udata}; + const Variant *vp[2] = { &v, &fi_callback->udata }; Variant::CallError ce; - int argc=(fi_callback->udata.get_type()==Variant::NIL)?1:2; - obj->call(fi_callback->method,vp,argc,ce); + int argc = (fi_callback->udata.get_type() == Variant::NIL) ? 1 : 2; + obj->call(fi_callback->method, vp, argc, ce); } - - } - - } +bool BodySW::sleep_test(real_t p_step) { -bool BodySW::sleep_test(real_t p_step) { - - if (mode==PhysicsServer::BODY_MODE_STATIC || mode==PhysicsServer::BODY_MODE_KINEMATIC) + if (mode == PhysicsServer::BODY_MODE_STATIC || mode == PhysicsServer::BODY_MODE_KINEMATIC) return true; // - else if (mode==PhysicsServer::BODY_MODE_CHARACTER) + else if (mode == PhysicsServer::BODY_MODE_CHARACTER) return !active; // characters don't sleep unless asked to sleep else if (!can_sleep) return false; + if (Math::abs(angular_velocity.length()) < get_space()->get_body_angular_velocity_sleep_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.length())<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 BodySW::set_force_integration_callback(ObjectID p_id,const StringName& p_method,const Variant& p_udata) { +void BodySW::set_force_integration_callback(ObjectID p_id, const StringName &p_method, const Variant &p_udata) { if (fi_callback) { memdelete(fi_callback); - fi_callback=NULL; + 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->udata=p_udata; + fi_callback = memnew(ForceIntegrationCallback); + fi_callback->id = p_id; + fi_callback->method = p_method; + fi_callback->udata = p_udata; } - } -BodySW::BodySW() : CollisionObjectSW(TYPE_BODY), active_list(this), inertia_update_list(this), direct_state_query_list(this) { - +BodySW::BodySW() + : CollisionObjectSW(TYPE_BODY), active_list(this), inertia_update_list(this), direct_state_query_list(this) { - mode=PhysicsServer::BODY_MODE_RIGID; - active=true; + mode = PhysicsServer::BODY_MODE_RIGID; + active = true; - mass=1; + mass = 1; //_inv_inertia=Transform(); - _inv_mass=1; - bounce=0; - friction=1; - omit_force_integration=false; + _inv_mass = 1; + bounce = 0; + friction = 1; + omit_force_integration = false; //applied_torque=0; - island_step=0; - island_next=NULL; - island_list_next=NULL; - first_time_kinematic=false; - first_integration=false; + island_step = 0; + island_next = NULL; + island_list_next = NULL; + first_time_kinematic = false; + first_integration = false; _set_static(false); - contact_count=0; - gravity_scale=1.0; + contact_count = 0; + gravity_scale = 1.0; - area_angular_damp=0; - area_linear_damp=0; - - still_time=0; - continuous_cd=false; - can_sleep=false; - fi_callback=NULL; - axis_lock=PhysicsServer::BODY_AXIS_LOCK_DISABLED; + area_angular_damp = 0; + area_linear_damp = 0; + still_time = 0; + continuous_cd = false; + can_sleep = false; + fi_callback = NULL; + axis_lock = PhysicsServer::BODY_AXIS_LOCK_DISABLED; } BodySW::~BodySW() { @@ -817,9 +773,9 @@ BodySW::~BodySW() { memdelete(fi_callback); } -PhysicsDirectBodyStateSW *PhysicsDirectBodyStateSW::singleton=NULL; +PhysicsDirectBodyStateSW *PhysicsDirectBodyStateSW::singleton = NULL; -PhysicsDirectSpaceState* PhysicsDirectBodyStateSW::get_space_state() { +PhysicsDirectSpaceState *PhysicsDirectBodyStateSW::get_space_state() { return body->get_space()->get_direct_state(); } diff --git a/servers/physics/body_sw.h b/servers/physics/body_sw.h index 2383d2d68..4b1af6fca 100644 --- a/servers/physics/body_sw.h +++ b/servers/physics/body_sw.h @@ -29,16 +29,14 @@ #ifndef BODY_SW_H #define BODY_SW_H +#include "area_sw.h" #include "collision_object_sw.h" #include "vset.h" -#include "area_sw.h" class ConstraintSW; - class BodySW : public CollisionObjectSW { - PhysicsServer::BodyMode mode; Vector3 linear_velocity; @@ -78,7 +76,6 @@ class BodySW : public CollisionObjectSW { real_t area_angular_damp; real_t area_linear_damp; - SelfList<BodySW> active_list; SelfList<BodySW> inertia_update_list; SelfList<BodySW> direct_state_query_list; @@ -96,23 +93,25 @@ class BodySW : public CollisionObjectSW { virtual void _shapes_changed(); Transform new_transform; - Map<ConstraintSW*,int> constraint_map; + Map<ConstraintSW *, int> constraint_map; struct AreaCMP { AreaSW *area; int refCount; - _FORCE_INLINE_ bool operator==(const AreaCMP& p_cmp) const { return area->get_self() == p_cmp.area->get_self();} - _FORCE_INLINE_ bool operator<(const AreaCMP& p_cmp) const { return area->get_priority() < p_cmp.area->get_priority();} + _FORCE_INLINE_ bool operator==(const AreaCMP &p_cmp) const { return area->get_self() == p_cmp.area->get_self(); } + _FORCE_INLINE_ bool operator<(const AreaCMP &p_cmp) const { return area->get_priority() < p_cmp.area->get_priority(); } _FORCE_INLINE_ AreaCMP() {} - _FORCE_INLINE_ AreaCMP(AreaSW *p_area) { area=p_area; refCount=1;} + _FORCE_INLINE_ AreaCMP(AreaSW *p_area) { + area = p_area; + refCount = 1; + } }; Vector<AreaCMP> areas; struct Contact { - Vector3 local_pos; Vector3 local_normal; real_t depth; @@ -136,7 +135,6 @@ class BodySW : public CollisionObjectSW { ForceIntegrationCallback *fi_callback; - uint64_t island_step; BodySW *island_next; BodySW *island_list_next; @@ -145,16 +143,14 @@ class BodySW : public CollisionObjectSW { _FORCE_INLINE_ void _update_transform_dependant(); -friend class PhysicsDirectBodyStateSW; // i give up, too many functions to expose + friend class PhysicsDirectBodyStateSW; // i give up, too many functions to expose public: - - - void set_force_integration_callback(ObjectID p_id,const StringName& p_method,const Variant& p_udata=Variant()); + void set_force_integration_callback(ObjectID p_id, const StringName &p_method, const Variant &p_udata = Variant()); _FORCE_INLINE_ void add_area(AreaSW *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)); @@ -163,77 +159,80 @@ public: _FORCE_INLINE_ void remove_area(AreaSW *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==PhysicsServer::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 == PhysicsServer::BODY_MODE_KINEMATIC && p_size) set_active(true); + } _FORCE_INLINE_ int get_max_contacts_reported() const { return contacts.size(); } _FORCE_INLINE_ bool can_report_contacts() const { return !contacts.empty(); } - _FORCE_INLINE_ void add_contact(const Vector3& p_local_pos,const Vector3& p_local_normal, real_t p_depth, int p_local_shape, const Vector3& p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID& p_collider,const Vector3& p_collider_velocity_at_pos); - + _FORCE_INLINE_ void add_contact(const Vector3 &p_local_pos, const Vector3 &p_local_normal, real_t p_depth, int p_local_shape, const Vector3 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector3 &p_collider_velocity_at_pos); - _FORCE_INLINE_ void add_exception(const RID& p_exception) { exceptions.insert(p_exception);} - _FORCE_INLINE_ void remove_exception(const RID& p_exception) { exceptions.erase(p_exception);} - _FORCE_INLINE_ bool has_exception(const RID& p_exception) const { return exceptions.has(p_exception);} - _FORCE_INLINE_ const VSet<RID>& get_exceptions() const { return exceptions;} + _FORCE_INLINE_ 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_ BodySW* get_island_next() const { return island_next; } - _FORCE_INLINE_ void set_island_next(BodySW* p_next) { island_next=p_next; } + _FORCE_INLINE_ BodySW *get_island_next() const { return island_next; } + _FORCE_INLINE_ void set_island_next(BodySW *p_next) { island_next = p_next; } - _FORCE_INLINE_ BodySW* get_island_list_next() const { return island_list_next; } - _FORCE_INLINE_ void set_island_list_next(BodySW* p_next) { island_list_next=p_next; } + _FORCE_INLINE_ BodySW *get_island_list_next() const { return island_list_next; } + _FORCE_INLINE_ void set_island_list_next(BodySW *p_next) { island_list_next = p_next; } - _FORCE_INLINE_ void add_constraint(ConstraintSW* p_constraint, int p_pos) { constraint_map[p_constraint]=p_pos; } - _FORCE_INLINE_ void remove_constraint(ConstraintSW* p_constraint) { constraint_map.erase(p_constraint); } - const Map<ConstraintSW*,int>& get_constraint_map() const { return constraint_map; } + _FORCE_INLINE_ void add_constraint(ConstraintSW *p_constraint, int p_pos) { constraint_map[p_constraint] = p_pos; } + _FORCE_INLINE_ void remove_constraint(ConstraintSW *p_constraint) { constraint_map.erase(p_constraint); } + const Map<ConstraintSW *, int> &get_constraint_map() const { return constraint_map; } - _FORCE_INLINE_ void 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_ Basis get_principal_inertia_axes() const { return principal_inertia_axes; } _FORCE_INLINE_ Vector3 get_center_of_mass() const { return center_of_mass; } - _FORCE_INLINE_ Vector3 xform_local_to_principal(const Vector3& p_pos) const { return principal_inertia_axes_local.xform(p_pos - center_of_mass_local); } + _FORCE_INLINE_ Vector3 xform_local_to_principal(const Vector3 &p_pos) const { return principal_inertia_axes_local.xform(p_pos - center_of_mass_local); } - _FORCE_INLINE_ void set_linear_velocity(const Vector3& p_velocity) {linear_velocity=p_velocity; } + _FORCE_INLINE_ void set_linear_velocity(const Vector3 &p_velocity) { linear_velocity = p_velocity; } _FORCE_INLINE_ Vector3 get_linear_velocity() const { return linear_velocity; } - _FORCE_INLINE_ void set_angular_velocity(const Vector3& p_velocity) { angular_velocity=p_velocity; } + _FORCE_INLINE_ void set_angular_velocity(const Vector3 &p_velocity) { angular_velocity = p_velocity; } _FORCE_INLINE_ Vector3 get_angular_velocity() const { return angular_velocity; } - _FORCE_INLINE_ const Vector3& get_biased_linear_velocity() const { return biased_linear_velocity; } - _FORCE_INLINE_ const Vector3& get_biased_angular_velocity() const { return biased_angular_velocity; } + _FORCE_INLINE_ const Vector3 &get_biased_linear_velocity() const { return biased_linear_velocity; } + _FORCE_INLINE_ const Vector3 &get_biased_angular_velocity() const { return biased_angular_velocity; } - _FORCE_INLINE_ void apply_impulse(const Vector3& p_pos, const Vector3& p_j) { + _FORCE_INLINE_ void apply_impulse(const Vector3 &p_pos, const Vector3 &p_j) { linear_velocity += p_j * _inv_mass; - angular_velocity += _inv_inertia_tensor.xform( (p_pos-center_of_mass).cross(p_j) ); + angular_velocity += _inv_inertia_tensor.xform((p_pos - center_of_mass).cross(p_j)); } - _FORCE_INLINE_ void apply_torque_impulse(const Vector3& p_j) { + _FORCE_INLINE_ void apply_torque_impulse(const Vector3 &p_j) { angular_velocity += _inv_inertia_tensor.xform(p_j); } - _FORCE_INLINE_ void apply_bias_impulse(const Vector3& p_pos, const Vector3& p_j) { + _FORCE_INLINE_ void apply_bias_impulse(const Vector3 &p_pos, const Vector3 &p_j) { biased_linear_velocity += p_j * _inv_mass; - biased_angular_velocity += _inv_inertia_tensor.xform( (p_pos-center_of_mass).cross(p_j) ); + biased_angular_velocity += _inv_inertia_tensor.xform((p_pos - center_of_mass).cross(p_j)); } - _FORCE_INLINE_ void apply_bias_torque_impulse(const Vector3& p_j) { + _FORCE_INLINE_ void apply_bias_torque_impulse(const Vector3 &p_j) { biased_angular_velocity += _inv_inertia_tensor.xform(p_j); } - _FORCE_INLINE_ void add_force(const Vector3& p_force, const Vector3& p_pos) { + _FORCE_INLINE_ void add_force(const Vector3 &p_force, const Vector3 &p_pos) { applied_force += p_force; applied_torque += p_pos.cross(p_force); @@ -243,7 +242,7 @@ public: _FORCE_INLINE_ bool is_active() const { return active; } _FORCE_INLINE_ void wakeup() { - if ((!get_space()) || mode==PhysicsServer::BODY_MODE_STATIC || mode==PhysicsServer::BODY_MODE_KINEMATIC) + if ((!get_space()) || mode == PhysicsServer::BODY_MODE_STATIC || mode == PhysicsServer::BODY_MODE_KINEMATIC) return; set_active(true); } @@ -254,16 +253,16 @@ public: void set_mode(PhysicsServer::BodyMode p_mode); PhysicsServer::BodyMode get_mode() const; - void set_state(PhysicsServer::BodyState p_state, const Variant& p_variant); + void set_state(PhysicsServer::BodyState p_state, const Variant &p_variant); Variant get_state(PhysicsServer::BodyState p_state) const; - void set_applied_force(const Vector3& p_force) { applied_force=p_force; } + void set_applied_force(const Vector3 &p_force) { applied_force = p_force; } Vector3 get_applied_force() const { return applied_force; } - void set_applied_torque(const Vector3& p_torque) { applied_torque=p_torque; } + void set_applied_torque(const Vector3 &p_torque) { applied_torque = p_torque; } Vector3 get_applied_torque() const { return applied_torque; } - _FORCE_INLINE_ void set_continuous_collision_detection(bool p_enable) { continuous_cd=p_enable; } + _FORCE_INLINE_ void set_continuous_collision_detection(bool p_enable) { continuous_cd = p_enable; } _FORCE_INLINE_ bool is_continuous_collision_detection_enabled() const { return continuous_cd; } void set_space(SpaceSW *p_space); @@ -277,33 +276,32 @@ public: _FORCE_INLINE_ Vector3 get_gravity() const { return gravity; } _FORCE_INLINE_ real_t get_bounce() const { return bounce; } - _FORCE_INLINE_ void set_axis_lock(PhysicsServer::BodyAxisLock p_lock) { axis_lock=p_lock; } + _FORCE_INLINE_ void set_axis_lock(PhysicsServer::BodyAxisLock p_lock) { axis_lock = p_lock; } _FORCE_INLINE_ PhysicsServer::BodyAxisLock get_axis_lock() const { return axis_lock; } void integrate_forces(real_t p_step); void integrate_velocities(real_t p_step); - _FORCE_INLINE_ Vector3 get_velocity_in_local_point(const Vector3& rel_pos) const { + _FORCE_INLINE_ Vector3 get_velocity_in_local_point(const Vector3 &rel_pos) const { - return linear_velocity + angular_velocity.cross(rel_pos-center_of_mass); + return linear_velocity + angular_velocity.cross(rel_pos - center_of_mass); } - _FORCE_INLINE_ real_t compute_impulse_denominator(const Vector3& p_pos, const Vector3& p_normal) const { + _FORCE_INLINE_ real_t compute_impulse_denominator(const Vector3 &p_pos, const Vector3 &p_normal) const { - Vector3 r0 = p_pos - get_transform().origin - center_of_mass; + Vector3 r0 = p_pos - get_transform().origin - center_of_mass; - Vector3 c0 = (r0).cross(p_normal); + Vector3 c0 = (r0).cross(p_normal); - Vector3 vec = (_inv_inertia_tensor.xform_inv(c0)).cross(r0); + Vector3 vec = (_inv_inertia_tensor.xform_inv(c0)).cross(r0); - return _inv_mass + p_normal.dot(vec); - - } + return _inv_mass + p_normal.dot(vec); + } - _FORCE_INLINE_ real_t compute_angular_impulse_denominator(const Vector3& p_axis) const { + _FORCE_INLINE_ real_t compute_angular_impulse_denominator(const Vector3 &p_axis) const { - return p_axis.dot( _inv_inertia_tensor.xform_inv(p_axis) ); - } + return p_axis.dot(_inv_inertia_tensor.xform_inv(p_axis)); + } //void simulate_motion(const Transform& p_xform,real_t p_step); void call_queries(); @@ -313,117 +311,133 @@ public: BodySW(); ~BodySW(); - }; - //add contact inline -void BodySW::add_contact(const Vector3& p_local_pos,const Vector3& p_local_normal, real_t p_depth, int p_local_shape, const Vector3& p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID& p_collider,const Vector3& p_collider_velocity_at_pos) { +void BodySW::add_contact(const Vector3 &p_local_pos, const Vector3 &p_local_normal, real_t p_depth, int p_local_shape, const Vector3 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector3 &p_collider_velocity_at_pos) { - int c_max=contacts.size(); + 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 PhysicsDirectBodyStateSW : public PhysicsDirectBodyState { - GDCLASS( PhysicsDirectBodyStateSW, PhysicsDirectBodyState ); + GDCLASS(PhysicsDirectBodyStateSW, PhysicsDirectBodyState); public: - static PhysicsDirectBodyStateSW *singleton; BodySW *body; real_t step; - virtual Vector3 get_total_gravity() const { return body->gravity; } // get gravity vector working on this body space/area - virtual real_t get_total_angular_damp() const { return body->area_angular_damp; } // get density of this body space/area - virtual real_t get_total_linear_damp() const { return body->area_linear_damp; } // get density of this body space/area + virtual Vector3 get_total_gravity() const { return body->gravity; } // get gravity vector working on this body space/area + virtual real_t get_total_angular_damp() const { return body->area_angular_damp; } // get density of this body space/area + virtual real_t get_total_linear_damp() const { return body->area_linear_damp; } // get density of this body space/area virtual Vector3 get_center_of_mass() const { return body->get_center_of_mass(); } virtual Basis get_principal_inertia_axes() const { return body->get_principal_inertia_axes(); } - virtual real_t get_inverse_mass() const { return body->get_inv_mass(); } // get the mass - virtual Vector3 get_inverse_inertia() const { return body->get_inv_inertia(); } // get density of this body space - virtual Basis get_inverse_inertia_tensor() const { return body->get_inv_inertia_tensor(); } // get density of this body space + virtual real_t get_inverse_mass() const { return body->get_inv_mass(); } // get the mass + virtual Vector3 get_inverse_inertia() const { return body->get_inv_inertia(); } // get density of this body space + virtual Basis get_inverse_inertia_tensor() const { return body->get_inv_inertia_tensor(); } // get density of this body space - virtual void set_linear_velocity(const Vector3& p_velocity) { body->set_linear_velocity(p_velocity); } - virtual Vector3 get_linear_velocity() const { return body->get_linear_velocity(); } + virtual void set_linear_velocity(const Vector3 &p_velocity) { body->set_linear_velocity(p_velocity); } + virtual Vector3 get_linear_velocity() const { return body->get_linear_velocity(); } - virtual void set_angular_velocity(const Vector3& p_velocity) { body->set_angular_velocity(p_velocity); } - virtual Vector3 get_angular_velocity() const { return body->get_angular_velocity(); } + virtual void set_angular_velocity(const Vector3 &p_velocity) { body->set_angular_velocity(p_velocity); } + virtual Vector3 get_angular_velocity() const { return body->get_angular_velocity(); } - virtual void set_transform(const Transform& p_transform) { body->set_state(PhysicsServer::BODY_STATE_TRANSFORM,p_transform); } - virtual Transform get_transform() const { return body->get_transform(); } + virtual void set_transform(const Transform &p_transform) { body->set_state(PhysicsServer::BODY_STATE_TRANSFORM, p_transform); } + virtual Transform get_transform() const { return body->get_transform(); } - virtual void add_force(const Vector3& p_force, const Vector3& p_pos) { body->add_force(p_force,p_pos); } - virtual void apply_impulse(const Vector3& p_pos, const Vector3& p_j) { body->apply_impulse(p_pos,p_j); } - virtual void apply_torque_impulse(const Vector3& p_j) { body->apply_torque_impulse(p_j); } + virtual void add_force(const Vector3 &p_force, const Vector3 &p_pos) { body->add_force(p_force, p_pos); } + virtual void apply_impulse(const Vector3 &p_pos, const Vector3 &p_j) { body->apply_impulse(p_pos, p_j); } + virtual void apply_torque_impulse(const Vector3 &p_j) { body->apply_torque_impulse(p_j); } - virtual void set_sleep_state(bool p_enable) { body->set_active(!p_enable); } - virtual bool is_sleeping() const { return !body->is_active(); } + virtual 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 Vector3 get_contact_local_pos(int p_contact_idx) const { - ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector3()); + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3()); return body->contacts[p_contact_idx].local_pos; } - virtual Vector3 get_contact_local_normal(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector3()); return body->contacts[p_contact_idx].local_normal; } - virtual int get_contact_local_shape(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,-1); return body->contacts[p_contact_idx].local_shape; } - - virtual RID get_contact_collider(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,RID()); return body->contacts[p_contact_idx].collider; } - virtual Vector3 get_contact_collider_pos(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector3()); return body->contacts[p_contact_idx].collider_pos; } - virtual ObjectID get_contact_collider_id(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,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 Vector3 get_contact_collider_velocity_at_pos(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector3()); return body->contacts[p_contact_idx].collider_velocity_at_pos; } + virtual Vector3 get_contact_local_normal(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3()); + return body->contacts[p_contact_idx].local_normal; + } + virtual 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 PhysicsDirectSpaceState* get_space_state(); + virtual RID get_contact_collider(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, RID()); + return body->contacts[p_contact_idx].collider; + } + virtual Vector3 get_contact_collider_pos(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3()); + return body->contacts[p_contact_idx].collider_pos; + } + virtual ObjectID get_contact_collider_id(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, 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 Vector3 get_contact_collider_velocity_at_pos(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3()); + return body->contacts[p_contact_idx].collider_velocity_at_pos; + } + virtual PhysicsDirectSpaceState *get_space_state(); virtual real_t get_step() const { return step; } - PhysicsDirectBodyStateSW() { singleton=this; body=NULL; } + PhysicsDirectBodyStateSW() { + singleton = this; + body = NULL; + } }; - #endif // BODY__SW_H diff --git a/servers/physics/broad_phase_basic.cpp b/servers/physics/broad_phase_basic.cpp index f1c22caae..ca9bb4084 100644 --- a/servers/physics/broad_phase_basic.cpp +++ b/servers/physics/broad_phase_basic.cpp @@ -31,117 +31,111 @@ #include "print_string.h" BroadPhaseSW::ID BroadPhaseBasic::create(CollisionObjectSW *p_object_, int p_subindex) { - if (p_object_==NULL) { + if (p_object_ == NULL) { - ERR_FAIL_COND_V(p_object_==NULL,0); + ERR_FAIL_COND_V(p_object_ == NULL, 0); } 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 BroadPhaseBasic::move(ID p_id, const Rect3& p_aabb) { +void BroadPhaseBasic::move(ID p_id, const Rect3 &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 BroadPhaseBasic::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 BroadPhaseBasic::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); List<PairKey> to_erase; //unpair must be done immediately on removal to avoid potential invalid pointers - for (Map<PairKey,void*>::Element *F=pair_map.front();F;F=F->next()) { + for (Map<PairKey, void *>::Element *F = pair_map.front(); F; F = F->next()) { - if (F->key().a==p_id || F->key().b==p_id) { + if (F->key().a == p_id || F->key().b == p_id) { if (unpair_callback) { - Element *elem_A=&element_map[F->key().a]; - Element *elem_B=&element_map[F->key().b]; - unpair_callback(elem_A->owner,elem_A->subindex,elem_B->owner,elem_B->subindex,F->get(),unpair_userdata); + Element *elem_A = &element_map[F->key().a]; + Element *elem_B = &element_map[F->key().b]; + unpair_callback(elem_A->owner, elem_A->subindex, elem_B->owner, elem_B->subindex, F->get(), unpair_userdata); } to_erase.push_back(F->key()); } } - while(to_erase.size()) { + while (to_erase.size()) { pair_map.erase(to_erase.front()->get()); to_erase.pop_front(); } element_map.erase(E); - } CollisionObjectSW *BroadPhaseBasic::get_object(ID p_id) const { - const Map<ID,Element>::Element *E=element_map.find(p_id); - ERR_FAIL_COND_V(!E,NULL); + const Map<ID, Element>::Element *E = element_map.find(p_id); + ERR_FAIL_COND_V(!E, NULL); return E->get().owner; - } bool BroadPhaseBasic::is_static(ID p_id) const { - const Map<ID,Element>::Element *E=element_map.find(p_id); - ERR_FAIL_COND_V(!E,false); + const Map<ID, Element>::Element *E = element_map.find(p_id); + ERR_FAIL_COND_V(!E, false); return E->get()._static; - } int BroadPhaseBasic::get_subindex(ID p_id) const { - const Map<ID,Element>::Element *E=element_map.find(p_id); - ERR_FAIL_COND_V(!E,-1); + const Map<ID, Element>::Element *E = element_map.find(p_id); + ERR_FAIL_COND_V(!E, -1); return E->get().subindex; } -int BroadPhaseBasic::cull_segment(const Vector3& p_from, const Vector3& p_to,CollisionObjectSW** p_results,int p_max_results,int *p_result_indices) { +int BroadPhaseBasic::cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices) { - int rc=0; + 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 Rect3 aabb=E->get().aabb; - if (aabb.intersects_segment(p_from,p_to)) { + const Rect3 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 BroadPhaseBasic::cull_aabb(const Rect3& p_aabb,CollisionObjectSW** p_results,int p_max_results,int *p_result_indices) { +int BroadPhaseBasic::cull_aabb(const Rect3 &p_aabb, CollisionObjectSW **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 Rect3 aabb=E->get().aabb; + const Rect3 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; } } @@ -149,68 +143,63 @@ int BroadPhaseBasic::cull_aabb(const Rect3& p_aabb,CollisionObjectSW** p_results return rc; } -void BroadPhaseBasic::set_pair_callback(PairCallback p_pair_callback,void *p_userdata) { - - pair_userdata=p_userdata; - pair_callback=p_pair_callback; +void BroadPhaseBasic::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) { + pair_userdata = p_userdata; + pair_callback = p_pair_callback; } -void BroadPhaseBasic::set_unpair_callback(UnpairCallback p_pair_callback,void *p_userdata) { - - unpair_userdata=p_userdata; - unpair_callback=p_pair_callback; +void BroadPhaseBasic::set_unpair_callback(UnpairCallback p_pair_callback, void *p_userdata) { + unpair_userdata = p_userdata; + unpair_callback = p_pair_callback; } void BroadPhaseBasic::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); } } } - } BroadPhaseSW *BroadPhaseBasic::_create() { - return memnew( BroadPhaseBasic ); + return memnew(BroadPhaseBasic); } BroadPhaseBasic::BroadPhaseBasic() { - 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/broad_phase_basic.h b/servers/physics/broad_phase_basic.h index 9f07e896c..2824af6b6 100644 --- a/servers/physics/broad_phase_basic.h +++ b/servers/physics/broad_phase_basic.h @@ -42,8 +42,7 @@ class BroadPhaseBasic : public BroadPhaseSW { int subindex; }; - - Map<ID,Element> element_map; + Map<ID, Element> element_map; ID current; @@ -57,17 +56,23 @@ class BroadPhaseBasic : public BroadPhaseSW { 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; @@ -75,10 +80,9 @@ class BroadPhaseBasic : public BroadPhaseSW { void *unpair_userdata; public: - // 0 is an invalid ID - virtual ID create(CollisionObjectSW *p_object_, int p_subindex=0); - virtual void move(ID p_id, const Rect3& p_aabb); + virtual ID create(CollisionObjectSW *p_object_, int p_subindex = 0); + virtual void move(ID p_id, const Rect3 &p_aabb); virtual void set_static(ID p_id, bool p_static); virtual void remove(ID p_id); @@ -86,11 +90,11 @@ public: virtual bool is_static(ID p_id) const; virtual int get_subindex(ID p_id) const; - virtual int cull_segment(const Vector3& p_from, const Vector3& p_to,CollisionObjectSW** p_results,int p_max_results,int *p_result_indices=NULL); - virtual int cull_aabb(const Rect3& p_aabb,CollisionObjectSW** p_results,int p_max_results,int *p_result_indices=NULL); + virtual int cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL); + virtual int cull_aabb(const Rect3 &p_aabb, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL); - virtual void set_pair_callback(PairCallback p_pair_callback,void *p_userdata); - virtual void set_unpair_callback(UnpairCallback p_unpair_callback,void *p_userdata); + virtual void 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/broad_phase_octree.cpp b/servers/physics/broad_phase_octree.cpp index 89581997a..cb64077d9 100644 --- a/servers/physics/broad_phase_octree.cpp +++ b/servers/physics/broad_phase_octree.cpp @@ -31,85 +31,77 @@ BroadPhaseSW::ID BroadPhaseOctree::create(CollisionObjectSW *p_object, int p_subindex) { - ID oid = octree.create(p_object,Rect3(),p_subindex,false,1<<p_object->get_type(),0); + ID oid = octree.create(p_object, Rect3(), p_subindex, false, 1 << p_object->get_type(), 0); return oid; } -void BroadPhaseOctree::move(ID p_id, const Rect3& p_aabb){ +void BroadPhaseOctree::move(ID p_id, const Rect3 &p_aabb) { - octree.move(p_id,p_aabb); + octree.move(p_id, p_aabb); } -void BroadPhaseOctree::set_static(ID p_id, bool p_static){ +void BroadPhaseOctree::set_static(ID p_id, bool p_static) { CollisionObjectSW *it = octree.get(p_id); - octree.set_pairable(p_id,p_static?false:true,1<<it->get_type(),p_static?0:0xFFFFF); //pair everything, don't care 1? - + octree.set_pairable(p_id, p_static ? false : true, 1 << it->get_type(), p_static ? 0 : 0xFFFFF); //pair everything, don't care 1? } -void BroadPhaseOctree::remove(ID p_id){ +void BroadPhaseOctree::remove(ID p_id) { octree.erase(p_id); } -CollisionObjectSW *BroadPhaseOctree::get_object(ID p_id) const{ +CollisionObjectSW *BroadPhaseOctree::get_object(ID p_id) const { CollisionObjectSW *it = octree.get(p_id); - ERR_FAIL_COND_V(!it,NULL); + ERR_FAIL_COND_V(!it, NULL); return it; } -bool BroadPhaseOctree::is_static(ID p_id) const{ +bool BroadPhaseOctree::is_static(ID p_id) const { return !octree.is_pairable(p_id); } -int BroadPhaseOctree::get_subindex(ID p_id) const{ +int BroadPhaseOctree::get_subindex(ID p_id) const { return octree.get_subindex(p_id); } -int BroadPhaseOctree::cull_segment(const Vector3& p_from, const Vector3& p_to,CollisionObjectSW** p_results,int p_max_results,int *p_result_indices){ +int BroadPhaseOctree::cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices) { - return octree.cull_segment(p_from,p_to,p_results,p_max_results,p_result_indices); + return octree.cull_segment(p_from, p_to, p_results, p_max_results, p_result_indices); } -int BroadPhaseOctree::cull_aabb(const Rect3& p_aabb,CollisionObjectSW** p_results,int p_max_results,int *p_result_indices) { - - return octree.cull_AABB(p_aabb,p_results,p_max_results,p_result_indices); +int BroadPhaseOctree::cull_aabb(const Rect3 &p_aabb, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices) { + return octree.cull_AABB(p_aabb, p_results, p_max_results, p_result_indices); } +void *BroadPhaseOctree::_pair_callback(void *self, OctreeElementID p_A, CollisionObjectSW *p_object_A, int subindex_A, OctreeElementID p_B, CollisionObjectSW *p_object_B, int subindex_B) { -void* BroadPhaseOctree::_pair_callback(void*self,OctreeElementID p_A, CollisionObjectSW*p_object_A,int subindex_A,OctreeElementID p_B, CollisionObjectSW*p_object_B,int subindex_B) { - - BroadPhaseOctree *bpo=(BroadPhaseOctree*)(self); + BroadPhaseOctree *bpo = (BroadPhaseOctree *)(self); if (!bpo->pair_callback) return NULL; - return bpo->pair_callback(p_object_A,subindex_A,p_object_B,subindex_B,bpo->pair_userdata); - + return bpo->pair_callback(p_object_A, subindex_A, p_object_B, subindex_B, bpo->pair_userdata); } -void BroadPhaseOctree::_unpair_callback(void*self,OctreeElementID p_A, CollisionObjectSW*p_object_A,int subindex_A,OctreeElementID p_B, CollisionObjectSW*p_object_B,int subindex_B,void*pairdata) { +void BroadPhaseOctree::_unpair_callback(void *self, OctreeElementID p_A, CollisionObjectSW *p_object_A, int subindex_A, OctreeElementID p_B, CollisionObjectSW *p_object_B, int subindex_B, void *pairdata) { - BroadPhaseOctree *bpo=(BroadPhaseOctree*)(self); + BroadPhaseOctree *bpo = (BroadPhaseOctree *)(self); if (!bpo->unpair_callback) return; - bpo->unpair_callback(p_object_A,subindex_A,p_object_B,subindex_B,pairdata,bpo->unpair_userdata); - + bpo->unpair_callback(p_object_A, subindex_A, p_object_B, subindex_B, pairdata, bpo->unpair_userdata); } +void BroadPhaseOctree::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) { -void BroadPhaseOctree::set_pair_callback(PairCallback p_pair_callback,void *p_userdata){ - - pair_callback=p_pair_callback; - pair_userdata=p_userdata; - + pair_callback = p_pair_callback; + pair_userdata = p_userdata; } -void BroadPhaseOctree::set_unpair_callback(UnpairCallback p_unpair_callback,void *p_userdata){ - - unpair_callback=p_unpair_callback; - unpair_userdata=p_userdata; +void BroadPhaseOctree::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) { + unpair_callback = p_unpair_callback; + unpair_userdata = p_userdata; } void BroadPhaseOctree::update() { @@ -118,16 +110,14 @@ void BroadPhaseOctree::update() { BroadPhaseSW *BroadPhaseOctree::_create() { - return memnew( BroadPhaseOctree ); + return memnew(BroadPhaseOctree); } BroadPhaseOctree::BroadPhaseOctree() { - octree.set_pair_callback(_pair_callback,this); - octree.set_unpair_callback(_unpair_callback,this); - pair_callback=NULL; - pair_userdata=NULL; - pair_callback=NULL; - unpair_userdata=NULL; + octree.set_pair_callback(_pair_callback, this); + octree.set_unpair_callback(_unpair_callback, this); + pair_callback = NULL; + pair_userdata = NULL; + pair_callback = NULL; + unpair_userdata = NULL; } - - diff --git a/servers/physics/broad_phase_octree.h b/servers/physics/broad_phase_octree.h index 29f1123ed..f9a8bd17e 100644 --- a/servers/physics/broad_phase_octree.h +++ b/servers/physics/broad_phase_octree.h @@ -34,12 +34,10 @@ class BroadPhaseOctree : public BroadPhaseSW { + Octree<CollisionObjectSW, true> octree; - Octree<CollisionObjectSW,true> octree; - - static void* _pair_callback(void*,OctreeElementID, CollisionObjectSW*,int,OctreeElementID, CollisionObjectSW*,int); - static void _unpair_callback(void*,OctreeElementID, CollisionObjectSW*,int,OctreeElementID, CollisionObjectSW*,int,void*); - + static void *_pair_callback(void *, OctreeElementID, CollisionObjectSW *, int, OctreeElementID, CollisionObjectSW *, int); + static void _unpair_callback(void *, OctreeElementID, CollisionObjectSW *, int, OctreeElementID, CollisionObjectSW *, int, void *); PairCallback pair_callback; void *pair_userdata; @@ -47,10 +45,9 @@ class BroadPhaseOctree : public BroadPhaseSW { void *unpair_userdata; public: - // 0 is an invalid ID - virtual ID create(CollisionObjectSW *p_object_, int p_subindex=0); - virtual void move(ID p_id, const Rect3& p_aabb); + virtual ID create(CollisionObjectSW *p_object_, int p_subindex = 0); + virtual void move(ID p_id, const Rect3 &p_aabb); virtual void set_static(ID p_id, bool p_static); virtual void remove(ID p_id); @@ -58,11 +55,11 @@ public: virtual bool is_static(ID p_id) const; virtual int get_subindex(ID p_id) const; - virtual int cull_segment(const Vector3& p_from, const Vector3& p_to,CollisionObjectSW** p_results,int p_max_results,int *p_result_indices=NULL); - virtual int cull_aabb(const Rect3& p_aabb,CollisionObjectSW** p_results,int p_max_results,int *p_result_indices=NULL); + virtual int cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL); + virtual int cull_aabb(const Rect3 &p_aabb, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL); - virtual void set_pair_callback(PairCallback p_pair_callback,void *p_userdata); - virtual void set_unpair_callback(UnpairCallback p_unpair_callback,void *p_userdata); + virtual void 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/broad_phase_sw.cpp b/servers/physics/broad_phase_sw.cpp index 2a897dea2..f9a19e558 100644 --- a/servers/physics/broad_phase_sw.cpp +++ b/servers/physics/broad_phase_sw.cpp @@ -28,8 +28,7 @@ /*************************************************************************/ #include "broad_phase_sw.h" -BroadPhaseSW::CreateFunction BroadPhaseSW::create_func=NULL; +BroadPhaseSW::CreateFunction BroadPhaseSW::create_func = NULL; -BroadPhaseSW::~BroadPhaseSW() -{ +BroadPhaseSW::~BroadPhaseSW() { } diff --git a/servers/physics/broad_phase_sw.h b/servers/physics/broad_phase_sw.h index 20b3efc7f..df6ea1cc7 100644 --- a/servers/physics/broad_phase_sw.h +++ b/servers/physics/broad_phase_sw.h @@ -34,40 +34,37 @@ class CollisionObjectSW; - class BroadPhaseSW { public: - typedef BroadPhaseSW* (*CreateFunction)(); + typedef BroadPhaseSW *(*CreateFunction)(); static CreateFunction create_func; typedef uint32_t ID; - - typedef void* (*PairCallback)(CollisionObjectSW *A,int p_subindex_A,CollisionObjectSW *B,int p_subindex_B,void *p_userdata); - typedef void (*UnpairCallback)(CollisionObjectSW *A,int p_subindex_A,CollisionObjectSW *B,int p_subindex_B,void *p_data,void *p_userdata); + typedef void *(*PairCallback)(CollisionObjectSW *A, int p_subindex_A, CollisionObjectSW *B, int p_subindex_B, void *p_userdata); + typedef void (*UnpairCallback)(CollisionObjectSW *A, int p_subindex_A, CollisionObjectSW *B, int p_subindex_B, void *p_data, void *p_userdata); // 0 is an invalid ID - virtual ID create(CollisionObjectSW *p_object_, int p_subindex=0)=0; - virtual void move(ID p_id, const Rect3& p_aabb)=0; - virtual void set_static(ID p_id, bool p_static)=0; - virtual void remove(ID p_id)=0; + virtual ID create(CollisionObjectSW *p_object_, int p_subindex = 0) = 0; + virtual void move(ID p_id, const Rect3 &p_aabb) = 0; + virtual void set_static(ID p_id, bool p_static) = 0; + virtual void remove(ID p_id) = 0; - virtual CollisionObjectSW *get_object(ID p_id) const=0; - virtual bool is_static(ID p_id) const=0; - virtual int get_subindex(ID p_id) const=0; + virtual CollisionObjectSW *get_object(ID p_id) const = 0; + virtual bool is_static(ID p_id) const = 0; + virtual int get_subindex(ID p_id) const = 0; - virtual int cull_segment(const Vector3& p_from, const Vector3& p_to,CollisionObjectSW** p_results,int p_max_results,int *p_result_indices=NULL)=0; - virtual int cull_aabb(const Rect3& p_aabb,CollisionObjectSW** p_results,int p_max_results,int *p_result_indices=NULL)=0; + virtual int cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL) = 0; + virtual int cull_aabb(const Rect3 &p_aabb, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL) = 0; - virtual void set_pair_callback(PairCallback p_pair_callback,void *p_userdata)=0; - virtual void set_unpair_callback(UnpairCallback p_unpair_callback,void *p_userdata)=0; + virtual void 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 ~BroadPhaseSW(); - }; #endif // BROAD_PHASE__SW_H diff --git a/servers/physics/collision_object_sw.cpp b/servers/physics/collision_object_sw.cpp index 88aa73757..36704b6eb 100644 --- a/servers/physics/collision_object_sw.cpp +++ b/servers/physics/collision_object_sw.cpp @@ -29,37 +29,35 @@ #include "collision_object_sw.h" #include "space_sw.h" -void CollisionObjectSW::add_shape(ShapeSW *p_shape,const Transform& p_transform) { +void CollisionObjectSW::add_shape(ShapeSW *p_shape, const Transform &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.shape = p_shape; + s.xform = p_transform; + s.xform_inv = s.xform.affine_inverse(); + s.bpid = 0; //needs update shapes.push_back(s); p_shape->add_owner(this); _update_shapes(); _shapes_changed(); - } -void CollisionObjectSW::set_shape(int p_index,ShapeSW *p_shape){ +void CollisionObjectSW::set_shape(int p_index, ShapeSW *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 CollisionObjectSW::set_shape_transform(int p_index,const Transform& p_transform){ +void CollisionObjectSW::set_shape_transform(int p_index, const Transform &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(); } @@ -67,61 +65,58 @@ void CollisionObjectSW::set_shape_transform(int p_index,const Transform& p_trans void CollisionObjectSW::remove_shape(ShapeSW *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 CollisionObjectSW::remove_shape(int p_index){ +void CollisionObjectSW::remove_shape(int p_index) { //remove anything from shape to be erased to end, so subindices don't change - ERR_FAIL_INDEX(p_index,shapes.size()); - for(int i=p_index;i<shapes.size();i++) { + 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 CollisionObjectSW::_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 CollisionObjectSW::_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 CollisionObjectSW::_update_shapes() { @@ -129,54 +124,50 @@ void CollisionObjectSW::_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.. - Rect3 shape_aabb=s.shape->get_aabb(); + Rect3 shape_aabb = s.shape->get_aabb(); Transform xform = transform * s.xform; - shape_aabb=xform.xform(shape_aabb); - s.aabb_cache=shape_aabb; - s.aabb_cache=s.aabb_cache.grow( (s.aabb_cache.size.x + s.aabb_cache.size.y)*0.5*0.05 ); + shape_aabb = xform.xform(shape_aabb); + s.aabb_cache = shape_aabb; + s.aabb_cache = s.aabb_cache.grow((s.aabb_cache.size.x + s.aabb_cache.size.y) * 0.5 * 0.05); Vector3 scale = xform.get_basis().get_scale(); - s.area_cache=s.shape->get_area()*scale.x*scale.y*scale.z; + s.area_cache = s.shape->get_area() * scale.x * scale.y * scale.z; - space->get_broadphase()->move(s.bpid,s.aabb_cache); + space->get_broadphase()->move(s.bpid, s.aabb_cache); } - } -void CollisionObjectSW::_update_shapes_with_motion(const Vector3& p_motion) { - +void CollisionObjectSW::_update_shapes_with_motion(const Vector3 &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.. - Rect3 shape_aabb=s.shape->get_aabb(); + Rect3 shape_aabb = s.shape->get_aabb(); Transform xform = transform * s.xform; - shape_aabb=xform.xform(shape_aabb); - shape_aabb=shape_aabb.merge(Rect3( 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(Rect3(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 CollisionObjectSW::_set_space(SpaceSW *p_space) { @@ -185,25 +176,23 @@ void CollisionObjectSW::_set_space(SpaceSW *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 CollisionObjectSW::_shape_changed() { @@ -214,11 +203,11 @@ void CollisionObjectSW::_shape_changed() { CollisionObjectSW::CollisionObjectSW(Type p_type) { - _static=true; - type=p_type; - space=NULL; - instance_id=0; - layer_mask=1; - collision_mask=1; - ray_pickable=true; + _static = true; + type = p_type; + space = NULL; + instance_id = 0; + layer_mask = 1; + collision_mask = 1; + ray_pickable = true; } diff --git a/servers/physics/collision_object_sw.h b/servers/physics/collision_object_sw.h index c46a7f4a5..9a7626d58 100644 --- a/servers/physics/collision_object_sw.h +++ b/servers/physics/collision_object_sw.h @@ -29,14 +29,14 @@ #ifndef COLLISION_OBJECT_SW_H #define COLLISION_OBJECT_SW_H -#include "shape_sw.h" -#include "servers/physics_server.h" -#include "self_list.h" #include "broad_phase_sw.h" +#include "self_list.h" +#include "servers/physics_server.h" +#include "shape_sw.h" #ifdef DEBUG_ENABLED #define MAX_OBJECT_DISTANCE 10000000.0 -#define MAX_OBJECT_DISTANCE_X2 (MAX_OBJECT_DISTANCE*MAX_OBJECT_DISTANCE) +#define MAX_OBJECT_DISTANCE_X2 (MAX_OBJECT_DISTANCE * MAX_OBJECT_DISTANCE) #endif class SpaceSW; @@ -47,8 +47,8 @@ public: TYPE_AREA, TYPE_BODY }; -private: +private: Type type; RID self; ObjectID instance_id; @@ -65,7 +65,7 @@ private: ShapeSW *shape; bool trigger; - Shape() { trigger=false; } + Shape() { trigger = false; } }; Vector<Shape> shapes; @@ -77,84 +77,80 @@ private: void _update_shapes(); protected: - - - void _update_shapes_with_motion(const Vector3& p_motion); + void _update_shapes_with_motion(const Vector3 &p_motion); void _unregister_shapes(); - _FORCE_INLINE_ void _set_transform(const Transform& p_transform,bool p_update_shapes=true) { + _FORCE_INLINE_ void _set_transform(const Transform &p_transform, bool p_update_shapes = true) { #ifdef DEBUG_ENABLED if (p_transform.origin.length_squared() > MAX_OBJECT_DISTANCE_X2) { - ERR_EXPLAIN("Object went too far away (more than "+itos(MAX_OBJECT_DISTANCE)+"mts from origin)."); + ERR_EXPLAIN("Object went too far away (more than " + itos(MAX_OBJECT_DISTANCE) + "mts from origin)."); ERR_FAIL(); } #endif - transform=p_transform; if (p_update_shapes) _update_shapes(); - + transform = p_transform; + if (p_update_shapes) _update_shapes(); } - _FORCE_INLINE_ void _set_inv_transform(const Transform& p_transform) { inv_transform=p_transform; } + _FORCE_INLINE_ void _set_inv_transform(const Transform &p_transform) { inv_transform = p_transform; } void _set_static(bool p_static); - virtual void _shapes_changed()=0; + virtual void _shapes_changed() = 0; void _set_space(SpaceSW *space); bool ray_pickable; - CollisionObjectSW(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(ShapeSW *p_shape,const Transform& p_transform=Transform()); - void set_shape(int p_index,ShapeSW *p_shape); - void set_shape_transform(int p_index,const Transform& p_transform); + void add_shape(ShapeSW *p_shape, const Transform &p_transform = Transform()); + void set_shape(int p_index, ShapeSW *p_shape); + void set_shape_transform(int p_index, const Transform &p_transform); _FORCE_INLINE_ int get_shape_count() const { return shapes.size(); } _FORCE_INLINE_ ShapeSW *get_shape(int p_index) const { return shapes[p_index].shape; } - _FORCE_INLINE_ const Transform& get_shape_transform(int p_index) const { return shapes[p_index].xform; } - _FORCE_INLINE_ const Transform& get_shape_inv_transform(int p_index) const { return shapes[p_index].xform_inv; } - _FORCE_INLINE_ const Rect3& get_shape_aabb(int p_index) const { return shapes[p_index].aabb_cache; } + _FORCE_INLINE_ const Transform &get_shape_transform(int p_index) const { return shapes[p_index].xform; } + _FORCE_INLINE_ const Transform &get_shape_inv_transform(int p_index) const { return shapes[p_index].xform_inv; } + _FORCE_INLINE_ const Rect3 &get_shape_aabb(int p_index) const { return shapes[p_index].aabb_cache; } _FORCE_INLINE_ const real_t get_shape_area(int p_index) const { return shapes[p_index].area_cache; } _FORCE_INLINE_ Transform get_transform() const { return transform; } _FORCE_INLINE_ Transform get_inv_transform() const { return inv_transform; } - _FORCE_INLINE_ SpaceSW* get_space() const { return space; } + _FORCE_INLINE_ SpaceSW *get_space() const { return space; } - _FORCE_INLINE_ void set_ray_pickable(bool p_enable) { ray_pickable=p_enable; } + _FORCE_INLINE_ void set_ray_pickable(bool p_enable) { ray_pickable = p_enable; } _FORCE_INLINE_ bool is_ray_pickable() const { return ray_pickable; } - _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; } - _FORCE_INLINE_ void set_layer_mask(uint32_t p_mask) { layer_mask=p_mask; } + _FORCE_INLINE_ void set_layer_mask(uint32_t p_mask) { layer_mask = p_mask; } _FORCE_INLINE_ uint32_t get_layer_mask() const { return layer_mask; } - _FORCE_INLINE_ void set_collision_mask(uint32_t p_mask) { collision_mask=p_mask; } + _FORCE_INLINE_ void set_collision_mask(uint32_t p_mask) { collision_mask = p_mask; } _FORCE_INLINE_ uint32_t get_collision_mask() const { return collision_mask; } - _FORCE_INLINE_ bool test_collision_mask(CollisionObjectSW* p_other) const { - return layer_mask&p_other->collision_mask || p_other->layer_mask&collision_mask; + _FORCE_INLINE_ bool test_collision_mask(CollisionObjectSW *p_other) const { + return layer_mask & p_other->collision_mask || p_other->layer_mask & collision_mask; } void remove_shape(ShapeSW *p_shape); void remove_shape(int p_index); - virtual void set_space(SpaceSW *p_space)=0; + virtual void set_space(SpaceSW *p_space) = 0; - _FORCE_INLINE_ bool is_static() const { return _static; } + _FORCE_INLINE_ bool is_static() const { return _static; } virtual ~CollisionObjectSW() {} - }; #endif // COLLISION_OBJECT_SW_H diff --git a/servers/physics/collision_solver_sat.cpp b/servers/physics/collision_solver_sat.cpp index 9d3e1db47..003e6b325 100644 --- a/servers/physics/collision_solver_sat.cpp +++ b/servers/physics/collision_solver_sat.cpp @@ -40,7 +40,7 @@ struct _CollectorCallback { Vector3 normal; Vector3 *prev_axis; - _FORCE_INLINE_ void call(const Vector3& p_point_A, const Vector3& p_point_B) { + _FORCE_INLINE_ void call(const Vector3 &p_point_A, const Vector3 &p_point_B) { /* if (normal.dot(p_point_A) >= normal.dot(p_point_B)) @@ -49,69 +49,61 @@ struct _CollectorCallback { */ 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 Vector3 *,int, const Vector3 *,int ,_CollectorCallback *); - +typedef void (*GenerateContactsFunc)(const Vector3 *, int, const Vector3 *, int, _CollectorCallback *); -static void _generate_contacts_point_point(const Vector3 * p_points_A,int p_point_count_A, const Vector3 * p_points_B,int p_point_count_B,_CollectorCallback *p_callback) { +static void _generate_contacts_point_point(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { #ifdef DEBUG_ENABLED - ERR_FAIL_COND( p_point_count_A != 1 ); - ERR_FAIL_COND( p_point_count_B != 1 ); + ERR_FAIL_COND(p_point_count_A != 1); + ERR_FAIL_COND(p_point_count_B != 1); #endif - p_callback->call(*p_points_A,*p_points_B); + p_callback->call(*p_points_A, *p_points_B); } -static void _generate_contacts_point_edge(const Vector3 * p_points_A,int p_point_count_A, const Vector3 * p_points_B,int p_point_count_B,_CollectorCallback *p_callback) { +static void _generate_contacts_point_edge(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { #ifdef DEBUG_ENABLED - ERR_FAIL_COND( p_point_count_A != 1 ); - ERR_FAIL_COND( p_point_count_B != 2 ); + ERR_FAIL_COND(p_point_count_A != 1); + ERR_FAIL_COND(p_point_count_B != 2); #endif - Vector3 closest_B = Geometry::get_closest_point_to_segment_uncapped(*p_points_A, p_points_B ); - p_callback->call(*p_points_A,closest_B); - + Vector3 closest_B = Geometry::get_closest_point_to_segment_uncapped(*p_points_A, p_points_B); + p_callback->call(*p_points_A, closest_B); } -static void _generate_contacts_point_face(const Vector3 * p_points_A,int p_point_count_A, const Vector3 * p_points_B,int p_point_count_B,_CollectorCallback *p_callback) { +static void _generate_contacts_point_face(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { #ifdef DEBUG_ENABLED - ERR_FAIL_COND( p_point_count_A != 1 ); - ERR_FAIL_COND( p_point_count_B < 3 ); + ERR_FAIL_COND(p_point_count_A != 1); + ERR_FAIL_COND(p_point_count_B < 3); #endif + Vector3 closest_B = Plane(p_points_B[0], p_points_B[1], p_points_B[2]).project(*p_points_A); - Vector3 closest_B=Plane(p_points_B[0],p_points_B[1],p_points_B[2]).project( *p_points_A ); - - p_callback->call(*p_points_A,closest_B); - + p_callback->call(*p_points_A, closest_B); } - -static void _generate_contacts_edge_edge(const Vector3 * p_points_A,int p_point_count_A, const Vector3 * p_points_B,int p_point_count_B,_CollectorCallback *p_callback) { +static void _generate_contacts_edge_edge(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { #ifdef DEBUG_ENABLED - ERR_FAIL_COND( p_point_count_A != 2 ); - ERR_FAIL_COND( p_point_count_B != 2 ); // circle is actually a 4x3 matrix + ERR_FAIL_COND(p_point_count_A != 2); + ERR_FAIL_COND(p_point_count_B != 2); // circle is actually a 4x3 matrix #endif + Vector3 rel_A = p_points_A[1] - p_points_A[0]; + Vector3 rel_B = p_points_B[1] - p_points_B[0]; - - Vector3 rel_A=p_points_A[1]-p_points_A[0]; - Vector3 rel_B=p_points_B[1]-p_points_B[0]; - - Vector3 c=rel_A.cross(rel_B).cross(rel_B); + Vector3 c = rel_A.cross(rel_B).cross(rel_B); //if ( Math::abs(rel_A.dot(c) )<_EDGE_IS_VALID_SUPPORT_TRESHOLD ) { - if ( Math::abs(rel_A.dot(c) )<CMP_EPSILON ) { + if (Math::abs(rel_A.dot(c)) < CMP_EPSILON) { // should handle somehow.. //ERR_PRINT("TODO FIX"); @@ -122,117 +114,110 @@ static void _generate_contacts_edge_edge(const Vector3 * p_points_A,int p_point_ Vector3 base_B = p_points_B[0] - axis * axis.dot(p_points_B[0]); //sort all 4 points in axis - real_t dvec[4]={ axis.dot(p_points_A[0]), axis.dot(p_points_A[1]), axis.dot(p_points_B[0]), axis.dot(p_points_B[1]) }; + real_t dvec[4] = { axis.dot(p_points_A[0]), axis.dot(p_points_A[1]), axis.dot(p_points_B[0]), axis.dot(p_points_B[1]) }; SortArray<real_t> sa; - sa.sort(dvec,4); + sa.sort(dvec, 4); //use the middle ones as contacts - p_callback->call(base_A+axis*dvec[1],base_B+axis*dvec[1]); - p_callback->call(base_A+axis*dvec[2],base_B+axis*dvec[2]); + p_callback->call(base_A + axis * dvec[1], base_B + axis * dvec[1]); + p_callback->call(base_A + axis * dvec[2], base_B + axis * dvec[2]); return; - } - real_t d = (c.dot( p_points_B[0] ) - p_points_A[0].dot(c))/rel_A.dot(c); + real_t d = (c.dot(p_points_B[0]) - p_points_A[0].dot(c)) / rel_A.dot(c); - if (d<0.0) - d=0.0; - else if (d>1.0) - d=1.0; - - Vector3 closest_A=p_points_A[0]+rel_A*d; - Vector3 closest_B=Geometry::get_closest_point_to_segment_uncapped(closest_A, p_points_B); - p_callback->call(closest_A,closest_B); + if (d < 0.0) + d = 0.0; + else if (d > 1.0) + d = 1.0; + Vector3 closest_A = p_points_A[0] + rel_A * d; + Vector3 closest_B = Geometry::get_closest_point_to_segment_uncapped(closest_A, p_points_B); + p_callback->call(closest_A, closest_B); } -static void _generate_contacts_face_face(const Vector3 * p_points_A,int p_point_count_A, const Vector3 * p_points_B,int p_point_count_B,_CollectorCallback *p_callback) { +static void _generate_contacts_face_face(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { #ifdef DEBUG_ENABLED - ERR_FAIL_COND( p_point_count_A <2 ); - ERR_FAIL_COND( p_point_count_B <3 ); + ERR_FAIL_COND(p_point_count_A < 2); + ERR_FAIL_COND(p_point_count_B < 3); #endif - static const int max_clip=32; + static const int max_clip = 32; Vector3 _clipbuf1[max_clip]; Vector3 _clipbuf2[max_clip]; - Vector3 *clipbuf_src=_clipbuf1; - Vector3 *clipbuf_dst=_clipbuf2; - int clipbuf_len=p_point_count_A; + Vector3 *clipbuf_src = _clipbuf1; + Vector3 *clipbuf_dst = _clipbuf2; + int clipbuf_len = p_point_count_A; // copy A points to clipbuf_src - for (int i=0;i<p_point_count_A;i++) { + for (int i = 0; i < p_point_count_A; i++) { - clipbuf_src[i]=p_points_A[i]; + clipbuf_src[i] = p_points_A[i]; } - Plane plane_B(p_points_B[0],p_points_B[1],p_points_B[2]); + Plane plane_B(p_points_B[0], p_points_B[1], p_points_B[2]); // go through all of B points - for (int i=0;i<p_point_count_B;i++) { + for (int i = 0; i < p_point_count_B; i++) { - int i_n=(i+1)%p_point_count_B; + int i_n = (i + 1) % p_point_count_B; - Vector3 edge0_B=p_points_B[i]; - Vector3 edge1_B=p_points_B[i_n]; + Vector3 edge0_B = p_points_B[i]; + Vector3 edge1_B = p_points_B[i_n]; - Vector3 clip_normal = (edge0_B - edge1_B).cross( plane_B.normal ).normalized(); + Vector3 clip_normal = (edge0_B - edge1_B).cross(plane_B.normal).normalized(); // make a clip plane - - Plane clip(edge0_B,clip_normal); + Plane clip(edge0_B, clip_normal); // avoid double clip if A is edge - int dst_idx=0; - bool edge = clipbuf_len==2; - for (int j=0;j<clipbuf_len;j++) { + int dst_idx = 0; + bool edge = clipbuf_len == 2; + for (int j = 0; j < clipbuf_len; j++) { - int j_n=(j+1)%clipbuf_len; + int j_n = (j + 1) % clipbuf_len; - Vector3 edge0_A=clipbuf_src[j]; - Vector3 edge1_A=clipbuf_src[j_n]; + Vector3 edge0_A = clipbuf_src[j]; + Vector3 edge1_A = clipbuf_src[j_n]; real_t dist0 = clip.distance_to(edge0_A); real_t dist1 = clip.distance_to(edge1_A); + if (dist0 <= 0) { // behind plane - if ( dist0 <= 0 ) { // behind plane - - ERR_FAIL_COND( dst_idx >= max_clip ); - clipbuf_dst[dst_idx++]=clipbuf_src[j]; - + ERR_FAIL_COND(dst_idx >= max_clip); + clipbuf_dst[dst_idx++] = clipbuf_src[j]; } - // check for different sides and non coplanar //if ( (dist0*dist1) < -CMP_EPSILON && !(edge && j)) { - if ( (dist0*dist1) < 0 && !(edge && j)) { + if ((dist0 * dist1) < 0 && !(edge && j)) { // calculate intersection Vector3 rel = edge1_A - edge0_A; - real_t den=clip.normal.dot( rel ); - real_t dist=-(clip.normal.dot( edge0_A )-clip.d)/den; - Vector3 inters = edge0_A+rel*dist; + real_t den = clip.normal.dot(rel); + real_t dist = -(clip.normal.dot(edge0_A) - clip.d) / den; + Vector3 inters = edge0_A + rel * dist; - ERR_FAIL_COND( dst_idx >= max_clip ); - clipbuf_dst[dst_idx]=inters; + ERR_FAIL_COND(dst_idx >= max_clip); + clipbuf_dst[dst_idx] = inters; dst_idx++; } } - clipbuf_len=dst_idx; - SWAP(clipbuf_src,clipbuf_dst); + clipbuf_len = dst_idx; + SWAP(clipbuf_src, clipbuf_dst); } - // generate contacts //Plane plane_A(p_points_A[0],p_points_A[1],p_points_A[2]); - int added=0; + int added = 0; - for (int i=0;i<clipbuf_len;i++) { + for (int i = 0; i < clipbuf_len; i++) { real_t d = plane_B.distance_to(clipbuf_src[i]); /* @@ -240,40 +225,37 @@ static void _generate_contacts_face_face(const Vector3 * p_points_A,int p_point_ continue; */ - Vector3 closest_B=clipbuf_src[i] - plane_B.normal*d; + Vector3 closest_B = clipbuf_src[i] - plane_B.normal * d; if (p_callback->normal.dot(clipbuf_src[i]) >= p_callback->normal.dot(closest_B)) continue; - p_callback->call(clipbuf_src[i],closest_B); + p_callback->call(clipbuf_src[i], closest_B); added++; - } - } - -static void _generate_contacts_from_supports(const Vector3 * p_points_A,int p_point_count_A, const Vector3 * p_points_B,int p_point_count_B,_CollectorCallback *p_callback) { - +static void _generate_contacts_from_supports(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { #ifdef DEBUG_ENABLED - ERR_FAIL_COND( p_point_count_A <1 ); - ERR_FAIL_COND( p_point_count_B <1 ); + ERR_FAIL_COND(p_point_count_A < 1); + ERR_FAIL_COND(p_point_count_B < 1); #endif - - static const GenerateContactsFunc generate_contacts_func_table[3][3]={ + static const GenerateContactsFunc generate_contacts_func_table[3][3] = { + { + _generate_contacts_point_point, + _generate_contacts_point_edge, + _generate_contacts_point_face, + }, { - _generate_contacts_point_point, - _generate_contacts_point_edge, - _generate_contacts_point_face, - },{ - 0, - _generate_contacts_edge_edge, - _generate_contacts_face_face, - },{ - 0,0, - _generate_contacts_face_face, + 0, + _generate_contacts_edge_edge, + _generate_contacts_face_face, + }, + { + 0, 0, + _generate_contacts_face_face, } }; @@ -289,28 +271,25 @@ static void _generate_contacts_from_supports(const Vector3 * 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_callback); - + contacts_func(points_A, pointcount_A, points_B, pointcount_B, p_callback); } - - -template<class ShapeA, class ShapeB, bool withMargin=false> +template <class ShapeA, class ShapeB, bool withMargin = false> class SeparatorAxisTest { const ShapeA *shape_A; @@ -325,46 +304,45 @@ class SeparatorAxisTest { Vector3 separator_axis; public: - _FORCE_INLINE_ bool test_previous_axis() { - if (callback && callback->prev_axis && *callback->prev_axis!=Vector3()) + if (callback && callback->prev_axis && *callback->prev_axis != Vector3()) return test_axis(*callback->prev_axis); else return true; } - _FORCE_INLINE_ bool test_axis(const Vector3& p_axis) { + _FORCE_INLINE_ bool test_axis(const Vector3 &p_axis) { - Vector3 axis=p_axis; + Vector3 axis = p_axis; - if ( Math::abs(axis.x)<CMP_EPSILON && - Math::abs(axis.y)<CMP_EPSILON && - Math::abs(axis.z)<CMP_EPSILON ) { + if (Math::abs(axis.x) < CMP_EPSILON && + Math::abs(axis.y) < CMP_EPSILON && + Math::abs(axis.z) < CMP_EPSILON) { // strange case, try an upwards separator - axis=Vector3(0.0,1.0,0.0); + axis = Vector3(0.0, 1.0, 0.0); } - real_t min_A,max_A,min_B,max_B; + real_t min_A, max_A, min_B, max_B; - shape_A->project_range(axis,*transform_A,min_A,max_A); - shape_B->project_range(axis,*transform_B,min_B,max_B); + shape_A->project_range(axis, *transform_A, min_A, max_A); + shape_B->project_range(axis, *transform_B, min_B, max_B); if (withMargin) { - min_A-=margin_A; - max_A+=margin_A; - min_B-=margin_B; - max_B+=margin_B; + min_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) { - separator_axis=axis; + separator_axis = axis; return false; // doesn't contain 0 } @@ -372,68 +350,65 @@ 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; } } 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 } } return true; } - _FORCE_INLINE_ void generate_contacts() { // nothing to do, don't generate - if (best_axis==Vector3(0.0,0.0,0.0)) + if (best_axis == Vector3(0.0, 0.0, 0.0)) return; if (!callback->callback) { //just was checking intersection? - callback->collided=true; + callback->collided = true; if (callback->prev_axis) - *callback->prev_axis=best_axis; + *callback->prev_axis = best_axis; return; } - static const int max_supports=16; + static const int max_supports = 16; Vector3 supports_A[max_supports]; int support_count_A; - shape_A->get_supports(transform_A->basis.xform_inv(-best_axis).normalized(),max_supports,supports_A,support_count_A); - for(int i=0;i<support_count_A;i++) { + shape_A->get_supports(transform_A->basis.xform_inv(-best_axis).normalized(), max_supports, supports_A, support_count_A); + for (int i = 0; i < support_count_A; i++) { supports_A[i] = transform_A->xform(supports_A[i]); } if (withMargin) { - for(int i=0;i<support_count_A;i++) { - supports_A[i]+=-best_axis*margin_A; + for (int i = 0; i < support_count_A; i++) { + supports_A[i] += -best_axis * margin_A; } - } - Vector3 supports_B[max_supports]; int support_count_B; - shape_B->get_supports(transform_B->basis.xform_inv(best_axis).normalized(),max_supports,supports_B,support_count_B); - for(int i=0;i<support_count_B;i++) { + shape_B->get_supports(transform_B->basis.xform_inv(best_axis).normalized(), max_supports, supports_B, support_count_B); + for (int i = 0; i < support_count_B; i++) { supports_B[i] = transform_B->xform(supports_B[i]); } if (withMargin) { - for(int i=0;i<support_count_B;i++) { - supports_B[i]+=best_axis*margin_B; + for (int i = 0; i < support_count_B; i++) { + supports_B[i] += best_axis * margin_B; } } -/* + /* print_line("best depth: "+rtos(best_depth)); print_line("best axis: "+(best_axis)); for(int i=0;i<support_count_A;i++) { @@ -445,29 +420,26 @@ public: print_line("B-"+itos(i)+": "+supports_B[i]); } */ - callback->normal=best_axis; + callback->normal = best_axis; if (callback->prev_axis) - *callback->prev_axis=best_axis; - _generate_contacts_from_supports(supports_A,support_count_A,supports_B,support_count_B,callback); + *callback->prev_axis = best_axis; + _generate_contacts_from_supports(supports_A, support_count_A, supports_B, support_count_B, callback); - callback->collided=true; + callback->collided = true; //CollisionSolverSW::CallbackResult cbk=NULL; //cbk(Vector3(),Vector3(),NULL); - } - _FORCE_INLINE_ SeparatorAxisTest(const ShapeA *p_shape_A,const Transform& p_transform_A, const ShapeB *p_shape_B,const Transform& p_transform_B,_CollectorCallback *p_callback,real_t p_margin_A=0,real_t p_margin_B=0) { - best_depth=1e15; - shape_A=p_shape_A; - shape_B=p_shape_B; - transform_A=&p_transform_A; - transform_B=&p_transform_B; - callback=p_callback; - margin_A=p_margin_A; - margin_B=p_margin_B; - + _FORCE_INLINE_ SeparatorAxisTest(const ShapeA *p_shape_A, const Transform &p_transform_A, const ShapeB *p_shape_B, const Transform &p_transform_B, _CollectorCallback *p_callback, real_t p_margin_A = 0, real_t p_margin_B = 0) { + best_depth = 1e15; + shape_A = p_shape_A; + shape_B = p_shape_B; + transform_A = &p_transform_A; + transform_B = &p_transform_B; + callback = p_callback; + margin_A = p_margin_A; + margin_B = p_margin_B; } - }; /****** SAT TESTS *******/ @@ -475,92 +447,84 @@ public: /****** SAT TESTS *******/ /****** SAT TESTS *******/ +typedef void (*CollisionFunc)(const ShapeSW *, const Transform &, const ShapeSW *, const Transform &, _CollectorCallback *p_callback, real_t, real_t); -typedef void (*CollisionFunc)(const ShapeSW*,const Transform&,const ShapeSW*,const Transform&,_CollectorCallback *p_callback,real_t,real_t); - - -template<bool withMargin> -static void _collision_sphere_sphere(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector,real_t p_margin_a,real_t p_margin_b) { - +template <bool withMargin> +static void _collision_sphere_sphere(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const SphereShapeSW *sphere_A = static_cast<const SphereShapeSW*>(p_a); - const SphereShapeSW *sphere_B = static_cast<const SphereShapeSW*>(p_b); + const SphereShapeSW *sphere_A = static_cast<const SphereShapeSW *>(p_a); + const SphereShapeSW *sphere_B = static_cast<const SphereShapeSW *>(p_b); - SeparatorAxisTest<SphereShapeSW,SphereShapeSW,withMargin> separator(sphere_A,p_transform_a,sphere_B,p_transform_b,p_collector,p_margin_a,p_margin_b); + SeparatorAxisTest<SphereShapeSW, SphereShapeSW, withMargin> separator(sphere_A, p_transform_a, sphere_B, p_transform_b, p_collector, p_margin_a, p_margin_b); // previous axis if (!separator.test_previous_axis()) return; - if (!separator.test_axis( (p_transform_a.origin-p_transform_b.origin).normalized() )) + if (!separator.test_axis((p_transform_a.origin - p_transform_b.origin).normalized())) return; separator.generate_contacts(); } -template<bool withMargin> -static void _collision_sphere_box(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector,real_t p_margin_a,real_t p_margin_b) { +template <bool withMargin> +static void _collision_sphere_box(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const SphereShapeSW *sphere_A = static_cast<const SphereShapeSW *>(p_a); + const BoxShapeSW *box_B = static_cast<const BoxShapeSW *>(p_b); - const SphereShapeSW *sphere_A = static_cast<const SphereShapeSW*>(p_a); - const BoxShapeSW *box_B = static_cast<const BoxShapeSW*>(p_b); - - SeparatorAxisTest<SphereShapeSW,BoxShapeSW,withMargin> separator(sphere_A,p_transform_a,box_B,p_transform_b,p_collector,p_margin_a,p_margin_b); + SeparatorAxisTest<SphereShapeSW, BoxShapeSW, withMargin> separator(sphere_A, p_transform_a, box_B, p_transform_b, p_collector, p_margin_a, p_margin_b); if (!separator.test_previous_axis()) return; // test faces - for (int i=0;i<3;i++) { + for (int i = 0; i < 3; i++) { Vector3 axis = p_transform_b.basis.get_axis(i).normalized(); - if (!separator.test_axis( axis )) + if (!separator.test_axis(axis)) return; - } // calculate closest point to sphere - Vector3 cnormal=p_transform_b.xform_inv( p_transform_a.origin ); + Vector3 cnormal = p_transform_b.xform_inv(p_transform_a.origin); - Vector3 cpoint=p_transform_b.xform( Vector3( + Vector3 cpoint = p_transform_b.xform(Vector3( - (cnormal.x<0) ? -box_B->get_half_extents().x : box_B->get_half_extents().x, - (cnormal.y<0) ? -box_B->get_half_extents().y : box_B->get_half_extents().y, - (cnormal.z<0) ? -box_B->get_half_extents().z : box_B->get_half_extents().z - ) ); + (cnormal.x < 0) ? -box_B->get_half_extents().x : box_B->get_half_extents().x, + (cnormal.y < 0) ? -box_B->get_half_extents().y : box_B->get_half_extents().y, + (cnormal.z < 0) ? -box_B->get_half_extents().z : box_B->get_half_extents().z)); // use point to test axis Vector3 point_axis = (p_transform_a.origin - cpoint).normalized(); - if (!separator.test_axis( point_axis )) + if (!separator.test_axis(point_axis)) return; // test edges - for (int i=0;i<3;i++) { + for (int i = 0; i < 3; i++) { - Vector3 axis = point_axis.cross( p_transform_b.basis.get_axis(i) ).cross( p_transform_b.basis.get_axis(i) ).normalized(); + Vector3 axis = point_axis.cross(p_transform_b.basis.get_axis(i)).cross(p_transform_b.basis.get_axis(i)).normalized(); - if (!separator.test_axis( axis )) + if (!separator.test_axis(axis)) return; } separator.generate_contacts(); - - } -template<bool withMargin> -static void _collision_sphere_capsule(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector,real_t p_margin_a,real_t p_margin_b) { +template <bool withMargin> +static void _collision_sphere_capsule(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const SphereShapeSW *sphere_A = static_cast<const SphereShapeSW*>(p_a); - const CapsuleShapeSW *capsule_B = static_cast<const CapsuleShapeSW*>(p_b); + const SphereShapeSW *sphere_A = static_cast<const SphereShapeSW *>(p_a); + const CapsuleShapeSW *capsule_B = static_cast<const CapsuleShapeSW *>(p_b); - SeparatorAxisTest<SphereShapeSW,CapsuleShapeSW,withMargin> separator(sphere_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_margin_a,p_margin_b); + SeparatorAxisTest<SphereShapeSW, CapsuleShapeSW, withMargin> separator(sphere_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_margin_a, p_margin_b); if (!separator.test_previous_axis()) return; @@ -571,38 +535,35 @@ static void _collision_sphere_capsule(const ShapeSW *p_a,const Transform &p_tran Vector3 capsule_ball_1 = p_transform_b.origin + capsule_axis; - if (!separator.test_axis( (capsule_ball_1 - p_transform_a.origin).normalized() ) ) + if (!separator.test_axis((capsule_ball_1 - p_transform_a.origin).normalized())) return; //capsule sphere 2, sphere Vector3 capsule_ball_2 = p_transform_b.origin - capsule_axis; - if (!separator.test_axis( (capsule_ball_2 - p_transform_a.origin).normalized() ) ) + if (!separator.test_axis((capsule_ball_2 - p_transform_a.origin).normalized())) return; //capsule edge, sphere Vector3 b2a = p_transform_a.origin - p_transform_b.origin; - Vector3 axis = b2a.cross( capsule_axis ).cross( capsule_axis ).normalized(); + Vector3 axis = b2a.cross(capsule_axis).cross(capsule_axis).normalized(); - - if (!separator.test_axis( axis )) + if (!separator.test_axis(axis)) return; separator.generate_contacts(); } -template<bool withMargin> -static void _collision_sphere_convex_polygon(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector,real_t p_margin_a,real_t p_margin_b) { - +template <bool withMargin> +static void _collision_sphere_convex_polygon(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const SphereShapeSW *sphere_A = static_cast<const SphereShapeSW*>(p_a); - const ConvexPolygonShapeSW *convex_polygon_B = static_cast<const ConvexPolygonShapeSW*>(p_b); - - SeparatorAxisTest<SphereShapeSW,ConvexPolygonShapeSW,withMargin> separator(sphere_A,p_transform_a,convex_polygon_B,p_transform_b,p_collector,p_margin_a,p_margin_b); + const SphereShapeSW *sphere_A = static_cast<const SphereShapeSW *>(p_a); + const ConvexPolygonShapeSW *convex_polygon_B = static_cast<const ConvexPolygonShapeSW *>(p_b); + SeparatorAxisTest<SphereShapeSW, ConvexPolygonShapeSW, withMargin> separator(sphere_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b); if (!separator.test_previous_axis()) return; @@ -616,146 +577,127 @@ static void _collision_sphere_convex_polygon(const ShapeSW *p_a,const Transform const Vector3 *vertices = mesh.vertices.ptr(); int vertex_count = mesh.vertices.size(); - // faces of B - for (int i=0;i<face_count;i++) { + for (int i = 0; i < face_count; i++) { - Vector3 axis = p_transform_b.xform( faces[i].plane ).normal; + Vector3 axis = p_transform_b.xform(faces[i].plane).normal; - if (!separator.test_axis( axis )) + if (!separator.test_axis(axis)) return; } - // edges of B - for(int i=0;i<edge_count;i++) { - + for (int i = 0; i < edge_count; i++) { - Vector3 v1=p_transform_b.xform( vertices[ edges[i].a ] ); - Vector3 v2=p_transform_b.xform( vertices[ edges[i].b ] ); - Vector3 v3=p_transform_a.origin; + Vector3 v1 = p_transform_b.xform(vertices[edges[i].a]); + Vector3 v2 = p_transform_b.xform(vertices[edges[i].b]); + Vector3 v3 = p_transform_a.origin; - - Vector3 n1=v2-v1; - Vector3 n2=v2-v3; + Vector3 n1 = v2 - v1; + Vector3 n2 = v2 - v3; Vector3 axis = n1.cross(n2).cross(n1).normalized(); - if (!separator.test_axis( axis )) + if (!separator.test_axis(axis)) return; - } // vertices of B - for(int i=0;i<vertex_count;i++) { + for (int i = 0; i < vertex_count; i++) { + Vector3 v1 = p_transform_b.xform(vertices[i]); + Vector3 v2 = p_transform_a.origin; - Vector3 v1=p_transform_b.xform( vertices[i] ); - Vector3 v2=p_transform_a.origin; + Vector3 axis = (v2 - v1).normalized(); - Vector3 axis = (v2-v1).normalized(); - - if (!separator.test_axis( axis )) + if (!separator.test_axis(axis)) return; - } separator.generate_contacts(); - - } -template<bool withMargin> -static void _collision_sphere_face(const ShapeSW *p_a,const Transform &p_transform_a, const ShapeSW *p_b,const Transform& p_transform_b,_CollectorCallback *p_collector,real_t p_margin_a,real_t p_margin_b) { +template <bool withMargin> +static void _collision_sphere_face(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const SphereShapeSW *sphere_A = static_cast<const SphereShapeSW*>(p_a); - const FaceShapeSW *face_B = static_cast<const FaceShapeSW*>(p_b); + const SphereShapeSW *sphere_A = static_cast<const SphereShapeSW *>(p_a); + const FaceShapeSW *face_B = static_cast<const FaceShapeSW *>(p_b); + SeparatorAxisTest<SphereShapeSW, FaceShapeSW, withMargin> separator(sphere_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b); - - SeparatorAxisTest<SphereShapeSW,FaceShapeSW,withMargin> separator(sphere_A,p_transform_a,face_B,p_transform_b,p_collector,p_margin_a,p_margin_b); - - - Vector3 vertex[3]={ - p_transform_b.xform( face_B->vertex[0] ), - p_transform_b.xform( face_B->vertex[1] ), - p_transform_b.xform( face_B->vertex[2] ), + Vector3 vertex[3] = { + p_transform_b.xform(face_B->vertex[0]), + p_transform_b.xform(face_B->vertex[1]), + p_transform_b.xform(face_B->vertex[2]), }; - if (!separator.test_axis( (vertex[0]-vertex[2]).cross(vertex[0]-vertex[1]).normalized() )) + if (!separator.test_axis((vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized())) return; // edges and points of B - for(int i=0;i<3;i++) { - + for (int i = 0; i < 3; i++) { - Vector3 n1=vertex[i]-p_transform_a.origin; + Vector3 n1 = vertex[i] - p_transform_a.origin; - if (!separator.test_axis( n1.normalized() )) { + if (!separator.test_axis(n1.normalized())) { return; } - Vector3 n2=vertex[(i+1)%3]-vertex[i]; + Vector3 n2 = vertex[(i + 1) % 3] - vertex[i]; Vector3 axis = n1.cross(n2).cross(n2).normalized(); - if (!separator.test_axis( axis )) { + if (!separator.test_axis(axis)) { return; } - } separator.generate_contacts(); } +template <bool withMargin> +static void _collision_box_box(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { -template<bool withMargin> -static void _collision_box_box(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector,real_t p_margin_a,real_t p_margin_b) { - + const BoxShapeSW *box_A = static_cast<const BoxShapeSW *>(p_a); + const BoxShapeSW *box_B = static_cast<const BoxShapeSW *>(p_b); - const BoxShapeSW *box_A = static_cast<const BoxShapeSW*>(p_a); - const BoxShapeSW *box_B = static_cast<const BoxShapeSW*>(p_b); - - SeparatorAxisTest<BoxShapeSW,BoxShapeSW,withMargin> separator(box_A,p_transform_a,box_B,p_transform_b,p_collector,p_margin_a,p_margin_b); + SeparatorAxisTest<BoxShapeSW, BoxShapeSW, withMargin> separator(box_A, p_transform_a, box_B, p_transform_b, p_collector, p_margin_a, p_margin_b); if (!separator.test_previous_axis()) return; // test faces of A - for (int i=0;i<3;i++) { + for (int i = 0; i < 3; i++) { Vector3 axis = p_transform_a.basis.get_axis(i).normalized(); - if (!separator.test_axis( axis )) + if (!separator.test_axis(axis)) return; - } // test faces of B - for (int i=0;i<3;i++) { + for (int i = 0; i < 3; i++) { Vector3 axis = p_transform_b.basis.get_axis(i).normalized(); - if (!separator.test_axis( axis )) + if (!separator.test_axis(axis)) return; - } // test combined edges - for (int i=0;i<3;i++) { + for (int i = 0; i < 3; i++) { - for (int j=0;j<3;j++) { + for (int j = 0; j < 3; j++) { - Vector3 axis = p_transform_a.basis.get_axis(i).cross( p_transform_b.basis.get_axis(j) ); + Vector3 axis = p_transform_a.basis.get_axis(i).cross(p_transform_b.basis.get_axis(j)); - if (axis.length_squared()<CMP_EPSILON) + if (axis.length_squared() < CMP_EPSILON) continue; axis.normalize(); - - if (!separator.test_axis( axis )) { + if (!separator.test_axis(axis)) { return; } } @@ -768,110 +710,103 @@ static void _collision_box_box(const ShapeSW *p_a,const Transform &p_transform_a Vector3 ab_vec = p_transform_b.origin - p_transform_a.origin; - Vector3 cnormal_a=p_transform_a.basis.xform_inv( ab_vec ); + Vector3 cnormal_a = p_transform_a.basis.xform_inv(ab_vec); - Vector3 support_a=p_transform_a.xform( Vector3( + Vector3 support_a = p_transform_a.xform(Vector3( - (cnormal_a.x<0) ? -box_A->get_half_extents().x : box_A->get_half_extents().x, - (cnormal_a.y<0) ? -box_A->get_half_extents().y : box_A->get_half_extents().y, - (cnormal_a.z<0) ? -box_A->get_half_extents().z : box_A->get_half_extents().z - ) ); + (cnormal_a.x < 0) ? -box_A->get_half_extents().x : box_A->get_half_extents().x, + (cnormal_a.y < 0) ? -box_A->get_half_extents().y : box_A->get_half_extents().y, + (cnormal_a.z < 0) ? -box_A->get_half_extents().z : box_A->get_half_extents().z)); + Vector3 cnormal_b = p_transform_b.basis.xform_inv(-ab_vec); - Vector3 cnormal_b=p_transform_b.basis.xform_inv( -ab_vec ); + Vector3 support_b = p_transform_b.xform(Vector3( - Vector3 support_b=p_transform_b.xform( Vector3( + (cnormal_b.x < 0) ? -box_B->get_half_extents().x : box_B->get_half_extents().x, + (cnormal_b.y < 0) ? -box_B->get_half_extents().y : box_B->get_half_extents().y, + (cnormal_b.z < 0) ? -box_B->get_half_extents().z : box_B->get_half_extents().z)); - (cnormal_b.x<0) ? -box_B->get_half_extents().x : box_B->get_half_extents().x, - (cnormal_b.y<0) ? -box_B->get_half_extents().y : box_B->get_half_extents().y, - (cnormal_b.z<0) ? -box_B->get_half_extents().z : box_B->get_half_extents().z - ) ); + Vector3 axis_ab = (support_a - support_b); - Vector3 axis_ab = (support_a-support_b); - - if (!separator.test_axis( axis_ab.normalized() )) { + if (!separator.test_axis(axis_ab.normalized())) { return; } //now try edges, which become cylinders! - for(int i=0;i<3;i++) { + for (int i = 0; i < 3; i++) { //a ->b Vector3 axis_a = p_transform_a.basis.get_axis(i); - if (!separator.test_axis( axis_ab.cross(axis_a).cross(axis_a).normalized() )) + if (!separator.test_axis(axis_ab.cross(axis_a).cross(axis_a).normalized())) return; //b ->a Vector3 axis_b = p_transform_b.basis.get_axis(i); - if (!separator.test_axis( axis_ab.cross(axis_b).cross(axis_b).normalized() )) + if (!separator.test_axis(axis_ab.cross(axis_b).cross(axis_b).normalized())) return; - } } separator.generate_contacts(); - - } -template<bool withMargin> -static void _collision_box_capsule(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector,real_t p_margin_a,real_t p_margin_b) { +template <bool withMargin> +static void _collision_box_capsule(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const BoxShapeSW *box_A = static_cast<const BoxShapeSW*>(p_a); - const CapsuleShapeSW *capsule_B = static_cast<const CapsuleShapeSW*>(p_b); + const BoxShapeSW *box_A = static_cast<const BoxShapeSW *>(p_a); + const CapsuleShapeSW *capsule_B = static_cast<const CapsuleShapeSW *>(p_b); - SeparatorAxisTest<BoxShapeSW,CapsuleShapeSW,withMargin> separator(box_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_margin_a,p_margin_b); + SeparatorAxisTest<BoxShapeSW, CapsuleShapeSW, withMargin> separator(box_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_margin_a, p_margin_b); if (!separator.test_previous_axis()) return; // faces of A - for (int i=0;i<3;i++) { + for (int i = 0; i < 3; i++) { Vector3 axis = p_transform_a.basis.get_axis(i); - if (!separator.test_axis( axis )) + if (!separator.test_axis(axis)) return; } - Vector3 cyl_axis = p_transform_b.basis.get_axis(2).normalized(); // edges of A, capsule cylinder - for (int i=0;i<3;i++) { + for (int i = 0; i < 3; i++) { // cylinder Vector3 box_axis = p_transform_a.basis.get_axis(i); - Vector3 axis = box_axis.cross( cyl_axis ); + Vector3 axis = box_axis.cross(cyl_axis); if (axis.length_squared() < CMP_EPSILON) continue; - if (!separator.test_axis( axis.normalized() )) + if (!separator.test_axis(axis.normalized())) return; } // points of A, capsule cylinder // this sure could be made faster somehow.. - for (int i=0;i<2;i++) { - for (int j=0;j<2;j++) { - for (int k=0;k<2;k++) { + for (int i = 0; i < 2; i++) { + for (int j = 0; j < 2; j++) { + for (int k = 0; k < 2; k++) { Vector3 he = box_A->get_half_extents(); - he.x*=(i*2-1); - he.y*=(j*2-1); - he.z*=(k*2-1); - Vector3 point=p_transform_a.origin; - for(int l=0;l<3;l++) - point+=p_transform_a.basis.get_axis(l)*he[l]; + he.x *= (i * 2 - 1); + he.y *= (j * 2 - 1); + he.z *= (k * 2 - 1); + Vector3 point = p_transform_a.origin; + for (int l = 0; l < 3; l++) + point += p_transform_a.basis.get_axis(l) * he[l]; //Vector3 axis = (point - cyl_axis * cyl_axis.dot(point)).normalized(); - Vector3 axis = Plane(cyl_axis,0).project(point).normalized(); + Vector3 axis = Plane(cyl_axis, 0).project(point).normalized(); - if (!separator.test_axis( axis )) + if (!separator.test_axis(axis)) return; } } @@ -879,58 +814,51 @@ static void _collision_box_capsule(const ShapeSW *p_a,const Transform &p_transfo // capsule balls, edges of A - for (int i=0;i<2;i++) { - - - Vector3 capsule_axis = p_transform_b.basis.get_axis(2)*(capsule_B->get_height()*0.5); + for (int i = 0; i < 2; i++) { - Vector3 sphere_pos = p_transform_b.origin + ((i==0)?capsule_axis:-capsule_axis); + Vector3 capsule_axis = p_transform_b.basis.get_axis(2) * (capsule_B->get_height() * 0.5); + Vector3 sphere_pos = p_transform_b.origin + ((i == 0) ? capsule_axis : -capsule_axis); - Vector3 cnormal=p_transform_a.xform_inv( sphere_pos ); + Vector3 cnormal = p_transform_a.xform_inv(sphere_pos); - Vector3 cpoint=p_transform_a.xform( Vector3( + Vector3 cpoint = p_transform_a.xform(Vector3( - (cnormal.x<0) ? -box_A->get_half_extents().x : box_A->get_half_extents().x, - (cnormal.y<0) ? -box_A->get_half_extents().y : box_A->get_half_extents().y, - (cnormal.z<0) ? -box_A->get_half_extents().z : box_A->get_half_extents().z - ) ); + (cnormal.x < 0) ? -box_A->get_half_extents().x : box_A->get_half_extents().x, + (cnormal.y < 0) ? -box_A->get_half_extents().y : box_A->get_half_extents().y, + (cnormal.z < 0) ? -box_A->get_half_extents().z : box_A->get_half_extents().z)); // use point to test axis Vector3 point_axis = (sphere_pos - cpoint).normalized(); - if (!separator.test_axis( point_axis )) + if (!separator.test_axis(point_axis)) return; // test edges of A - for (int i=0;i<3;i++) { + for (int i = 0; i < 3; i++) { - Vector3 axis = point_axis.cross( p_transform_a.basis.get_axis(i) ).cross( p_transform_a.basis.get_axis(i) ).normalized(); + Vector3 axis = point_axis.cross(p_transform_a.basis.get_axis(i)).cross(p_transform_a.basis.get_axis(i)).normalized(); - if (!separator.test_axis( axis )) + if (!separator.test_axis(axis)) return; } } - separator.generate_contacts(); } -template<bool withMargin> -static void _collision_box_convex_polygon(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector,real_t p_margin_a,real_t p_margin_b) { - +template <bool withMargin> +static void _collision_box_convex_polygon(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const BoxShapeSW *box_A = static_cast<const BoxShapeSW *>(p_a); + const ConvexPolygonShapeSW *convex_polygon_B = static_cast<const ConvexPolygonShapeSW *>(p_b); - const BoxShapeSW *box_A = static_cast<const BoxShapeSW*>(p_a); - const ConvexPolygonShapeSW *convex_polygon_B = static_cast<const ConvexPolygonShapeSW*>(p_b); - - SeparatorAxisTest<BoxShapeSW,ConvexPolygonShapeSW,withMargin> separator(box_A,p_transform_a,convex_polygon_B,p_transform_b,p_collector,p_margin_a,p_margin_b); + SeparatorAxisTest<BoxShapeSW, ConvexPolygonShapeSW, withMargin> separator(box_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b); if (!separator.test_previous_axis()) return; - const Geometry::MeshData &mesh = convex_polygon_B->get_mesh(); const Geometry::MeshData::Face *faces = mesh.faces.ptr(); @@ -941,97 +869,92 @@ static void _collision_box_convex_polygon(const ShapeSW *p_a,const Transform &p_ int vertex_count = mesh.vertices.size(); // faces of A - for (int i=0;i<3;i++) { + for (int i = 0; i < 3; i++) { Vector3 axis = p_transform_a.basis.get_axis(i).normalized(); - if (!separator.test_axis( axis )) + if (!separator.test_axis(axis)) return; } // faces of B - for (int i=0;i<face_count;i++) { + for (int i = 0; i < face_count; i++) { - Vector3 axis = p_transform_b.xform( faces[i].plane ).normal; + Vector3 axis = p_transform_b.xform(faces[i].plane).normal; - if (!separator.test_axis( axis )) + if (!separator.test_axis(axis)) return; } // A<->B edges - for (int i=0;i<3;i++) { + for (int i = 0; i < 3; i++) { Vector3 e1 = p_transform_a.basis.get_axis(i); - for (int j=0;j<edge_count;j++) { + for (int j = 0; j < edge_count; j++) { - Vector3 e2=p_transform_b.basis.xform(vertices[edges[j].a]) - p_transform_b.basis.xform(vertices[edges[j].b]); + Vector3 e2 = p_transform_b.basis.xform(vertices[edges[j].a]) - p_transform_b.basis.xform(vertices[edges[j].b]); - Vector3 axis=e1.cross( e2 ).normalized(); + Vector3 axis = e1.cross(e2).normalized(); - if (!separator.test_axis( axis )) + if (!separator.test_axis(axis)) return; - } } if (withMargin) { // calculate closest points between vertices and box edges - for(int v=0;v<vertex_count;v++) { - + for (int v = 0; v < vertex_count; v++) { Vector3 vtxb = p_transform_b.xform(vertices[v]); Vector3 ab_vec = vtxb - p_transform_a.origin; - Vector3 cnormal_a=p_transform_a.basis.xform_inv( ab_vec ); + Vector3 cnormal_a = p_transform_a.basis.xform_inv(ab_vec); - Vector3 support_a=p_transform_a.xform( Vector3( + Vector3 support_a = p_transform_a.xform(Vector3( - (cnormal_a.x<0) ? -box_A->get_half_extents().x : box_A->get_half_extents().x, - (cnormal_a.y<0) ? -box_A->get_half_extents().y : box_A->get_half_extents().y, - (cnormal_a.z<0) ? -box_A->get_half_extents().z : box_A->get_half_extents().z - ) ); + (cnormal_a.x < 0) ? -box_A->get_half_extents().x : box_A->get_half_extents().x, + (cnormal_a.y < 0) ? -box_A->get_half_extents().y : box_A->get_half_extents().y, + (cnormal_a.z < 0) ? -box_A->get_half_extents().z : box_A->get_half_extents().z)); + Vector3 axis_ab = support_a - vtxb; - Vector3 axis_ab = support_a-vtxb; - - if (!separator.test_axis( axis_ab.normalized() )) { + if (!separator.test_axis(axis_ab.normalized())) { return; } //now try edges, which become cylinders! - for(int i=0;i<3;i++) { + for (int i = 0; i < 3; i++) { //a ->b Vector3 axis_a = p_transform_a.basis.get_axis(i); - if (!separator.test_axis( axis_ab.cross(axis_a).cross(axis_a).normalized() )) + if (!separator.test_axis(axis_ab.cross(axis_a).cross(axis_a).normalized())) return; } } //convex edges and box points - for (int i=0;i<2;i++) { - for (int j=0;j<2;j++) { - for (int k=0;k<2;k++) { + for (int i = 0; i < 2; i++) { + for (int j = 0; j < 2; j++) { + for (int k = 0; k < 2; k++) { Vector3 he = box_A->get_half_extents(); - he.x*=(i*2-1); - he.y*=(j*2-1); - he.z*=(k*2-1); - Vector3 point=p_transform_a.origin; - for(int l=0;l<3;l++) - point+=p_transform_a.basis.get_axis(l)*he[l]; - - for(int e=0;e<edge_count;e++) { + he.x *= (i * 2 - 1); + he.y *= (j * 2 - 1); + he.z *= (k * 2 - 1); + Vector3 point = p_transform_a.origin; + for (int l = 0; l < 3; l++) + point += p_transform_a.basis.get_axis(l) * he[l]; - Vector3 p1=p_transform_b.xform(vertices[edges[e].a]); - Vector3 p2=p_transform_b.xform(vertices[edges[e].b]); - Vector3 n = (p2-p1); + for (int e = 0; e < edge_count; e++) { + Vector3 p1 = p_transform_b.xform(vertices[edges[e].a]); + Vector3 p2 = p_transform_b.xform(vertices[edges[e].b]); + Vector3 n = (p2 - p1); - if (!separator.test_axis( (point-p2).cross(n).cross(n).normalized() )) + if (!separator.test_axis((point - p2).cross(n).cross(n).normalized())) return; } } @@ -1040,130 +963,119 @@ static void _collision_box_convex_polygon(const ShapeSW *p_a,const Transform &p_ } separator.generate_contacts(); - - } +template <bool withMargin> +static void _collision_box_face(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { -template<bool withMargin> -static void _collision_box_face(const ShapeSW *p_a,const Transform &p_transform_a, const ShapeSW *p_b,const Transform& p_transform_b,_CollectorCallback *p_collector,real_t p_margin_a,real_t p_margin_b) { - - - const BoxShapeSW *box_A = static_cast<const BoxShapeSW*>(p_a); - const FaceShapeSW *face_B = static_cast<const FaceShapeSW*>(p_b); + const BoxShapeSW *box_A = static_cast<const BoxShapeSW *>(p_a); + const FaceShapeSW *face_B = static_cast<const FaceShapeSW *>(p_b); - SeparatorAxisTest<BoxShapeSW,FaceShapeSW,withMargin> separator(box_A,p_transform_a,face_B,p_transform_b,p_collector,p_margin_a,p_margin_b); + SeparatorAxisTest<BoxShapeSW, FaceShapeSW, withMargin> separator(box_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b); - Vector3 vertex[3]={ - p_transform_b.xform( face_B->vertex[0] ), - p_transform_b.xform( face_B->vertex[1] ), - p_transform_b.xform( face_B->vertex[2] ), + Vector3 vertex[3] = { + p_transform_b.xform(face_B->vertex[0]), + p_transform_b.xform(face_B->vertex[1]), + p_transform_b.xform(face_B->vertex[2]), }; - if (!separator.test_axis( (vertex[0]-vertex[2]).cross(vertex[0]-vertex[1]).normalized() )) + if (!separator.test_axis((vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized())) return; // faces of A - for (int i=0;i<3;i++) { + for (int i = 0; i < 3; i++) { Vector3 axis = p_transform_a.basis.get_axis(i).normalized(); - if (!separator.test_axis( axis )) + if (!separator.test_axis(axis)) return; } // combined edges - for(int i=0;i<3;i++) { + for (int i = 0; i < 3; i++) { - Vector3 e=vertex[i]-vertex[(i+1)%3]; + Vector3 e = vertex[i] - vertex[(i + 1) % 3]; - for (int j=0;j<3;j++) { + for (int j = 0; j < 3; j++) { Vector3 axis = p_transform_a.basis.get_axis(j); - if (!separator.test_axis( e.cross(axis).normalized() )) + if (!separator.test_axis(e.cross(axis).normalized())) return; } - } if (withMargin) { // calculate closest points between vertices and box edges - for(int v=0;v<3;v++) { - + for (int v = 0; v < 3; v++) { Vector3 ab_vec = vertex[v] - p_transform_a.origin; - Vector3 cnormal_a=p_transform_a.basis.xform_inv( ab_vec ); + Vector3 cnormal_a = p_transform_a.basis.xform_inv(ab_vec); - Vector3 support_a=p_transform_a.xform( Vector3( + Vector3 support_a = p_transform_a.xform(Vector3( - (cnormal_a.x<0) ? -box_A->get_half_extents().x : box_A->get_half_extents().x, - (cnormal_a.y<0) ? -box_A->get_half_extents().y : box_A->get_half_extents().y, - (cnormal_a.z<0) ? -box_A->get_half_extents().z : box_A->get_half_extents().z - ) ); + (cnormal_a.x < 0) ? -box_A->get_half_extents().x : box_A->get_half_extents().x, + (cnormal_a.y < 0) ? -box_A->get_half_extents().y : box_A->get_half_extents().y, + (cnormal_a.z < 0) ? -box_A->get_half_extents().z : box_A->get_half_extents().z)); + Vector3 axis_ab = support_a - vertex[v]; - Vector3 axis_ab = support_a-vertex[v]; - - if (!separator.test_axis( axis_ab.normalized() )) { + if (!separator.test_axis(axis_ab.normalized())) { return; } //now try edges, which become cylinders! - for(int i=0;i<3;i++) { + for (int i = 0; i < 3; i++) { //a ->b Vector3 axis_a = p_transform_a.basis.get_axis(i); - if (!separator.test_axis( axis_ab.cross(axis_a).cross(axis_a).normalized() )) + if (!separator.test_axis(axis_ab.cross(axis_a).cross(axis_a).normalized())) return; } } //convex edges and box points, there has to be a way to speed up this (get closest point?) - for (int i=0;i<2;i++) { - for (int j=0;j<2;j++) { - for (int k=0;k<2;k++) { + for (int i = 0; i < 2; i++) { + for (int j = 0; j < 2; j++) { + for (int k = 0; k < 2; k++) { Vector3 he = box_A->get_half_extents(); - he.x*=(i*2-1); - he.y*=(j*2-1); - he.z*=(k*2-1); - Vector3 point=p_transform_a.origin; - for(int l=0;l<3;l++) - point+=p_transform_a.basis.get_axis(l)*he[l]; + he.x *= (i * 2 - 1); + he.y *= (j * 2 - 1); + he.z *= (k * 2 - 1); + Vector3 point = p_transform_a.origin; + for (int l = 0; l < 3; l++) + point += p_transform_a.basis.get_axis(l) * he[l]; - for(int e=0;e<3;e++) { + for (int e = 0; e < 3; e++) { - Vector3 p1=vertex[e]; - Vector3 p2=vertex[(e+1)%3]; + Vector3 p1 = vertex[e]; + Vector3 p2 = vertex[(e + 1) % 3]; - Vector3 n = (p2-p1); + Vector3 n = (p2 - p1); - if (!separator.test_axis( (point-p2).cross(n).cross(n).normalized() )) + if (!separator.test_axis((point - p2).cross(n).cross(n).normalized())) return; } } } } - } separator.generate_contacts(); - } +template <bool withMargin> +static void _collision_capsule_capsule(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { -template<bool withMargin> -static void _collision_capsule_capsule(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector,real_t p_margin_a,real_t p_margin_b) { - - const CapsuleShapeSW *capsule_A = static_cast<const CapsuleShapeSW*>(p_a); - const CapsuleShapeSW *capsule_B = static_cast<const CapsuleShapeSW*>(p_b); + const CapsuleShapeSW *capsule_A = static_cast<const CapsuleShapeSW *>(p_a); + const CapsuleShapeSW *capsule_B = static_cast<const CapsuleShapeSW *>(p_b); - SeparatorAxisTest<CapsuleShapeSW,CapsuleShapeSW,withMargin> separator(capsule_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_margin_a,p_margin_b); + SeparatorAxisTest<CapsuleShapeSW, CapsuleShapeSW, withMargin> separator(capsule_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_margin_a, p_margin_b); if (!separator.test_previous_axis()) return; @@ -1180,49 +1092,45 @@ static void _collision_capsule_capsule(const ShapeSW *p_a,const Transform &p_tra //balls-balls - if (!separator.test_axis( (capsule_A_ball_1 - capsule_B_ball_1 ).normalized() ) ) + if (!separator.test_axis((capsule_A_ball_1 - capsule_B_ball_1).normalized())) return; - if (!separator.test_axis( (capsule_A_ball_1 - capsule_B_ball_2 ).normalized() ) ) + if (!separator.test_axis((capsule_A_ball_1 - capsule_B_ball_2).normalized())) return; - if (!separator.test_axis( (capsule_A_ball_2 - capsule_B_ball_1 ).normalized() ) ) + if (!separator.test_axis((capsule_A_ball_2 - capsule_B_ball_1).normalized())) return; - if (!separator.test_axis( (capsule_A_ball_2 - capsule_B_ball_2 ).normalized() ) ) + if (!separator.test_axis((capsule_A_ball_2 - capsule_B_ball_2).normalized())) return; - // edges-balls - if (!separator.test_axis( (capsule_A_ball_1 - capsule_B_ball_1 ).cross(capsule_A_axis).cross(capsule_A_axis).normalized() ) ) + if (!separator.test_axis((capsule_A_ball_1 - capsule_B_ball_1).cross(capsule_A_axis).cross(capsule_A_axis).normalized())) return; - if (!separator.test_axis( (capsule_A_ball_1 - capsule_B_ball_2 ).cross(capsule_A_axis).cross(capsule_A_axis).normalized() ) ) + if (!separator.test_axis((capsule_A_ball_1 - capsule_B_ball_2).cross(capsule_A_axis).cross(capsule_A_axis).normalized())) return; - if (!separator.test_axis( (capsule_B_ball_1 - capsule_A_ball_1 ).cross(capsule_B_axis).cross(capsule_B_axis).normalized() ) ) + if (!separator.test_axis((capsule_B_ball_1 - capsule_A_ball_1).cross(capsule_B_axis).cross(capsule_B_axis).normalized())) return; - if (!separator.test_axis( (capsule_B_ball_1 - capsule_A_ball_2 ).cross(capsule_B_axis).cross(capsule_B_axis).normalized() ) ) + if (!separator.test_axis((capsule_B_ball_1 - capsule_A_ball_2).cross(capsule_B_axis).cross(capsule_B_axis).normalized())) return; // edges - if (!separator.test_axis( capsule_A_axis.cross(capsule_B_axis).normalized() ) ) + if (!separator.test_axis(capsule_A_axis.cross(capsule_B_axis).normalized())) return; - separator.generate_contacts(); - } -template<bool withMargin> -static void _collision_capsule_convex_polygon(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector,real_t p_margin_a,real_t p_margin_b) { - +template <bool withMargin> +static void _collision_capsule_convex_polygon(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const CapsuleShapeSW *capsule_A = static_cast<const CapsuleShapeSW*>(p_a); - const ConvexPolygonShapeSW *convex_polygon_B = static_cast<const ConvexPolygonShapeSW*>(p_b); + const CapsuleShapeSW *capsule_A = static_cast<const CapsuleShapeSW *>(p_a); + const ConvexPolygonShapeSW *convex_polygon_B = static_cast<const ConvexPolygonShapeSW *>(p_b); - SeparatorAxisTest<CapsuleShapeSW,ConvexPolygonShapeSW,withMargin> separator(capsule_A,p_transform_a,convex_polygon_B,p_transform_b,p_collector,p_margin_a,p_margin_b); + SeparatorAxisTest<CapsuleShapeSW, ConvexPolygonShapeSW, withMargin> separator(capsule_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b); if (!separator.test_previous_axis()) return; @@ -1236,127 +1144,113 @@ static void _collision_capsule_convex_polygon(const ShapeSW *p_a,const Transform const Vector3 *vertices = mesh.vertices.ptr(); // faces of B - for (int i=0;i<face_count;i++) { + for (int i = 0; i < face_count; i++) { - Vector3 axis = p_transform_b.xform( faces[i].plane ).normal; + Vector3 axis = p_transform_b.xform(faces[i].plane).normal; - if (!separator.test_axis( axis )) + if (!separator.test_axis(axis)) return; } // edges of B, capsule cylinder - for (int i=0;i<edge_count;i++) { + for (int i = 0; i < edge_count; i++) { // cylinder - Vector3 edge_axis = p_transform_b.basis.xform( vertices[ edges[i].a] ) - p_transform_b.basis.xform( vertices[ edges[i].b] ); - Vector3 axis = edge_axis.cross( p_transform_a.basis.get_axis(2) ).normalized(); + Vector3 edge_axis = p_transform_b.basis.xform(vertices[edges[i].a]) - p_transform_b.basis.xform(vertices[edges[i].b]); + Vector3 axis = edge_axis.cross(p_transform_a.basis.get_axis(2)).normalized(); - - if (!separator.test_axis( axis )) + if (!separator.test_axis(axis)) return; } // capsule balls, edges of B - for (int i=0;i<2;i++) { + for (int i = 0; i < 2; i++) { // edges of B, capsule cylinder - Vector3 capsule_axis = p_transform_a.basis.get_axis(2)*(capsule_A->get_height()*0.5); - - Vector3 sphere_pos = p_transform_a.origin + ((i==0)?capsule_axis:-capsule_axis); + Vector3 capsule_axis = p_transform_a.basis.get_axis(2) * (capsule_A->get_height() * 0.5); - for (int j=0;j<edge_count;j++) { + Vector3 sphere_pos = p_transform_a.origin + ((i == 0) ? capsule_axis : -capsule_axis); + for (int j = 0; j < edge_count; j++) { - Vector3 n1=sphere_pos - p_transform_b.xform( vertices[ edges[j].a] ); - Vector3 n2=p_transform_b.basis.xform( vertices[ edges[j].a] ) - p_transform_b.basis.xform( vertices[ edges[j].b] ); + Vector3 n1 = sphere_pos - p_transform_b.xform(vertices[edges[j].a]); + Vector3 n2 = p_transform_b.basis.xform(vertices[edges[j].a]) - p_transform_b.basis.xform(vertices[edges[j].b]); Vector3 axis = n1.cross(n2).cross(n2).normalized(); - if (!separator.test_axis( axis )) + if (!separator.test_axis(axis)) return; } } - separator.generate_contacts(); - } +template <bool withMargin> +static void _collision_capsule_face(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { -template<bool withMargin> -static void _collision_capsule_face(const ShapeSW *p_a,const Transform &p_transform_a, const ShapeSW *p_b,const Transform& p_transform_b,_CollectorCallback *p_collector,real_t p_margin_a,real_t p_margin_b) { - - const CapsuleShapeSW *capsule_A = static_cast<const CapsuleShapeSW*>(p_a); - const FaceShapeSW *face_B = static_cast<const FaceShapeSW*>(p_b); - - SeparatorAxisTest<CapsuleShapeSW,FaceShapeSW,withMargin> separator(capsule_A,p_transform_a,face_B,p_transform_b,p_collector,p_margin_a,p_margin_b); + const CapsuleShapeSW *capsule_A = static_cast<const CapsuleShapeSW *>(p_a); + const FaceShapeSW *face_B = static_cast<const FaceShapeSW *>(p_b); + SeparatorAxisTest<CapsuleShapeSW, FaceShapeSW, withMargin> separator(capsule_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b); - - Vector3 vertex[3]={ - p_transform_b.xform( face_B->vertex[0] ), - p_transform_b.xform( face_B->vertex[1] ), - p_transform_b.xform( face_B->vertex[2] ), + Vector3 vertex[3] = { + p_transform_b.xform(face_B->vertex[0]), + p_transform_b.xform(face_B->vertex[1]), + p_transform_b.xform(face_B->vertex[2]), }; - if (!separator.test_axis( (vertex[0]-vertex[2]).cross(vertex[0]-vertex[1]).normalized() )) + if (!separator.test_axis((vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized())) return; // edges of B, capsule cylinder - Vector3 capsule_axis = p_transform_a.basis.get_axis(2)*(capsule_A->get_height()*0.5); + Vector3 capsule_axis = p_transform_a.basis.get_axis(2) * (capsule_A->get_height() * 0.5); - for (int i=0;i<3;i++) { + for (int i = 0; i < 3; i++) { // edge-cylinder - Vector3 edge_axis = vertex[i]-vertex[(i+1)%3]; - Vector3 axis = edge_axis.cross( capsule_axis ).normalized(); + Vector3 edge_axis = vertex[i] - vertex[(i + 1) % 3]; + Vector3 axis = edge_axis.cross(capsule_axis).normalized(); - if (!separator.test_axis( axis )) + if (!separator.test_axis(axis)) return; - if (!separator.test_axis( (p_transform_a.origin-vertex[i]).cross(capsule_axis).cross(capsule_axis).normalized() )) + if (!separator.test_axis((p_transform_a.origin - vertex[i]).cross(capsule_axis).cross(capsule_axis).normalized())) return; - for (int j=0;j<2;j++) { + for (int j = 0; j < 2; j++) { // point-spheres - Vector3 sphere_pos = p_transform_a.origin + ( (j==0) ? capsule_axis : -capsule_axis ); + Vector3 sphere_pos = p_transform_a.origin + ((j == 0) ? capsule_axis : -capsule_axis); - Vector3 n1=sphere_pos - vertex[i]; + Vector3 n1 = sphere_pos - vertex[i]; - if (!separator.test_axis( n1.normalized() )) + if (!separator.test_axis(n1.normalized())) return; - Vector3 n2=edge_axis; + Vector3 n2 = edge_axis; axis = n1.cross(n2).cross(n2); - if (!separator.test_axis( axis.normalized() )) + if (!separator.test_axis(axis.normalized())) return; - - } - } - separator.generate_contacts(); - } +template <bool withMargin> +static void _collision_convex_polygon_convex_polygon(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { -template<bool withMargin> -static void _collision_convex_polygon_convex_polygon(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector,real_t p_margin_a,real_t p_margin_b) { - + const ConvexPolygonShapeSW *convex_polygon_A = static_cast<const ConvexPolygonShapeSW *>(p_a); + const ConvexPolygonShapeSW *convex_polygon_B = static_cast<const ConvexPolygonShapeSW *>(p_b); - const ConvexPolygonShapeSW *convex_polygon_A = static_cast<const ConvexPolygonShapeSW*>(p_a); - const ConvexPolygonShapeSW *convex_polygon_B = static_cast<const ConvexPolygonShapeSW*>(p_b); - - SeparatorAxisTest<ConvexPolygonShapeSW,ConvexPolygonShapeSW,withMargin> separator(convex_polygon_A,p_transform_a,convex_polygon_B,p_transform_b,p_collector,p_margin_a,p_margin_b); + SeparatorAxisTest<ConvexPolygonShapeSW, ConvexPolygonShapeSW, withMargin> separator(convex_polygon_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b); if (!separator.test_previous_axis()) return; @@ -1380,107 +1274,97 @@ static void _collision_convex_polygon_convex_polygon(const ShapeSW *p_a,const Tr int vertex_count_B = mesh_B.vertices.size(); // faces of A - for (int i=0;i<face_count_A;i++) { + for (int i = 0; i < face_count_A; i++) { - Vector3 axis = p_transform_a.xform( faces_A[i].plane ).normal; + Vector3 axis = p_transform_a.xform(faces_A[i].plane).normal; //Vector3 axis = p_transform_a.basis.xform( faces_A[i].plane.normal ).normalized(); - if (!separator.test_axis( axis )) + if (!separator.test_axis(axis)) return; } // faces of B - for (int i=0;i<face_count_B;i++) { + for (int i = 0; i < face_count_B; i++) { - Vector3 axis = p_transform_b.xform( faces_B[i].plane ).normal; + Vector3 axis = p_transform_b.xform(faces_B[i].plane).normal; //Vector3 axis = p_transform_b.basis.xform( faces_B[i].plane.normal ).normalized(); - - if (!separator.test_axis( axis )) + if (!separator.test_axis(axis)) return; } // A<->B edges - for (int i=0;i<edge_count_A;i++) { + for (int i = 0; i < edge_count_A; i++) { - Vector3 e1=p_transform_a.basis.xform( vertices_A[ edges_A[i].a] ) -p_transform_a.basis.xform( vertices_A[ edges_A[i].b] ); + Vector3 e1 = p_transform_a.basis.xform(vertices_A[edges_A[i].a]) - p_transform_a.basis.xform(vertices_A[edges_A[i].b]); - for (int j=0;j<edge_count_B;j++) { + for (int j = 0; j < edge_count_B; j++) { - Vector3 e2=p_transform_b.basis.xform( vertices_B[ edges_B[j].a] ) -p_transform_b.basis.xform( vertices_B[ edges_B[j].b] ); + Vector3 e2 = p_transform_b.basis.xform(vertices_B[edges_B[j].a]) - p_transform_b.basis.xform(vertices_B[edges_B[j].b]); - Vector3 axis=e1.cross( e2 ).normalized(); + Vector3 axis = e1.cross(e2).normalized(); - if (!separator.test_axis( axis )) + if (!separator.test_axis(axis)) return; - } } if (withMargin) { //vertex-vertex - for(int i=0;i<vertex_count_A;i++) { + for (int i = 0; i < vertex_count_A; i++) { Vector3 va = p_transform_a.xform(vertices_A[i]); - for(int j=0;j<vertex_count_B;j++) { + for (int j = 0; j < vertex_count_B; j++) { - if (!separator.test_axis( (va-p_transform_b.xform(vertices_B[j])).normalized() )) + if (!separator.test_axis((va - p_transform_b.xform(vertices_B[j])).normalized())) return; - } } //edge-vertex( hsell) - for (int i=0;i<edge_count_A;i++) { + for (int i = 0; i < edge_count_A; i++) { - Vector3 e1=p_transform_a.basis.xform( vertices_A[ edges_A[i].a] ); - Vector3 e2=p_transform_a.basis.xform( vertices_A[ edges_A[i].b] ); - Vector3 n = (e2-e1); + Vector3 e1 = p_transform_a.basis.xform(vertices_A[edges_A[i].a]); + Vector3 e2 = p_transform_a.basis.xform(vertices_A[edges_A[i].b]); + Vector3 n = (e2 - e1); - for(int j=0;j<vertex_count_B;j++) { + for (int j = 0; j < vertex_count_B; j++) { - Vector3 e3=p_transform_b.xform(vertices_B[j]); + Vector3 e3 = p_transform_b.xform(vertices_B[j]); - - if (!separator.test_axis( (e1-e3).cross(n).cross(n).normalized() )) + if (!separator.test_axis((e1 - e3).cross(n).cross(n).normalized())) return; } } - for (int i=0;i<edge_count_B;i++) { - - Vector3 e1=p_transform_b.basis.xform( vertices_B[ edges_B[i].a] ); - Vector3 e2=p_transform_b.basis.xform( vertices_B[ edges_B[i].b] ); - Vector3 n = (e2-e1); + for (int i = 0; i < edge_count_B; i++) { - for(int j=0;j<vertex_count_A;j++) { + Vector3 e1 = p_transform_b.basis.xform(vertices_B[edges_B[i].a]); + Vector3 e2 = p_transform_b.basis.xform(vertices_B[edges_B[i].b]); + Vector3 n = (e2 - e1); - Vector3 e3=p_transform_a.xform(vertices_A[j]); + for (int j = 0; j < vertex_count_A; j++) { + Vector3 e3 = p_transform_a.xform(vertices_A[j]); - if (!separator.test_axis( (e1-e3).cross(n).cross(n).normalized() )) + if (!separator.test_axis((e1 - e3).cross(n).cross(n).normalized())) return; } } - - } separator.generate_contacts(); - } +template <bool withMargin> +static void _collision_convex_polygon_face(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { -template<bool withMargin> -static void _collision_convex_polygon_face(const ShapeSW *p_a,const Transform &p_transform_a, const ShapeSW *p_b,const Transform& p_transform_b,_CollectorCallback *p_collector,real_t p_margin_a,real_t p_margin_b) { - + const ConvexPolygonShapeSW *convex_polygon_A = static_cast<const ConvexPolygonShapeSW *>(p_a); + const FaceShapeSW *face_B = static_cast<const FaceShapeSW *>(p_b); - const ConvexPolygonShapeSW *convex_polygon_A = static_cast<const ConvexPolygonShapeSW*>(p_a); - const FaceShapeSW *face_B = static_cast<const FaceShapeSW*>(p_b); - - SeparatorAxisTest<ConvexPolygonShapeSW,FaceShapeSW,withMargin> separator(convex_polygon_A,p_transform_a,face_B,p_transform_b,p_collector,p_margin_a,p_margin_b); + SeparatorAxisTest<ConvexPolygonShapeSW, FaceShapeSW, withMargin> separator(convex_polygon_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b); const Geometry::MeshData &mesh = convex_polygon_A->get_mesh(); @@ -1491,207 +1375,192 @@ static void _collision_convex_polygon_face(const ShapeSW *p_a,const Transform &p const Vector3 *vertices = mesh.vertices.ptr(); int vertex_count = mesh.vertices.size(); - - - Vector3 vertex[3]={ - p_transform_b.xform( face_B->vertex[0] ), - p_transform_b.xform( face_B->vertex[1] ), - p_transform_b.xform( face_B->vertex[2] ), + Vector3 vertex[3] = { + p_transform_b.xform(face_B->vertex[0]), + p_transform_b.xform(face_B->vertex[1]), + p_transform_b.xform(face_B->vertex[2]), }; - if (!separator.test_axis( (vertex[0]-vertex[2]).cross(vertex[0]-vertex[1]).normalized() )) + if (!separator.test_axis((vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized())) return; - // faces of A - for (int i=0;i<face_count;i++) { + for (int i = 0; i < face_count; i++) { //Vector3 axis = p_transform_a.xform( faces[i].plane ).normal; - Vector3 axis = p_transform_a.basis.xform( faces[i].plane.normal ).normalized(); + Vector3 axis = p_transform_a.basis.xform(faces[i].plane.normal).normalized(); - if (!separator.test_axis( axis )) + if (!separator.test_axis(axis)) return; } - // A<->B edges - for (int i=0;i<edge_count;i++) { + for (int i = 0; i < edge_count; i++) { - Vector3 e1=p_transform_a.xform( vertices[edges[i].a] ) - p_transform_a.xform( vertices[edges[i].b] ); + Vector3 e1 = p_transform_a.xform(vertices[edges[i].a]) - p_transform_a.xform(vertices[edges[i].b]); - for (int j=0;j<3;j++) { + for (int j = 0; j < 3; j++) { - Vector3 e2=vertex[j]-vertex[(j+1)%3]; + Vector3 e2 = vertex[j] - vertex[(j + 1) % 3]; - Vector3 axis=e1.cross( e2 ).normalized(); + Vector3 axis = e1.cross(e2).normalized(); - if (!separator.test_axis( axis )) + if (!separator.test_axis(axis)) return; } } - if (withMargin) { //vertex-vertex - for(int i=0;i<vertex_count;i++) { + for (int i = 0; i < vertex_count; i++) { Vector3 va = p_transform_a.xform(vertices[i]); - for(int j=0;j<3;j++) { + for (int j = 0; j < 3; j++) { - if (!separator.test_axis( (va-vertex[j]).normalized() )) + if (!separator.test_axis((va - vertex[j]).normalized())) return; - } } //edge-vertex( hsell) - for (int i=0;i<edge_count;i++) { - - Vector3 e1=p_transform_a.basis.xform( vertices[ edges[i].a] ); - Vector3 e2=p_transform_a.basis.xform( vertices[ edges[i].b] ); - Vector3 n = (e2-e1); + for (int i = 0; i < edge_count; i++) { - for(int j=0;j<3;j++) { + Vector3 e1 = p_transform_a.basis.xform(vertices[edges[i].a]); + Vector3 e2 = p_transform_a.basis.xform(vertices[edges[i].b]); + Vector3 n = (e2 - e1); - Vector3 e3=vertex[j]; + for (int j = 0; j < 3; j++) { + Vector3 e3 = vertex[j]; - if (!separator.test_axis( (e1-e3).cross(n).cross(n).normalized() )) + if (!separator.test_axis((e1 - e3).cross(n).cross(n).normalized())) return; } } - for (int i=0;i<3;i++) { + for (int i = 0; i < 3; i++) { - Vector3 e1=vertex[i]; - Vector3 e2=vertex[(i+1)%3]; - Vector3 n = (e2-e1); + Vector3 e1 = vertex[i]; + Vector3 e2 = vertex[(i + 1) % 3]; + Vector3 n = (e2 - e1); - for(int j=0;j<vertex_count;j++) { + for (int j = 0; j < vertex_count; j++) { - Vector3 e3=p_transform_a.xform(vertices[j]); + Vector3 e3 = p_transform_a.xform(vertices[j]); - - if (!separator.test_axis( (e1-e3).cross(n).cross(n).normalized() )) + if (!separator.test_axis((e1 - e3).cross(n).cross(n).normalized())) return; } } } separator.generate_contacts(); - } +bool sat_calculate_penetration(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CollisionSolverSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap, Vector3 *r_prev_axis, real_t p_margin_a, real_t p_margin_b) { -bool sat_calculate_penetration(const ShapeSW *p_shape_A, const Transform& p_transform_A, const ShapeSW *p_shape_B, const Transform& p_transform_B, CollisionSolverSW::CallbackResult p_result_callback,void *p_userdata,bool p_swap,Vector3* r_prev_axis,real_t p_margin_a,real_t p_margin_b) { - - PhysicsServer::ShapeType type_A=p_shape_A->get_type(); - - ERR_FAIL_COND_V(type_A==PhysicsServer::SHAPE_PLANE,false); - ERR_FAIL_COND_V(type_A==PhysicsServer::SHAPE_RAY,false); - ERR_FAIL_COND_V(p_shape_A->is_concave(),false); + PhysicsServer::ShapeType type_A = p_shape_A->get_type(); - PhysicsServer::ShapeType type_B=p_shape_B->get_type(); + ERR_FAIL_COND_V(type_A == PhysicsServer::SHAPE_PLANE, false); + ERR_FAIL_COND_V(type_A == PhysicsServer::SHAPE_RAY, false); + ERR_FAIL_COND_V(p_shape_A->is_concave(), false); - ERR_FAIL_COND_V(type_B==PhysicsServer::SHAPE_PLANE,false); - ERR_FAIL_COND_V(type_B==PhysicsServer::SHAPE_RAY,false); - ERR_FAIL_COND_V(p_shape_B->is_concave(),false); + PhysicsServer::ShapeType type_B = p_shape_B->get_type(); + ERR_FAIL_COND_V(type_B == PhysicsServer::SHAPE_PLANE, false); + ERR_FAIL_COND_V(type_B == PhysicsServer::SHAPE_RAY, false); + ERR_FAIL_COND_V(p_shape_B->is_concave(), false); - static const CollisionFunc collision_table[5][5]={ - {_collision_sphere_sphere<false>, - _collision_sphere_box<false>, - _collision_sphere_capsule<false>, - _collision_sphere_convex_polygon<false>, - _collision_sphere_face<false>}, - {0, - _collision_box_box<false>, - _collision_box_capsule<false>, - _collision_box_convex_polygon<false>, - _collision_box_face<false>}, - {0, - 0, - _collision_capsule_capsule<false>, - _collision_capsule_convex_polygon<false>, - _collision_capsule_face<false>}, - {0, - 0, - 0, - _collision_convex_polygon_convex_polygon<false>, - _collision_convex_polygon_face<false>}, - {0, - 0, - 0, - 0, - 0}, + static const CollisionFunc collision_table[5][5] = { + { _collision_sphere_sphere<false>, + _collision_sphere_box<false>, + _collision_sphere_capsule<false>, + _collision_sphere_convex_polygon<false>, + _collision_sphere_face<false> }, + { 0, + _collision_box_box<false>, + _collision_box_capsule<false>, + _collision_box_convex_polygon<false>, + _collision_box_face<false> }, + { 0, + 0, + _collision_capsule_capsule<false>, + _collision_capsule_convex_polygon<false>, + _collision_capsule_face<false> }, + { 0, + 0, + 0, + _collision_convex_polygon_convex_polygon<false>, + _collision_convex_polygon_face<false> }, + { 0, + 0, + 0, + 0, + 0 }, }; - static const CollisionFunc collision_table_margin[5][5]={ - {_collision_sphere_sphere<true>, - _collision_sphere_box<true>, - _collision_sphere_capsule<true>, - _collision_sphere_convex_polygon<true>, - _collision_sphere_face<true>}, - {0, - _collision_box_box<true>, - _collision_box_capsule<true>, - _collision_box_convex_polygon<true>, - _collision_box_face<true>}, - {0, - 0, - _collision_capsule_capsule<true>, - _collision_capsule_convex_polygon<true>, - _collision_capsule_face<true>}, - {0, - 0, - 0, - _collision_convex_polygon_convex_polygon<true>, - _collision_convex_polygon_face<true>}, - {0, - 0, - 0, - 0, - 0}, + static const CollisionFunc collision_table_margin[5][5] = { + { _collision_sphere_sphere<true>, + _collision_sphere_box<true>, + _collision_sphere_capsule<true>, + _collision_sphere_convex_polygon<true>, + _collision_sphere_face<true> }, + { 0, + _collision_box_box<true>, + _collision_box_capsule<true>, + _collision_box_convex_polygon<true>, + _collision_box_face<true> }, + { 0, + 0, + _collision_capsule_capsule<true>, + _collision_capsule_convex_polygon<true>, + _collision_capsule_face<true> }, + { 0, + 0, + 0, + _collision_convex_polygon_convex_polygon<true>, + _collision_convex_polygon_face<true> }, + { 0, + 0, + 0, + 0, + 0 }, }; _CollectorCallback callback; - callback.callback=p_result_callback; - callback.swap=p_swap; - callback.userdata=p_userdata; - callback.collided=false; - callback.prev_axis=r_prev_axis; + callback.callback = p_result_callback; + callback.swap = p_swap; + callback.userdata = p_userdata; + callback.collided = false; + callback.prev_axis = r_prev_axis; - const ShapeSW *A=p_shape_A; - const ShapeSW *B=p_shape_B; - const Transform *transform_A=&p_transform_A; - const Transform *transform_B=&p_transform_B; - real_t margin_A=p_margin_a; - real_t margin_B=p_margin_b; + const ShapeSW *A = p_shape_A; + const ShapeSW *B = p_shape_B; + const Transform *transform_A = &p_transform_A; + const Transform *transform_B = &p_transform_B; + real_t margin_A = p_margin_a; + real_t margin_B = p_margin_b; if (type_A > type_B) { - SWAP(A,B); - SWAP(transform_A,transform_B); - SWAP(type_A,type_B); - SWAP(margin_A,margin_B); + SWAP(A, B); + SWAP(transform_A, transform_B); + SWAP(type_A, type_B); + SWAP(margin_A, margin_B); callback.swap = !callback.swap; } - CollisionFunc collision_func; - if (margin_A!=0.0 || margin_B!=0.0) { - collision_func = collision_table_margin[type_A-2][type_B-2]; + if (margin_A != 0.0 || margin_B != 0.0) { + collision_func = collision_table_margin[type_A - 2][type_B - 2]; } else { - collision_func = collision_table[type_A-2][type_B-2]; - + collision_func = collision_table[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,margin_A,margin_B); + collision_func(A, *transform_A, B, *transform_B, &callback, margin_A, margin_B); return callback.collided; - } diff --git a/servers/physics/collision_solver_sat.h b/servers/physics/collision_solver_sat.h index 15fe7dc34..67ffb0b06 100644 --- a/servers/physics/collision_solver_sat.h +++ b/servers/physics/collision_solver_sat.h @@ -31,7 +31,6 @@ #include "collision_solver_sw.h" - -bool sat_calculate_penetration(const ShapeSW *p_shape_A, const Transform& p_transform_A, const ShapeSW *p_shape_B, const Transform& p_transform_B, CollisionSolverSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap=false,Vector3* r_prev_axis=NULL,real_t p_margin_a=0,real_t p_margin_b=0); +bool sat_calculate_penetration(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CollisionSolverSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap = false, Vector3 *r_prev_axis = NULL, real_t p_margin_a = 0, real_t p_margin_b = 0); #endif // COLLISION_SOLVER_SAT_H diff --git a/servers/physics/collision_solver_sw.cpp b/servers/physics/collision_solver_sw.cpp index f0ddde3a7..0f6e96435 100644 --- a/servers/physics/collision_solver_sw.cpp +++ b/servers/physics/collision_solver_sw.cpp @@ -29,18 +29,16 @@ #include "collision_solver_sw.h" #include "collision_solver_sat.h" -#include "gjk_epa.h" #include "collision_solver_sat.h" - +#include "gjk_epa.h" #define collision_solver sat_calculate_penetration //#define collision_solver gjk_epa_calculate_penetration +bool CollisionSolverSW::solve_static_plane(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) { -bool CollisionSolverSW::solve_static_plane(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result) { - - const PlaneShapeSW *plane = static_cast<const PlaneShapeSW*>(p_shape_A); - if (p_shape_B->get_type()==PhysicsServer::SHAPE_PLANE) + const PlaneShapeSW *plane = static_cast<const PlaneShapeSW *>(p_shape_A); + if (p_shape_B->get_type() == PhysicsServer::SHAPE_PLANE) return false; Plane p = p_transform_A.xform(plane->get_plane()); @@ -48,57 +46,54 @@ bool CollisionSolverSW::solve_static_plane(const ShapeSW *p_shape_A,const Transf Vector3 supports[max_supports]; int support_count; - p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(),max_supports,supports,support_count); + p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(), max_supports, supports, support_count); - bool found=false; + 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] ); - if (p.distance_to(supports[i])>=0) + supports[i] = p_transform_B.xform(supports[i]); + if (p.distance_to(supports[i]) >= 0) continue; - found=true; + found = true; Vector3 support_A = p.project(supports[i]); if (p_result_callback) { if (p_swap_result) - p_result_callback(supports[i],support_A,p_userdata); + 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 CollisionSolverSW::solve_ray(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result) { +bool CollisionSolverSW::solve_ray(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) { - - const RayShapeSW *ray = static_cast<const RayShapeSW*>(p_shape_A); + const RayShapeSW *ray = static_cast<const RayShapeSW *>(p_shape_A); Vector3 from = p_transform_A.origin; - Vector3 to = from+p_transform_A.basis.get_axis(2)*ray->get_length(); - Vector3 support_A=to; + Vector3 to = from + p_transform_A.basis.get_axis(2) * ray->get_length(); + Vector3 support_A = to; Transform ai = p_transform_B.affine_inverse(); from = ai.xform(from); to = ai.xform(to); - Vector3 p,n; - if (!p_shape_B->intersect_segment(from,to,p,n)) + Vector3 p, n; + if (!p_shape_B->intersect_segment(from, to, p, n)) return false; - Vector3 support_B=p_transform_B.xform(p); + Vector3 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; } @@ -117,169 +112,153 @@ struct _ConcaveCollisionInfo { bool tested; real_t margin_A; real_t margin_B; - Vector3 close_A,close_B; - + Vector3 close_A, close_B; }; void CollisionSolverSW::concave_callback(void *p_userdata, ShapeSW *p_convex) { - - _ConcaveCollisionInfo &cinfo = *(_ConcaveCollisionInfo*)(p_userdata); + _ConcaveCollisionInfo &cinfo = *(_ConcaveCollisionInfo *)(p_userdata); cinfo.aabb_tests++; - bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, p_convex,*cinfo.transform_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result,NULL,cinfo.margin_A,cinfo.margin_B); + bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, p_convex, *cinfo.transform_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result, NULL, cinfo.margin_A, cinfo.margin_B); if (!collided) return; - cinfo.collided=true; + cinfo.collided = true; cinfo.collisions++; - } -bool CollisionSolverSW::solve_concave(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,real_t p_margin_A,real_t p_margin_B) { +bool CollisionSolverSW::solve_concave(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin_A, real_t p_margin_B) { - - const ConcaveShapeSW *concave_B=static_cast<const ConcaveShapeSW*>(p_shape_B); + const ConcaveShapeSW *concave_B = static_cast<const ConcaveShapeSW *>(p_shape_B); _ConcaveCollisionInfo cinfo; - cinfo.transform_A=&p_transform_A; - cinfo.shape_A=p_shape_A; - cinfo.transform_B=&p_transform_B; - cinfo.result_callback=p_result_callback; - cinfo.userdata=p_userdata; - cinfo.swap_result=p_swap_result; - cinfo.collided=false; - cinfo.collisions=0; - cinfo.margin_A=p_margin_A; - cinfo.margin_B=p_margin_B; + cinfo.transform_A = &p_transform_A; + cinfo.shape_A = p_shape_A; + cinfo.transform_B = &p_transform_B; + cinfo.result_callback = p_result_callback; + cinfo.userdata = p_userdata; + cinfo.swap_result = p_swap_result; + cinfo.collided = false; + cinfo.collisions = 0; + cinfo.margin_A = p_margin_A; + cinfo.margin_B = p_margin_B; - cinfo.aabb_tests=0; + cinfo.aabb_tests = 0; Transform rel_transform = p_transform_A; - rel_transform.origin-=p_transform_B.origin; + rel_transform.origin -= p_transform_B.origin; //quickly compute a local AABB Rect3 local_aabb; - for(int i=0;i<3;i++) { - - Vector3 axis( p_transform_B.basis.get_axis(i) ); - real_t axis_scale = 1.0/axis.length(); - axis*=axis_scale; + for (int i = 0; i < 3; i++) { - real_t smin,smax; - p_shape_A->project_range(axis,rel_transform,smin,smax); - smin-=p_margin_A; - smax+=p_margin_A; - smin*=axis_scale; - smax*=axis_scale; + Vector3 axis(p_transform_B.basis.get_axis(i)); + real_t axis_scale = 1.0 / axis.length(); + axis *= axis_scale; + real_t smin, smax; + p_shape_A->project_range(axis, rel_transform, smin, smax); + smin -= p_margin_A; + smax += p_margin_A; + smin *= axis_scale; + smax *= axis_scale; - local_aabb.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("COL AABB TESTS: "+itos(cinfo.aabb_tests)); return cinfo.collided; } +bool CollisionSolverSW::solve_static(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, Vector3 *r_sep_axis, real_t p_margin_A, real_t p_margin_B) { -bool CollisionSolverSW::solve_static(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,Vector3 *r_sep_axis,real_t p_margin_A,real_t p_margin_B) { - - - PhysicsServer::ShapeType type_A=p_shape_A->get_type(); - PhysicsServer::ShapeType type_B=p_shape_B->get_type(); - bool concave_A=p_shape_A->is_concave(); - bool concave_B=p_shape_B->is_concave(); + PhysicsServer::ShapeType type_A = p_shape_A->get_type(); + PhysicsServer::ShapeType type_B = p_shape_B->get_type(); + bool concave_A = p_shape_A->is_concave(); + bool concave_B = p_shape_B->is_concave(); bool swap = false; - if (type_A>type_B) { - SWAP(type_A,type_B); - SWAP(concave_A,concave_B); - swap=true; + if (type_A > type_B) { + SWAP(type_A, type_B); + SWAP(concave_A, concave_B); + swap = true; } - if (type_A==PhysicsServer::SHAPE_PLANE) { + if (type_A == PhysicsServer::SHAPE_PLANE) { - if (type_B==PhysicsServer::SHAPE_PLANE) + if (type_B == PhysicsServer::SHAPE_PLANE) return false; - if (type_B==PhysicsServer::SHAPE_RAY) { + if (type_B == PhysicsServer::SHAPE_RAY) { return false; } if (swap) { - return solve_static_plane(p_shape_B,p_transform_B,p_shape_A,p_transform_A,p_result_callback,p_userdata,true); + return solve_static_plane(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true); } else { - return solve_static_plane(p_shape_A,p_transform_A,p_shape_B,p_transform_B,p_result_callback,p_userdata,false); + return solve_static_plane(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false); } - } else if (type_A==PhysicsServer::SHAPE_RAY) { + } else if (type_A == PhysicsServer::SHAPE_RAY) { - if (type_B==PhysicsServer::SHAPE_RAY) + if (type_B == PhysicsServer::SHAPE_RAY) return false; if (swap) { - return solve_ray(p_shape_B,p_transform_B,p_shape_A,p_transform_A,p_result_callback,p_userdata,true); + return solve_ray(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true); } else { - return solve_ray(p_shape_A,p_transform_A,p_shape_B,p_transform_B,p_result_callback,p_userdata,false); + return solve_ray(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false); } } else if (concave_B) { - if (concave_A) return false; if (!swap) - return solve_concave(p_shape_A,p_transform_A,p_shape_B,p_transform_B,p_result_callback,p_userdata,false,p_margin_A,p_margin_B); + return solve_concave(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false, p_margin_A, p_margin_B); else - return solve_concave(p_shape_B,p_transform_B,p_shape_A,p_transform_A,p_result_callback,p_userdata,true,p_margin_A,p_margin_B); - - + return solve_concave(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true, p_margin_A, p_margin_B); } else { - return collision_solver(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback,p_userdata,false,r_sep_axis,p_margin_A,p_margin_B); + return collision_solver(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false, r_sep_axis, p_margin_A, p_margin_B); } - return false; } - void CollisionSolverSW::concave_distance_callback(void *p_userdata, ShapeSW *p_convex) { - - _ConcaveCollisionInfo &cinfo = *(_ConcaveCollisionInfo*)(p_userdata); + _ConcaveCollisionInfo &cinfo = *(_ConcaveCollisionInfo *)(p_userdata); cinfo.aabb_tests++; if (cinfo.collided) return; - Vector3 close_A,close_B; - cinfo.collided = !gjk_epa_calculate_distance(cinfo.shape_A,*cinfo.transform_A,p_convex,*cinfo.transform_B,close_A,close_B); + Vector3 close_A, close_B; + cinfo.collided = !gjk_epa_calculate_distance(cinfo.shape_A, *cinfo.transform_A, p_convex, *cinfo.transform_B, close_A, close_B); if (cinfo.collided) return; if (!cinfo.tested || close_A.distance_squared_to(close_B) < cinfo.close_A.distance_squared_to(cinfo.close_B)) { - cinfo.close_A=close_A; - cinfo.close_B=close_B; - cinfo.tested=true; + cinfo.close_A = close_A; + cinfo.close_B = close_B; + cinfo.tested = true; } cinfo.collisions++; - } +bool CollisionSolverSW::solve_distance_plane(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B) { - -bool CollisionSolverSW::solve_distance_plane(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,Vector3& r_point_A,Vector3& r_point_B) { - - const PlaneShapeSW *plane = static_cast<const PlaneShapeSW*>(p_shape_A); - if (p_shape_B->get_type()==PhysicsServer::SHAPE_PLANE) + const PlaneShapeSW *plane = static_cast<const PlaneShapeSW *>(p_shape_A); + if (p_shape_B->get_type() == PhysicsServer::SHAPE_PLANE) return false; Plane p = p_transform_A.xform(plane->get_plane()); @@ -287,43 +266,41 @@ bool CollisionSolverSW::solve_distance_plane(const ShapeSW *p_shape_A,const Tran Vector3 supports[max_supports]; int support_count; - p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(),max_supports,supports,support_count); + p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(), max_supports, supports, support_count); - bool collided=false; + bool collided = false; Vector3 closest; real_t closest_d; + 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 d = p.distance_to(supports[i]); - if (i==0 || d<closest_d) { - closest=supports[i]; - closest_d=d; - if (d<=0) - collided=true; + if (i == 0 || d < closest_d) { + closest = supports[i]; + closest_d = d; + if (d <= 0) + collided = true; } - } - r_point_A=p.project(closest); - r_point_B=closest; + r_point_A = p.project(closest); + r_point_B = closest; return collided; } -bool CollisionSolverSW::solve_distance(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,Vector3& r_point_A,Vector3& r_point_B,const Rect3& p_concave_hint,Vector3 *r_sep_axis) { +bool CollisionSolverSW::solve_distance(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B, const Rect3 &p_concave_hint, Vector3 *r_sep_axis) { if (p_shape_A->is_concave()) return false; - if (p_shape_B->get_type()==PhysicsServer::SHAPE_PLANE) { + if (p_shape_B->get_type() == PhysicsServer::SHAPE_PLANE) { - Vector3 a,b; - bool col = solve_distance_plane(p_shape_B,p_transform_B,p_shape_A,p_transform_A,a,b); - r_point_A=b; - r_point_B=a; + Vector3 a, b; + bool col = solve_distance_plane(p_shape_B, p_transform_B, p_shape_A, p_transform_A, a, b); + r_point_A = b; + r_point_B = a; return !col; } else if (p_shape_B->is_concave()) { @@ -331,62 +308,59 @@ bool CollisionSolverSW::solve_distance(const ShapeSW *p_shape_A,const Transform& if (p_shape_A->is_concave()) return false; - - const ConcaveShapeSW *concave_B=static_cast<const ConcaveShapeSW*>(p_shape_B); + const ConcaveShapeSW *concave_B = static_cast<const ConcaveShapeSW *>(p_shape_B); _ConcaveCollisionInfo cinfo; - cinfo.transform_A=&p_transform_A; - cinfo.shape_A=p_shape_A; - cinfo.transform_B=&p_transform_B; - cinfo.result_callback=NULL; - cinfo.userdata=NULL; - cinfo.swap_result=false; - cinfo.collided=false; - cinfo.collisions=0; - cinfo.aabb_tests=0; - cinfo.tested=false; + cinfo.transform_A = &p_transform_A; + cinfo.shape_A = p_shape_A; + cinfo.transform_B = &p_transform_B; + cinfo.result_callback = NULL; + cinfo.userdata = NULL; + cinfo.swap_result = false; + cinfo.collided = false; + cinfo.collisions = 0; + cinfo.aabb_tests = 0; + cinfo.tested = false; Transform rel_transform = p_transform_A; - rel_transform.origin-=p_transform_B.origin; + rel_transform.origin -= p_transform_B.origin; //quickly compute a local AABB - bool use_cc_hint=p_concave_hint!=Rect3(); + bool use_cc_hint = p_concave_hint != Rect3(); Rect3 cc_hint_aabb; if (use_cc_hint) { - cc_hint_aabb=p_concave_hint; - cc_hint_aabb.pos-=p_transform_B.origin; + cc_hint_aabb = p_concave_hint; + cc_hint_aabb.pos -= p_transform_B.origin; } Rect3 local_aabb; - for(int i=0;i<3;i++) { + for (int i = 0; i < 3; i++) { - Vector3 axis( p_transform_B.basis.get_axis(i) ); - real_t axis_scale = ((real_t)1.0)/axis.length(); - axis*=axis_scale; + Vector3 axis(p_transform_B.basis.get_axis(i)); + real_t axis_scale = ((real_t)1.0) / axis.length(); + axis *= axis_scale; - real_t smin,smax; + real_t smin, smax; - if (use_cc_hint) { - cc_hint_aabb.project_range_in_plane(Plane(axis,0),smin,smax); - } else { - p_shape_A->project_range(axis,rel_transform,smin,smax); - } + if (use_cc_hint) { + cc_hint_aabb.project_range_in_plane(Plane(axis, 0), smin, smax); + } else { + p_shape_A->project_range(axis, rel_transform, smin, smax); + } - smin*=axis_scale; - smax*=axis_scale; + 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_distance_callback,&cinfo); + concave_B->cull(local_aabb, concave_distance_callback, &cinfo); if (!cinfo.collided) { //print_line(itos(cinfo.tested)); - r_point_A=cinfo.close_A; - r_point_B=cinfo.close_B; - + r_point_A = cinfo.close_A; + r_point_B = cinfo.close_B; } //print_line("DIST AABB TESTS: "+itos(cinfo.aabb_tests)); @@ -394,10 +368,8 @@ bool CollisionSolverSW::solve_distance(const ShapeSW *p_shape_A,const Transform& return !cinfo.collided; } else { - return gjk_epa_calculate_distance(p_shape_A,p_transform_A,p_shape_B,p_transform_B,r_point_A,r_point_B); //should pass sepaxis.. + return gjk_epa_calculate_distance(p_shape_A, p_transform_A, p_shape_B, p_transform_B, r_point_A, r_point_B); //should pass sepaxis.. } - return false; } - diff --git a/servers/physics/collision_solver_sw.h b/servers/physics/collision_solver_sw.h index b8d37a8a9..b0f18dc0a 100644 --- a/servers/physics/collision_solver_sw.h +++ b/servers/physics/collision_solver_sw.h @@ -31,25 +31,21 @@ #include "shape_sw.h" -class CollisionSolverSW -{ +class CollisionSolverSW { public: - typedef void (*CallbackResult)(const Vector3& p_point_A,const Vector3& p_point_B,void *p_userdata); -private: + typedef void (*CallbackResult)(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata); +private: static void concave_callback(void *p_userdata, ShapeSW *p_convex); - static bool solve_static_plane(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result); - static bool solve_ray(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result); - static bool solve_concave(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,real_t p_margin_A=0,real_t p_margin_B=0); + static bool solve_static_plane(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); + static bool solve_ray(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); + static bool solve_concave(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin_A = 0, real_t p_margin_B = 0); static void concave_distance_callback(void *p_userdata, ShapeSW *p_convex); - static bool solve_distance_plane(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,Vector3& r_point_A,Vector3& r_point_B); + static bool solve_distance_plane(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B); public: - - - static bool solve_static(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,Vector3 *r_sep_axis=NULL,real_t p_margin_A=0,real_t p_margin_B=0); - static bool solve_distance(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,Vector3& r_point_A,Vector3& r_point_B,const Rect3& p_concave_hint,Vector3 *r_sep_axis=NULL); - + static bool solve_static(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, Vector3 *r_sep_axis = NULL, real_t p_margin_A = 0, real_t p_margin_B = 0); + static bool solve_distance(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B, const Rect3 &p_concave_hint, Vector3 *r_sep_axis = NULL); }; #endif // COLLISION_SOLVER__SW_H diff --git a/servers/physics/constraint_sw.h b/servers/physics/constraint_sw.h index f5bbe4bbd..2cd0e1a42 100644 --- a/servers/physics/constraint_sw.h +++ b/servers/physics/constraint_sw.h @@ -40,35 +40,37 @@ class ConstraintSW : public RID_Data { ConstraintSW *island_list_next; int priority; - RID self; protected: - ConstraintSW(BodySW **p_body_ptr=NULL,int p_body_count=0) { _body_ptr=p_body_ptr; _body_count=p_body_count; island_step=0; priority=1; } -public: + ConstraintSW(BodySW **p_body_ptr = NULL, int p_body_count = 0) { + _body_ptr = p_body_ptr; + _body_count = p_body_count; + island_step = 0; + priority = 1; + } - _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_ ConstraintSW *get_island_next() const { return island_next; } + _FORCE_INLINE_ void set_island_next(ConstraintSW *p_next) { island_next = p_next; } - _FORCE_INLINE_ ConstraintSW* get_island_next() const { return island_next; } - _FORCE_INLINE_ void set_island_next(ConstraintSW* p_next) { island_next=p_next; } - - _FORCE_INLINE_ ConstraintSW* get_island_list_next() const { return island_list_next; } - _FORCE_INLINE_ void set_island_list_next(ConstraintSW* p_next) { island_list_next=p_next; } + _FORCE_INLINE_ ConstraintSW *get_island_list_next() const { return island_list_next; } + _FORCE_INLINE_ void set_island_list_next(ConstraintSW *p_next) { island_list_next = p_next; } _FORCE_INLINE_ BodySW **get_body_ptr() const { return _body_ptr; } _FORCE_INLINE_ int get_body_count() const { return _body_count; } - _FORCE_INLINE_ void set_priority(int p_priority) { priority=p_priority; } + _FORCE_INLINE_ void set_priority(int p_priority) { priority = p_priority; } _FORCE_INLINE_ int get_priority() const { return priority; } - - 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 ~ConstraintSW() {} }; diff --git a/servers/physics/gjk_epa.cpp b/servers/physics/gjk_epa.cpp index 06c2c2382..f65e6768a 100644 --- a/servers/physics/gjk_epa.cpp +++ b/servers/physics/gjk_epa.cpp @@ -884,33 +884,30 @@ bool Penetration( const ShapeSW* shape0, /* clang-format on */ - -bool gjk_epa_calculate_distance(const ShapeSW *p_shape_A, const Transform& p_transform_A, const ShapeSW *p_shape_B, const Transform& p_transform_B, Vector3& r_result_A, Vector3& r_result_B) { - +bool gjk_epa_calculate_distance(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_result_A, Vector3 &r_result_B) { GjkEpa2::sResults res; - if (GjkEpa2::Distance(p_shape_A,p_transform_A,p_shape_B,p_transform_B,p_transform_B.origin-p_transform_A.origin,res)) { + if (GjkEpa2::Distance(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_transform_B.origin - p_transform_A.origin, res)) { - r_result_A=res.witnesses[0]; - r_result_B=res.witnesses[1]; + r_result_A = res.witnesses[0]; + r_result_B = res.witnesses[1]; return true; } return false; - } -bool gjk_epa_calculate_penetration(const ShapeSW *p_shape_A, const Transform& p_transform_A, const ShapeSW *p_shape_B, const Transform& p_transform_B, CollisionSolverSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap ) { +bool gjk_epa_calculate_penetration(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CollisionSolverSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap) { GjkEpa2::sResults res; - if (GjkEpa2::Penetration(p_shape_A,p_transform_A,p_shape_B,p_transform_B,p_transform_B.origin-p_transform_A.origin,res)) { + if (GjkEpa2::Penetration(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_transform_B.origin - p_transform_A.origin, res)) { if (p_result_callback) { if (p_swap) - p_result_callback(res.witnesses[1],res.witnesses[0],p_userdata); + p_result_callback(res.witnesses[1], res.witnesses[0], p_userdata); else - p_result_callback(res.witnesses[0],res.witnesses[1],p_userdata); + p_result_callback(res.witnesses[0], res.witnesses[1], p_userdata); } return true; } diff --git a/servers/physics/gjk_epa.h b/servers/physics/gjk_epa.h index 58cf8f50c..ae5db733b 100644 --- a/servers/physics/gjk_epa.h +++ b/servers/physics/gjk_epa.h @@ -35,7 +35,7 @@ */ #include "collision_solver_sw.h" -bool gjk_epa_calculate_penetration(const ShapeSW *p_shape_A, const Transform& p_transform_A, const ShapeSW *p_shape_B, const Transform& p_transform_B, CollisionSolverSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap=false); -bool gjk_epa_calculate_distance(const ShapeSW *p_shape_A, const Transform& p_transform_A, const ShapeSW *p_shape_B, const Transform& p_transform_B, Vector3& r_result_A, Vector3& r_result_B); +bool gjk_epa_calculate_penetration(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CollisionSolverSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap = false); +bool gjk_epa_calculate_distance(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_result_A, Vector3 &r_result_B); #endif diff --git a/servers/physics/joints/cone_twist_joint_sw.cpp b/servers/physics/joints/cone_twist_joint_sw.cpp index d78bcbd16..8cab81de2 100644 --- a/servers/physics/joints/cone_twist_joint_sw.cpp +++ b/servers/physics/joints/cone_twist_joint_sw.cpp @@ -34,29 +34,26 @@ See corresponding header file for licensing info. #include "cone_twist_joint_sw.h" -static void plane_space(const Vector3& n, Vector3& p, Vector3& q) { +static void plane_space(const Vector3 &n, Vector3 &p, Vector3 &q) { - if (Math::abs(n.z) > 0.707106781186547524400844362) { - // choose p in y-z plane - real_t a = n[1]*n[1] + n[2]*n[2]; - real_t k = 1.0/Math::sqrt(a); - p=Vector3(0,-n[2]*k,n[1]*k); - // set q = n x p - q=Vector3(a*k,-n[0]*p[2],n[0]*p[1]); - } - else { - // choose p in x-y plane - real_t a = n.x*n.x + n.y*n.y; - real_t k = 1.0/Math::sqrt(a); - p=Vector3(-n.y*k,n.x*k,0); - // set q = n x p - q=Vector3(-n.z*p.y,n.z*p.x,a*k); - } + if (Math::abs(n.z) > 0.707106781186547524400844362) { + // choose p in y-z plane + real_t a = n[1] * n[1] + n[2] * n[2]; + real_t k = 1.0 / Math::sqrt(a); + p = Vector3(0, -n[2] * k, n[1] * k); + // set q = n x p + q = Vector3(a * k, -n[0] * p[2], n[0] * p[1]); + } else { + // choose p in x-y plane + real_t a = n.x * n.x + n.y * n.y; + real_t k = 1.0 / Math::sqrt(a); + p = Vector3(-n.y * k, n.x * k, 0); + // set q = n x p + q = Vector3(-n.z * p.y, n.z * p.x, a * k); + } } - -static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x) -{ +static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x) { real_t coeff_1 = Math_PI / 4.0f; real_t coeff_2 = 3.0f * coeff_1; real_t abs_y = Math::abs(y); @@ -71,32 +68,31 @@ static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x) return (y < 0.0f) ? -angle : angle; } -ConeTwistJointSW::ConeTwistJointSW(BodySW* rbA,BodySW* rbB,const Transform& rbAFrame, const Transform& rbBFrame) : JointSW(_arr,2) { +ConeTwistJointSW::ConeTwistJointSW(BodySW *rbA, BodySW *rbB, const Transform &rbAFrame, const Transform &rbBFrame) + : JointSW(_arr, 2) { - A=rbA; - B=rbB; + A = rbA; + B = rbB; + m_rbAFrame = rbAFrame; + m_rbBFrame = rbBFrame; - m_rbAFrame=rbAFrame; - m_rbBFrame=rbBFrame; - - m_swingSpan1 = Math_PI/4.0; - m_swingSpan2 = Math_PI/4.0; - m_twistSpan = Math_PI*2; + m_swingSpan1 = Math_PI / 4.0; + m_swingSpan2 = Math_PI / 4.0; + m_twistSpan = Math_PI * 2; m_biasFactor = 0.3f; m_relaxationFactor = 1.0f; m_solveTwistLimit = false; m_solveSwingLimit = false; - A->add_constraint(this,0); - B->add_constraint(this,1); + A->add_constraint(this, 0); + B->add_constraint(this, 1); - m_appliedImpulse=0; + m_appliedImpulse = 0; } - -bool ConeTwistJointSW::setup(real_t p_step) { +bool ConeTwistJointSW::setup(real_t p_step) { m_appliedImpulse = real_t(0.); //set bias, sign, clear accumulator @@ -107,109 +103,97 @@ bool ConeTwistJointSW::setup(real_t p_step) { m_accTwistLimitImpulse = real_t(0.); m_accSwingLimitImpulse = real_t(0.); - if (!m_angularOnly) - { + if (!m_angularOnly) { Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin); Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin); Vector3 relPos = pivotBInW - pivotAInW; Vector3 normal[3]; - if (relPos.length_squared() > CMP_EPSILON) - { + if (relPos.length_squared() > CMP_EPSILON) { normal[0] = relPos.normalized(); - } - else - { - normal[0]=Vector3(real_t(1.0),0,0); + } else { + normal[0] = Vector3(real_t(1.0), 0, 0); } plane_space(normal[0], normal[1], normal[2]); - for (int i=0;i<3;i++) - { + for (int i = 0; i < 3; i++) { memnew_placement(&m_jac[i], JacobianEntrySW( - A->get_principal_inertia_axes().transposed(), - B->get_principal_inertia_axes().transposed(), - pivotAInW - A->get_transform().origin - A->get_center_of_mass(), - pivotBInW - B->get_transform().origin - B->get_center_of_mass(), - normal[i], - A->get_inv_inertia(), - A->get_inv_mass(), - B->get_inv_inertia(), - B->get_inv_mass())); + A->get_principal_inertia_axes().transposed(), + B->get_principal_inertia_axes().transposed(), + pivotAInW - A->get_transform().origin - A->get_center_of_mass(), + pivotBInW - B->get_transform().origin - B->get_center_of_mass(), + normal[i], + A->get_inv_inertia(), + A->get_inv_mass(), + B->get_inv_inertia(), + B->get_inv_mass())); } } - Vector3 b1Axis1,b1Axis2,b1Axis3; - Vector3 b2Axis1,b2Axis2; + Vector3 b1Axis1, b1Axis2, b1Axis3; + Vector3 b2Axis1, b2Axis2; - b1Axis1 = A->get_transform().basis.xform( this->m_rbAFrame.basis.get_axis(0) ); - b2Axis1 = B->get_transform().basis.xform( this->m_rbBFrame.basis.get_axis(0) ); + b1Axis1 = A->get_transform().basis.xform(this->m_rbAFrame.basis.get_axis(0)); + b2Axis1 = B->get_transform().basis.xform(this->m_rbBFrame.basis.get_axis(0)); - real_t swing1=real_t(0.),swing2 = real_t(0.); + real_t swing1 = real_t(0.), swing2 = real_t(0.); - real_t swx=real_t(0.),swy = real_t(0.); + real_t swx = real_t(0.), swy = real_t(0.); real_t thresh = real_t(10.); real_t fact; // Get Frame into world space - if (m_swingSpan1 >= real_t(0.05f)) - { - b1Axis2 = A->get_transform().basis.xform( this->m_rbAFrame.basis.get_axis(1) ); + if (m_swingSpan1 >= real_t(0.05f)) { + b1Axis2 = A->get_transform().basis.xform(this->m_rbAFrame.basis.get_axis(1)); //swing1 = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) ); swx = b2Axis1.dot(b1Axis1); swy = b2Axis1.dot(b1Axis2); - swing1 = atan2fast(swy, swx); - fact = (swy*swy + swx*swx) * thresh * thresh; + swing1 = atan2fast(swy, swx); + fact = (swy * swy + swx * swx) * thresh * thresh; fact = fact / (fact + real_t(1.0)); swing1 *= fact; - } - if (m_swingSpan2 >= real_t(0.05f)) - { - b1Axis3 = A->get_transform().basis.xform( this->m_rbAFrame.basis.get_axis(2) ); + if (m_swingSpan2 >= real_t(0.05f)) { + b1Axis3 = A->get_transform().basis.xform(this->m_rbAFrame.basis.get_axis(2)); //swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) ); swx = b2Axis1.dot(b1Axis1); swy = b2Axis1.dot(b1Axis3); - swing2 = atan2fast(swy, swx); - fact = (swy*swy + swx*swx) * thresh * thresh; + swing2 = atan2fast(swy, swx); + fact = (swy * swy + swx * swx) * thresh * thresh; fact = fact / (fact + real_t(1.0)); swing2 *= fact; } - real_t RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1); - real_t RMaxAngle2Sq = 1.0f / (m_swingSpan2*m_swingSpan2); - real_t EllipseAngle = Math::abs(swing1*swing1)* RMaxAngle1Sq + Math::abs(swing2*swing2) * RMaxAngle2Sq; + real_t RMaxAngle1Sq = 1.0f / (m_swingSpan1 * m_swingSpan1); + real_t RMaxAngle2Sq = 1.0f / (m_swingSpan2 * m_swingSpan2); + real_t EllipseAngle = Math::abs(swing1 * swing1) * RMaxAngle1Sq + Math::abs(swing2 * swing2) * RMaxAngle2Sq; - if (EllipseAngle > 1.0f) - { - m_swingCorrection = EllipseAngle-1.0f; + if (EllipseAngle > 1.0f) { + m_swingCorrection = EllipseAngle - 1.0f; m_solveSwingLimit = true; // Calculate necessary axis & factors - m_swingAxis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3)); + m_swingAxis = b2Axis1.cross(b1Axis2 * b2Axis1.dot(b1Axis2) + b1Axis3 * b2Axis1.dot(b1Axis3)); m_swingAxis.normalize(); real_t swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f; m_swingAxis *= swingAxisSign; - m_kSwing = real_t(1.) / (A->compute_angular_impulse_denominator(m_swingAxis) + - B->compute_angular_impulse_denominator(m_swingAxis)); - + m_kSwing = real_t(1.) / (A->compute_angular_impulse_denominator(m_swingAxis) + + B->compute_angular_impulse_denominator(m_swingAxis)); } // Twist limits - if (m_twistSpan >= real_t(0.)) - { - Vector3 b2Axis2 = B->get_transform().basis.xform( this->m_rbBFrame.basis.get_axis(1) ); - Quat rotationArc = Quat(b2Axis1,b1Axis1); + if (m_twistSpan >= real_t(0.)) { + Vector3 b2Axis2 = B->get_transform().basis.xform(this->m_rbBFrame.basis.get_axis(1)); + Quat rotationArc = Quat(b2Axis1, b1Axis1); Vector3 TwistRef = rotationArc.xform(b2Axis2); - real_t twist = atan2fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) ); + real_t twist = atan2fast(TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2)); real_t lockedFreeFactor = (m_twistSpan > real_t(0.05f)) ? m_limitSoftness : real_t(0.); - if (twist <= -m_twistSpan*lockedFreeFactor) - { + if (twist <= -m_twistSpan * lockedFreeFactor) { m_twistCorrection = -(twist + m_twistSpan); m_solveTwistLimit = true; @@ -218,28 +202,24 @@ bool ConeTwistJointSW::setup(real_t p_step) { m_twistAxis *= -1.0f; m_kTwist = real_t(1.) / (A->compute_angular_impulse_denominator(m_twistAxis) + - B->compute_angular_impulse_denominator(m_twistAxis)); - - } else - if (twist > m_twistSpan*lockedFreeFactor) - { - m_twistCorrection = (twist - m_twistSpan); - m_solveTwistLimit = true; + B->compute_angular_impulse_denominator(m_twistAxis)); - m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f; - m_twistAxis.normalize(); + } else if (twist > m_twistSpan * lockedFreeFactor) { + m_twistCorrection = (twist - m_twistSpan); + m_solveTwistLimit = true; - m_kTwist = real_t(1.) / (A->compute_angular_impulse_denominator(m_twistAxis) + - B->compute_angular_impulse_denominator(m_twistAxis)); + m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f; + m_twistAxis.normalize(); - } + m_kTwist = real_t(1.) / (A->compute_angular_impulse_denominator(m_twistAxis) + + B->compute_angular_impulse_denominator(m_twistAxis)); + } } return true; } -void ConeTwistJointSW::solve(real_t timeStep) -{ +void ConeTwistJointSW::solve(real_t timeStep) { Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin); Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin); @@ -247,8 +227,7 @@ void ConeTwistJointSW::solve(real_t timeStep) real_t tau = real_t(0.3); //linear part - if (!m_angularOnly) - { + if (!m_angularOnly) { Vector3 rel_pos1 = pivotAInW - A->get_transform().origin; Vector3 rel_pos2 = pivotBInW - B->get_transform().origin; @@ -256,16 +235,15 @@ void ConeTwistJointSW::solve(real_t timeStep) Vector3 vel2 = B->get_velocity_in_local_point(rel_pos2); Vector3 vel = vel1 - vel2; - for (int i=0;i<3;i++) - { - const Vector3& normal = m_jac[i].m_linearJointAxis; + for (int i = 0; i < 3; i++) { + const Vector3 &normal = m_jac[i].m_linearJointAxis; real_t jacDiagABInv = real_t(1.) / m_jac[i].getDiagonal(); real_t rel_vel; rel_vel = normal.dot(vel); //positional error (zeroth order error) real_t depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal - real_t impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv; + real_t impulse = depth * tau / timeStep * jacDiagABInv - rel_vel * jacDiagABInv; m_appliedImpulse += impulse; Vector3 impulse_vector = normal * impulse; A->apply_impulse(pivotAInW - A->get_transform().origin, impulse_vector); @@ -275,79 +253,73 @@ void ConeTwistJointSW::solve(real_t timeStep) { ///solve angular part - const Vector3& angVelA = A->get_angular_velocity(); - const Vector3& angVelB = B->get_angular_velocity(); + const Vector3 &angVelA = A->get_angular_velocity(); + const Vector3 &angVelB = B->get_angular_velocity(); // solve swing limit - if (m_solveSwingLimit) - { - real_t amplitude = ((angVelB - angVelA).dot( m_swingAxis )*m_relaxationFactor*m_relaxationFactor + m_swingCorrection*(real_t(1.)/timeStep)*m_biasFactor); + if (m_solveSwingLimit) { + real_t amplitude = ((angVelB - angVelA).dot(m_swingAxis) * m_relaxationFactor * m_relaxationFactor + m_swingCorrection * (real_t(1.) / timeStep) * m_biasFactor); real_t impulseMag = amplitude * m_kSwing; // Clamp the accumulated impulse real_t temp = m_accSwingLimitImpulse; - m_accSwingLimitImpulse = MAX(m_accSwingLimitImpulse + impulseMag, real_t(0.0) ); + m_accSwingLimitImpulse = MAX(m_accSwingLimitImpulse + impulseMag, real_t(0.0)); impulseMag = m_accSwingLimitImpulse - temp; Vector3 impulse = m_swingAxis * impulseMag; A->apply_torque_impulse(impulse); B->apply_torque_impulse(-impulse); - } // solve twist limit - if (m_solveTwistLimit) - { - real_t amplitude = ((angVelB - angVelA).dot( m_twistAxis )*m_relaxationFactor*m_relaxationFactor + m_twistCorrection*(real_t(1.)/timeStep)*m_biasFactor ); + if (m_solveTwistLimit) { + real_t amplitude = ((angVelB - angVelA).dot(m_twistAxis) * m_relaxationFactor * m_relaxationFactor + m_twistCorrection * (real_t(1.) / timeStep) * m_biasFactor); real_t impulseMag = amplitude * m_kTwist; // Clamp the accumulated impulse real_t temp = m_accTwistLimitImpulse; - m_accTwistLimitImpulse = MAX(m_accTwistLimitImpulse + impulseMag, real_t(0.0) ); + m_accTwistLimitImpulse = MAX(m_accTwistLimitImpulse + impulseMag, real_t(0.0)); impulseMag = m_accTwistLimitImpulse - temp; Vector3 impulse = m_twistAxis * impulseMag; A->apply_torque_impulse(impulse); B->apply_torque_impulse(-impulse); - } - } - } void ConeTwistJointSW::set_param(PhysicsServer::ConeTwistJointParam p_param, real_t p_value) { - switch(p_param) { + switch (p_param) { case PhysicsServer::CONE_TWIST_JOINT_SWING_SPAN: { - m_swingSpan1=p_value; - m_swingSpan2=p_value; + m_swingSpan1 = p_value; + m_swingSpan2 = p_value; } break; case PhysicsServer::CONE_TWIST_JOINT_TWIST_SPAN: { - m_twistSpan=p_value; + m_twistSpan = p_value; } break; case PhysicsServer::CONE_TWIST_JOINT_BIAS: { - m_biasFactor=p_value; + m_biasFactor = p_value; } break; case PhysicsServer::CONE_TWIST_JOINT_SOFTNESS: { - m_limitSoftness=p_value; + m_limitSoftness = p_value; } break; case PhysicsServer::CONE_TWIST_JOINT_RELAXATION: { - m_relaxationFactor=p_value; + m_relaxationFactor = p_value; } break; } } -real_t ConeTwistJointSW::get_param(PhysicsServer::ConeTwistJointParam p_param) const{ +real_t ConeTwistJointSW::get_param(PhysicsServer::ConeTwistJointParam p_param) const { - switch(p_param) { + switch (p_param) { case PhysicsServer::CONE_TWIST_JOINT_SWING_SPAN: { return m_swingSpan1; diff --git a/servers/physics/joints/cone_twist_joint_sw.h b/servers/physics/joints/cone_twist_joint_sw.h index eb7a5cd1b..c122c2225 100644 --- a/servers/physics/joints/cone_twist_joint_sw.h +++ b/servers/physics/joints/cone_twist_joint_sw.h @@ -51,14 +51,11 @@ Written by: Marcus Hennix #ifndef CONE_TWIST_JOINT_SW_H #define CONE_TWIST_JOINT_SW_H -#include "servers/physics/joints_sw.h" #include "servers/physics/joints/jacobian_entry_sw.h" - - +#include "servers/physics/joints_sw.h" ///ConeTwistJointSW can be used to simulate ragdoll joints (upper arm, leg etc) -class ConeTwistJointSW : public JointSW -{ +class ConeTwistJointSW : public JointSW { #ifdef IN_PARALLELL_SOLVER public: #endif @@ -72,86 +69,73 @@ public: BodySW *_arr[2]; }; - JacobianEntrySW m_jac[3]; //3 orthogonal linear constraints - + JacobianEntrySW m_jac[3]; //3 orthogonal linear constraints real_t m_appliedImpulse; Transform m_rbAFrame; Transform m_rbBFrame; - real_t m_limitSoftness; - real_t m_biasFactor; - real_t m_relaxationFactor; - - real_t m_swingSpan1; - real_t m_swingSpan2; - real_t m_twistSpan; + real_t m_limitSoftness; + real_t m_biasFactor; + real_t m_relaxationFactor; - Vector3 m_swingAxis; - Vector3 m_twistAxis; + real_t m_swingSpan1; + real_t m_swingSpan2; + real_t m_twistSpan; - real_t m_kSwing; - real_t m_kTwist; + Vector3 m_swingAxis; + Vector3 m_twistAxis; - real_t m_twistLimitSign; - real_t m_swingCorrection; - real_t m_twistCorrection; + real_t m_kSwing; + real_t m_kTwist; - real_t m_accSwingLimitImpulse; - real_t m_accTwistLimitImpulse; + real_t m_twistLimitSign; + real_t m_swingCorrection; + real_t m_twistCorrection; - bool m_angularOnly; - bool m_solveTwistLimit; - bool m_solveSwingLimit; + real_t m_accSwingLimitImpulse; + real_t m_accTwistLimitImpulse; + bool m_angularOnly; + bool m_solveTwistLimit; + bool m_solveSwingLimit; public: - virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_CONE_TWIST; } virtual bool setup(real_t p_step); virtual void solve(real_t p_step); - ConeTwistJointSW(BodySW* rbA,BodySW* rbB,const Transform& rbAFrame, const Transform& rbBFrame); - + ConeTwistJointSW(BodySW *rbA, BodySW *rbB, const Transform &rbAFrame, const Transform &rbBFrame); - void setAngularOnly(bool angularOnly) - { + void setAngularOnly(bool angularOnly) { m_angularOnly = angularOnly; } - void setLimit(real_t _swingSpan1,real_t _swingSpan2,real_t _twistSpan, real_t _softness = 0.8f, real_t _biasFactor = 0.3f, real_t _relaxationFactor = 1.0f) - { + void setLimit(real_t _swingSpan1, real_t _swingSpan2, real_t _twistSpan, real_t _softness = 0.8f, real_t _biasFactor = 0.3f, real_t _relaxationFactor = 1.0f) { m_swingSpan1 = _swingSpan1; m_swingSpan2 = _swingSpan2; - m_twistSpan = _twistSpan; + m_twistSpan = _twistSpan; - m_limitSoftness = _softness; + m_limitSoftness = _softness; m_biasFactor = _biasFactor; m_relaxationFactor = _relaxationFactor; } - inline int getSolveTwistLimit() - { + inline int getSolveTwistLimit() { return m_solveTwistLimit; } - inline int getSolveSwingLimit() - { + inline int getSolveSwingLimit() { return m_solveTwistLimit; } - inline real_t getTwistLimitSign() - { + inline real_t getTwistLimitSign() { return m_twistLimitSign; } void set_param(PhysicsServer::ConeTwistJointParam p_param, real_t p_value); real_t get_param(PhysicsServer::ConeTwistJointParam p_param) const; - - }; - - #endif // CONE_TWIST_JOINT_SW_H diff --git a/servers/physics/joints/generic_6dof_joint_sw.cpp b/servers/physics/joints/generic_6dof_joint_sw.cpp index bd07f3122..1e07bc73f 100644 --- a/servers/physics/joints/generic_6dof_joint_sw.cpp +++ b/servers/physics/joints/generic_6dof_joint_sw.cpp @@ -34,118 +34,90 @@ See corresponding header file for licensing info. #include "generic_6dof_joint_sw.h" - - #define GENERIC_D6_DISABLE_WARMSTARTING 1 - //////////////////////////// G6DOFRotationalLimitMotorSW //////////////////////////////////// - -int G6DOFRotationalLimitMotorSW::testLimitValue(real_t test_value) -{ - if(m_loLimit>m_hiLimit) - { - m_currentLimit = 0;//Free from violation +int G6DOFRotationalLimitMotorSW::testLimitValue(real_t test_value) { + if (m_loLimit > m_hiLimit) { + m_currentLimit = 0; //Free from violation return 0; } - if (test_value < m_loLimit) - { - m_currentLimit = 1;//low limit violation - m_currentLimitError = test_value - m_loLimit; + if (test_value < m_loLimit) { + m_currentLimit = 1; //low limit violation + m_currentLimitError = test_value - m_loLimit; return 1; - } - else if (test_value> m_hiLimit) - { - m_currentLimit = 2;//High limit violation + } else if (test_value > m_hiLimit) { + m_currentLimit = 2; //High limit violation m_currentLimitError = test_value - m_hiLimit; return 2; }; - m_currentLimit = 0;//Free from violation + m_currentLimit = 0; //Free from violation return 0; - } - real_t G6DOFRotationalLimitMotorSW::solveAngularLimits( - real_t timeStep,Vector3& axis,real_t jacDiagABInv, - BodySW * body0, BodySW * body1) -{ - if (needApplyTorques()==false) return 0.0f; + real_t timeStep, Vector3 &axis, real_t jacDiagABInv, + BodySW *body0, BodySW *body1) { + if (needApplyTorques() == false) return 0.0f; - real_t target_velocity = m_targetVelocity; - real_t maxMotorForce = m_maxMotorForce; + real_t target_velocity = m_targetVelocity; + real_t maxMotorForce = m_maxMotorForce; //current error correction - if (m_currentLimit!=0) - { - target_velocity = -m_ERP*m_currentLimitError/(timeStep); - maxMotorForce = m_maxLimitForce; - } - - maxMotorForce *= timeStep; - - // current velocity difference - Vector3 vel_diff = body0->get_angular_velocity(); - if (body1) - { - vel_diff -= body1->get_angular_velocity(); - } + if (m_currentLimit != 0) { + target_velocity = -m_ERP * m_currentLimitError / (timeStep); + maxMotorForce = m_maxLimitForce; + } + maxMotorForce *= timeStep; + // current velocity difference + Vector3 vel_diff = body0->get_angular_velocity(); + if (body1) { + vel_diff -= body1->get_angular_velocity(); + } - real_t rel_vel = axis.dot(vel_diff); + real_t rel_vel = axis.dot(vel_diff); // correction velocity - real_t motor_relvel = m_limitSoftness*(target_velocity - m_damping*rel_vel); - - - if ( motor_relvel < CMP_EPSILON && motor_relvel > -CMP_EPSILON ) - { - return 0.0f;//no need for applying force - } + real_t motor_relvel = m_limitSoftness * (target_velocity - m_damping * rel_vel); + if (motor_relvel < CMP_EPSILON && motor_relvel > -CMP_EPSILON) { + return 0.0f; //no need for applying force + } // correction impulse - real_t unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv; + real_t unclippedMotorImpulse = (1 + m_bounce) * motor_relvel * jacDiagABInv; // clip correction impulse - real_t clippedMotorImpulse; - - ///@todo: should clip against accumulated impulse - if (unclippedMotorImpulse>0.0f) - { - clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse; - } - else - { - clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse; - } + real_t clippedMotorImpulse; + ///@todo: should clip against accumulated impulse + if (unclippedMotorImpulse > 0.0f) { + clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce ? maxMotorForce : unclippedMotorImpulse; + } else { + clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce : unclippedMotorImpulse; + } // sort with accumulated impulses - real_t lo = real_t(-1e30); - real_t hi = real_t(1e30); - - real_t oldaccumImpulse = m_accumulatedImpulse; - real_t sum = oldaccumImpulse + clippedMotorImpulse; - m_accumulatedImpulse = sum > hi ? real_t(0.) : sum < lo ? real_t(0.) : sum; - - clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse; + real_t lo = real_t(-1e30); + real_t hi = real_t(1e30); + real_t oldaccumImpulse = m_accumulatedImpulse; + real_t sum = oldaccumImpulse + clippedMotorImpulse; + m_accumulatedImpulse = sum > hi ? real_t(0.) : sum < lo ? real_t(0.) : sum; + clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse; - Vector3 motorImp = clippedMotorImpulse * axis; - - - body0->apply_torque_impulse(motorImp); - if (body1) body1->apply_torque_impulse(-motorImp); - - return clippedMotorImpulse; + Vector3 motorImp = clippedMotorImpulse * axis; + body0->apply_torque_impulse(motorImp); + if (body1) body1->apply_torque_impulse(-motorImp); + return clippedMotorImpulse; } //////////////////////////// End G6DOFRotationalLimitMotorSW //////////////////////////////////// @@ -153,120 +125,96 @@ real_t G6DOFRotationalLimitMotorSW::solveAngularLimits( //////////////////////////// G6DOFTranslationalLimitMotorSW //////////////////////////////////// real_t G6DOFTranslationalLimitMotorSW::solveLinearAxis( real_t timeStep, - real_t jacDiagABInv, - BodySW* body1,const Vector3 &pointInA, - BodySW* body2,const Vector3 &pointInB, - int limit_index, - const Vector3 & axis_normal_on_a, - const Vector3 & anchorPos) -{ - -///find relative velocity -// Vector3 rel_pos1 = pointInA - body1->get_transform().origin; -// Vector3 rel_pos2 = pointInB - body2->get_transform().origin; - Vector3 rel_pos1 = anchorPos - body1->get_transform().origin; - Vector3 rel_pos2 = anchorPos - body2->get_transform().origin; + real_t jacDiagABInv, + BodySW *body1, const Vector3 &pointInA, + BodySW *body2, const Vector3 &pointInB, + int limit_index, + const Vector3 &axis_normal_on_a, + const Vector3 &anchorPos) { - Vector3 vel1 = body1->get_velocity_in_local_point(rel_pos1); - Vector3 vel2 = body2->get_velocity_in_local_point(rel_pos2); - Vector3 vel = vel1 - vel2; + ///find relative velocity + // Vector3 rel_pos1 = pointInA - body1->get_transform().origin; + // Vector3 rel_pos2 = pointInB - body2->get_transform().origin; + Vector3 rel_pos1 = anchorPos - body1->get_transform().origin; + Vector3 rel_pos2 = anchorPos - body2->get_transform().origin; - real_t rel_vel = axis_normal_on_a.dot(vel); + Vector3 vel1 = body1->get_velocity_in_local_point(rel_pos1); + Vector3 vel2 = body2->get_velocity_in_local_point(rel_pos2); + Vector3 vel = vel1 - vel2; + real_t rel_vel = axis_normal_on_a.dot(vel); + /// apply displacement correction -/// apply displacement correction + //positional error (zeroth order error) + real_t depth = -(pointInA - pointInB).dot(axis_normal_on_a); + real_t lo = real_t(-1e30); + real_t hi = real_t(1e30); -//positional error (zeroth order error) - real_t depth = -(pointInA - pointInB).dot(axis_normal_on_a); - real_t lo = real_t(-1e30); - real_t hi = real_t(1e30); + real_t minLimit = m_lowerLimit[limit_index]; + real_t maxLimit = m_upperLimit[limit_index]; - real_t minLimit = m_lowerLimit[limit_index]; - real_t maxLimit = m_upperLimit[limit_index]; - - //handle the limits - if (minLimit < maxLimit) - { - { - if (depth > maxLimit) - { - depth -= maxLimit; - lo = real_t(0.); - - } - else - { - if (depth < minLimit) - { - depth -= minLimit; - hi = real_t(0.); - } - else + //handle the limits + if (minLimit < maxLimit) { { - return 0.0f; + if (depth > maxLimit) { + depth -= maxLimit; + lo = real_t(0.); + + } else { + if (depth < minLimit) { + depth -= minLimit; + hi = real_t(0.); + } else { + return 0.0f; + } + } } - } } - } - - real_t normalImpulse= m_limitSoftness[limit_index]*(m_restitution[limit_index]*depth/timeStep - m_damping[limit_index]*rel_vel) * jacDiagABInv; - + real_t normalImpulse = m_limitSoftness[limit_index] * (m_restitution[limit_index] * depth / timeStep - m_damping[limit_index] * rel_vel) * jacDiagABInv; + real_t oldNormalImpulse = m_accumulatedImpulse[limit_index]; + real_t sum = oldNormalImpulse + normalImpulse; + m_accumulatedImpulse[limit_index] = sum > hi ? real_t(0.) : sum < lo ? real_t(0.) : sum; + normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse; - real_t oldNormalImpulse = m_accumulatedImpulse[limit_index]; - real_t sum = oldNormalImpulse + normalImpulse; - m_accumulatedImpulse[limit_index] = sum > hi ? real_t(0.) : sum < lo ? real_t(0.) : sum; - normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse; - - Vector3 impulse_vector = axis_normal_on_a * normalImpulse; - body1->apply_impulse( rel_pos1, impulse_vector); - body2->apply_impulse( rel_pos2, -impulse_vector); - return normalImpulse; + Vector3 impulse_vector = axis_normal_on_a * normalImpulse; + body1->apply_impulse(rel_pos1, impulse_vector); + body2->apply_impulse(rel_pos2, -impulse_vector); + return normalImpulse; } //////////////////////////// G6DOFTranslationalLimitMotorSW //////////////////////////////////// - -Generic6DOFJointSW::Generic6DOFJointSW(BodySW* rbA, BodySW* rbB, const Transform& frameInA, const Transform& frameInB, bool useLinearReferenceFrameA) - : JointSW(_arr,2) - , m_frameInA(frameInA) - , m_frameInB(frameInB), - m_useLinearReferenceFrameA(useLinearReferenceFrameA) -{ - A=rbA; - B=rbB; - A->add_constraint(this,0); - B->add_constraint(this,1); +Generic6DOFJointSW::Generic6DOFJointSW(BodySW *rbA, BodySW *rbB, const Transform &frameInA, const Transform &frameInB, bool useLinearReferenceFrameA) + : JointSW(_arr, 2), m_frameInA(frameInA), m_frameInB(frameInB), + m_useLinearReferenceFrameA(useLinearReferenceFrameA) { + A = rbA; + B = rbB; + A->add_constraint(this, 0); + B->add_constraint(this, 1); } - - - - -void Generic6DOFJointSW::calculateAngleInfo() -{ - Basis relative_frame = m_calculatedTransformA.basis.inverse()*m_calculatedTransformB.basis; +void Generic6DOFJointSW::calculateAngleInfo() { + Basis relative_frame = m_calculatedTransformA.basis.inverse() * m_calculatedTransformB.basis; m_calculatedAxisAngleDiff = relative_frame.get_euler(); - - // in euler angle mode we do not actually constrain the angular velocity - // along the axes axis[0] and axis[2] (although we do use axis[1]) : - // - // to get constrain w2-w1 along ...not - // ------ --------------------- ------ - // d(angle[0])/dt = 0 ax[1] x ax[2] ax[0] - // d(angle[1])/dt = 0 ax[1] - // d(angle[2])/dt = 0 ax[0] x ax[1] ax[2] - // - // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0. - // to prove the result for angle[0], write the expression for angle[0] from - // GetInfo1 then take the derivative. to prove this for angle[2] it is - // easier to take the euler rate expression for d(angle[2])/dt with respect - // to the components of w and set that to 0. + // along the axes axis[0] and axis[2] (although we do use axis[1]) : + // + // to get constrain w2-w1 along ...not + // ------ --------------------- ------ + // d(angle[0])/dt = 0 ax[1] x ax[2] ax[0] + // d(angle[1])/dt = 0 ax[1] + // d(angle[2])/dt = 0 ax[0] x ax[1] ax[2] + // + // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0. + // to prove the result for angle[0], write the expression for angle[0] from + // GetInfo1 then take the derivative. to prove this for angle[2] it is + // easier to take the euler rate expression for d(angle[2])/dt with respect + // to the components of w and set that to 0. Vector3 axis0 = m_calculatedTransformB.basis.get_axis(0); Vector3 axis2 = m_calculatedTransformA.basis.get_axis(2); @@ -275,7 +223,6 @@ void Generic6DOFJointSW::calculateAngleInfo() m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2); m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]); - /* if(m_debugDrawer) { @@ -288,280 +235,250 @@ void Generic6DOFJointSW::calculateAngleInfo() m_debugDrawer->reportErrorWarning(buff); } */ - } -void Generic6DOFJointSW::calculateTransforms() -{ - m_calculatedTransformA = A->get_transform() * m_frameInA; - m_calculatedTransformB = B->get_transform() * m_frameInB; +void Generic6DOFJointSW::calculateTransforms() { + m_calculatedTransformA = A->get_transform() * m_frameInA; + m_calculatedTransformB = B->get_transform() * m_frameInB; - calculateAngleInfo(); + calculateAngleInfo(); } - void Generic6DOFJointSW::buildLinearJacobian( - JacobianEntrySW & jacLinear,const Vector3 & normalWorld, - const Vector3 & pivotAInW,const Vector3 & pivotBInW) -{ - memnew_placement(&jacLinear, JacobianEntrySW( - A->get_principal_inertia_axes().transposed(), - B->get_principal_inertia_axes().transposed(), - pivotAInW - A->get_transform().origin - A->get_center_of_mass(), - pivotBInW - B->get_transform().origin - B->get_center_of_mass(), - normalWorld, - A->get_inv_inertia(), - A->get_inv_mass(), - B->get_inv_inertia(), - B->get_inv_mass())); - + JacobianEntrySW &jacLinear, const Vector3 &normalWorld, + const Vector3 &pivotAInW, const Vector3 &pivotBInW) { + memnew_placement(&jacLinear, JacobianEntrySW( + A->get_principal_inertia_axes().transposed(), + B->get_principal_inertia_axes().transposed(), + pivotAInW - A->get_transform().origin - A->get_center_of_mass(), + pivotBInW - B->get_transform().origin - B->get_center_of_mass(), + normalWorld, + A->get_inv_inertia(), + A->get_inv_mass(), + B->get_inv_inertia(), + B->get_inv_mass())); } void Generic6DOFJointSW::buildAngularJacobian( - JacobianEntrySW & jacAngular,const Vector3 & jointAxisW) -{ - memnew_placement(&jacAngular, JacobianEntrySW(jointAxisW, - A->get_principal_inertia_axes().transposed(), - B->get_principal_inertia_axes().transposed(), - A->get_inv_inertia(), - B->get_inv_inertia())); - + JacobianEntrySW &jacAngular, const Vector3 &jointAxisW) { + memnew_placement(&jacAngular, JacobianEntrySW(jointAxisW, + A->get_principal_inertia_axes().transposed(), + B->get_principal_inertia_axes().transposed(), + A->get_inv_inertia(), + B->get_inv_inertia())); } -bool Generic6DOFJointSW::testAngularLimitMotor(int axis_index) -{ - real_t angle = m_calculatedAxisAngleDiff[axis_index]; +bool Generic6DOFJointSW::testAngularLimitMotor(int axis_index) { + real_t angle = m_calculatedAxisAngleDiff[axis_index]; - //test limits - m_angularLimits[axis_index].testLimitValue(angle); - return m_angularLimits[axis_index].needApplyTorques(); + //test limits + m_angularLimits[axis_index].testLimitValue(angle); + return m_angularLimits[axis_index].needApplyTorques(); } bool Generic6DOFJointSW::setup(real_t p_step) { // Clear accumulated impulses for the next simulation step - m_linearLimits.m_accumulatedImpulse=Vector3(real_t(0.), real_t(0.), real_t(0.)); - int i; - for(i = 0; i < 3; i++) - { - m_angularLimits[i].m_accumulatedImpulse = real_t(0.); - } - //calculates transform - calculateTransforms(); + m_linearLimits.m_accumulatedImpulse = Vector3(real_t(0.), real_t(0.), real_t(0.)); + int i; + for (i = 0; i < 3; i++) { + m_angularLimits[i].m_accumulatedImpulse = real_t(0.); + } + //calculates transform + calculateTransforms(); -// const Vector3& pivotAInW = m_calculatedTransformA.origin; -// const Vector3& pivotBInW = m_calculatedTransformB.origin; + // const Vector3& pivotAInW = m_calculatedTransformA.origin; + // const Vector3& pivotBInW = m_calculatedTransformB.origin; calcAnchorPos(); Vector3 pivotAInW = m_AnchorPos; Vector3 pivotBInW = m_AnchorPos; -// not used here -// Vector3 rel_pos1 = pivotAInW - A->get_transform().origin; -// Vector3 rel_pos2 = pivotBInW - B->get_transform().origin; + // not used here + // Vector3 rel_pos1 = pivotAInW - A->get_transform().origin; + // Vector3 rel_pos2 = pivotBInW - B->get_transform().origin; - Vector3 normalWorld; - //linear part - for (i=0;i<3;i++) - { - if (m_linearLimits.enable_limit[i] && m_linearLimits.isLimited(i)) - { + Vector3 normalWorld; + //linear part + for (i = 0; i < 3; i++) { + if (m_linearLimits.enable_limit[i] && m_linearLimits.isLimited(i)) { if (m_useLinearReferenceFrameA) - normalWorld = m_calculatedTransformA.basis.get_axis(i); + normalWorld = m_calculatedTransformA.basis.get_axis(i); else - normalWorld = m_calculatedTransformB.basis.get_axis(i); - - buildLinearJacobian( - m_jacLinear[i],normalWorld , - pivotAInW,pivotBInW); + normalWorld = m_calculatedTransformB.basis.get_axis(i); + buildLinearJacobian( + m_jacLinear[i], normalWorld, + pivotAInW, pivotBInW); + } } - } - // angular part - for (i=0;i<3;i++) - { - //calculates error angle - if (m_angularLimits[i].m_enableLimit && testAngularLimitMotor(i)) - { - normalWorld = this->getAxis(i); - // Create angular atom - buildAngularJacobian(m_jacAng[i],normalWorld); + // angular part + for (i = 0; i < 3; i++) { + //calculates error angle + if (m_angularLimits[i].m_enableLimit && testAngularLimitMotor(i)) { + normalWorld = this->getAxis(i); + // Create angular atom + buildAngularJacobian(m_jacAng[i], normalWorld); + } } - } return true; } +void Generic6DOFJointSW::solve(real_t timeStep) { + m_timeStep = timeStep; -void Generic6DOFJointSW::solve(real_t timeStep) -{ - m_timeStep = timeStep; - - //calculateTransforms(); + //calculateTransforms(); - int i; + int i; - // linear + // linear - Vector3 pointInA = m_calculatedTransformA.origin; + Vector3 pointInA = m_calculatedTransformA.origin; Vector3 pointInB = m_calculatedTransformB.origin; real_t jacDiagABInv; Vector3 linear_axis; - for (i=0;i<3;i++) - { - if (m_linearLimits.enable_limit[i] && m_linearLimits.isLimited(i)) - { - jacDiagABInv = real_t(1.) / m_jacLinear[i].getDiagonal(); + for (i = 0; i < 3; i++) { + if (m_linearLimits.enable_limit[i] && m_linearLimits.isLimited(i)) { + jacDiagABInv = real_t(1.) / m_jacLinear[i].getDiagonal(); if (m_useLinearReferenceFrameA) - linear_axis = m_calculatedTransformA.basis.get_axis(i); + linear_axis = m_calculatedTransformA.basis.get_axis(i); else - linear_axis = m_calculatedTransformB.basis.get_axis(i); - - m_linearLimits.solveLinearAxis( - m_timeStep, - jacDiagABInv, - A,pointInA, - B,pointInB, - i,linear_axis, m_AnchorPos); + linear_axis = m_calculatedTransformB.basis.get_axis(i); + m_linearLimits.solveLinearAxis( + m_timeStep, + jacDiagABInv, + A, pointInA, + B, pointInB, + i, linear_axis, m_AnchorPos); + } } - } - // angular - Vector3 angular_axis; - real_t angularJacDiagABInv; - for (i=0;i<3;i++) - { - if (m_angularLimits[i].m_enableLimit && m_angularLimits[i].needApplyTorques()) - { + // angular + Vector3 angular_axis; + real_t angularJacDiagABInv; + for (i = 0; i < 3; i++) { + if (m_angularLimits[i].m_enableLimit && m_angularLimits[i].needApplyTorques()) { // get axis angular_axis = getAxis(i); angularJacDiagABInv = real_t(1.) / m_jacAng[i].getDiagonal(); - m_angularLimits[i].solveAngularLimits(m_timeStep,angular_axis,angularJacDiagABInv, A,B); + m_angularLimits[i].solveAngularLimits(m_timeStep, angular_axis, angularJacDiagABInv, A, B); + } } - } } -void Generic6DOFJointSW::updateRHS(real_t timeStep) -{ - (void)timeStep; - +void Generic6DOFJointSW::updateRHS(real_t timeStep) { + (void)timeStep; } -Vector3 Generic6DOFJointSW::getAxis(int axis_index) const -{ - return m_calculatedAxis[axis_index]; +Vector3 Generic6DOFJointSW::getAxis(int axis_index) const { + return m_calculatedAxis[axis_index]; } -real_t Generic6DOFJointSW::getAngle(int axis_index) const -{ - return m_calculatedAxisAngleDiff[axis_index]; +real_t Generic6DOFJointSW::getAngle(int axis_index) const { + return m_calculatedAxisAngleDiff[axis_index]; } -void Generic6DOFJointSW::calcAnchorPos(void) -{ +void Generic6DOFJointSW::calcAnchorPos(void) { real_t imA = A->get_inv_mass(); real_t imB = B->get_inv_mass(); real_t weight; - if(imB == real_t(0.0)) - { + if (imB == real_t(0.0)) { weight = real_t(1.0); - } - else - { + } else { weight = imA / (imA + imB); } - const Vector3& pA = m_calculatedTransformA.origin; - const Vector3& pB = m_calculatedTransformB.origin; + const Vector3 &pA = m_calculatedTransformA.origin; + const Vector3 &pB = m_calculatedTransformB.origin; m_AnchorPos = pA * weight + pB * (real_t(1.0) - weight); return; } // Generic6DOFJointSW::calcAnchorPos() +void Generic6DOFJointSW::set_param(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisParam p_param, real_t p_value) { -void Generic6DOFJointSW::set_param(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisParam p_param, real_t p_value) { - - ERR_FAIL_INDEX(p_axis,3); - switch(p_param) { + ERR_FAIL_INDEX(p_axis, 3); + switch (p_param) { case PhysicsServer::G6DOF_JOINT_LINEAR_LOWER_LIMIT: { - m_linearLimits.m_lowerLimit[p_axis]=p_value; + m_linearLimits.m_lowerLimit[p_axis] = p_value; } break; case PhysicsServer::G6DOF_JOINT_LINEAR_UPPER_LIMIT: { - m_linearLimits.m_upperLimit[p_axis]=p_value; + m_linearLimits.m_upperLimit[p_axis] = p_value; } break; case PhysicsServer::G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS: { - m_linearLimits.m_limitSoftness[p_axis]=p_value; + m_linearLimits.m_limitSoftness[p_axis] = p_value; } break; case PhysicsServer::G6DOF_JOINT_LINEAR_RESTITUTION: { - m_linearLimits.m_restitution[p_axis]=p_value; + m_linearLimits.m_restitution[p_axis] = p_value; } break; case PhysicsServer::G6DOF_JOINT_LINEAR_DAMPING: { - m_linearLimits.m_damping[p_axis]=p_value; + m_linearLimits.m_damping[p_axis] = p_value; } break; case PhysicsServer::G6DOF_JOINT_ANGULAR_LOWER_LIMIT: { - m_angularLimits[p_axis].m_loLimit=p_value; + m_angularLimits[p_axis].m_loLimit = p_value; } break; case PhysicsServer::G6DOF_JOINT_ANGULAR_UPPER_LIMIT: { - m_angularLimits[p_axis].m_hiLimit=p_value; + m_angularLimits[p_axis].m_hiLimit = p_value; } break; case PhysicsServer::G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS: { - m_angularLimits[p_axis].m_limitSoftness=p_value; + m_angularLimits[p_axis].m_limitSoftness = p_value; } break; case PhysicsServer::G6DOF_JOINT_ANGULAR_DAMPING: { - m_angularLimits[p_axis].m_damping=p_value; + m_angularLimits[p_axis].m_damping = p_value; } break; case PhysicsServer::G6DOF_JOINT_ANGULAR_RESTITUTION: { - m_angularLimits[p_axis].m_bounce=p_value; + m_angularLimits[p_axis].m_bounce = p_value; } break; case PhysicsServer::G6DOF_JOINT_ANGULAR_FORCE_LIMIT: { - m_angularLimits[p_axis].m_maxLimitForce=p_value; + m_angularLimits[p_axis].m_maxLimitForce = p_value; } break; case PhysicsServer::G6DOF_JOINT_ANGULAR_ERP: { - m_angularLimits[p_axis].m_ERP=p_value; + m_angularLimits[p_axis].m_ERP = p_value; } break; case PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY: { - m_angularLimits[p_axis].m_targetVelocity=p_value; + m_angularLimits[p_axis].m_targetVelocity = p_value; } break; case PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT: { - m_angularLimits[p_axis].m_maxLimitForce=p_value; + m_angularLimits[p_axis].m_maxLimitForce = p_value; } break; } } -real_t Generic6DOFJointSW::get_param(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisParam p_param) const{ - ERR_FAIL_INDEX_V(p_axis,3,0); - switch(p_param) { +real_t Generic6DOFJointSW::get_param(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisParam p_param) const { + ERR_FAIL_INDEX_V(p_axis, 3, 0); + switch (p_param) { case PhysicsServer::G6DOF_JOINT_LINEAR_LOWER_LIMIT: { return m_linearLimits.m_lowerLimit[p_axis]; @@ -635,31 +552,29 @@ real_t Generic6DOFJointSW::get_param(Vector3::Axis p_axis,PhysicsServer::G6DOFJo return 0; } -void Generic6DOFJointSW::set_flag(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisFlag p_flag, bool p_value){ +void Generic6DOFJointSW::set_flag(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisFlag p_flag, bool p_value) { - ERR_FAIL_INDEX(p_axis,3); + ERR_FAIL_INDEX(p_axis, 3); - switch(p_flag) { + switch (p_flag) { case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT: { - m_linearLimits.enable_limit[p_axis]=p_value; + m_linearLimits.enable_limit[p_axis] = p_value; } break; case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT: { - m_angularLimits[p_axis].m_enableLimit=p_value; + m_angularLimits[p_axis].m_enableLimit = p_value; } break; case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_MOTOR: { - m_angularLimits[p_axis].m_enableMotor=p_value; + m_angularLimits[p_axis].m_enableMotor = p_value; } break; } - - } -bool Generic6DOFJointSW::get_flag(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisFlag p_flag) const{ +bool Generic6DOFJointSW::get_flag(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisFlag p_flag) const { - ERR_FAIL_INDEX_V(p_axis,3,0); - switch(p_flag) { + ERR_FAIL_INDEX_V(p_axis, 3, 0); + switch (p_flag) { case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT: { return m_linearLimits.enable_limit[p_axis]; diff --git a/servers/physics/joints/generic_6dof_joint_sw.h b/servers/physics/joints/generic_6dof_joint_sw.h index 207ae85a4..87245c6ff 100644 --- a/servers/physics/joints/generic_6dof_joint_sw.h +++ b/servers/physics/joints/generic_6dof_joint_sw.h @@ -34,9 +34,8 @@ Adapted to Godot from the Bullet library. #ifndef GENERIC_6DOF_JOINT_SW_H #define GENERIC_6DOF_JOINT_SW_H -#include "servers/physics/joints_sw.h" #include "servers/physics/joints/jacobian_entry_sw.h" - +#include "servers/physics/joints_sw.h" /* Bullet Continuous Collision Detection and Physics Library @@ -53,7 +52,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - /* 2007-09-09 Generic6DOFJointSW Refactored by Francisco Le?n @@ -61,80 +59,73 @@ email: projectileman@yahoo.com http://gimpact.sf.net */ - //! Rotation Limit structure for generic joints class G6DOFRotationalLimitMotorSW { public: - //! limit_parameters - //!@{ - real_t m_loLimit;//!< joint limit - real_t m_hiLimit;//!< joint limit - real_t m_targetVelocity;//!< target motor velocity - real_t m_maxMotorForce;//!< max force on motor - real_t m_maxLimitForce;//!< max force on limit - real_t m_damping;//!< Damping. - real_t m_limitSoftness;//! Relaxation factor - real_t m_ERP;//!< Error tolerance factor when joint is at limit - real_t m_bounce;//!< restitution factor - bool m_enableMotor; - bool m_enableLimit; + //! limit_parameters + //!@{ + real_t m_loLimit; //!< joint limit + real_t m_hiLimit; //!< joint limit + real_t m_targetVelocity; //!< target motor velocity + real_t m_maxMotorForce; //!< max force on motor + real_t m_maxLimitForce; //!< max force on limit + real_t m_damping; //!< Damping. + real_t m_limitSoftness; //! Relaxation factor + real_t m_ERP; //!< Error tolerance factor when joint is at limit + real_t m_bounce; //!< restitution factor + bool m_enableMotor; + bool m_enableLimit; - //!@} - - //! temp_variables - //!@{ - real_t m_currentLimitError;//! How much is violated this limit - int m_currentLimit;//!< 0=free, 1=at lo limit, 2=at hi limit - real_t m_accumulatedImpulse; - //!@} - - G6DOFRotationalLimitMotorSW() - { - m_accumulatedImpulse = 0.f; - m_targetVelocity = 0; - m_maxMotorForce = 0.1f; - m_maxLimitForce = 300.0f; - m_loLimit = -1e30; - m_hiLimit = 1e30; - m_ERP = 0.5f; - m_bounce = 0.0f; - m_damping = 1.0f; - m_limitSoftness = 0.5f; - m_currentLimit = 0; - m_currentLimitError = 0; - m_enableMotor = false; - m_enableLimit=false; - } + //!@} - G6DOFRotationalLimitMotorSW(const G6DOFRotationalLimitMotorSW & limot) - { - m_targetVelocity = limot.m_targetVelocity; - m_maxMotorForce = limot.m_maxMotorForce; - m_limitSoftness = limot.m_limitSoftness; - m_loLimit = limot.m_loLimit; - m_hiLimit = limot.m_hiLimit; - m_ERP = limot.m_ERP; - m_bounce = limot.m_bounce; - m_currentLimit = limot.m_currentLimit; - m_currentLimitError = limot.m_currentLimitError; - m_enableMotor = limot.m_enableMotor; - } + //! temp_variables + //!@{ + real_t m_currentLimitError; //! How much is violated this limit + int m_currentLimit; //!< 0=free, 1=at lo limit, 2=at hi limit + real_t m_accumulatedImpulse; + //!@} + G6DOFRotationalLimitMotorSW() { + m_accumulatedImpulse = 0.f; + m_targetVelocity = 0; + m_maxMotorForce = 0.1f; + m_maxLimitForce = 300.0f; + m_loLimit = -1e30; + m_hiLimit = 1e30; + m_ERP = 0.5f; + m_bounce = 0.0f; + m_damping = 1.0f; + m_limitSoftness = 0.5f; + m_currentLimit = 0; + m_currentLimitError = 0; + m_enableMotor = false; + m_enableLimit = false; + } + G6DOFRotationalLimitMotorSW(const G6DOFRotationalLimitMotorSW &limot) { + m_targetVelocity = limot.m_targetVelocity; + m_maxMotorForce = limot.m_maxMotorForce; + m_limitSoftness = limot.m_limitSoftness; + m_loLimit = limot.m_loLimit; + m_hiLimit = limot.m_hiLimit; + m_ERP = limot.m_ERP; + m_bounce = limot.m_bounce; + m_currentLimit = limot.m_currentLimit; + m_currentLimitError = limot.m_currentLimitError; + m_enableMotor = limot.m_enableMotor; + } //! Is limited - bool isLimited() - { - if(m_loLimit>=m_hiLimit) return false; - return true; - } + bool isLimited() { + if (m_loLimit >= m_hiLimit) return false; + return true; + } //! Need apply correction - bool needApplyTorques() - { - if(m_currentLimit == 0 && m_enableMotor == false) return false; - return true; - } + bool needApplyTorques() { + if (m_currentLimit == 0 && m_enableMotor == false) return false; + return true; + } //! calculates error /*! @@ -143,84 +134,69 @@ public: int testLimitValue(real_t test_value); //! apply the correction impulses for two bodies - real_t solveAngularLimits(real_t timeStep,Vector3& axis, real_t jacDiagABInv,BodySW * body0, BodySW * body1); - - + real_t solveAngularLimits(real_t timeStep, Vector3 &axis, real_t jacDiagABInv, BodySW *body0, BodySW *body1); }; - - -class G6DOFTranslationalLimitMotorSW -{ +class G6DOFTranslationalLimitMotorSW { public: - Vector3 m_lowerLimit;//!< the constraint lower limits - Vector3 m_upperLimit;//!< the constraint upper limits - Vector3 m_accumulatedImpulse; - //! Linear_Limit_parameters - //!@{ - Vector3 m_limitSoftness;//!< Softness for linear limit - Vector3 m_damping;//!< Damping for linear limit - Vector3 m_restitution;//! Bounce parameter for linear limit - //!@} - bool enable_limit[3]; + Vector3 m_lowerLimit; //!< the constraint lower limits + Vector3 m_upperLimit; //!< the constraint upper limits + Vector3 m_accumulatedImpulse; + //! Linear_Limit_parameters + //!@{ + Vector3 m_limitSoftness; //!< Softness for linear limit + Vector3 m_damping; //!< Damping for linear limit + Vector3 m_restitution; //! Bounce parameter for linear limit + //!@} + bool enable_limit[3]; - G6DOFTranslationalLimitMotorSW() - { - m_lowerLimit=Vector3(0.f,0.f,0.f); - m_upperLimit=Vector3(0.f,0.f,0.f); - m_accumulatedImpulse=Vector3(0.f,0.f,0.f); + G6DOFTranslationalLimitMotorSW() { + m_lowerLimit = Vector3(0.f, 0.f, 0.f); + m_upperLimit = Vector3(0.f, 0.f, 0.f); + m_accumulatedImpulse = Vector3(0.f, 0.f, 0.f); - m_limitSoftness = Vector3(1,1,1)*0.7f; - m_damping = Vector3(1,1,1)*real_t(1.0f); - m_restitution = Vector3(1,1,1)*real_t(0.5f); + m_limitSoftness = Vector3(1, 1, 1) * 0.7f; + m_damping = Vector3(1, 1, 1) * real_t(1.0f); + m_restitution = Vector3(1, 1, 1) * real_t(0.5f); - enable_limit[0]=true; - enable_limit[1]=true; - enable_limit[2]=true; - } + enable_limit[0] = true; + enable_limit[1] = true; + enable_limit[2] = true; + } - G6DOFTranslationalLimitMotorSW(const G6DOFTranslationalLimitMotorSW & other ) - { - m_lowerLimit = other.m_lowerLimit; - m_upperLimit = other.m_upperLimit; - m_accumulatedImpulse = other.m_accumulatedImpulse; + G6DOFTranslationalLimitMotorSW(const G6DOFTranslationalLimitMotorSW &other) { + m_lowerLimit = other.m_lowerLimit; + m_upperLimit = other.m_upperLimit; + m_accumulatedImpulse = other.m_accumulatedImpulse; - m_limitSoftness = other.m_limitSoftness ; - m_damping = other.m_damping; - m_restitution = other.m_restitution; - } + m_limitSoftness = other.m_limitSoftness; + m_damping = other.m_damping; + m_restitution = other.m_restitution; + } - //! Test limit + //! Test limit /*! - free means upper < lower, - locked means upper == lower - limited means upper > lower - limitIndex: first 3 are linear, next 3 are angular */ - inline bool isLimited(int limitIndex) - { - return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]); - } - - - real_t solveLinearAxis( - real_t timeStep, - real_t jacDiagABInv, - BodySW* body1,const Vector3 &pointInA, - BodySW* body2,const Vector3 &pointInB, - int limit_index, - const Vector3 & axis_normal_on_a, - const Vector3 & anchorPos); - + inline bool isLimited(int limitIndex) { + return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]); + } + real_t solveLinearAxis( + real_t timeStep, + real_t jacDiagABInv, + BodySW *body1, const Vector3 &pointInA, + BodySW *body2, const Vector3 &pointInB, + int limit_index, + const Vector3 &axis_normal_on_a, + const Vector3 &anchorPos); }; - -class Generic6DOFJointSW : public JointSW -{ +class Generic6DOFJointSW : public JointSW { protected: - - union { struct { BodySW *A; @@ -231,195 +207,167 @@ protected: }; //! relative_frames - //!@{ - Transform m_frameInA;//!< the constraint space w.r.t body A - Transform m_frameInB;//!< the constraint space w.r.t body B - //!@} + //!@{ + Transform m_frameInA; //!< the constraint space w.r.t body A + Transform m_frameInB; //!< the constraint space w.r.t body B + //!@} - //! Jacobians - //!@{ - JacobianEntrySW m_jacLinear[3];//!< 3 orthogonal linear constraints - JacobianEntrySW m_jacAng[3];//!< 3 orthogonal angular constraints - //!@} + //! Jacobians + //!@{ + JacobianEntrySW m_jacLinear[3]; //!< 3 orthogonal linear constraints + JacobianEntrySW m_jacAng[3]; //!< 3 orthogonal angular constraints + //!@} //! Linear_Limit_parameters - //!@{ - G6DOFTranslationalLimitMotorSW m_linearLimits; - //!@} - - - //! hinge_parameters - //!@{ - G6DOFRotationalLimitMotorSW m_angularLimits[3]; + //!@{ + G6DOFTranslationalLimitMotorSW m_linearLimits; //!@} + //! hinge_parameters + //!@{ + G6DOFRotationalLimitMotorSW m_angularLimits[3]; + //!@} protected: - //! temporal variables - //!@{ - real_t m_timeStep; - Transform m_calculatedTransformA; - Transform m_calculatedTransformB; - Vector3 m_calculatedAxisAngleDiff; - Vector3 m_calculatedAxis[3]; + //! temporal variables + //!@{ + real_t m_timeStep; + Transform m_calculatedTransformA; + Transform m_calculatedTransformB; + Vector3 m_calculatedAxisAngleDiff; + Vector3 m_calculatedAxis[3]; Vector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes - bool m_useLinearReferenceFrameA; - - //!@} - - Generic6DOFJointSW& operator=(Generic6DOFJointSW& other) - { - ERR_PRINT("pito"); - (void) other; - return *this; - } - + bool m_useLinearReferenceFrameA; + //!@} - void buildLinearJacobian( - JacobianEntrySW & jacLinear,const Vector3 & normalWorld, - const Vector3 & pivotAInW,const Vector3 & pivotBInW); + Generic6DOFJointSW &operator=(Generic6DOFJointSW &other) { + ERR_PRINT("pito"); + (void)other; + return *this; + } - void buildAngularJacobian(JacobianEntrySW & jacAngular,const Vector3 & jointAxisW); + void buildLinearJacobian( + JacobianEntrySW &jacLinear, const Vector3 &normalWorld, + const Vector3 &pivotAInW, const Vector3 &pivotBInW); + void buildAngularJacobian(JacobianEntrySW &jacAngular, const Vector3 &jointAxisW); //! calcs the euler angles between the two bodies. - void calculateAngleInfo(); - - + void calculateAngleInfo(); public: - Generic6DOFJointSW(BodySW* rbA, BodySW* rbB, const Transform& frameInA, const Transform& frameInB ,bool useLinearReferenceFrameA); - - virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_6DOF; } + Generic6DOFJointSW(BodySW *rbA, BodySW *rbB, const Transform &frameInA, const Transform &frameInB, bool useLinearReferenceFrameA); - virtual bool setup(real_t p_step); - virtual void solve(real_t p_step); + virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_6DOF; } + virtual bool setup(real_t p_step); + virtual void solve(real_t p_step); //! Calcs global transform of the offsets /*! Calcs the global transform for the joint offset for body A an B, and also calcs the agle differences between the bodies. \sa Generic6DOFJointSW.getCalculatedTransformA , Generic6DOFJointSW.getCalculatedTransformB, Generic6DOFJointSW.calculateAngleInfo */ - void calculateTransforms(); + void calculateTransforms(); //! Gets the global transform of the offset for body A - /*! + /*! \sa Generic6DOFJointSW.getFrameOffsetA, Generic6DOFJointSW.getFrameOffsetB, Generic6DOFJointSW.calculateAngleInfo. */ - const Transform & getCalculatedTransformA() const - { - return m_calculatedTransformA; - } + const Transform &getCalculatedTransformA() const { + return m_calculatedTransformA; + } - //! Gets the global transform of the offset for body B - /*! + //! Gets the global transform of the offset for body B + /*! \sa Generic6DOFJointSW.getFrameOffsetA, Generic6DOFJointSW.getFrameOffsetB, Generic6DOFJointSW.calculateAngleInfo. */ - const Transform & getCalculatedTransformB() const - { - return m_calculatedTransformB; - } - - const Transform & getFrameOffsetA() const - { - return m_frameInA; - } - - const Transform & getFrameOffsetB() const - { - return m_frameInB; - } + const Transform &getCalculatedTransformB() const { + return m_calculatedTransformB; + } + const Transform &getFrameOffsetA() const { + return m_frameInA; + } - Transform & getFrameOffsetA() - { - return m_frameInA; - } + const Transform &getFrameOffsetB() const { + return m_frameInB; + } - Transform & getFrameOffsetB() - { - return m_frameInB; - } + Transform &getFrameOffsetA() { + return m_frameInA; + } + Transform &getFrameOffsetB() { + return m_frameInB; + } //! performs Jacobian calculation, and also calculates angle differences and axis - - void updateRHS(real_t timeStep); + void updateRHS(real_t timeStep); //! Get the rotation axis in global coordinates /*! \pre Generic6DOFJointSW.buildJacobian must be called previously. */ - Vector3 getAxis(int axis_index) const; + Vector3 getAxis(int axis_index) const; - //! Get the relative Euler angle - /*! + //! Get the relative Euler angle + /*! \pre Generic6DOFJointSW.buildJacobian must be called previously. */ - real_t getAngle(int axis_index) const; + real_t getAngle(int axis_index) const; //! Test angular limit. /*! Calculates angular correction and returns true if limit needs to be corrected. \pre Generic6DOFJointSW.buildJacobian must be called previously. */ - bool testAngularLimitMotor(int axis_index); + bool testAngularLimitMotor(int axis_index); - void setLinearLowerLimit(const Vector3& linearLower) - { - m_linearLimits.m_lowerLimit = linearLower; - } + void setLinearLowerLimit(const Vector3 &linearLower) { + m_linearLimits.m_lowerLimit = linearLower; + } - void setLinearUpperLimit(const Vector3& linearUpper) - { - m_linearLimits.m_upperLimit = linearUpper; - } + void setLinearUpperLimit(const Vector3 &linearUpper) { + m_linearLimits.m_upperLimit = linearUpper; + } - void setAngularLowerLimit(const Vector3& angularLower) - { - m_angularLimits[0].m_loLimit = angularLower.x; - m_angularLimits[1].m_loLimit = angularLower.y; - m_angularLimits[2].m_loLimit = angularLower.z; - } + void setAngularLowerLimit(const Vector3 &angularLower) { + m_angularLimits[0].m_loLimit = angularLower.x; + m_angularLimits[1].m_loLimit = angularLower.y; + m_angularLimits[2].m_loLimit = angularLower.z; + } - void setAngularUpperLimit(const Vector3& angularUpper) - { - m_angularLimits[0].m_hiLimit = angularUpper.x; - m_angularLimits[1].m_hiLimit = angularUpper.y; - m_angularLimits[2].m_hiLimit = angularUpper.z; - } + void setAngularUpperLimit(const Vector3 &angularUpper) { + m_angularLimits[0].m_hiLimit = angularUpper.x; + m_angularLimits[1].m_hiLimit = angularUpper.y; + m_angularLimits[2].m_hiLimit = angularUpper.z; + } //! Retrieves the angular limit informacion - G6DOFRotationalLimitMotorSW * getRotationalLimitMotor(int index) - { - return &m_angularLimits[index]; - } - - //! Retrieves the limit informacion - G6DOFTranslationalLimitMotorSW * getTranslationalLimitMotor() - { - return &m_linearLimits; - } + G6DOFRotationalLimitMotorSW *getRotationalLimitMotor(int index) { + return &m_angularLimits[index]; + } - //first 3 are linear, next 3 are angular - void setLimit(int axis, real_t lo, real_t hi) - { - if(axis<3) - { - m_linearLimits.m_lowerLimit[axis] = lo; - m_linearLimits.m_upperLimit[axis] = hi; + //! Retrieves the limit informacion + G6DOFTranslationalLimitMotorSW *getTranslationalLimitMotor() { + return &m_linearLimits; } - else - { - m_angularLimits[axis-3].m_loLimit = lo; - m_angularLimits[axis-3].m_hiLimit = hi; + + //first 3 are linear, next 3 are angular + void setLimit(int axis, real_t lo, real_t hi) { + if (axis < 3) { + m_linearLimits.m_lowerLimit[axis] = lo; + m_linearLimits.m_upperLimit[axis] = hi; + } else { + m_angularLimits[axis - 3].m_loLimit = lo; + m_angularLimits[axis - 3].m_hiLimit = hi; + } } - } //! Test limit /*! @@ -428,34 +376,27 @@ public: - limited means upper > lower - limitIndex: first 3 are linear, next 3 are angular */ - bool isLimited(int limitIndex) - { - if(limitIndex<3) - { + bool isLimited(int limitIndex) { + if (limitIndex < 3) { return m_linearLimits.isLimited(limitIndex); - + } + return m_angularLimits[limitIndex - 3].isLimited(); } - return m_angularLimits[limitIndex-3].isLimited(); - } - const BodySW* getRigidBodyA() const - { - return A; - } - const BodySW* getRigidBodyB() const - { - return B; - } + const BodySW *getRigidBodyA() const { + return A; + } + const BodySW *getRigidBodyB() const { + return B; + } virtual void calcAnchorPos(void); // overridable - void set_param(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisParam p_param, real_t p_value); - real_t get_param(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisParam p_param) const; - - void set_flag(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisFlag p_flag, bool p_value); - bool get_flag(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisFlag p_flag) const; + void set_param(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisParam p_param, real_t p_value); + real_t get_param(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisParam p_param) const; + void set_flag(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisFlag p_flag, bool p_value); + bool get_flag(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisFlag p_flag) const; }; - #endif // GENERIC_6DOF_JOINT_SW_H diff --git a/servers/physics/joints/hinge_joint_sw.cpp b/servers/physics/joints/hinge_joint_sw.cpp index 9617eb879..eaa57af87 100644 --- a/servers/physics/joints/hinge_joint_sw.cpp +++ b/servers/physics/joints/hinge_joint_sw.cpp @@ -34,65 +34,63 @@ See corresponding header file for licensing info. #include "hinge_joint_sw.h" -static void plane_space(const Vector3& n, Vector3& p, Vector3& q) { +static void plane_space(const Vector3 &n, Vector3 &p, Vector3 &q) { - if (Math::abs(n.z) > 0.707106781186547524400844362) { - // choose p in y-z plane - real_t a = n[1]*n[1] + n[2]*n[2]; - real_t k = 1.0/Math::sqrt(a); - p=Vector3(0,-n[2]*k,n[1]*k); - // set q = n x p - q=Vector3(a*k,-n[0]*p[2],n[0]*p[1]); - } - else { - // choose p in x-y plane - real_t a = n.x*n.x + n.y*n.y; - real_t k = 1.0/Math::sqrt(a); - p=Vector3(-n.y*k,n.x*k,0); - // set q = n x p - q=Vector3(-n.z*p.y,n.z*p.x,a*k); - } + if (Math::abs(n.z) > 0.707106781186547524400844362) { + // choose p in y-z plane + real_t a = n[1] * n[1] + n[2] * n[2]; + real_t k = 1.0 / Math::sqrt(a); + p = Vector3(0, -n[2] * k, n[1] * k); + // set q = n x p + q = Vector3(a * k, -n[0] * p[2], n[0] * p[1]); + } else { + // choose p in x-y plane + real_t a = n.x * n.x + n.y * n.y; + real_t k = 1.0 / Math::sqrt(a); + p = Vector3(-n.y * k, n.x * k, 0); + // set q = n x p + q = Vector3(-n.z * p.y, n.z * p.x, a * k); + } } -HingeJointSW::HingeJointSW(BodySW* rbA,BodySW* rbB, const Transform& frameA, const Transform& frameB) : JointSW(_arr,2) { +HingeJointSW::HingeJointSW(BodySW *rbA, BodySW *rbB, const Transform &frameA, const Transform &frameB) + : JointSW(_arr, 2) { - A=rbA; - B=rbB; + A = rbA; + B = rbB; - m_rbAFrame=frameA; - m_rbBFrame=frameB; + m_rbAFrame = frameA; + m_rbBFrame = frameB; // flip axis m_rbBFrame.basis[0][2] *= real_t(-1.); m_rbBFrame.basis[1][2] *= real_t(-1.); m_rbBFrame.basis[2][2] *= real_t(-1.); - //start with free m_lowerLimit = Math_PI; m_upperLimit = -Math_PI; - m_useLimit = false; m_biasFactor = 0.3f; m_relaxationFactor = 1.0f; m_limitSoftness = 0.9f; m_solveLimit = false; - tau=0.3; - - m_angularOnly=false; - m_enableAngularMotor=false; + tau = 0.3; - A->add_constraint(this,0); - B->add_constraint(this,1); + m_angularOnly = false; + m_enableAngularMotor = false; + A->add_constraint(this, 0); + B->add_constraint(this, 1); } -HingeJointSW::HingeJointSW(BodySW* rbA,BodySW* rbB, const Vector3& pivotInA,const Vector3& pivotInB, - const Vector3& axisInA,const Vector3& axisInB) : JointSW(_arr,2) { +HingeJointSW::HingeJointSW(BodySW *rbA, BodySW *rbB, const Vector3 &pivotInA, const Vector3 &pivotInB, + const Vector3 &axisInA, const Vector3 &axisInB) + : JointSW(_arr, 2) { - A=rbA; - B=rbB; + A = rbA; + B = rbB; m_rbAFrame.origin = pivotInA; @@ -112,76 +110,67 @@ HingeJointSW::HingeJointSW(BodySW* rbA,BodySW* rbB, const Vector3& pivotInA,cons rbAxisA1 = rbAxisA2.cross(axisInA); } - m_rbAFrame.basis=Basis( rbAxisA1.x,rbAxisA2.x,axisInA.x, - rbAxisA1.y,rbAxisA2.y,axisInA.y, - rbAxisA1.z,rbAxisA2.z,axisInA.z ); + m_rbAFrame.basis = Basis(rbAxisA1.x, rbAxisA2.x, axisInA.x, + rbAxisA1.y, rbAxisA2.y, axisInA.y, + rbAxisA1.z, rbAxisA2.z, axisInA.z); - Quat rotationArc = Quat(axisInA,axisInB); - Vector3 rbAxisB1 = rotationArc.xform(rbAxisA1); - Vector3 rbAxisB2 = axisInB.cross(rbAxisB1); + Quat rotationArc = Quat(axisInA, axisInB); + Vector3 rbAxisB1 = rotationArc.xform(rbAxisA1); + Vector3 rbAxisB2 = axisInB.cross(rbAxisB1); m_rbBFrame.origin = pivotInB; - m_rbBFrame.basis=Basis( rbAxisB1.x,rbAxisB2.x,-axisInB.x, - rbAxisB1.y,rbAxisB2.y,-axisInB.y, - rbAxisB1.z,rbAxisB2.z,-axisInB.z ); + m_rbBFrame.basis = Basis(rbAxisB1.x, rbAxisB2.x, -axisInB.x, + rbAxisB1.y, rbAxisB2.y, -axisInB.y, + rbAxisB1.z, rbAxisB2.z, -axisInB.z); //start with free m_lowerLimit = Math_PI; m_upperLimit = -Math_PI; - m_useLimit = false; m_biasFactor = 0.3f; m_relaxationFactor = 1.0f; m_limitSoftness = 0.9f; m_solveLimit = false; - tau=0.3; - - m_angularOnly=false; - m_enableAngularMotor=false; + tau = 0.3; - A->add_constraint(this,0); - B->add_constraint(this,1); + m_angularOnly = false; + m_enableAngularMotor = false; + A->add_constraint(this, 0); + B->add_constraint(this, 1); } - - bool HingeJointSW::setup(real_t p_step) { m_appliedImpulse = real_t(0.); - if (!m_angularOnly) - { + if (!m_angularOnly) { Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin); Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin); Vector3 relPos = pivotBInW - pivotAInW; Vector3 normal[3]; - if (relPos.length_squared() > CMP_EPSILON) - { + if (relPos.length_squared() > CMP_EPSILON) { normal[0] = relPos.normalized(); - } - else - { - normal[0]=Vector3(real_t(1.0),0,0); + } else { + normal[0] = Vector3(real_t(1.0), 0, 0); } plane_space(normal[0], normal[1], normal[2]); - for (int i=0;i<3;i++) - { + for (int i = 0; i < 3; i++) { memnew_placement(&m_jac[i], JacobianEntrySW( - A->get_principal_inertia_axes().transposed(), - B->get_principal_inertia_axes().transposed(), - pivotAInW - A->get_transform().origin - A->get_center_of_mass(), - pivotBInW - B->get_transform().origin - B->get_center_of_mass(), - normal[i], - A->get_inv_inertia(), - A->get_inv_mass(), - B->get_inv_inertia(), - B->get_inv_mass()) ); + A->get_principal_inertia_axes().transposed(), + B->get_principal_inertia_axes().transposed(), + pivotAInW - A->get_transform().origin - A->get_center_of_mass(), + pivotBInW - B->get_transform().origin - B->get_center_of_mass(), + normal[i], + A->get_inv_inertia(), + A->get_inv_mass(), + B->get_inv_inertia(), + B->get_inv_mass())); } } @@ -192,31 +181,30 @@ bool HingeJointSW::setup(real_t p_step) { Vector3 jointAxis0local; Vector3 jointAxis1local; - plane_space(m_rbAFrame.basis.get_axis(2),jointAxis0local,jointAxis1local); - - A->get_transform().basis.xform( m_rbAFrame.basis.get_axis(2) ); - Vector3 jointAxis0 = A->get_transform().basis.xform( jointAxis0local ); - Vector3 jointAxis1 = A->get_transform().basis.xform( jointAxis1local ); - Vector3 hingeAxisWorld = A->get_transform().basis.xform( m_rbAFrame.basis.get_axis(2) ); + plane_space(m_rbAFrame.basis.get_axis(2), jointAxis0local, jointAxis1local); - memnew_placement(&m_jacAng[0], JacobianEntrySW(jointAxis0, - A->get_principal_inertia_axes().transposed(), - B->get_principal_inertia_axes().transposed(), - A->get_inv_inertia(), - B->get_inv_inertia())); + A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(2)); + Vector3 jointAxis0 = A->get_transform().basis.xform(jointAxis0local); + Vector3 jointAxis1 = A->get_transform().basis.xform(jointAxis1local); + Vector3 hingeAxisWorld = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(2)); - memnew_placement(&m_jacAng[1], JacobianEntrySW(jointAxis1, - A->get_principal_inertia_axes().transposed(), - B->get_principal_inertia_axes().transposed(), - A->get_inv_inertia(), - B->get_inv_inertia())); + memnew_placement(&m_jacAng[0], JacobianEntrySW(jointAxis0, + A->get_principal_inertia_axes().transposed(), + B->get_principal_inertia_axes().transposed(), + A->get_inv_inertia(), + B->get_inv_inertia())); - memnew_placement(&m_jacAng[2], JacobianEntrySW(hingeAxisWorld, - A->get_principal_inertia_axes().transposed(), - B->get_principal_inertia_axes().transposed(), - A->get_inv_inertia(), - B->get_inv_inertia())); + memnew_placement(&m_jacAng[1], JacobianEntrySW(jointAxis1, + A->get_principal_inertia_axes().transposed(), + B->get_principal_inertia_axes().transposed(), + A->get_inv_inertia(), + B->get_inv_inertia())); + memnew_placement(&m_jacAng[2], JacobianEntrySW(hingeAxisWorld, + A->get_principal_inertia_axes().transposed(), + B->get_principal_inertia_axes().transposed(), + A->get_inv_inertia(), + B->get_inv_inertia())); // Compute limit information real_t hingeAngle = get_hinge_angle(); @@ -228,26 +216,21 @@ bool HingeJointSW::setup(real_t p_step) { m_solveLimit = false; m_accLimitImpulse = real_t(0.); - - /*if (m_useLimit) { print_line("low: "+rtos(m_lowerLimit)); print_line("hi: "+rtos(m_upperLimit)); }*/ //if (m_lowerLimit < m_upperLimit) - if (m_useLimit && m_lowerLimit <= m_upperLimit) - { + if (m_useLimit && m_lowerLimit <= m_upperLimit) { //if (hingeAngle <= m_lowerLimit*m_limitSoftness) - if (hingeAngle <= m_lowerLimit) - { + if (hingeAngle <= m_lowerLimit) { m_correction = (m_lowerLimit - hingeAngle); m_limitSign = 1.0f; m_solveLimit = true; } //else if (hingeAngle >= m_upperLimit*m_limitSoftness) - else if (hingeAngle >= m_upperLimit) - { + else if (hingeAngle >= m_upperLimit) { m_correction = m_upperLimit - hingeAngle; m_limitSign = -1.0f; m_solveLimit = true; @@ -255,9 +238,9 @@ bool HingeJointSW::setup(real_t p_step) { } //Compute K = J*W*J' for hinge axis - Vector3 axisA = A->get_transform().basis.xform( m_rbAFrame.basis.get_axis(2) ); - m_kHinge = 1.0f / (A->compute_angular_impulse_denominator(axisA) + - B->compute_angular_impulse_denominator(axisA)); + Vector3 axisA = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(2)); + m_kHinge = 1.0f / (A->compute_angular_impulse_denominator(axisA) + + B->compute_angular_impulse_denominator(axisA)); return true; } @@ -270,8 +253,7 @@ void HingeJointSW::solve(real_t p_step) { //real_t tau = real_t(0.3); //linear part - if (!m_angularOnly) - { + if (!m_angularOnly) { Vector3 rel_pos1 = pivotAInW - A->get_transform().origin; Vector3 rel_pos2 = pivotBInW - B->get_transform().origin; @@ -279,80 +261,74 @@ void HingeJointSW::solve(real_t p_step) { Vector3 vel2 = B->get_velocity_in_local_point(rel_pos2); Vector3 vel = vel1 - vel2; - for (int i=0;i<3;i++) - { - const Vector3& normal = m_jac[i].m_linearJointAxis; + for (int i = 0; i < 3; i++) { + const Vector3 &normal = m_jac[i].m_linearJointAxis; real_t jacDiagABInv = real_t(1.) / m_jac[i].getDiagonal(); real_t rel_vel; rel_vel = normal.dot(vel); //positional error (zeroth order error) real_t depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal - real_t impulse = depth*tau/p_step * jacDiagABInv - rel_vel * jacDiagABInv; + real_t impulse = depth * tau / p_step * jacDiagABInv - rel_vel * jacDiagABInv; m_appliedImpulse += impulse; Vector3 impulse_vector = normal * impulse; - A->apply_impulse(pivotAInW - A->get_transform().origin,impulse_vector); - B->apply_impulse(pivotBInW - B->get_transform().origin,-impulse_vector); + A->apply_impulse(pivotAInW - A->get_transform().origin, impulse_vector); + B->apply_impulse(pivotBInW - B->get_transform().origin, -impulse_vector); } } - { ///solve angular part // get axes in world space - Vector3 axisA = A->get_transform().basis.xform( m_rbAFrame.basis.get_axis(2) ); - Vector3 axisB = B->get_transform().basis.xform( m_rbBFrame.basis.get_axis(2) ); + Vector3 axisA = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(2)); + Vector3 axisB = B->get_transform().basis.xform(m_rbBFrame.basis.get_axis(2)); - const Vector3& angVelA = A->get_angular_velocity(); - const Vector3& angVelB = B->get_angular_velocity(); + const Vector3 &angVelA = A->get_angular_velocity(); + const Vector3 &angVelB = B->get_angular_velocity(); Vector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA); Vector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB); Vector3 angAorthog = angVelA - angVelAroundHingeAxisA; Vector3 angBorthog = angVelB - angVelAroundHingeAxisB; - Vector3 velrelOrthog = angAorthog-angBorthog; + Vector3 velrelOrthog = angAorthog - angBorthog; { //solve orthogonal angular velocity correction real_t relaxation = real_t(1.); real_t len = velrelOrthog.length(); - if (len > real_t(0.00001)) - { + if (len > real_t(0.00001)) { Vector3 normal = velrelOrthog.normalized(); real_t denom = A->compute_angular_impulse_denominator(normal) + - B->compute_angular_impulse_denominator(normal); + B->compute_angular_impulse_denominator(normal); // scale for mass and relaxation - velrelOrthog *= (real_t(1.)/denom) * m_relaxationFactor; + velrelOrthog *= (real_t(1.) / denom) * m_relaxationFactor; } //solve angular positional correction - Vector3 angularError = -axisA.cross(axisB) *(real_t(1.)/p_step); + Vector3 angularError = -axisA.cross(axisB) * (real_t(1.) / p_step); real_t len2 = angularError.length(); - if (len2>real_t(0.00001)) - { + if (len2 > real_t(0.00001)) { Vector3 normal2 = angularError.normalized(); real_t denom2 = A->compute_angular_impulse_denominator(normal2) + - B->compute_angular_impulse_denominator(normal2); - angularError *= (real_t(1.)/denom2) * relaxation; + B->compute_angular_impulse_denominator(normal2); + angularError *= (real_t(1.) / denom2) * relaxation; } - A->apply_torque_impulse(-velrelOrthog+angularError); - B->apply_torque_impulse(velrelOrthog-angularError); + A->apply_torque_impulse(-velrelOrthog + angularError); + B->apply_torque_impulse(velrelOrthog - angularError); // solve limit - if (m_solveLimit) - { - real_t amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (real_t(1.)/p_step)*m_biasFactor ) * m_limitSign; + if (m_solveLimit) { + real_t amplitude = ((angVelB - angVelA).dot(axisA) * m_relaxationFactor + m_correction * (real_t(1.) / p_step) * m_biasFactor) * m_limitSign; real_t impulseMag = amplitude * m_kHinge; // Clamp the accumulated impulse real_t temp = m_accLimitImpulse; - m_accLimitImpulse = MAX(m_accLimitImpulse + impulseMag, real_t(0) ); + m_accLimitImpulse = MAX(m_accLimitImpulse + impulseMag, real_t(0)); impulseMag = m_accLimitImpulse - temp; - Vector3 impulse = axisA * impulseMag * m_limitSign; A->apply_torque_impulse(impulse); B->apply_torque_impulse(-impulse); @@ -360,10 +336,9 @@ void HingeJointSW::solve(real_t p_step) { } //apply motor - if (m_enableAngularMotor) - { + if (m_enableAngularMotor) { //todo: add limits too - Vector3 angularLimit(0,0,0); + Vector3 angularLimit(0, 0, 0); Vector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB; real_t projRelVel = velrel.dot(axisA); @@ -377,12 +352,10 @@ void HingeJointSW::solve(real_t p_step) { clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse; Vector3 motorImp = clippedMotorImpulse * axisA; - A->apply_torque_impulse(motorImp+angularLimit); - B->apply_torque_impulse(-motorImp-angularLimit); - + A->apply_torque_impulse(motorImp + angularLimit); + B->apply_torque_impulse(-motorImp - angularLimit); } } - } /* void HingeJointSW::updateRHS(real_t timeStep) @@ -392,8 +365,7 @@ void HingeJointSW::updateRHS(real_t timeStep) } */ -static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x) -{ +static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x) { real_t coeff_1 = Math_PI / 4.0f; real_t coeff_2 = 3.0f * coeff_1; real_t abs_y = Math::abs(y); @@ -408,33 +380,30 @@ static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x) return (y < 0.0f) ? -angle : angle; } - real_t HingeJointSW::get_hinge_angle() { - const Vector3 refAxis0 = A->get_transform().basis.xform( m_rbAFrame.basis.get_axis(0) ); - const Vector3 refAxis1 = A->get_transform().basis.xform( m_rbAFrame.basis.get_axis(1) ); - const Vector3 swingAxis = B->get_transform().basis.xform( m_rbBFrame.basis.get_axis(1) ); + const Vector3 refAxis0 = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(0)); + const Vector3 refAxis1 = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(1)); + const Vector3 swingAxis = B->get_transform().basis.xform(m_rbBFrame.basis.get_axis(1)); - return atan2fast( swingAxis.dot(refAxis0), swingAxis.dot(refAxis1) ); + return atan2fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)); } - void HingeJointSW::set_param(PhysicsServer::HingeJointParam p_param, real_t p_value) { switch (p_param) { - case PhysicsServer::HINGE_JOINT_BIAS: tau=p_value; break; - case PhysicsServer::HINGE_JOINT_LIMIT_UPPER: m_upperLimit=p_value; break; - case PhysicsServer::HINGE_JOINT_LIMIT_LOWER: m_lowerLimit=p_value; break; - case PhysicsServer::HINGE_JOINT_LIMIT_BIAS: m_biasFactor=p_value; break; - case PhysicsServer::HINGE_JOINT_LIMIT_SOFTNESS: m_limitSoftness=p_value; break; - case PhysicsServer::HINGE_JOINT_LIMIT_RELAXATION: m_relaxationFactor=p_value; break; - case PhysicsServer::HINGE_JOINT_MOTOR_TARGET_VELOCITY: m_motorTargetVelocity=p_value; break; - case PhysicsServer::HINGE_JOINT_MOTOR_MAX_IMPULSE: m_maxMotorImpulse=p_value; break; - + case PhysicsServer::HINGE_JOINT_BIAS: tau = p_value; break; + case PhysicsServer::HINGE_JOINT_LIMIT_UPPER: m_upperLimit = p_value; break; + case PhysicsServer::HINGE_JOINT_LIMIT_LOWER: m_lowerLimit = p_value; break; + case PhysicsServer::HINGE_JOINT_LIMIT_BIAS: m_biasFactor = p_value; break; + case PhysicsServer::HINGE_JOINT_LIMIT_SOFTNESS: m_limitSoftness = p_value; break; + case PhysicsServer::HINGE_JOINT_LIMIT_RELAXATION: m_relaxationFactor = p_value; break; + case PhysicsServer::HINGE_JOINT_MOTOR_TARGET_VELOCITY: m_motorTargetVelocity = p_value; break; + case PhysicsServer::HINGE_JOINT_MOTOR_MAX_IMPULSE: m_maxMotorImpulse = p_value; break; } } -real_t HingeJointSW::get_param(PhysicsServer::HingeJointParam p_param) const{ +real_t HingeJointSW::get_param(PhysicsServer::HingeJointParam p_param) const { switch (p_param) { @@ -446,25 +415,23 @@ real_t HingeJointSW::get_param(PhysicsServer::HingeJointParam p_param) const{ case PhysicsServer::HINGE_JOINT_LIMIT_RELAXATION: return m_relaxationFactor; case PhysicsServer::HINGE_JOINT_MOTOR_TARGET_VELOCITY: return m_motorTargetVelocity; case PhysicsServer::HINGE_JOINT_MOTOR_MAX_IMPULSE: return m_maxMotorImpulse; - } return 0; } -void HingeJointSW::set_flag(PhysicsServer::HingeJointFlag p_flag, bool p_value){ +void HingeJointSW::set_flag(PhysicsServer::HingeJointFlag p_flag, bool p_value) { switch (p_flag) { - case PhysicsServer::HINGE_JOINT_FLAG_USE_LIMIT: m_useLimit=p_value; break; - case PhysicsServer::HINGE_JOINT_FLAG_ENABLE_MOTOR: m_enableAngularMotor=p_value; break; + case PhysicsServer::HINGE_JOINT_FLAG_USE_LIMIT: m_useLimit = p_value; break; + case PhysicsServer::HINGE_JOINT_FLAG_ENABLE_MOTOR: m_enableAngularMotor = p_value; break; } - } -bool HingeJointSW::get_flag(PhysicsServer::HingeJointFlag p_flag) const{ +bool HingeJointSW::get_flag(PhysicsServer::HingeJointFlag p_flag) const { switch (p_flag) { case PhysicsServer::HINGE_JOINT_FLAG_USE_LIMIT: return m_useLimit; - case PhysicsServer::HINGE_JOINT_FLAG_ENABLE_MOTOR:return m_enableAngularMotor; + case PhysicsServer::HINGE_JOINT_FLAG_ENABLE_MOTOR: return m_enableAngularMotor; } return false; diff --git a/servers/physics/joints/hinge_joint_sw.h b/servers/physics/joints/hinge_joint_sw.h index 8469fd1ca..013d9afdb 100644 --- a/servers/physics/joints/hinge_joint_sw.h +++ b/servers/physics/joints/hinge_joint_sw.h @@ -34,9 +34,8 @@ Adapted to Godot from the Bullet library. #ifndef HINGE_JOINT_SW_H #define HINGE_JOINT_SW_H -#include "servers/physics/joints_sw.h" #include "servers/physics/joints/jacobian_entry_sw.h" - +#include "servers/physics/joints_sw.h" /* Bullet Continuous Collision Detection and Physics Library @@ -53,8 +52,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - - class HingeJointSW : public JointSW { union { @@ -66,41 +63,39 @@ class HingeJointSW : public JointSW { BodySW *_arr[2]; }; - JacobianEntrySW m_jac[3]; //3 orthogonal linear constraints - JacobianEntrySW m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor + JacobianEntrySW m_jac[3]; //3 orthogonal linear constraints + JacobianEntrySW m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor Transform m_rbAFrame; // constraint axii. Assumes z is hinge axis. Transform m_rbBFrame; - real_t m_motorTargetVelocity; - real_t m_maxMotorImpulse; + real_t m_motorTargetVelocity; + real_t m_maxMotorImpulse; - real_t m_limitSoftness; - real_t m_biasFactor; - real_t m_relaxationFactor; + real_t m_limitSoftness; + real_t m_biasFactor; + real_t m_relaxationFactor; - real_t m_lowerLimit; - real_t m_upperLimit; + real_t m_lowerLimit; + real_t m_upperLimit; - real_t m_kHinge; + real_t m_kHinge; - real_t m_limitSign; - real_t m_correction; + real_t m_limitSign; + real_t m_correction; - real_t m_accLimitImpulse; + real_t m_accLimitImpulse; real_t tau; - bool m_useLimit; - bool m_angularOnly; - bool m_enableAngularMotor; - bool m_solveLimit; + bool m_useLimit; + bool m_angularOnly; + bool m_enableAngularMotor; + bool m_solveLimit; real_t m_appliedImpulse; - public: - virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_HINGE; } virtual bool setup(real_t p_step); @@ -114,9 +109,8 @@ public: void set_flag(PhysicsServer::HingeJointFlag p_flag, bool p_value); bool get_flag(PhysicsServer::HingeJointFlag p_flag) const; - HingeJointSW(BodySW* rbA,BodySW* rbB, const Transform& frameA, const Transform& frameB); - HingeJointSW(BodySW* rbA,BodySW* rbB, const Vector3& pivotInA,const Vector3& pivotInB, const Vector3& axisInA,const Vector3& axisInB); - + HingeJointSW(BodySW *rbA, BodySW *rbB, const Transform &frameA, const Transform &frameB); + HingeJointSW(BodySW *rbA, BodySW *rbB, const Vector3 &pivotInA, const Vector3 &pivotInB, const Vector3 &axisInA, const Vector3 &axisInB); }; #endif // HINGE_JOINT_SW_H diff --git a/servers/physics/joints/jacobian_entry_sw.h b/servers/physics/joints/jacobian_entry_sw.h index cd85162ba..b0b31ed79 100644 --- a/servers/physics/joints/jacobian_entry_sw.h +++ b/servers/physics/joints/jacobian_entry_sw.h @@ -53,22 +53,21 @@ subject to the following restrictions: class JacobianEntrySW { public: - JacobianEntrySW() {}; + JacobianEntrySW(){}; //constraint between two different rigidbodies JacobianEntrySW( - const Basis& world2A, - const Basis& world2B, - const Vector3& rel_pos1,const Vector3& rel_pos2, - const Vector3& jointAxis, - const Vector3& inertiaInvA, - const real_t massInvA, - const Vector3& inertiaInvB, - const real_t massInvB) - :m_linearJointAxis(jointAxis) - { + const Basis &world2A, + const Basis &world2B, + const Vector3 &rel_pos1, const Vector3 &rel_pos2, + const Vector3 &jointAxis, + const Vector3 &inertiaInvA, + const real_t massInvA, + const Vector3 &inertiaInvB, + const real_t massInvB) + : m_linearJointAxis(jointAxis) { m_aJ = world2A.xform(rel_pos1.cross(m_linearJointAxis)); m_bJ = world2B.xform(rel_pos2.cross(-m_linearJointAxis)); - m_0MinvJt = inertiaInvA * m_aJ; + m_0MinvJt = inertiaInvA * m_aJ; m_1MinvJt = inertiaInvB * m_bJ; m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ); @@ -76,104 +75,92 @@ public: } //angular constraint between two different rigidbodies - JacobianEntrySW(const Vector3& jointAxis, - const Basis& world2A, - const Basis& world2B, - const Vector3& inertiaInvA, - const Vector3& inertiaInvB) - :m_linearJointAxis(Vector3(real_t(0.),real_t(0.),real_t(0.))) - { - m_aJ= world2A.xform(jointAxis); + JacobianEntrySW(const Vector3 &jointAxis, + const Basis &world2A, + const Basis &world2B, + const Vector3 &inertiaInvA, + const Vector3 &inertiaInvB) + : m_linearJointAxis(Vector3(real_t(0.), real_t(0.), real_t(0.))) { + m_aJ = world2A.xform(jointAxis); m_bJ = world2B.xform(-jointAxis); - m_0MinvJt = inertiaInvA * m_aJ; + m_0MinvJt = inertiaInvA * m_aJ; m_1MinvJt = inertiaInvB * m_bJ; - m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ); + m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ); ERR_FAIL_COND(m_Adiag <= real_t(0.0)); } //angular constraint between two different rigidbodies - JacobianEntrySW(const Vector3& axisInA, - const Vector3& axisInB, - const Vector3& inertiaInvA, - const Vector3& inertiaInvB) - : m_linearJointAxis(Vector3(real_t(0.),real_t(0.),real_t(0.))) - , m_aJ(axisInA) - , m_bJ(-axisInB) - { - m_0MinvJt = inertiaInvA * m_aJ; + JacobianEntrySW(const Vector3 &axisInA, + const Vector3 &axisInB, + const Vector3 &inertiaInvA, + const Vector3 &inertiaInvB) + : m_linearJointAxis(Vector3(real_t(0.), real_t(0.), real_t(0.))), m_aJ(axisInA), m_bJ(-axisInB) { + m_0MinvJt = inertiaInvA * m_aJ; m_1MinvJt = inertiaInvB * m_bJ; - m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ); + m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ); ERR_FAIL_COND(m_Adiag <= real_t(0.0)); } //constraint on one rigidbody JacobianEntrySW( - const Basis& world2A, - const Vector3& rel_pos1,const Vector3& rel_pos2, - const Vector3& jointAxis, - const Vector3& inertiaInvA, - const real_t massInvA) - :m_linearJointAxis(jointAxis) - { - m_aJ= world2A.xform(rel_pos1.cross(jointAxis)); + const Basis &world2A, + const Vector3 &rel_pos1, const Vector3 &rel_pos2, + const Vector3 &jointAxis, + const Vector3 &inertiaInvA, + const real_t massInvA) + : m_linearJointAxis(jointAxis) { + m_aJ = world2A.xform(rel_pos1.cross(jointAxis)); m_bJ = world2A.xform(rel_pos2.cross(-jointAxis)); - m_0MinvJt = inertiaInvA * m_aJ; - m_1MinvJt = Vector3(real_t(0.),real_t(0.),real_t(0.)); + m_0MinvJt = inertiaInvA * m_aJ; + m_1MinvJt = Vector3(real_t(0.), real_t(0.), real_t(0.)); m_Adiag = massInvA + m_0MinvJt.dot(m_aJ); ERR_FAIL_COND(m_Adiag <= real_t(0.0)); } - real_t getDiagonal() const { return m_Adiag; } + real_t getDiagonal() const { return m_Adiag; } // for two constraints on the same rigidbody (for example vehicle friction) - real_t getNonDiagonal(const JacobianEntrySW& jacB, const real_t massInvA) const - { - const JacobianEntrySW& jacA = *this; + real_t getNonDiagonal(const JacobianEntrySW &jacB, const real_t massInvA) const { + const JacobianEntrySW &jacA = *this; real_t lin = massInvA * jacA.m_linearJointAxis.dot(jacB.m_linearJointAxis); real_t ang = jacA.m_0MinvJt.dot(jacB.m_aJ); return lin + ang; } - - // for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies) - real_t getNonDiagonal(const JacobianEntrySW& jacB,const real_t massInvA,const real_t massInvB) const - { - const JacobianEntrySW& jacA = *this; + real_t getNonDiagonal(const JacobianEntrySW &jacB, const real_t massInvA, const real_t massInvB) const { + const JacobianEntrySW &jacA = *this; Vector3 lin = jacA.m_linearJointAxis * jacB.m_linearJointAxis; Vector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ; Vector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ; - Vector3 lin0 = massInvA * lin ; + Vector3 lin0 = massInvA * lin; Vector3 lin1 = massInvB * lin; - Vector3 sum = ang0+ang1+lin0+lin1; - return sum[0]+sum[1]+sum[2]; + Vector3 sum = ang0 + ang1 + lin0 + lin1; + return sum[0] + sum[1] + sum[2]; } - real_t getRelativeVelocity(const Vector3& linvelA,const Vector3& angvelA,const Vector3& linvelB,const Vector3& angvelB) - { + real_t getRelativeVelocity(const Vector3 &linvelA, const Vector3 &angvelA, const Vector3 &linvelB, const Vector3 &angvelB) { Vector3 linrel = linvelA - linvelB; - Vector3 angvela = angvelA * m_aJ; - Vector3 angvelb = angvelB * m_bJ; + Vector3 angvela = angvelA * m_aJ; + Vector3 angvelb = angvelB * m_bJ; linrel *= m_linearJointAxis; angvela += angvelb; angvela += linrel; - real_t rel_vel2 = angvela[0]+angvela[1]+angvela[2]; + real_t rel_vel2 = angvela[0] + angvela[1] + angvela[2]; return rel_vel2 + CMP_EPSILON; } -//private: + //private: - Vector3 m_linearJointAxis; - Vector3 m_aJ; - Vector3 m_bJ; - Vector3 m_0MinvJt; - Vector3 m_1MinvJt; + Vector3 m_linearJointAxis; + Vector3 m_aJ; + Vector3 m_bJ; + Vector3 m_0MinvJt; + Vector3 m_1MinvJt; //Optimization: can be stored in the w/last component of one of the vectors - real_t m_Adiag; - + real_t m_Adiag; }; - #endif // JACOBIAN_ENTRY_SW_H diff --git a/servers/physics/joints/pin_joint_sw.cpp b/servers/physics/joints/pin_joint_sw.cpp index b545ae630..e01514f4b 100644 --- a/servers/physics/joints/pin_joint_sw.cpp +++ b/servers/physics/joints/pin_joint_sw.cpp @@ -38,41 +38,37 @@ bool PinJointSW::setup(real_t p_step) { m_appliedImpulse = real_t(0.); - Vector3 normal(0,0,0); + Vector3 normal(0, 0, 0); - for (int i=0;i<3;i++) - { + for (int i = 0; i < 3; i++) { normal[i] = 1; - memnew_placement(&m_jac[i],JacobianEntrySW( - A->get_principal_inertia_axes().transposed(), - B->get_principal_inertia_axes().transposed(), - A->get_transform().xform(m_pivotInA) - A->get_transform().origin - A->get_center_of_mass(), - B->get_transform().xform(m_pivotInB) - B->get_transform().origin - B->get_center_of_mass(), - normal, - A->get_inv_inertia(), - A->get_inv_mass(), - B->get_inv_inertia(), - B->get_inv_mass())); + memnew_placement(&m_jac[i], JacobianEntrySW( + A->get_principal_inertia_axes().transposed(), + B->get_principal_inertia_axes().transposed(), + A->get_transform().xform(m_pivotInA) - A->get_transform().origin - A->get_center_of_mass(), + B->get_transform().xform(m_pivotInB) - B->get_transform().origin - B->get_center_of_mass(), + normal, + A->get_inv_inertia(), + A->get_inv_mass(), + B->get_inv_inertia(), + B->get_inv_mass())); normal[i] = 0; } return true; } -void PinJointSW::solve(real_t p_step){ +void PinJointSW::solve(real_t p_step) { Vector3 pivotAInW = A->get_transform().xform(m_pivotInA); Vector3 pivotBInW = B->get_transform().xform(m_pivotInB); - - Vector3 normal(0,0,0); - + Vector3 normal(0, 0, 0); //Vector3 angvelA = A->get_transform().origin.getBasis().transpose() * A->getAngularVelocity(); //Vector3 angvelB = B->get_transform().origin.getBasis().transpose() * B->getAngularVelocity(); - for (int i=0;i<3;i++) - { + for (int i = 0; i < 3; i++) { normal[i] = 1; real_t jacDiagABInv = real_t(1.) / m_jac[i].getDiagonal(); @@ -87,7 +83,7 @@ void PinJointSW::solve(real_t p_step){ real_t rel_vel; rel_vel = normal.dot(vel); - /* + /* //velocity error (first order error) real_t rel_vel = m_jac[i].getRelativeVelocity(A->getLinearVelocity(),angvelA, B->getLinearVelocity(),angvelB); @@ -96,38 +92,37 @@ void PinJointSW::solve(real_t p_step){ //positional error (zeroth order error) real_t depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal - real_t impulse = depth*m_tau/p_step * jacDiagABInv - m_damping * rel_vel * jacDiagABInv; + real_t impulse = depth * m_tau / p_step * jacDiagABInv - m_damping * rel_vel * jacDiagABInv; real_t impulseClamp = m_impulseClamp; - if (impulseClamp > 0) - { + if (impulseClamp > 0) { if (impulse < -impulseClamp) impulse = -impulseClamp; if (impulse > impulseClamp) impulse = impulseClamp; } - m_appliedImpulse+=impulse; + m_appliedImpulse += impulse; Vector3 impulse_vector = normal * impulse; - A->apply_impulse(pivotAInW - A->get_transform().origin,impulse_vector); - B->apply_impulse(pivotBInW - B->get_transform().origin,-impulse_vector); + A->apply_impulse(pivotAInW - A->get_transform().origin, impulse_vector); + B->apply_impulse(pivotBInW - B->get_transform().origin, -impulse_vector); normal[i] = 0; } } -void PinJointSW::set_param(PhysicsServer::PinJointParam p_param,real_t p_value) { +void PinJointSW::set_param(PhysicsServer::PinJointParam p_param, real_t p_value) { - switch(p_param) { - case PhysicsServer::PIN_JOINT_BIAS: m_tau=p_value; break; - case PhysicsServer::PIN_JOINT_DAMPING: m_damping=p_value; break; - case PhysicsServer::PIN_JOINT_IMPULSE_CLAMP: m_impulseClamp=p_value; break; + switch (p_param) { + case PhysicsServer::PIN_JOINT_BIAS: m_tau = p_value; break; + case PhysicsServer::PIN_JOINT_DAMPING: m_damping = p_value; break; + case PhysicsServer::PIN_JOINT_IMPULSE_CLAMP: m_impulseClamp = p_value; break; } } -real_t PinJointSW::get_param(PhysicsServer::PinJointParam p_param) const{ +real_t PinJointSW::get_param(PhysicsServer::PinJointParam p_param) const { - switch(p_param) { + switch (p_param) { case PhysicsServer::PIN_JOINT_BIAS: return m_tau; case PhysicsServer::PIN_JOINT_DAMPING: return m_damping; case PhysicsServer::PIN_JOINT_IMPULSE_CLAMP: return m_impulseClamp; @@ -136,26 +131,22 @@ real_t PinJointSW::get_param(PhysicsServer::PinJointParam p_param) const{ return 0; } -PinJointSW::PinJointSW(BodySW* p_body_a,const Vector3& p_pos_a,BodySW* p_body_b,const Vector3& p_pos_b) : JointSW(_arr,2) { - - A=p_body_a; - B=p_body_b; - m_pivotInA=p_pos_a; - m_pivotInB=p_pos_b; - - m_tau=0.3; - m_damping=1; - m_impulseClamp=0; - m_appliedImpulse=0; +PinJointSW::PinJointSW(BodySW *p_body_a, const Vector3 &p_pos_a, BodySW *p_body_b, const Vector3 &p_pos_b) + : JointSW(_arr, 2) { - A->add_constraint(this,0); - B->add_constraint(this,1); + A = p_body_a; + B = p_body_b; + m_pivotInA = p_pos_a; + m_pivotInB = p_pos_b; + m_tau = 0.3; + m_damping = 1; + m_impulseClamp = 0; + m_appliedImpulse = 0; + A->add_constraint(this, 0); + B->add_constraint(this, 1); } PinJointSW::~PinJointSW() { - - - } diff --git a/servers/physics/joints/pin_joint_sw.h b/servers/physics/joints/pin_joint_sw.h index b72b21219..9500d4b46 100644 --- a/servers/physics/joints/pin_joint_sw.h +++ b/servers/physics/joints/pin_joint_sw.h @@ -34,8 +34,8 @@ Adapted to Godot from the Bullet library. #ifndef PIN_JOINT_SW_H #define PIN_JOINT_SW_H -#include "servers/physics/joints_sw.h" #include "servers/physics/joints/jacobian_entry_sw.h" +#include "servers/physics/joints_sw.h" /* Bullet Continuous Collision Detection and Physics Library @@ -52,7 +52,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - class PinJointSW : public JointSW { union { @@ -64,37 +63,33 @@ class PinJointSW : public JointSW { BodySW *_arr[2]; }; + real_t m_tau; //bias + real_t m_damping; + real_t m_impulseClamp; + real_t m_appliedImpulse; - real_t m_tau; //bias - real_t m_damping; - real_t m_impulseClamp; - real_t m_appliedImpulse; - - JacobianEntrySW m_jac[3]; //3 orthogonal linear constraints + JacobianEntrySW m_jac[3]; //3 orthogonal linear constraints - Vector3 m_pivotInA; - Vector3 m_pivotInB; + Vector3 m_pivotInA; + Vector3 m_pivotInB; public: - virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_PIN; } virtual bool setup(real_t p_step); virtual void solve(real_t p_step); - void set_param(PhysicsServer::PinJointParam p_param,real_t p_value); + void set_param(PhysicsServer::PinJointParam p_param, real_t p_value); real_t get_param(PhysicsServer::PinJointParam p_param) const; - void set_pos_A(const Vector3& p_pos) { m_pivotInA=p_pos; } - void set_pos_B(const Vector3& p_pos) { m_pivotInB=p_pos; } + void set_pos_A(const Vector3 &p_pos) { m_pivotInA = p_pos; } + void set_pos_B(const Vector3 &p_pos) { m_pivotInB = p_pos; } Vector3 get_pos_A() { return m_pivotInB; } Vector3 get_pos_B() { return m_pivotInA; } - PinJointSW(BodySW* p_body_a,const Vector3& p_pos_a,BodySW* p_body_b,const Vector3& p_pos_b); + PinJointSW(BodySW *p_body_a, const Vector3 &p_pos_a, BodySW *p_body_b, const Vector3 &p_pos_b); ~PinJointSW(); }; - - #endif // PIN_JOINT_SW_H diff --git a/servers/physics/joints/slider_joint_sw.cpp b/servers/physics/joints/slider_joint_sw.cpp index fc728ed0b..b8a6c1eca 100644 --- a/servers/physics/joints/slider_joint_sw.cpp +++ b/servers/physics/joints/slider_joint_sw.cpp @@ -36,8 +36,7 @@ See corresponding header file for licensing info. //----------------------------------------------------------------------------- -static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x) -{ +static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x) { real_t coeff_1 = Math_PI / 4.0f; real_t coeff_2 = 3.0f * coeff_1; real_t abs_y = Math::abs(y); @@ -52,13 +51,11 @@ static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x) return (y < 0.0f) ? -angle : angle; } - -void SliderJointSW::initParams() -{ - m_lowerLinLimit = real_t(1.0); - m_upperLinLimit = real_t(-1.0); - m_lowerAngLimit = real_t(0.); - m_upperAngLimit = real_t(0.); +void SliderJointSW::initParams() { + m_lowerLinLimit = real_t(1.0); + m_upperLinLimit = real_t(-1.0); + m_lowerAngLimit = real_t(0.); + m_upperAngLimit = real_t(0.); m_softnessDirLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; m_restitutionDirLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; m_dampingDirLin = real_t(0.); @@ -84,40 +81,35 @@ void SliderJointSW::initParams() m_accumulatedLinMotorImpulse = real_t(0.0); m_poweredAngMotor = false; - m_targetAngMotorVelocity = real_t(0.); - m_maxAngMotorForce = real_t(0.); + m_targetAngMotorVelocity = real_t(0.); + m_maxAngMotorForce = real_t(0.); m_accumulatedAngMotorImpulse = real_t(0.0); } // SliderJointSW::initParams() //----------------------------------------------------------------------------- - //----------------------------------------------------------------------------- -SliderJointSW::SliderJointSW(BodySW* rbA, BodySW* rbB, const Transform& frameInA, const Transform& frameInB) - : JointSW(_arr,2) - , m_frameInA(frameInA) - , m_frameInB(frameInB) -{ +SliderJointSW::SliderJointSW(BodySW *rbA, BodySW *rbB, const Transform &frameInA, const Transform &frameInB) + : JointSW(_arr, 2), m_frameInA(frameInA), m_frameInB(frameInB) { - A=rbA; - B=rbB; + A = rbA; + B = rbB; - A->add_constraint(this,0); - B->add_constraint(this,1); + A->add_constraint(this, 0); + B->add_constraint(this, 1); initParams(); } // SliderJointSW::SliderJointSW() //----------------------------------------------------------------------------- -bool SliderJointSW::setup(real_t p_step) -{ +bool SliderJointSW::setup(real_t p_step) { //calculate transforms - m_calculatedTransformA = A->get_transform() * m_frameInA; - m_calculatedTransformB = B->get_transform() * m_frameInB; + m_calculatedTransformA = A->get_transform() * m_frameInA; + m_calculatedTransformB = B->get_transform() * m_frameInB; m_realPivotAInW = m_calculatedTransformA.origin; m_realPivotBInW = m_calculatedTransformB.origin; m_sliderAxis = m_calculatedTransformA.basis.get_axis(0); // along X @@ -125,42 +117,38 @@ bool SliderJointSW::setup(real_t p_step) m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis; m_relPosA = m_projPivotInW - A->get_transform().origin; m_relPosB = m_realPivotBInW - B->get_transform().origin; - Vector3 normalWorld; - int i; - //linear part - for(i = 0; i < 3; i++) - { + Vector3 normalWorld; + int i; + //linear part + for (i = 0; i < 3; i++) { normalWorld = m_calculatedTransformA.basis.get_axis(i); memnew_placement(&m_jacLin[i], JacobianEntrySW( - A->get_principal_inertia_axes().transposed(), - B->get_principal_inertia_axes().transposed(), - m_relPosA - A->get_center_of_mass(), - m_relPosB - B->get_center_of_mass(), - normalWorld, - A->get_inv_inertia(), - A->get_inv_mass(), - B->get_inv_inertia(), - B->get_inv_mass() - )); + A->get_principal_inertia_axes().transposed(), + B->get_principal_inertia_axes().transposed(), + m_relPosA - A->get_center_of_mass(), + m_relPosB - B->get_center_of_mass(), + normalWorld, + A->get_inv_inertia(), + A->get_inv_mass(), + B->get_inv_inertia(), + B->get_inv_mass())); m_jacLinDiagABInv[i] = real_t(1.) / m_jacLin[i].getDiagonal(); m_depth[i] = m_delta.dot(normalWorld); - } + } testLinLimits(); - // angular part - for(i = 0; i < 3; i++) - { + // angular part + for (i = 0; i < 3; i++) { normalWorld = m_calculatedTransformA.basis.get_axis(i); - memnew_placement(&m_jacAng[i], JacobianEntrySW( - normalWorld, - A->get_principal_inertia_axes().transposed(), - B->get_principal_inertia_axes().transposed(), - A->get_inv_inertia(), - B->get_inv_inertia() - )); + memnew_placement(&m_jacAng[i], JacobianEntrySW( + normalWorld, + A->get_principal_inertia_axes().transposed(), + B->get_principal_inertia_axes().transposed(), + A->get_inv_inertia(), + B->get_inv_inertia())); } testAngLimits(); Vector3 axisA = m_calculatedTransformA.basis.get_axis(0); - m_kAngle = real_t(1.0 )/ (A->compute_angular_impulse_denominator(axisA) + B->compute_angular_impulse_denominator(axisA)); + m_kAngle = real_t(1.0) / (A->compute_angular_impulse_denominator(axisA) + B->compute_angular_impulse_denominator(axisA)); // clear accumulator for motors m_accumulatedLinMotorImpulse = real_t(0.0); m_accumulatedAngMotorImpulse = real_t(0.0); @@ -172,14 +160,13 @@ bool SliderJointSW::setup(real_t p_step) void SliderJointSW::solve(real_t p_step) { - int i; - // linear - Vector3 velA = A->get_velocity_in_local_point(m_relPosA); - Vector3 velB = B->get_velocity_in_local_point(m_relPosB); - Vector3 vel = velA - velB; - for(i = 0; i < 3; i++) - { - const Vector3& normal = m_jacLin[i].m_linearJointAxis; + int i; + // linear + Vector3 velA = A->get_velocity_in_local_point(m_relPosA); + Vector3 velB = B->get_velocity_in_local_point(m_relPosB); + Vector3 vel = velA - velB; + for (i = 0; i < 3; i++) { + const Vector3 &normal = m_jacLin[i].m_linearJointAxis; real_t rel_vel = normal.dot(vel); // calculate positional error real_t depth = m_depth[i]; @@ -190,81 +177,70 @@ void SliderJointSW::solve(real_t p_step) { // calcutate and apply impulse real_t normalImpulse = softness * (restitution * depth / p_step - damping * rel_vel) * m_jacLinDiagABInv[i]; Vector3 impulse_vector = normal * normalImpulse; - A->apply_impulse( m_relPosA, impulse_vector); - B->apply_impulse(m_relPosB,-impulse_vector); - if(m_poweredLinMotor && (!i)) - { // apply linear motor - if(m_accumulatedLinMotorImpulse < m_maxLinMotorForce) - { + A->apply_impulse(m_relPosA, impulse_vector); + B->apply_impulse(m_relPosB, -impulse_vector); + if (m_poweredLinMotor && (!i)) { // apply linear motor + if (m_accumulatedLinMotorImpulse < m_maxLinMotorForce) { real_t desiredMotorVel = m_targetLinMotorVelocity; real_t motor_relvel = desiredMotorVel + rel_vel; normalImpulse = -motor_relvel * m_jacLinDiagABInv[i]; // clamp accumulated impulse real_t new_acc = m_accumulatedLinMotorImpulse + Math::abs(normalImpulse); - if(new_acc > m_maxLinMotorForce) - { + if (new_acc > m_maxLinMotorForce) { new_acc = m_maxLinMotorForce; } - real_t del = new_acc - m_accumulatedLinMotorImpulse; - if(normalImpulse < real_t(0.0)) - { + real_t del = new_acc - m_accumulatedLinMotorImpulse; + if (normalImpulse < real_t(0.0)) { normalImpulse = -del; - } - else - { + } else { normalImpulse = del; } m_accumulatedLinMotorImpulse = new_acc; // apply clamped impulse impulse_vector = normal * normalImpulse; - A->apply_impulse( m_relPosA, impulse_vector); - B->apply_impulse( m_relPosB,-impulse_vector); + A->apply_impulse(m_relPosA, impulse_vector); + B->apply_impulse(m_relPosB, -impulse_vector); } } - } + } // angular // get axes in world space - Vector3 axisA = m_calculatedTransformA.basis.get_axis(0); - Vector3 axisB = m_calculatedTransformB.basis.get_axis(0); + Vector3 axisA = m_calculatedTransformA.basis.get_axis(0); + Vector3 axisB = m_calculatedTransformB.basis.get_axis(0); - const Vector3& angVelA = A->get_angular_velocity(); - const Vector3& angVelB = B->get_angular_velocity(); + const Vector3 &angVelA = A->get_angular_velocity(); + const Vector3 &angVelB = B->get_angular_velocity(); Vector3 angVelAroundAxisA = axisA * axisA.dot(angVelA); Vector3 angVelAroundAxisB = axisB * axisB.dot(angVelB); Vector3 angAorthog = angVelA - angVelAroundAxisA; Vector3 angBorthog = angVelB - angVelAroundAxisB; - Vector3 velrelOrthog = angAorthog-angBorthog; + Vector3 velrelOrthog = angAorthog - angBorthog; //solve orthogonal angular velocity correction real_t len = velrelOrthog.length(); - if (len > real_t(0.00001)) - { + if (len > real_t(0.00001)) { Vector3 normal = velrelOrthog.normalized(); real_t denom = A->compute_angular_impulse_denominator(normal) + B->compute_angular_impulse_denominator(normal); - velrelOrthog *= (real_t(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng; + velrelOrthog *= (real_t(1.) / denom) * m_dampingOrthoAng * m_softnessOrthoAng; } //solve angular positional correction - Vector3 angularError = axisA.cross(axisB) *(real_t(1.)/p_step); + Vector3 angularError = axisA.cross(axisB) * (real_t(1.) / p_step); real_t len2 = angularError.length(); - if (len2>real_t(0.00001)) - { + if (len2 > real_t(0.00001)) { Vector3 normal2 = angularError.normalized(); real_t denom2 = A->compute_angular_impulse_denominator(normal2) + B->compute_angular_impulse_denominator(normal2); - angularError *= (real_t(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng; + angularError *= (real_t(1.) / denom2) * m_restitutionOrthoAng * m_softnessOrthoAng; } // apply impulse - A->apply_torque_impulse(-velrelOrthog+angularError); - B->apply_torque_impulse(velrelOrthog-angularError); + A->apply_torque_impulse(-velrelOrthog + angularError); + B->apply_torque_impulse(velrelOrthog - angularError); real_t impulseMag; //solve angular limits - if(m_solveAngLim) - { + if (m_solveAngLim) { impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingLimAng + m_angDepth * m_restitutionLimAng / p_step; impulseMag *= m_kAngle * m_softnessLimAng; - } - else - { + } else { impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingDirAng + m_angDepth * m_restitutionDirAng / p_step; impulseMag *= m_kAngle * m_softnessDirAng; } @@ -272,10 +248,8 @@ void SliderJointSW::solve(real_t p_step) { A->apply_torque_impulse(impulse); B->apply_torque_impulse(-impulse); //apply angular motor - if(m_poweredAngMotor) - { - if(m_accumulatedAngMotorImpulse < m_maxAngMotorForce) - { + if (m_poweredAngMotor) { + if (m_accumulatedAngMotorImpulse < m_maxAngMotorForce) { Vector3 velrel = angVelAroundAxisA - angVelAroundAxisB; real_t projRelVel = velrel.dot(axisA); @@ -285,17 +259,13 @@ void SliderJointSW::solve(real_t p_step) { real_t angImpulse = m_kAngle * motor_relvel; // clamp accumulated impulse real_t new_acc = m_accumulatedAngMotorImpulse + Math::abs(angImpulse); - if(new_acc > m_maxAngMotorForce) - { + if (new_acc > m_maxAngMotorForce) { new_acc = m_maxAngMotorForce; } - real_t del = new_acc - m_accumulatedAngMotorImpulse; - if(angImpulse < real_t(0.0)) - { + real_t del = new_acc - m_accumulatedAngMotorImpulse; + if (angImpulse < real_t(0.0)) { angImpulse = -del; - } - else - { + } else { angImpulse = del; } m_accumulatedAngMotorImpulse = new_acc; @@ -311,96 +281,75 @@ void SliderJointSW::solve(real_t p_step) { //----------------------------------------------------------------------------- -void SliderJointSW::calculateTransforms(void){ - m_calculatedTransformA = A->get_transform() * m_frameInA ; +void SliderJointSW::calculateTransforms(void) { + m_calculatedTransformA = A->get_transform() * m_frameInA; m_calculatedTransformB = B->get_transform() * m_frameInB; m_realPivotAInW = m_calculatedTransformA.origin; m_realPivotBInW = m_calculatedTransformB.origin; m_sliderAxis = m_calculatedTransformA.basis.get_axis(0); // along X m_delta = m_realPivotBInW - m_realPivotAInW; m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis; - Vector3 normalWorld; - int i; - //linear part - for(i = 0; i < 3; i++) - { + Vector3 normalWorld; + int i; + //linear part + for (i = 0; i < 3; i++) { normalWorld = m_calculatedTransformA.basis.get_axis(i); m_depth[i] = m_delta.dot(normalWorld); - } + } } // SliderJointSW::calculateTransforms() //----------------------------------------------------------------------------- -void SliderJointSW::testLinLimits(void) -{ +void SliderJointSW::testLinLimits(void) { m_solveLinLim = false; m_linPos = m_depth[0]; - if(m_lowerLinLimit <= m_upperLinLimit) - { - if(m_depth[0] > m_upperLinLimit) - { + if (m_lowerLinLimit <= m_upperLinLimit) { + if (m_depth[0] > m_upperLinLimit) { m_depth[0] -= m_upperLinLimit; m_solveLinLim = true; - } - else if(m_depth[0] < m_lowerLinLimit) - { + } else if (m_depth[0] < m_lowerLinLimit) { m_depth[0] -= m_lowerLinLimit; m_solveLinLim = true; - } - else - { + } else { m_depth[0] = real_t(0.); } - } - else - { + } else { m_depth[0] = real_t(0.); } } // SliderJointSW::testLinLimits() //----------------------------------------------------------------------------- - -void SliderJointSW::testAngLimits(void) -{ +void SliderJointSW::testAngLimits(void) { m_angDepth = real_t(0.); m_solveAngLim = false; - if(m_lowerAngLimit <= m_upperAngLimit) - { + if (m_lowerAngLimit <= m_upperAngLimit) { const Vector3 axisA0 = m_calculatedTransformA.basis.get_axis(1); const Vector3 axisA1 = m_calculatedTransformA.basis.get_axis(2); const Vector3 axisB0 = m_calculatedTransformB.basis.get_axis(1); real_t rot = atan2fast(axisB0.dot(axisA1), axisB0.dot(axisA0)); - if(rot < m_lowerAngLimit) - { + if (rot < m_lowerAngLimit) { m_angDepth = rot - m_lowerAngLimit; m_solveAngLim = true; - } - else if(rot > m_upperAngLimit) - { + } else if (rot > m_upperAngLimit) { m_angDepth = rot - m_upperAngLimit; m_solveAngLim = true; } } } // SliderJointSW::testAngLimits() - //----------------------------------------------------------------------------- - - -Vector3 SliderJointSW::getAncorInA(void) -{ +Vector3 SliderJointSW::getAncorInA(void) { Vector3 ancorInA; ancorInA = m_realPivotAInW + (m_lowerLinLimit + m_upperLinLimit) * real_t(0.5) * m_sliderAxis; - ancorInA = A->get_transform().inverse().xform( ancorInA ); + ancorInA = A->get_transform().inverse().xform(ancorInA); return ancorInA; } // SliderJointSW::getAncorInA() //----------------------------------------------------------------------------- -Vector3 SliderJointSW::getAncorInB(void) -{ +Vector3 SliderJointSW::getAncorInB(void) { Vector3 ancorInB; ancorInB = m_frameInB.origin; return ancorInB; @@ -408,38 +357,36 @@ Vector3 SliderJointSW::getAncorInB(void) void SliderJointSW::set_param(PhysicsServer::SliderJointParam p_param, real_t p_value) { - switch(p_param) { - case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_UPPER: m_upperLinLimit=p_value; break; - case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_LOWER: m_lowerLinLimit=p_value; break; - case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS: m_softnessLimLin=p_value; break; - case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION: m_restitutionLimLin=p_value; break; - case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_DAMPING: m_dampingLimLin=p_value; break; - case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_SOFTNESS: m_softnessDirLin=p_value; break; - case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_RESTITUTION: m_restitutionDirLin=p_value; break; - case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_DAMPING: m_dampingDirLin=p_value; break; - case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS: m_softnessOrthoLin=p_value; break; - case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION: m_restitutionOrthoLin=p_value; break; - case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING: m_dampingOrthoLin=p_value; break; - - case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_UPPER: m_upperAngLimit=p_value; break; - case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_LOWER: m_lowerAngLimit=p_value; break; - case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS: m_softnessLimAng=p_value; break; - case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION: m_restitutionLimAng=p_value; break; - case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_DAMPING: m_dampingLimAng=p_value; break; - case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS: m_softnessDirAng=p_value; break; - case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION: m_restitutionDirAng=p_value; break; - case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_DAMPING: m_dampingDirAng=p_value; break; - case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS: m_softnessOrthoAng=p_value; break; - case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION: m_restitutionOrthoAng=p_value; break; - case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING: m_dampingOrthoAng=p_value; break; + switch (p_param) { + case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_UPPER: m_upperLinLimit = p_value; break; + case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_LOWER: m_lowerLinLimit = p_value; break; + case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS: m_softnessLimLin = p_value; break; + case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION: m_restitutionLimLin = p_value; break; + case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_DAMPING: m_dampingLimLin = p_value; break; + case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_SOFTNESS: m_softnessDirLin = p_value; break; + case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_RESTITUTION: m_restitutionDirLin = p_value; break; + case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_DAMPING: m_dampingDirLin = p_value; break; + case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS: m_softnessOrthoLin = p_value; break; + case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION: m_restitutionOrthoLin = p_value; break; + case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING: m_dampingOrthoLin = p_value; break; + case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_UPPER: m_upperAngLimit = p_value; break; + case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_LOWER: m_lowerAngLimit = p_value; break; + case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS: m_softnessLimAng = p_value; break; + case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION: m_restitutionLimAng = p_value; break; + case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_DAMPING: m_dampingLimAng = p_value; break; + case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS: m_softnessDirAng = p_value; break; + case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION: m_restitutionDirAng = p_value; break; + case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_DAMPING: m_dampingDirAng = p_value; break; + case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS: m_softnessOrthoAng = p_value; break; + case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION: m_restitutionOrthoAng = p_value; break; + case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING: m_dampingOrthoAng = p_value; break; } - } real_t SliderJointSW::get_param(PhysicsServer::SliderJointParam p_param) const { - switch(p_param) { + switch (p_param) { case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_UPPER: return m_upperLinLimit; case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_LOWER: return m_lowerLinLimit; case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS: return m_softnessLimLin; @@ -463,11 +410,7 @@ real_t SliderJointSW::get_param(PhysicsServer::SliderJointParam p_param) const { case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS: return m_softnessOrthoAng; case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION: return m_restitutionOrthoAng; case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING: return m_dampingOrthoAng; - } return 0; - } - - diff --git a/servers/physics/joints/slider_joint_sw.h b/servers/physics/joints/slider_joint_sw.h index d01038df5..faf36bfe9 100644 --- a/servers/physics/joints/slider_joint_sw.h +++ b/servers/physics/joints/slider_joint_sw.h @@ -34,9 +34,8 @@ Adapted to Godot from the Bullet library. #ifndef SLIDER_JOINT_SW_H #define SLIDER_JOINT_SW_H -#include "servers/physics/joints_sw.h" #include "servers/physics/joints/jacobian_entry_sw.h" - +#include "servers/physics/joints_sw.h" /* Bullet Continuous Collision Detection and Physics Library @@ -59,15 +58,14 @@ April 04, 2008 */ -#define SLIDER_CONSTRAINT_DEF_SOFTNESS (real_t(1.0)) -#define SLIDER_CONSTRAINT_DEF_DAMPING (real_t(1.0)) -#define SLIDER_CONSTRAINT_DEF_RESTITUTION (real_t(0.7)) +#define SLIDER_CONSTRAINT_DEF_SOFTNESS (real_t(1.0)) +#define SLIDER_CONSTRAINT_DEF_DAMPING (real_t(1.0)) +#define SLIDER_CONSTRAINT_DEF_RESTITUTION (real_t(0.7)) //----------------------------------------------------------------------------- class SliderJointSW : public JointSW { protected: - union { struct { BodySW *A; @@ -77,8 +75,8 @@ protected: BodySW *_arr[2]; }; - Transform m_frameInA; - Transform m_frameInB; + Transform m_frameInA; + Transform m_frameInB; // linear limits real_t m_lowerLinLimit; @@ -115,14 +113,14 @@ protected: bool m_solveLinLim; bool m_solveAngLim; - JacobianEntrySW m_jacLin[3]; - real_t m_jacLinDiagABInv[3]; + JacobianEntrySW m_jacLin[3]; + real_t m_jacLinDiagABInv[3]; - JacobianEntrySW m_jacAng[3]; + JacobianEntrySW m_jacAng[3]; real_t m_timeStep; - Transform m_calculatedTransformA; - Transform m_calculatedTransformB; + Transform m_calculatedTransformA; + Transform m_calculatedTransformB; Vector3 m_sliderAxis; Vector3 m_realPivotAInW; @@ -138,45 +136,46 @@ protected: real_t m_angDepth; real_t m_kAngle; - bool m_poweredLinMotor; - real_t m_targetLinMotorVelocity; - real_t m_maxLinMotorForce; - real_t m_accumulatedLinMotorImpulse; + bool m_poweredLinMotor; + real_t m_targetLinMotorVelocity; + real_t m_maxLinMotorForce; + real_t m_accumulatedLinMotorImpulse; - bool m_poweredAngMotor; - real_t m_targetAngMotorVelocity; - real_t m_maxAngMotorForce; - real_t m_accumulatedAngMotorImpulse; + bool m_poweredAngMotor; + real_t m_targetAngMotorVelocity; + real_t m_maxAngMotorForce; + real_t m_accumulatedAngMotorImpulse; //------------------------ void initParams(); + public: // constructors - SliderJointSW(BodySW* rbA, BodySW* rbB, const Transform& frameInA, const Transform& frameInB); - //SliderJointSW(); + SliderJointSW(BodySW *rbA, BodySW *rbB, const Transform &frameInA, const Transform &frameInB); + //SliderJointSW(); // overrides // access - const BodySW* getRigidBodyA() const { return A; } - const BodySW* getRigidBodyB() const { return B; } - const Transform & getCalculatedTransformA() const { return m_calculatedTransformA; } - const Transform & getCalculatedTransformB() const { return m_calculatedTransformB; } - const Transform & getFrameOffsetA() const { return m_frameInA; } - const Transform & getFrameOffsetB() const { return m_frameInB; } - Transform & getFrameOffsetA() { return m_frameInA; } - Transform & getFrameOffsetB() { return m_frameInB; } - real_t getLowerLinLimit() { return m_lowerLinLimit; } - void setLowerLinLimit(real_t lowerLimit) { m_lowerLinLimit = lowerLimit; } - real_t getUpperLinLimit() { return m_upperLinLimit; } - void setUpperLinLimit(real_t upperLimit) { m_upperLinLimit = upperLimit; } - real_t getLowerAngLimit() { return m_lowerAngLimit; } - void setLowerAngLimit(real_t lowerLimit) { m_lowerAngLimit = lowerLimit; } - real_t getUpperAngLimit() { return m_upperAngLimit; } - void setUpperAngLimit(real_t upperLimit) { m_upperAngLimit = upperLimit; } + const BodySW *getRigidBodyA() const { return A; } + const BodySW *getRigidBodyB() const { return B; } + const Transform &getCalculatedTransformA() const { return m_calculatedTransformA; } + const Transform &getCalculatedTransformB() const { return m_calculatedTransformB; } + const Transform &getFrameOffsetA() const { return m_frameInA; } + const Transform &getFrameOffsetB() const { return m_frameInB; } + Transform &getFrameOffsetA() { return m_frameInA; } + Transform &getFrameOffsetB() { return m_frameInB; } + real_t getLowerLinLimit() { return m_lowerLinLimit; } + void setLowerLinLimit(real_t lowerLimit) { m_lowerLinLimit = lowerLimit; } + real_t getUpperLinLimit() { return m_upperLinLimit; } + void setUpperLinLimit(real_t upperLimit) { m_upperLinLimit = upperLimit; } + real_t getLowerAngLimit() { return m_lowerAngLimit; } + void setLowerAngLimit(real_t lowerLimit) { m_lowerAngLimit = lowerLimit; } + real_t getUpperAngLimit() { return m_upperAngLimit; } + void setUpperAngLimit(real_t upperLimit) { m_upperAngLimit = upperLimit; } real_t getSoftnessDirLin() { return m_softnessDirLin; } real_t getRestitutionDirLin() { return m_restitutionDirLin; } - real_t getDampingDirLin() { return m_dampingDirLin ; } + real_t getDampingDirLin() { return m_dampingDirLin; } real_t getSoftnessDirAng() { return m_softnessDirAng; } real_t getRestitutionDirAng() { return m_restitutionDirAng; } real_t getDampingDirAng() { return m_dampingDirAng; } @@ -230,9 +229,9 @@ public: bool getSolveAngLimit() { return m_solveAngLim; } real_t getAngDepth() { return m_angDepth; } // shared code used by ODE solver - void calculateTransforms(void); - void testLinLimits(void); - void testAngLimits(void); + void calculateTransforms(void); + void testLinLimits(void); + void testAngLimits(void); // access for PE Solver Vector3 getAncorInA(void); Vector3 getAncorInB(void); @@ -244,8 +243,6 @@ public: void solve(real_t p_step); virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_SLIDER; } - }; - #endif // SLIDER_JOINT_SW_H diff --git a/servers/physics/joints_sw.h b/servers/physics/joints_sw.h index c87c86b59..0f637faf7 100644 --- a/servers/physics/joints_sw.h +++ b/servers/physics/joints_sw.h @@ -29,19 +29,16 @@ #ifndef JOINTS_SW_H #define JOINTS_SW_H -#include "constraint_sw.h" #include "body_sw.h" - +#include "constraint_sw.h" class JointSW : public ConstraintSW { - public: - - virtual PhysicsServer::JointType get_type() const=0; - _FORCE_INLINE_ JointSW(BodySW **p_body_ptr=NULL,int p_body_count=0) : ConstraintSW(p_body_ptr,p_body_count) { + virtual PhysicsServer::JointType get_type() const = 0; + _FORCE_INLINE_ JointSW(BodySW **p_body_ptr = NULL, int p_body_count = 0) + : ConstraintSW(p_body_ptr, p_body_count) { } - }; #endif // JOINTS_SW_H diff --git a/servers/physics/physics_server_sw.cpp b/servers/physics/physics_server_sw.cpp index 67e3b2785..37be0a8a1 100644 --- a/servers/physics/physics_server_sw.cpp +++ b/servers/physics/physics_server_sw.cpp @@ -29,57 +29,56 @@ #include "physics_server_sw.h" #include "broad_phase_basic.h" #include "broad_phase_octree.h" -#include "joints/pin_joint_sw.h" -#include "joints/hinge_joint_sw.h" -#include "joints/slider_joint_sw.h" #include "joints/cone_twist_joint_sw.h" #include "joints/generic_6dof_joint_sw.h" -#include "script_language.h" +#include "joints/hinge_joint_sw.h" +#include "joints/pin_joint_sw.h" +#include "joints/slider_joint_sw.h" #include "os/os.h" +#include "script_language.h" RID PhysicsServerSW::shape_create(ShapeType p_shape) { - ShapeSW *shape=NULL; - switch(p_shape) { + ShapeSW *shape = NULL; + switch (p_shape) { case SHAPE_PLANE: { - shape=memnew( PlaneShapeSW ); + shape = memnew(PlaneShapeSW); } break; case SHAPE_RAY: { - shape=memnew( RayShapeSW ); + shape = memnew(RayShapeSW); } break; case SHAPE_SPHERE: { - shape=memnew( SphereShapeSW); + shape = memnew(SphereShapeSW); } break; case SHAPE_BOX: { - shape=memnew( BoxShapeSW); + shape = memnew(BoxShapeSW); } break; case SHAPE_CAPSULE: { - shape=memnew( CapsuleShapeSW ); + shape = memnew(CapsuleShapeSW); } break; case SHAPE_CONVEX_POLYGON: { - shape=memnew( ConvexPolygonShapeSW ); + shape = memnew(ConvexPolygonShapeSW); } break; case SHAPE_CONCAVE_POLYGON: { - shape=memnew( ConcavePolygonShapeSW ); + shape = memnew(ConcavePolygonShapeSW); } break; case SHAPE_HEIGHTMAP: { - shape=memnew( HeightMapShapeSW ); + shape = memnew(HeightMapShapeSW); } break; case SHAPE_CUSTOM: { ERR_FAIL_V(RID()); } break; - } RID id = shape_owner.make_rid(shape); @@ -88,71 +87,62 @@ RID PhysicsServerSW::shape_create(ShapeType p_shape) { return id; }; -void PhysicsServerSW::shape_set_data(RID p_shape, const Variant& p_data) { +void PhysicsServerSW::shape_set_data(RID p_shape, const Variant &p_data) { ShapeSW *shape = shape_owner.get(p_shape); ERR_FAIL_COND(!shape); shape->set_data(p_data); - - }; - void PhysicsServerSW::shape_set_custom_solver_bias(RID p_shape, real_t p_bias) { ShapeSW *shape = shape_owner.get(p_shape); ERR_FAIL_COND(!shape); shape->set_custom_bias(p_bias); - } - PhysicsServer::ShapeType PhysicsServerSW::shape_get_type(RID p_shape) const { const ShapeSW *shape = shape_owner.get(p_shape); - ERR_FAIL_COND_V(!shape,SHAPE_CUSTOM); + ERR_FAIL_COND_V(!shape, SHAPE_CUSTOM); return shape->get_type(); - }; Variant PhysicsServerSW::shape_get_data(RID p_shape) const { const ShapeSW *shape = shape_owner.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 PhysicsServerSW::shape_get_custom_solver_bias(RID p_shape) const { const ShapeSW *shape = shape_owner.get(p_shape); - ERR_FAIL_COND_V(!shape,0); + ERR_FAIL_COND_V(!shape, 0); return shape->get_custom_bias(); - } - RID PhysicsServerSW::space_create() { - SpaceSW *space = memnew( SpaceSW ); + SpaceSW *space = memnew(SpaceSW); RID id = space_owner.make_rid(space); space->set_self(id); RID area_id = area_create(); AreaSW *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); RID sgb = body_create(); - body_set_space(sgb,id); - body_set_mode(sgb,BODY_MODE_STATIC); + body_set_space(sgb, id); + body_set_mode(sgb, BODY_MODE_STATIC); space->set_static_global_body(sgb); return id; }; -void PhysicsServerSW::space_set_active(RID p_space,bool p_active) { +void PhysicsServerSW::space_set_active(RID p_space, bool p_active) { SpaceSW *space = space_owner.get(p_space); ERR_FAIL_COND(!space); @@ -165,70 +155,63 @@ void PhysicsServerSW::space_set_active(RID p_space,bool p_active) { bool PhysicsServerSW::space_is_active(RID p_space) const { const SpaceSW *space = space_owner.get(p_space); - ERR_FAIL_COND_V(!space,false); + ERR_FAIL_COND_V(!space, false); return active_spaces.has(space); - } -void PhysicsServerSW::space_set_param(RID p_space,SpaceParameter p_param, real_t p_value) { +void PhysicsServerSW::space_set_param(RID p_space, SpaceParameter p_param, real_t p_value) { SpaceSW *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 PhysicsServerSW::space_get_param(RID p_space,SpaceParameter p_param) const { +real_t PhysicsServerSW::space_get_param(RID p_space, SpaceParameter p_param) const { const SpaceSW *space = space_owner.get(p_space); - ERR_FAIL_COND_V(!space,0); + ERR_FAIL_COND_V(!space, 0); return space->get_param(p_param); } -PhysicsDirectSpaceState* PhysicsServerSW::space_get_direct_state(RID p_space) { +PhysicsDirectSpaceState *PhysicsServerSW::space_get_direct_state(RID p_space) { SpaceSW *space = space_owner.get(p_space); - ERR_FAIL_COND_V(!space,NULL); + ERR_FAIL_COND_V(!space, NULL); if (!doing_sync || space->is_locked()) { ERR_EXPLAIN("Space state is inaccesible right now, wait for iteration or fixed process notification."); ERR_FAIL_V(NULL); - - } return space->get_direct_state(); } -void PhysicsServerSW::space_set_debug_contacts(RID p_space,int p_max_contacts) { +void PhysicsServerSW::space_set_debug_contacts(RID p_space, int p_max_contacts) { SpaceSW *space = space_owner.get(p_space); ERR_FAIL_COND(!space); space->set_debug_contacts(p_max_contacts); - } Vector<Vector3> PhysicsServerSW::space_get_contacts(RID p_space) const { SpaceSW *space = space_owner.get(p_space); - ERR_FAIL_COND_V(!space,Vector<Vector3>()); + ERR_FAIL_COND_V(!space, Vector<Vector3>()); return space->get_debug_contacts(); - } int PhysicsServerSW::space_get_contact_count(RID p_space) const { SpaceSW *space = space_owner.get(p_space); - ERR_FAIL_COND_V(!space,0); + ERR_FAIL_COND_V(!space, 0); return space->get_debug_contact_count(); - } RID PhysicsServerSW::area_create() { - AreaSW *area = memnew( AreaSW ); + AreaSW *area = memnew(AreaSW); RID rid = area_owner.make_rid(area); area->set_self(rid); return rid; @@ -238,20 +221,19 @@ void PhysicsServerSW::area_set_space(RID p_area, RID p_space) { AreaSW *area = area_owner.get(p_area); ERR_FAIL_COND(!area); - SpaceSW *space=NULL; + SpaceSW *space = NULL; if (p_space.is_valid()) { space = space_owner.get(p_space); ERR_FAIL_COND(!space); } area->set_space(space); - }; RID PhysicsServerSW::area_get_space(RID p_area) const { AreaSW *area = area_owner.get(p_area); - ERR_FAIL_COND_V(!area,RID()); + ERR_FAIL_COND_V(!area, RID()); SpaceSW *space = area->get_space(); if (!space) @@ -261,7 +243,6 @@ RID PhysicsServerSW::area_get_space(RID p_area) const { void PhysicsServerSW::area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode) { - AreaSW *area = area_owner.get(p_area); ERR_FAIL_COND(!area); @@ -271,13 +252,12 @@ void PhysicsServerSW::area_set_space_override_mode(RID p_area, AreaSpaceOverride PhysicsServer::AreaSpaceOverrideMode PhysicsServerSW::area_get_space_override_mode(RID p_area) const { const AreaSW *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 PhysicsServerSW::area_add_shape(RID p_area, RID p_shape, const Transform& p_transform) { +void PhysicsServerSW::area_add_shape(RID p_area, RID p_shape, const Transform &p_transform) { AreaSW *area = area_owner.get(p_area); ERR_FAIL_COND(!area); @@ -285,11 +265,10 @@ void PhysicsServerSW::area_add_shape(RID p_area, RID p_shape, const Transform& p ShapeSW *shape = shape_owner.get(p_shape); ERR_FAIL_COND(!shape); - area->add_shape(shape,p_transform); - + area->add_shape(shape, p_transform); } -void PhysicsServerSW::area_set_shape(RID p_area, int p_shape_idx,RID p_shape) { +void PhysicsServerSW::area_set_shape(RID p_area, int p_shape_idx, RID p_shape) { AreaSW *area = area_owner.get(p_area); ERR_FAIL_COND(!area); @@ -298,39 +277,37 @@ void PhysicsServerSW::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 PhysicsServerSW::area_set_shape_transform(RID p_area, int p_shape_idx, const Transform& p_transform) { +void PhysicsServerSW::area_set_shape_transform(RID p_area, int p_shape_idx, const Transform &p_transform) { AreaSW *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 PhysicsServerSW::area_get_shape_count(RID p_area) const { AreaSW *area = area_owner.get(p_area); - ERR_FAIL_COND_V(!area,-1); + ERR_FAIL_COND_V(!area, -1); return area->get_shape_count(); - } RID PhysicsServerSW::area_get_shape(RID p_area, int p_shape_idx) const { AreaSW *area = area_owner.get(p_area); - ERR_FAIL_COND_V(!area,RID()); + ERR_FAIL_COND_V(!area, RID()); ShapeSW *shape = area->get_shape(p_shape_idx); - ERR_FAIL_COND_V(!shape,RID()); + ERR_FAIL_COND_V(!shape, RID()); return shape->get_self(); } Transform PhysicsServerSW::area_get_shape_transform(RID p_area, int p_shape_idx) const { AreaSW *area = area_owner.get(p_area); - ERR_FAIL_COND_V(!area,Transform()); + ERR_FAIL_COND_V(!area, Transform()); return area->get_shape_transform(p_shape_idx); } @@ -348,65 +325,57 @@ void PhysicsServerSW::area_clear_shapes(RID p_area) { AreaSW *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 PhysicsServerSW::area_attach_object_instance_ID(RID p_area,ObjectID p_ID) { +void PhysicsServerSW::area_attach_object_instance_ID(RID p_area, ObjectID p_ID) { if (space_owner.owns(p_area)) { - SpaceSW *space=space_owner.get(p_area); - p_area=space->get_default_area()->get_self(); + SpaceSW *space = space_owner.get(p_area); + p_area = space->get_default_area()->get_self(); } AreaSW *area = area_owner.get(p_area); ERR_FAIL_COND(!area); area->set_instance_id(p_ID); - } ObjectID PhysicsServerSW::area_get_object_instance_ID(RID p_area) const { if (space_owner.owns(p_area)) { - SpaceSW *space=space_owner.get(p_area); - p_area=space->get_default_area()->get_self(); + SpaceSW *space = space_owner.get(p_area); + p_area = space->get_default_area()->get_self(); } AreaSW *area = area_owner.get(p_area); - ERR_FAIL_COND_V(!area,0); + ERR_FAIL_COND_V(!area, 0); return area->get_instance_id(); - - } - -void PhysicsServerSW::area_set_param(RID p_area,AreaParameter p_param,const Variant& p_value) { +void PhysicsServerSW::area_set_param(RID p_area, AreaParameter p_param, const Variant &p_value) { if (space_owner.owns(p_area)) { - SpaceSW *space=space_owner.get(p_area); - p_area=space->get_default_area()->get_self(); + SpaceSW *space = space_owner.get(p_area); + p_area = space->get_default_area()->get_self(); } AreaSW *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 PhysicsServerSW::area_set_transform(RID p_area, const Transform& p_transform) { +void PhysicsServerSW::area_set_transform(RID p_area, const Transform &p_transform) { AreaSW *area = area_owner.get(p_area); ERR_FAIL_COND(!area); area->set_transform(p_transform); - }; -Variant PhysicsServerSW::area_get_param(RID p_area,AreaParameter p_param) const { +Variant PhysicsServerSW::area_get_param(RID p_area, AreaParameter p_param) const { if (space_owner.owns(p_area)) { - SpaceSW *space=space_owner.get(p_area); - p_area=space->get_default_area()->get_self(); + SpaceSW *space = space_owner.get(p_area); + p_area = space->get_default_area()->get_self(); } AreaSW *area = area_owner.get(p_area); - ERR_FAIL_COND_V(!area,Variant()); + ERR_FAIL_COND_V(!area, Variant()); return area->get_param(p_param); }; @@ -414,12 +383,12 @@ Variant PhysicsServerSW::area_get_param(RID p_area,AreaParameter p_param) const Transform PhysicsServerSW::area_get_transform(RID p_area) const { AreaSW *area = area_owner.get(p_area); - ERR_FAIL_COND_V(!area,Transform()); + ERR_FAIL_COND_V(!area, Transform()); return area->get_transform(); }; -void PhysicsServerSW::area_set_layer_mask(RID p_area,uint32_t p_mask) { +void PhysicsServerSW::area_set_layer_mask(RID p_area, uint32_t p_mask) { AreaSW *area = area_owner.get(p_area); ERR_FAIL_COND(!area); @@ -427,7 +396,7 @@ void PhysicsServerSW::area_set_layer_mask(RID p_area,uint32_t p_mask) { area->set_layer_mask(p_mask); } -void PhysicsServerSW::area_set_collision_mask(RID p_area,uint32_t p_mask) { +void PhysicsServerSW::area_set_collision_mask(RID p_area, uint32_t p_mask) { AreaSW *area = area_owner.get(p_area); ERR_FAIL_COND(!area); @@ -435,7 +404,7 @@ void PhysicsServerSW::area_set_collision_mask(RID p_area,uint32_t p_mask) { area->set_collision_mask(p_mask); } -void PhysicsServerSW::area_set_monitorable(RID p_area,bool p_monitorable) { +void PhysicsServerSW::area_set_monitorable(RID p_area, bool p_monitorable) { AreaSW *area = area_owner.get(p_area); ERR_FAIL_COND(!area); @@ -443,81 +412,73 @@ void PhysicsServerSW::area_set_monitorable(RID p_area,bool p_monitorable) { area->set_monitorable(p_monitorable); } -void PhysicsServerSW::area_set_monitor_callback(RID p_area,Object *p_receiver,const StringName& p_method) { +void PhysicsServerSW::area_set_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) { AreaSW *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 PhysicsServerSW::area_set_ray_pickable(RID p_area,bool p_enable) { +void PhysicsServerSW::area_set_ray_pickable(RID p_area, bool p_enable) { AreaSW *area = area_owner.get(p_area); ERR_FAIL_COND(!area); area->set_ray_pickable(p_enable); - } -bool PhysicsServerSW::area_is_ray_pickable(RID p_area) const{ +bool PhysicsServerSW::area_is_ray_pickable(RID p_area) const { AreaSW *area = area_owner.get(p_area); - ERR_FAIL_COND_V(!area,false); + ERR_FAIL_COND_V(!area, false); return area->is_ray_pickable(); - } - -void PhysicsServerSW::area_set_area_monitor_callback(RID p_area,Object *p_receiver,const StringName& p_method) { - +void PhysicsServerSW::area_set_area_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) { AreaSW *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 PhysicsServerSW::body_create(BodyMode p_mode,bool p_init_sleeping) { +RID PhysicsServerSW::body_create(BodyMode p_mode, bool p_init_sleeping) { - BodySW *body = memnew( BodySW ); - if (p_mode!=BODY_MODE_RIGID) + BodySW *body = memnew(BodySW); + if (p_mode != BODY_MODE_RIGID) body->set_mode(p_mode); if (p_init_sleeping) - body->set_state(BODY_STATE_SLEEPING,p_init_sleeping); + body->set_state(BODY_STATE_SLEEPING, p_init_sleeping); RID rid = body_owner.make_rid(body); body->set_self(rid); return rid; }; - void PhysicsServerSW::body_set_space(RID p_body, RID p_space) { BodySW *body = body_owner.get(p_body); ERR_FAIL_COND(!body); - SpaceSW *space=NULL; + SpaceSW *space = NULL; if (p_space.is_valid()) { space = space_owner.get(p_space); ERR_FAIL_COND(!space); } - if (body->get_space()==space) + if (body->get_space() == space) return; //pointles body->set_space(space); - }; RID PhysicsServerSW::body_get_space(RID p_body) const { BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body,RID()); + ERR_FAIL_COND_V(!body, RID()); SpaceSW *space = body->get_space(); if (!space) @@ -525,7 +486,6 @@ RID PhysicsServerSW::body_get_space(RID p_body) const { return space->get_self(); }; - void PhysicsServerSW::body_set_mode(RID p_body, BodyMode p_mode) { BodySW *body = body_owner.get(p_body); @@ -537,12 +497,12 @@ void PhysicsServerSW::body_set_mode(RID p_body, BodyMode p_mode) { PhysicsServer::BodyMode PhysicsServerSW::body_get_mode(RID p_body) const { BodySW *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 PhysicsServerSW::body_add_shape(RID p_body, RID p_shape, const Transform& p_transform) { +void PhysicsServerSW::body_add_shape(RID p_body, RID p_shape, const Transform &p_transform) { BodySW *body = body_owner.get(p_body); ERR_FAIL_COND(!body); @@ -550,11 +510,10 @@ void PhysicsServerSW::body_add_shape(RID p_body, RID p_shape, const Transform& p ShapeSW *shape = shape_owner.get(p_shape); ERR_FAIL_COND(!shape); - body->add_shape(shape,p_transform); - + body->add_shape(shape, p_transform); } -void PhysicsServerSW::body_set_shape(RID p_body, int p_shape_idx,RID p_shape) { +void PhysicsServerSW::body_set_shape(RID p_body, int p_shape_idx, RID p_shape) { BodySW *body = body_owner.get(p_body); ERR_FAIL_COND(!body); @@ -563,59 +522,55 @@ void PhysicsServerSW::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 PhysicsServerSW::body_set_shape_transform(RID p_body, int p_shape_idx, const Transform& p_transform) { +void PhysicsServerSW::body_set_shape_transform(RID p_body, int p_shape_idx, const Transform &p_transform) { BodySW *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); } int PhysicsServerSW::body_get_shape_count(RID p_body) const { BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body,-1); + ERR_FAIL_COND_V(!body, -1); return body->get_shape_count(); - } RID PhysicsServerSW::body_get_shape(RID p_body, int p_shape_idx) const { BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body,RID()); + ERR_FAIL_COND_V(!body, RID()); ShapeSW *shape = body->get_shape(p_shape_idx); - ERR_FAIL_COND_V(!shape,RID()); + ERR_FAIL_COND_V(!shape, RID()); return shape->get_self(); } -void PhysicsServerSW::body_set_shape_as_trigger(RID p_body, int p_shape_idx,bool p_enable) { +void PhysicsServerSW::body_set_shape_as_trigger(RID p_body, int p_shape_idx, bool p_enable) { BodySW *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 PhysicsServerSW::body_is_shape_set_as_trigger(RID p_body, int p_shape_idx) const{ +bool PhysicsServerSW::body_is_shape_set_as_trigger(RID p_body, int p_shape_idx) const { BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body,false); - ERR_FAIL_INDEX_V(p_shape_idx,body->get_shape_count(),false); + ERR_FAIL_COND_V(!body, false); + ERR_FAIL_INDEX_V(p_shape_idx, body->get_shape_count(), false); return body->is_shape_set_as_trigger(p_shape_idx); } - Transform PhysicsServerSW::body_get_shape_transform(RID p_body, int p_shape_idx) const { BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body,Transform()); + ERR_FAIL_COND_V(!body, Transform()); return body->get_shape_transform(p_shape_idx); } @@ -633,24 +588,22 @@ void PhysicsServerSW::body_clear_shapes(RID p_body) { BodySW *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 PhysicsServerSW::body_set_enable_continuous_collision_detection(RID p_body,bool p_enable) { +void PhysicsServerSW::body_set_enable_continuous_collision_detection(RID p_body, bool p_enable) { BodySW *body = body_owner.get(p_body); ERR_FAIL_COND(!body); body->set_continuous_collision_detection(p_enable); - } bool PhysicsServerSW::body_is_continuous_collision_detection_enabled(RID p_body) const { BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body,false); + ERR_FAIL_COND_V(!body, false); return body->is_continuous_collision_detection_enabled(); } @@ -662,16 +615,14 @@ void PhysicsServerSW::body_set_layer_mask(RID p_body, uint32_t p_mask) { body->set_layer_mask(p_mask); body->wakeup(); - } -uint32_t PhysicsServerSW::body_get_layer_mask(RID p_body, uint32_t p_mask) const{ +uint32_t PhysicsServerSW::body_get_layer_mask(RID p_body, uint32_t p_mask) const { const BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body,0); + ERR_FAIL_COND_V(!body, 0); return body->get_layer_mask(); - } void PhysicsServerSW::body_set_collision_mask(RID p_body, uint32_t p_mask) { @@ -681,48 +632,42 @@ void PhysicsServerSW::body_set_collision_mask(RID p_body, uint32_t p_mask) { body->set_collision_mask(p_mask); body->wakeup(); - } -uint32_t PhysicsServerSW::body_get_collision_mask(RID p_body, uint32_t p_mask) const{ +uint32_t PhysicsServerSW::body_get_collision_mask(RID p_body, uint32_t p_mask) const { const BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body,0); + ERR_FAIL_COND_V(!body, 0); return body->get_collision_mask(); - } - -void PhysicsServerSW::body_attach_object_instance_ID(RID p_body,uint32_t p_ID) { +void PhysicsServerSW::body_attach_object_instance_ID(RID p_body, uint32_t p_ID) { BodySW *body = body_owner.get(p_body); ERR_FAIL_COND(!body); body->set_instance_id(p_ID); - }; uint32_t PhysicsServerSW::body_get_object_instance_ID(RID p_body) const { BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body,0); + ERR_FAIL_COND_V(!body, 0); return body->get_instance_id(); }; - void PhysicsServerSW::body_set_user_flags(RID p_body, uint32_t p_flags) { BodySW *body = body_owner.get(p_body); ERR_FAIL_COND(!body); - }; uint32_t PhysicsServerSW::body_get_user_flags(RID p_body, uint32_t p_flags) const { BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body,0); + ERR_FAIL_COND_V(!body, 0); return 0; }; @@ -732,38 +677,34 @@ void PhysicsServerSW::body_set_param(RID p_body, BodyParameter p_param, real_t p BodySW *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 PhysicsServerSW::body_get_param(RID p_body, BodyParameter p_param) const { BodySW *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 PhysicsServerSW::body_set_state(RID p_body, BodyState p_state, const Variant& p_variant) { +void PhysicsServerSW::body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) { BodySW *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 PhysicsServerSW::body_get_state(RID p_body, BodyState p_state) const { BodySW *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 PhysicsServerSW::body_set_applied_force(RID p_body, const Vector3& p_force) { +void PhysicsServerSW::body_set_applied_force(RID p_body, const Vector3 &p_force) { BodySW *body = body_owner.get(p_body); ERR_FAIL_COND(!body); @@ -775,11 +716,11 @@ void PhysicsServerSW::body_set_applied_force(RID p_body, const Vector3& p_force) Vector3 PhysicsServerSW::body_get_applied_force(RID p_body) const { BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body,Vector3()); + ERR_FAIL_COND_V(!body, Vector3()); return body->get_applied_force(); }; -void PhysicsServerSW::body_set_applied_torque(RID p_body, const Vector3& p_torque) { +void PhysicsServerSW::body_set_applied_torque(RID p_body, const Vector3 &p_torque) { BodySW *body = body_owner.get(p_body); ERR_FAIL_COND(!body); @@ -791,21 +732,21 @@ void PhysicsServerSW::body_set_applied_torque(RID p_body, const Vector3& p_torqu Vector3 PhysicsServerSW::body_get_applied_torque(RID p_body) const { BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body,Vector3()); + ERR_FAIL_COND_V(!body, Vector3()); return body->get_applied_torque(); }; -void PhysicsServerSW::body_apply_impulse(RID p_body, const Vector3& p_pos, const Vector3& p_impulse) { +void PhysicsServerSW::body_apply_impulse(RID p_body, const Vector3 &p_pos, const Vector3 &p_impulse) { BodySW *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 PhysicsServerSW::body_apply_torque_impulse(RID p_body, const Vector3& p_impulse) { +void PhysicsServerSW::body_apply_torque_impulse(RID p_body, const Vector3 &p_impulse) { BodySW *body = body_owner.get(p_body); ERR_FAIL_COND(!body); @@ -814,40 +755,34 @@ void PhysicsServerSW::body_apply_torque_impulse(RID p_body, const Vector3& p_imp body->wakeup(); }; -void PhysicsServerSW::body_set_axis_velocity(RID p_body, const Vector3& p_axis_velocity) { +void PhysicsServerSW::body_set_axis_velocity(RID p_body, const Vector3 &p_axis_velocity) { BodySW *body = body_owner.get(p_body); ERR_FAIL_COND(!body); Vector3 v = body->get_linear_velocity(); Vector3 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(); - }; - -void PhysicsServerSW::body_set_axis_lock(RID p_body,BodyAxisLock p_lock) { +void PhysicsServerSW::body_set_axis_lock(RID p_body, BodyAxisLock p_lock) { BodySW *body = body_owner.get(p_body); ERR_FAIL_COND(!body); body->set_axis_lock(p_lock); body->wakeup(); - } -PhysicsServerSW::BodyAxisLock PhysicsServerSW::body_get_axis_lock(RID p_body) const{ +PhysicsServerSW::BodyAxisLock PhysicsServerSW::body_get_axis_lock(RID p_body) const { const BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body,BODY_AXIS_LOCK_DISABLED); + ERR_FAIL_COND_V(!body, BODY_AXIS_LOCK_DISABLED); return body->get_axis_lock(); - } - - void PhysicsServerSW::body_add_collision_exception(RID p_body, RID p_body_b) { BodySW *body = body_owner.get(p_body); @@ -855,7 +790,6 @@ void PhysicsServerSW::body_add_collision_exception(RID p_body, RID p_body_b) { body->add_exception(p_body_b); body->wakeup(); - }; void PhysicsServerSW::body_remove_collision_exception(RID p_body, RID p_body_b) { @@ -865,7 +799,6 @@ void PhysicsServerSW::body_remove_collision_exception(RID p_body, RID p_body_b) body->remove_exception(p_body_b); body->wakeup(); - }; void PhysicsServerSW::body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) { @@ -873,27 +806,25 @@ void PhysicsServerSW::body_get_collision_exceptions(RID p_body, List<RID> *p_exc BodySW *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 PhysicsServerSW::body_set_contacts_reported_depth_treshold(RID p_body, real_t p_treshold) { BodySW *body = body_owner.get(p_body); ERR_FAIL_COND(!body); - }; real_t PhysicsServerSW::body_get_contacts_reported_depth_treshold(RID p_body) const { BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body,0); + ERR_FAIL_COND_V(!body, 0); return 0; }; -void PhysicsServerSW::body_set_omit_force_integration(RID p_body,bool p_omit) { +void PhysicsServerSW::body_set_omit_force_integration(RID p_body, bool p_omit) { BodySW *body = body_owner.get(p_body); ERR_FAIL_COND(!body); @@ -904,7 +835,7 @@ void PhysicsServerSW::body_set_omit_force_integration(RID p_body,bool p_omit) { bool PhysicsServerSW::body_is_omitting_force_integration(RID p_body) const { BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body,false); + ERR_FAIL_COND_V(!body, false); return body->get_omit_force_integration(); }; @@ -918,355 +849,333 @@ void PhysicsServerSW::body_set_max_contacts_reported(RID p_body, int p_contacts) int PhysicsServerSW::body_get_max_contacts_reported(RID p_body) const { BodySW *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 PhysicsServerSW::body_set_force_integration_callback(RID p_body,Object *p_receiver,const StringName& p_method,const Variant& p_udata) { - +void PhysicsServerSW::body_set_force_integration_callback(RID p_body, Object *p_receiver, const StringName &p_method, const Variant &p_udata) { BodySW *body = body_owner.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); } -void PhysicsServerSW::body_set_ray_pickable(RID p_body,bool p_enable) { +void PhysicsServerSW::body_set_ray_pickable(RID p_body, bool p_enable) { BodySW *body = body_owner.get(p_body); ERR_FAIL_COND(!body); body->set_ray_pickable(p_enable); - } -bool PhysicsServerSW::body_is_ray_pickable(RID p_body) const{ +bool PhysicsServerSW::body_is_ray_pickable(RID p_body) const { BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body,false); + ERR_FAIL_COND_V(!body, false); return body->is_ray_pickable(); - } - /* JOINT API */ -RID PhysicsServerSW::joint_create_pin(RID p_body_A,const Vector3& p_local_A,RID p_body_B,const Vector3& p_local_B) { +RID PhysicsServerSW::joint_create_pin(RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B) { BodySW *body_A = body_owner.get(p_body_A); - ERR_FAIL_COND_V(!body_A,RID()); + ERR_FAIL_COND_V(!body_A, RID()); if (!p_body_B.is_valid()) { - ERR_FAIL_COND_V(!body_A->get_space(),RID()); - p_body_B=body_A->get_space()->get_static_global_body(); + ERR_FAIL_COND_V(!body_A->get_space(), RID()); + p_body_B = body_A->get_space()->get_static_global_body(); } BodySW *body_B = body_owner.get(p_body_B); - ERR_FAIL_COND_V(!body_B,RID()); + ERR_FAIL_COND_V(!body_B, RID()); - ERR_FAIL_COND_V(body_A==body_B,RID()); + ERR_FAIL_COND_V(body_A == body_B, RID()); - JointSW *joint = memnew( PinJointSW(body_A,p_local_A,body_B,p_local_B) ); + JointSW *joint = memnew(PinJointSW(body_A, p_local_A, body_B, p_local_B)); RID rid = joint_owner.make_rid(joint); joint->set_self(rid); return rid; } -void PhysicsServerSW::pin_joint_set_param(RID p_joint,PinJointParam p_param, real_t p_value){ +void PhysicsServerSW::pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value) { JointSW *joint = joint_owner.get(p_joint); ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type()!=JOINT_PIN); - PinJointSW *pin_joint = static_cast<PinJointSW*>(joint); - pin_joint->set_param(p_param,p_value); - + ERR_FAIL_COND(joint->get_type() != JOINT_PIN); + PinJointSW *pin_joint = static_cast<PinJointSW *>(joint); + pin_joint->set_param(p_param, p_value); } -real_t PhysicsServerSW::pin_joint_get_param(RID p_joint,PinJointParam p_param) const{ +real_t PhysicsServerSW::pin_joint_get_param(RID p_joint, PinJointParam p_param) const { JointSW *joint = joint_owner.get(p_joint); - ERR_FAIL_COND_V(!joint,0); - ERR_FAIL_COND_V(joint->get_type()!=JOINT_PIN,0); - PinJointSW *pin_joint = static_cast<PinJointSW*>(joint); + ERR_FAIL_COND_V(!joint, 0); + ERR_FAIL_COND_V(joint->get_type() != JOINT_PIN, 0); + PinJointSW *pin_joint = static_cast<PinJointSW *>(joint); return pin_joint->get_param(p_param); - } -void PhysicsServerSW::pin_joint_set_local_A(RID p_joint, const Vector3& p_A){ +void PhysicsServerSW::pin_joint_set_local_A(RID p_joint, const Vector3 &p_A) { JointSW *joint = joint_owner.get(p_joint); ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type()!=JOINT_PIN); - PinJointSW *pin_joint = static_cast<PinJointSW*>(joint); + ERR_FAIL_COND(joint->get_type() != JOINT_PIN); + PinJointSW *pin_joint = static_cast<PinJointSW *>(joint); pin_joint->set_pos_A(p_A); - } -Vector3 PhysicsServerSW::pin_joint_get_local_A(RID p_joint) const{ +Vector3 PhysicsServerSW::pin_joint_get_local_A(RID p_joint) const { JointSW *joint = joint_owner.get(p_joint); - ERR_FAIL_COND_V(!joint,Vector3()); - ERR_FAIL_COND_V(joint->get_type()!=JOINT_PIN,Vector3()); - PinJointSW *pin_joint = static_cast<PinJointSW*>(joint); + ERR_FAIL_COND_V(!joint, Vector3()); + ERR_FAIL_COND_V(joint->get_type() != JOINT_PIN, Vector3()); + PinJointSW *pin_joint = static_cast<PinJointSW *>(joint); return pin_joint->get_pos_A(); - } -void PhysicsServerSW::pin_joint_set_local_B(RID p_joint, const Vector3& p_B){ +void PhysicsServerSW::pin_joint_set_local_B(RID p_joint, const Vector3 &p_B) { JointSW *joint = joint_owner.get(p_joint); ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type()!=JOINT_PIN); - PinJointSW *pin_joint = static_cast<PinJointSW*>(joint); + ERR_FAIL_COND(joint->get_type() != JOINT_PIN); + PinJointSW *pin_joint = static_cast<PinJointSW *>(joint); pin_joint->set_pos_B(p_B); - } -Vector3 PhysicsServerSW::pin_joint_get_local_B(RID p_joint) const{ +Vector3 PhysicsServerSW::pin_joint_get_local_B(RID p_joint) const { JointSW *joint = joint_owner.get(p_joint); - ERR_FAIL_COND_V(!joint,Vector3()); - ERR_FAIL_COND_V(joint->get_type()!=JOINT_PIN,Vector3()); - PinJointSW *pin_joint = static_cast<PinJointSW*>(joint); + ERR_FAIL_COND_V(!joint, Vector3()); + ERR_FAIL_COND_V(joint->get_type() != JOINT_PIN, Vector3()); + PinJointSW *pin_joint = static_cast<PinJointSW *>(joint); return pin_joint->get_pos_B(); - } - -RID PhysicsServerSW::joint_create_hinge(RID p_body_A,const Transform& p_frame_A,RID p_body_B,const Transform& p_frame_B) { +RID PhysicsServerSW::joint_create_hinge(RID p_body_A, const Transform &p_frame_A, RID p_body_B, const Transform &p_frame_B) { BodySW *body_A = body_owner.get(p_body_A); - ERR_FAIL_COND_V(!body_A,RID()); + ERR_FAIL_COND_V(!body_A, RID()); if (!p_body_B.is_valid()) { - ERR_FAIL_COND_V(!body_A->get_space(),RID()); - p_body_B=body_A->get_space()->get_static_global_body(); + ERR_FAIL_COND_V(!body_A->get_space(), RID()); + p_body_B = body_A->get_space()->get_static_global_body(); } BodySW *body_B = body_owner.get(p_body_B); - ERR_FAIL_COND_V(!body_B,RID()); + ERR_FAIL_COND_V(!body_B, RID()); - ERR_FAIL_COND_V(body_A==body_B,RID()); + ERR_FAIL_COND_V(body_A == body_B, RID()); - JointSW *joint = memnew( HingeJointSW(body_A,body_B,p_frame_A,p_frame_B) ); + JointSW *joint = memnew(HingeJointSW(body_A, body_B, p_frame_A, p_frame_B)); RID rid = joint_owner.make_rid(joint); joint->set_self(rid); return rid; - } - -RID PhysicsServerSW::joint_create_hinge_simple(RID p_body_A,const Vector3& p_pivot_A,const Vector3& p_axis_A,RID p_body_B,const Vector3& p_pivot_B,const Vector3& p_axis_B) { +RID PhysicsServerSW::joint_create_hinge_simple(RID p_body_A, const Vector3 &p_pivot_A, const Vector3 &p_axis_A, RID p_body_B, const Vector3 &p_pivot_B, const Vector3 &p_axis_B) { BodySW *body_A = body_owner.get(p_body_A); - ERR_FAIL_COND_V(!body_A,RID()); + ERR_FAIL_COND_V(!body_A, RID()); if (!p_body_B.is_valid()) { - ERR_FAIL_COND_V(!body_A->get_space(),RID()); - p_body_B=body_A->get_space()->get_static_global_body(); + ERR_FAIL_COND_V(!body_A->get_space(), RID()); + p_body_B = body_A->get_space()->get_static_global_body(); } BodySW *body_B = body_owner.get(p_body_B); - ERR_FAIL_COND_V(!body_B,RID()); + ERR_FAIL_COND_V(!body_B, RID()); - ERR_FAIL_COND_V(body_A==body_B,RID()); + ERR_FAIL_COND_V(body_A == body_B, RID()); - JointSW *joint = memnew( HingeJointSW(body_A,body_B,p_pivot_A,p_pivot_B,p_axis_A,p_axis_B) ); + JointSW *joint = memnew(HingeJointSW(body_A, body_B, p_pivot_A, p_pivot_B, p_axis_A, p_axis_B)); RID rid = joint_owner.make_rid(joint); joint->set_self(rid); return rid; - } -void PhysicsServerSW::hinge_joint_set_param(RID p_joint,HingeJointParam p_param, real_t p_value){ +void PhysicsServerSW::hinge_joint_set_param(RID p_joint, HingeJointParam p_param, real_t p_value) { JointSW *joint = joint_owner.get(p_joint); ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type()!=JOINT_HINGE); - HingeJointSW *hinge_joint = static_cast<HingeJointSW*>(joint); - hinge_joint->set_param(p_param,p_value); - + ERR_FAIL_COND(joint->get_type() != JOINT_HINGE); + HingeJointSW *hinge_joint = static_cast<HingeJointSW *>(joint); + hinge_joint->set_param(p_param, p_value); } -real_t PhysicsServerSW::hinge_joint_get_param(RID p_joint,HingeJointParam p_param) const{ +real_t PhysicsServerSW::hinge_joint_get_param(RID p_joint, HingeJointParam p_param) const { JointSW *joint = joint_owner.get(p_joint); - ERR_FAIL_COND_V(!joint,0); - ERR_FAIL_COND_V(joint->get_type()!=JOINT_HINGE,0); - HingeJointSW *hinge_joint = static_cast<HingeJointSW*>(joint); + ERR_FAIL_COND_V(!joint, 0); + ERR_FAIL_COND_V(joint->get_type() != JOINT_HINGE, 0); + HingeJointSW *hinge_joint = static_cast<HingeJointSW *>(joint); return hinge_joint->get_param(p_param); - } -void PhysicsServerSW::hinge_joint_set_flag(RID p_joint,HingeJointFlag p_flag, bool p_value){ +void PhysicsServerSW::hinge_joint_set_flag(RID p_joint, HingeJointFlag p_flag, bool p_value) { JointSW *joint = joint_owner.get(p_joint); ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type()!=JOINT_HINGE); - HingeJointSW *hinge_joint = static_cast<HingeJointSW*>(joint); - hinge_joint->set_flag(p_flag,p_value); - + ERR_FAIL_COND(joint->get_type() != JOINT_HINGE); + HingeJointSW *hinge_joint = static_cast<HingeJointSW *>(joint); + hinge_joint->set_flag(p_flag, p_value); } -bool PhysicsServerSW::hinge_joint_get_flag(RID p_joint,HingeJointFlag p_flag) const{ +bool PhysicsServerSW::hinge_joint_get_flag(RID p_joint, HingeJointFlag p_flag) const { JointSW *joint = joint_owner.get(p_joint); - ERR_FAIL_COND_V(!joint,false); - ERR_FAIL_COND_V(joint->get_type()!=JOINT_HINGE,false); - HingeJointSW *hinge_joint = static_cast<HingeJointSW*>(joint); + ERR_FAIL_COND_V(!joint, false); + ERR_FAIL_COND_V(joint->get_type() != JOINT_HINGE, false); + HingeJointSW *hinge_joint = static_cast<HingeJointSW *>(joint); return hinge_joint->get_flag(p_flag); } -void PhysicsServerSW::joint_set_solver_priority(RID p_joint,int p_priority) { +void PhysicsServerSW::joint_set_solver_priority(RID p_joint, int p_priority) { JointSW *joint = joint_owner.get(p_joint); ERR_FAIL_COND(!joint); joint->set_priority(p_priority); } -int PhysicsServerSW::joint_get_solver_priority(RID p_joint) const{ +int PhysicsServerSW::joint_get_solver_priority(RID p_joint) const { JointSW *joint = joint_owner.get(p_joint); - ERR_FAIL_COND_V(!joint,0); + ERR_FAIL_COND_V(!joint, 0); return joint->get_priority(); - } PhysicsServerSW::JointType PhysicsServerSW::joint_get_type(RID p_joint) const { JointSW *joint = joint_owner.get(p_joint); - ERR_FAIL_COND_V(!joint,JOINT_PIN); + ERR_FAIL_COND_V(!joint, JOINT_PIN); return joint->get_type(); } -RID PhysicsServerSW::joint_create_slider(RID p_body_A,const Transform& p_local_frame_A,RID p_body_B,const Transform& p_local_frame_B) { +RID PhysicsServerSW::joint_create_slider(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) { BodySW *body_A = body_owner.get(p_body_A); - ERR_FAIL_COND_V(!body_A,RID()); + ERR_FAIL_COND_V(!body_A, RID()); if (!p_body_B.is_valid()) { - ERR_FAIL_COND_V(!body_A->get_space(),RID()); - p_body_B=body_A->get_space()->get_static_global_body(); + ERR_FAIL_COND_V(!body_A->get_space(), RID()); + p_body_B = body_A->get_space()->get_static_global_body(); } BodySW *body_B = body_owner.get(p_body_B); - ERR_FAIL_COND_V(!body_B,RID()); + ERR_FAIL_COND_V(!body_B, RID()); - ERR_FAIL_COND_V(body_A==body_B,RID()); + ERR_FAIL_COND_V(body_A == body_B, RID()); - JointSW *joint = memnew( SliderJointSW(body_A,body_B,p_local_frame_A,p_local_frame_B) ); + JointSW *joint = memnew(SliderJointSW(body_A, body_B, p_local_frame_A, p_local_frame_B)); RID rid = joint_owner.make_rid(joint); joint->set_self(rid); return rid; } -void PhysicsServerSW::slider_joint_set_param(RID p_joint,SliderJointParam p_param, real_t p_value){ +void PhysicsServerSW::slider_joint_set_param(RID p_joint, SliderJointParam p_param, real_t p_value) { JointSW *joint = joint_owner.get(p_joint); ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type()!=JOINT_SLIDER); - SliderJointSW *slider_joint = static_cast<SliderJointSW*>(joint); - slider_joint->set_param(p_param,p_value); + ERR_FAIL_COND(joint->get_type() != JOINT_SLIDER); + SliderJointSW *slider_joint = static_cast<SliderJointSW *>(joint); + slider_joint->set_param(p_param, p_value); } -real_t PhysicsServerSW::slider_joint_get_param(RID p_joint,SliderJointParam p_param) const{ +real_t PhysicsServerSW::slider_joint_get_param(RID p_joint, SliderJointParam p_param) const { JointSW *joint = joint_owner.get(p_joint); - ERR_FAIL_COND_V(!joint,0); - ERR_FAIL_COND_V(joint->get_type()!=JOINT_CONE_TWIST,0); - SliderJointSW *slider_joint = static_cast<SliderJointSW*>(joint); + ERR_FAIL_COND_V(!joint, 0); + ERR_FAIL_COND_V(joint->get_type() != JOINT_CONE_TWIST, 0); + SliderJointSW *slider_joint = static_cast<SliderJointSW *>(joint); return slider_joint->get_param(p_param); } - -RID PhysicsServerSW::joint_create_cone_twist(RID p_body_A,const Transform& p_local_frame_A,RID p_body_B,const Transform& p_local_frame_B) { +RID PhysicsServerSW::joint_create_cone_twist(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) { BodySW *body_A = body_owner.get(p_body_A); - ERR_FAIL_COND_V(!body_A,RID()); + ERR_FAIL_COND_V(!body_A, RID()); if (!p_body_B.is_valid()) { - ERR_FAIL_COND_V(!body_A->get_space(),RID()); - p_body_B=body_A->get_space()->get_static_global_body(); + ERR_FAIL_COND_V(!body_A->get_space(), RID()); + p_body_B = body_A->get_space()->get_static_global_body(); } BodySW *body_B = body_owner.get(p_body_B); - ERR_FAIL_COND_V(!body_B,RID()); + ERR_FAIL_COND_V(!body_B, RID()); - ERR_FAIL_COND_V(body_A==body_B,RID()); + ERR_FAIL_COND_V(body_A == body_B, RID()); - JointSW *joint = memnew( ConeTwistJointSW(body_A,body_B,p_local_frame_A,p_local_frame_B) ); + JointSW *joint = memnew(ConeTwistJointSW(body_A, body_B, p_local_frame_A, p_local_frame_B)); RID rid = joint_owner.make_rid(joint); joint->set_self(rid); return rid; } -void PhysicsServerSW::cone_twist_joint_set_param(RID p_joint,ConeTwistJointParam p_param, real_t p_value) { +void PhysicsServerSW::cone_twist_joint_set_param(RID p_joint, ConeTwistJointParam p_param, real_t p_value) { JointSW *joint = joint_owner.get(p_joint); ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type()!=JOINT_CONE_TWIST); - ConeTwistJointSW *cone_twist_joint = static_cast<ConeTwistJointSW*>(joint); - cone_twist_joint->set_param(p_param,p_value); + ERR_FAIL_COND(joint->get_type() != JOINT_CONE_TWIST); + ConeTwistJointSW *cone_twist_joint = static_cast<ConeTwistJointSW *>(joint); + cone_twist_joint->set_param(p_param, p_value); } -real_t PhysicsServerSW::cone_twist_joint_get_param(RID p_joint,ConeTwistJointParam p_param) const { +real_t PhysicsServerSW::cone_twist_joint_get_param(RID p_joint, ConeTwistJointParam p_param) const { JointSW *joint = joint_owner.get(p_joint); - ERR_FAIL_COND_V(!joint,0); - ERR_FAIL_COND_V(joint->get_type()!=JOINT_CONE_TWIST,0); - ConeTwistJointSW *cone_twist_joint = static_cast<ConeTwistJointSW*>(joint); + ERR_FAIL_COND_V(!joint, 0); + ERR_FAIL_COND_V(joint->get_type() != JOINT_CONE_TWIST, 0); + ConeTwistJointSW *cone_twist_joint = static_cast<ConeTwistJointSW *>(joint); return cone_twist_joint->get_param(p_param); } - -RID PhysicsServerSW::joint_create_generic_6dof(RID p_body_A,const Transform& p_local_frame_A,RID p_body_B,const Transform& p_local_frame_B) { +RID PhysicsServerSW::joint_create_generic_6dof(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) { BodySW *body_A = body_owner.get(p_body_A); - ERR_FAIL_COND_V(!body_A,RID()); + ERR_FAIL_COND_V(!body_A, RID()); if (!p_body_B.is_valid()) { - ERR_FAIL_COND_V(!body_A->get_space(),RID()); - p_body_B=body_A->get_space()->get_static_global_body(); + ERR_FAIL_COND_V(!body_A->get_space(), RID()); + p_body_B = body_A->get_space()->get_static_global_body(); } BodySW *body_B = body_owner.get(p_body_B); - ERR_FAIL_COND_V(!body_B,RID()); + ERR_FAIL_COND_V(!body_B, RID()); - ERR_FAIL_COND_V(body_A==body_B,RID()); + ERR_FAIL_COND_V(body_A == body_B, RID()); - JointSW *joint = memnew( Generic6DOFJointSW(body_A,body_B,p_local_frame_A,p_local_frame_B,true) ); + JointSW *joint = memnew(Generic6DOFJointSW(body_A, body_B, p_local_frame_A, p_local_frame_B, true)); RID rid = joint_owner.make_rid(joint); joint->set_self(rid); return rid; } -void PhysicsServerSW::generic_6dof_joint_set_param(RID p_joint,Vector3::Axis p_axis,G6DOFJointAxisParam p_param, real_t p_value){ +void PhysicsServerSW::generic_6dof_joint_set_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParam p_param, real_t p_value) { JointSW *joint = joint_owner.get(p_joint); ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type()!=JOINT_6DOF); - Generic6DOFJointSW *generic_6dof_joint = static_cast<Generic6DOFJointSW*>(joint); - generic_6dof_joint->set_param(p_axis,p_param,p_value); + ERR_FAIL_COND(joint->get_type() != JOINT_6DOF); + Generic6DOFJointSW *generic_6dof_joint = static_cast<Generic6DOFJointSW *>(joint); + generic_6dof_joint->set_param(p_axis, p_param, p_value); } -real_t PhysicsServerSW::generic_6dof_joint_get_param(RID p_joint,Vector3::Axis p_axis,G6DOFJointAxisParam p_param){ +real_t PhysicsServerSW::generic_6dof_joint_get_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParam p_param) { JointSW *joint = joint_owner.get(p_joint); - ERR_FAIL_COND_V(!joint,0); - ERR_FAIL_COND_V(joint->get_type()!=JOINT_6DOF,0); - Generic6DOFJointSW *generic_6dof_joint = static_cast<Generic6DOFJointSW*>(joint); - return generic_6dof_joint->get_param(p_axis,p_param); + ERR_FAIL_COND_V(!joint, 0); + ERR_FAIL_COND_V(joint->get_type() != JOINT_6DOF, 0); + Generic6DOFJointSW *generic_6dof_joint = static_cast<Generic6DOFJointSW *>(joint); + return generic_6dof_joint->get_param(p_axis, p_param); } -void PhysicsServerSW::generic_6dof_joint_set_flag(RID p_joint,Vector3::Axis p_axis,G6DOFJointAxisFlag p_flag, bool p_enable){ +void PhysicsServerSW::generic_6dof_joint_set_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlag p_flag, bool p_enable) { JointSW *joint = joint_owner.get(p_joint); ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type()!=JOINT_6DOF); - Generic6DOFJointSW *generic_6dof_joint = static_cast<Generic6DOFJointSW*>(joint); - generic_6dof_joint->set_flag(p_axis,p_flag,p_enable); + ERR_FAIL_COND(joint->get_type() != JOINT_6DOF); + Generic6DOFJointSW *generic_6dof_joint = static_cast<Generic6DOFJointSW *>(joint); + generic_6dof_joint->set_flag(p_axis, p_flag, p_enable); } -bool PhysicsServerSW::generic_6dof_joint_get_flag(RID p_joint,Vector3::Axis p_axis,G6DOFJointAxisFlag p_flag){ +bool PhysicsServerSW::generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlag p_flag) { JointSW *joint = joint_owner.get(p_joint); - ERR_FAIL_COND_V(!joint,false); - ERR_FAIL_COND_V(joint->get_type()!=JOINT_6DOF,false); - Generic6DOFJointSW *generic_6dof_joint = static_cast<Generic6DOFJointSW*>(joint); - return generic_6dof_joint->get_flag(p_axis,p_flag); + ERR_FAIL_COND_V(!joint, false); + ERR_FAIL_COND_V(joint->get_type() != JOINT_6DOF, false); + Generic6DOFJointSW *generic_6dof_joint = static_cast<Generic6DOFJointSW *>(joint); + return generic_6dof_joint->get_flag(p_axis, p_flag); } - #if 0 void PhysicsServerSW::joint_set_param(RID p_joint, JointParam p_param, real_t p_value) { @@ -1384,8 +1293,8 @@ void PhysicsServerSW::free(RID p_rid) { ShapeSW *shape = shape_owner.get(p_rid); - while(shape->get_owners().size()) { - ShapeOwnerSW *so=shape->get_owners().front()->key(); + while (shape->get_owners().size()) { + ShapeOwnerSW *so = shape->get_owners().front()->key(); so->remove_shape(shape); } @@ -1405,8 +1314,7 @@ void PhysicsServerSW::free(RID p_rid) { body->set_space(NULL); - - while( body->get_shape_count() ) { + while (body->get_shape_count()) { body->remove_shape(0); } @@ -1431,7 +1339,7 @@ void PhysicsServerSW::free(RID p_rid) { area->set_space(NULL); - while( area->get_shape_count() ) { + while (area->get_shape_count()) { area->remove_shape(0); } @@ -1442,7 +1350,7 @@ void PhysicsServerSW::free(RID p_rid) { SpaceSW *space = space_owner.get(p_rid); - while(space->get_objects().size()) { + while (space->get_objects().size()) { CollisionObjectSW *co = (CollisionObjectSW *)space->get_objects().front()->get(); co->set_space(NULL); } @@ -1457,7 +1365,7 @@ void PhysicsServerSW::free(RID p_rid) { JointSW *joint = joint_owner.get(p_rid); - for(int i=0;i<joint->get_body_count();i++) { + for (int i = 0; i < joint->get_body_count(); i++) { joint->get_body_ptr()[i]->remove_constraint(joint); } @@ -1469,51 +1377,45 @@ void PhysicsServerSW::free(RID p_rid) { ERR_EXPLAIN("Invalid ID"); ERR_FAIL(); } - - }; void PhysicsServerSW::set_active(bool p_active) { - active=p_active; + active = p_active; }; void PhysicsServerSW::init() { - doing_sync=true; - last_step=0.001; - iterations=8;// 8? - stepper = memnew( StepSW ); - direct_state = memnew( PhysicsDirectBodyStateSW ); + doing_sync = true; + last_step = 0.001; + iterations = 8; // 8? + stepper = memnew(StepSW); + direct_state = memnew(PhysicsDirectBodyStateSW); }; - void PhysicsServerSW::step(real_t p_step) { - if (!active) return; + doing_sync = false; - doing_sync=false; - - last_step=p_step; - PhysicsDirectBodyStateSW::singleton->step=p_step; + last_step = p_step; + PhysicsDirectBodyStateSW::singleton->step = p_step; - island_count=0; - active_objects=0; - collision_pairs=0; - for( Set<const SpaceSW*>::Element *E=active_spaces.front();E;E=E->next()) { + island_count = 0; + active_objects = 0; + collision_pairs = 0; + for (Set<const SpaceSW *>::Element *E = active_spaces.front(); E; E = E->next()) { - stepper->step((SpaceSW*)E->get(),p_step,iterations); - island_count+=E->get()->get_island_count(); - active_objects+=E->get()->get_active_objects(); - collision_pairs+=E->get()->get_collision_pairs(); + stepper->step((SpaceSW *)E->get(), p_step, iterations); + island_count += E->get()->get_island_count(); + active_objects += E->get()->get_active_objects(); + collision_pairs += E->get()->get_collision_pairs(); } - } -void PhysicsServerSW::sync() { +void PhysicsServerSW::sync(){ }; @@ -1522,21 +1424,20 @@ void PhysicsServerSW::flush_queries() { if (!active) return; - doing_sync=true; + doing_sync = true; uint64_t time_beg = OS::get_singleton()->get_ticks_usec(); - for( Set<const SpaceSW*>::Element *E=active_spaces.front();E;E=E->next()) { + for (Set<const SpaceSW *>::Element *E = active_spaces.front(); E; E = E->next()) { - SpaceSW *space=(SpaceSW *)E->get(); + SpaceSW *space = (SpaceSW *)E->get(); space->call_queries(); } - if (ScriptDebugger::get_singleton() && ScriptDebugger::get_singleton()->is_profiling()) { uint64_t total_time[SpaceSW::ELAPSED_TIME_MAX]; - static const char* time_name[SpaceSW::ELAPSED_TIME_MAX]={ + static const char *time_name[SpaceSW::ELAPSED_TIME_MAX] = { "integrate_forces", "generate_islands", "setup_constraints", @@ -1544,44 +1445,39 @@ void PhysicsServerSW::flush_queries() { "integrate_velocities" }; - for(int i=0;i<SpaceSW::ELAPSED_TIME_MAX;i++) { - total_time[i]=0; + for (int i = 0; i < SpaceSW::ELAPSED_TIME_MAX; i++) { + total_time[i] = 0; } - for( Set<const SpaceSW*>::Element *E=active_spaces.front();E;E=E->next()) { + for (Set<const SpaceSW *>::Element *E = active_spaces.front(); E; E = E->next()) { - for(int i=0;i<SpaceSW::ELAPSED_TIME_MAX;i++) { - total_time[i]+=E->get()->get_elapsed_time(SpaceSW::ElapsedTime(i)); + for (int i = 0; i < SpaceSW::ELAPSED_TIME_MAX; i++) { + total_time[i] += E->get()->get_elapsed_time(SpaceSW::ElapsedTime(i)); } - } Array values; - values.resize(SpaceSW::ELAPSED_TIME_MAX*2); - for(int i=0;i<SpaceSW::ELAPSED_TIME_MAX;i++) { - values[i*2+0]=time_name[i]; - values[i*2+1]=USEC_TO_SEC(total_time[i]); + values.resize(SpaceSW::ELAPSED_TIME_MAX * 2); + for (int i = 0; i < SpaceSW::ELAPSED_TIME_MAX; i++) { + values[i * 2 + 0] = time_name[i]; + values[i * 2 + 1] = USEC_TO_SEC(total_time[i]); } values.push_back("flush_queries"); - values.push_back(USEC_TO_SEC(OS::get_singleton()->get_ticks_usec()-time_beg)); - - ScriptDebugger::get_singleton()->add_profiling_frame_data("physics",values); + values.push_back(USEC_TO_SEC(OS::get_singleton()->get_ticks_usec() - time_beg)); + ScriptDebugger::get_singleton()->add_profiling_frame_data("physics", values); } }; - - void PhysicsServerSW::finish() { memdelete(stepper); memdelete(direct_state); }; - int PhysicsServerSW::get_process_info(ProcessInfo p_info) { - switch(p_info) { + switch (p_info) { case INFO_ACTIVE_OBJECTS: { @@ -1594,64 +1490,55 @@ int PhysicsServerSW::get_process_info(ProcessInfo p_info) { return island_count; } break; - } return 0; } +void PhysicsServerSW::_shape_col_cbk(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata) { -void PhysicsServerSW::_shape_col_cbk(const Vector3& p_point_A,const Vector3& 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->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 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 = 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++; } } - PhysicsServerSW::PhysicsServerSW() { - BroadPhaseSW::create_func=BroadPhaseOctree::_create; - island_count=0; - active_objects=0; - collision_pairs=0; - - active=true; + BroadPhaseSW::create_func = BroadPhaseOctree::_create; + island_count = 0; + active_objects = 0; + collision_pairs = 0; + active = true; }; -PhysicsServerSW::~PhysicsServerSW() { +PhysicsServerSW::~PhysicsServerSW(){ }; - - diff --git a/servers/physics/physics_server_sw.h b/servers/physics/physics_server_sw.h index 6e2047435..cb5a339ee 100644 --- a/servers/physics/physics_server_sw.h +++ b/servers/physics/physics_server_sw.h @@ -29,19 +29,17 @@ #ifndef PHYSICS_SERVER_SW #define PHYSICS_SERVER_SW - +#include "joints_sw.h" #include "servers/physics_server.h" #include "shape_sw.h" #include "space_sw.h" #include "step_sw.h" -#include "joints_sw.h" - class PhysicsServerSW : public PhysicsServer { - GDCLASS( PhysicsServerSW, PhysicsServer ); + GDCLASS(PhysicsServerSW, PhysicsServer); -friend class PhysicsDirectSpaceStateSW; + friend class PhysicsDirectSpaceStateSW; bool active; int iterations; bool doing_sync; @@ -52,7 +50,7 @@ friend class PhysicsDirectSpaceStateSW; int collision_pairs; StepSW *stepper; - Set<const SpaceSW*> active_spaces; + Set<const SpaceSW *> active_spaces; PhysicsDirectBodyStateSW *direct_state; @@ -64,7 +62,6 @@ friend class PhysicsDirectSpaceStateSW; //void _clear_query(QuerySW *p_query); public: - struct CollCbkData { int max; @@ -72,10 +69,10 @@ public: Vector3 *ptr; }; - static void _shape_col_cbk(const Vector3& p_point_A,const Vector3& p_point_B,void *p_userdata); + static void _shape_col_cbk(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata); virtual RID shape_create(ShapeType p_shape); - virtual void shape_set_data(RID p_shape, const Variant& p_data); + virtual void shape_set_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; @@ -85,16 +82,16 @@ public: /* 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; // this function only works on fixed process, errors and returns null otherwise - virtual PhysicsDirectSpaceState* space_get_direct_state(RID p_space); + virtual PhysicsDirectSpaceState *space_get_direct_state(RID p_space); - 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<Vector3> space_get_contacts(RID p_space) const; virtual int space_get_contact_count(RID p_space) const; @@ -108,9 +105,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 Transform& p_transform=Transform()); - virtual void area_set_shape(RID p_area, int p_shape_idx,RID p_shape); - virtual void area_set_shape_transform(RID p_area, int p_shape_idx, const Transform& p_transform); + virtual void area_add_shape(RID p_area, RID p_shape, const Transform &p_transform = Transform()); + virtual void area_set_shape(RID p_area, int p_shape_idx, RID p_shape); + virtual void area_set_shape_transform(RID p_area, int p_shape_idx, const Transform &p_transform); virtual int area_get_shape_count(RID p_area) const; virtual RID area_get_shape(RID p_area, int p_shape_idx) const; @@ -119,31 +116,30 @@ 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 Transform& 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 Transform &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 Transform area_get_transform(RID p_area) const; - virtual void area_set_ray_pickable(RID p_area,bool p_enable); + virtual void area_set_ray_pickable(RID p_area, bool p_enable); virtual bool area_is_ray_pickable(RID p_area) const; - virtual void area_set_collision_mask(RID p_area,uint32_t p_mask); - virtual void area_set_layer_mask(RID p_area,uint32_t p_mask); + 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_monitorable(RID p_area,bool p_monitorable); - - virtual void area_set_monitor_callback(RID p_area,Object *p_receiver,const StringName& p_method); - virtual void area_set_area_monitor_callback(RID p_area,Object *p_receiver,const StringName& p_method); + virtual void area_set_monitorable(RID p_area, bool p_monitorable); + virtual void area_set_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method); + virtual void area_set_area_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method); /* BODY API */ // create a body of a given type - virtual RID body_create(BodyMode p_mode=BODY_MODE_RIGID,bool p_init_sleeping=false); + virtual 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; @@ -151,24 +147,24 @@ 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 Transform& p_transform=Transform()); - virtual void body_set_shape(RID p_body, int p_shape_idx,RID p_shape); - virtual void body_set_shape_transform(RID p_body, int p_shape_idx, const Transform& p_transform); + virtual void body_add_shape(RID p_body, RID p_shape, const Transform &p_transform = Transform()); + virtual void body_set_shape(RID p_body, int p_shape_idx, RID p_shape); + virtual void body_set_shape_transform(RID p_body, int p_shape_idx, const Transform &p_transform); virtual int body_get_shape_count(RID p_body) const; virtual RID body_get_shape(RID p_body, int p_shape_idx) const; virtual Transform body_get_shape_transform(RID p_body, int p_shape_idx) const; - virtual void body_set_shape_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_remove_shape(RID p_body, int p_shape_idx); virtual void body_clear_shapes(RID p_body); - virtual void body_attach_object_instance_ID(RID p_body,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_enable_continuous_collision_detection(RID p_body,bool p_enable); + virtual void body_set_enable_continuous_collision_detection(RID p_body, bool p_enable); virtual bool body_is_continuous_collision_detection_enabled(RID p_body) const; virtual void body_set_layer_mask(RID p_body, uint32_t p_mask); @@ -183,20 +179,20 @@ 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 Vector3& p_force); + virtual void body_set_applied_force(RID p_body, const Vector3 &p_force); virtual Vector3 body_get_applied_force(RID p_body) const; - virtual void body_set_applied_torque(RID p_body, const Vector3& p_torque); + virtual void body_set_applied_torque(RID p_body, const Vector3 &p_torque); virtual Vector3 body_get_applied_torque(RID p_body) const; - virtual void body_apply_impulse(RID p_body, const Vector3& p_pos, const Vector3& p_impulse); - virtual void body_apply_torque_impulse(RID p_body, const Vector3& p_impulse); - virtual void body_set_axis_velocity(RID p_body, const Vector3& p_axis_velocity); + virtual void body_apply_impulse(RID p_body, const Vector3 &p_pos, const Vector3 &p_impulse); + virtual void body_apply_torque_impulse(RID p_body, const Vector3 &p_impulse); + virtual void body_set_axis_velocity(RID p_body, const Vector3 &p_axis_velocity); - virtual void body_set_axis_lock(RID p_body,BodyAxisLock p_lock); + virtual void body_set_axis_lock(RID p_body, BodyAxisLock p_lock); virtual BodyAxisLock body_get_axis_lock(RID p_body) const; virtual void body_add_collision_exception(RID p_body, RID p_body_b); @@ -206,61 +202,60 @@ 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_force_integration_callback(RID p_body,Object *p_receiver,const StringName& p_method,const Variant& p_udata=Variant()); + virtual void body_set_force_integration_callback(RID p_body, Object *p_receiver, const StringName &p_method, const Variant &p_udata = Variant()); - virtual void body_set_ray_pickable(RID p_body,bool p_enable); + virtual void body_set_ray_pickable(RID p_body, bool p_enable); virtual bool body_is_ray_pickable(RID p_body) const; /* JOINT API */ - virtual RID joint_create_pin(RID p_body_A,const Vector3& p_local_A,RID p_body_B,const Vector3& p_local_B); + virtual RID joint_create_pin(RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B); - virtual void pin_joint_set_param(RID p_joint,PinJointParam p_param, real_t p_value); - virtual real_t pin_joint_get_param(RID p_joint,PinJointParam p_param) const; + virtual void pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value); + virtual real_t pin_joint_get_param(RID p_joint, PinJointParam p_param) const; - virtual void pin_joint_set_local_A(RID p_joint, const Vector3& p_A); + virtual void pin_joint_set_local_A(RID p_joint, const Vector3 &p_A); virtual Vector3 pin_joint_get_local_A(RID p_joint) const; - virtual void pin_joint_set_local_B(RID p_joint, const Vector3& p_B); + virtual void pin_joint_set_local_B(RID p_joint, const Vector3 &p_B); virtual Vector3 pin_joint_get_local_B(RID p_joint) const; - virtual RID joint_create_hinge(RID p_body_A,const Transform& p_frame_A,RID p_body_B,const Transform& p_frame_B); - virtual RID joint_create_hinge_simple(RID p_body_A,const Vector3& p_pivot_A,const Vector3& p_axis_A,RID p_body_B,const Vector3& p_pivot_B,const Vector3& p_axis_B); - - virtual void hinge_joint_set_param(RID p_joint,HingeJointParam p_param, real_t p_value); - virtual real_t hinge_joint_get_param(RID p_joint,HingeJointParam p_param) const; + virtual RID joint_create_hinge(RID p_body_A, const Transform &p_frame_A, RID p_body_B, const Transform &p_frame_B); + virtual RID joint_create_hinge_simple(RID p_body_A, const Vector3 &p_pivot_A, const Vector3 &p_axis_A, RID p_body_B, const Vector3 &p_pivot_B, const Vector3 &p_axis_B); - virtual void hinge_joint_set_flag(RID p_joint,HingeJointFlag p_flag, bool p_value); - virtual bool hinge_joint_get_flag(RID p_joint,HingeJointFlag p_flag) const; + virtual void hinge_joint_set_param(RID p_joint, HingeJointParam p_param, real_t p_value); + virtual real_t hinge_joint_get_param(RID p_joint, HingeJointParam p_param) const; + virtual void hinge_joint_set_flag(RID p_joint, HingeJointFlag p_flag, bool p_value); + virtual bool hinge_joint_get_flag(RID p_joint, HingeJointFlag p_flag) const; - virtual RID joint_create_slider(RID p_body_A,const Transform& p_local_frame_A,RID p_body_B,const Transform& p_local_frame_B); //reference frame is A + virtual RID joint_create_slider(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B); //reference frame is A - virtual void slider_joint_set_param(RID p_joint,SliderJointParam p_param, real_t p_value); - virtual real_t slider_joint_get_param(RID p_joint,SliderJointParam p_param) const; + virtual void slider_joint_set_param(RID p_joint, SliderJointParam p_param, real_t p_value); + virtual real_t slider_joint_get_param(RID p_joint, SliderJointParam p_param) const; - virtual RID joint_create_cone_twist(RID p_body_A,const Transform& p_local_frame_A,RID p_body_B,const Transform& p_local_frame_B); //reference frame is A + virtual RID joint_create_cone_twist(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B); //reference frame is A - virtual void cone_twist_joint_set_param(RID p_joint,ConeTwistJointParam p_param, real_t p_value); - virtual real_t cone_twist_joint_get_param(RID p_joint,ConeTwistJointParam p_param) const; + virtual void cone_twist_joint_set_param(RID p_joint, ConeTwistJointParam p_param, real_t p_value); + virtual real_t cone_twist_joint_get_param(RID p_joint, ConeTwistJointParam p_param) const; - virtual RID joint_create_generic_6dof(RID p_body_A,const Transform& p_local_frame_A,RID p_body_B,const Transform& p_local_frame_B); //reference frame is A + virtual RID joint_create_generic_6dof(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B); //reference frame is A - virtual void generic_6dof_joint_set_param(RID p_joint,Vector3::Axis,G6DOFJointAxisParam p_param, real_t p_value); - virtual real_t generic_6dof_joint_get_param(RID p_joint,Vector3::Axis,G6DOFJointAxisParam p_param); + virtual void generic_6dof_joint_set_param(RID p_joint, Vector3::Axis, G6DOFJointAxisParam p_param, real_t p_value); + virtual real_t generic_6dof_joint_get_param(RID p_joint, Vector3::Axis, G6DOFJointAxisParam p_param); - virtual void generic_6dof_joint_set_flag(RID p_joint,Vector3::Axis,G6DOFJointAxisFlag p_flag, bool p_enable); - virtual bool generic_6dof_joint_get_flag(RID p_joint,Vector3::Axis,G6DOFJointAxisFlag p_flag); + virtual void generic_6dof_joint_set_flag(RID p_joint, Vector3::Axis, G6DOFJointAxisFlag p_flag, bool p_enable); + virtual bool generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis, G6DOFJointAxisFlag p_flag); virtual JointType joint_get_type(RID p_joint) const; - virtual void joint_set_solver_priority(RID p_joint,int p_priority); + virtual void joint_set_solver_priority(RID p_joint, int p_priority); virtual int joint_get_solver_priority(RID p_joint) const; #if 0 @@ -290,8 +285,6 @@ public: PhysicsServerSW(); ~PhysicsServerSW(); - }; #endif - diff --git a/servers/physics/shape_sw.cpp b/servers/physics/shape_sw.cpp index dec1c75d9..4ce716c70 100644 --- a/servers/physics/shape_sw.cpp +++ b/servers/physics/shape_sw.cpp @@ -28,100 +28,91 @@ /*************************************************************************/ #include "shape_sw.h" #include "geometry.h" -#include "sort.h" #include "quick_hull.h" +#include "sort.h" #define _POINT_SNAP 0.001953125 #define _EDGE_IS_VALID_SUPPORT_TRESHOLD 0.0002 #define _FACE_IS_VALID_SUPPORT_TRESHOLD 0.9998 - -void ShapeSW::configure(const Rect3& p_aabb) { - aabb=p_aabb; - configured=true; - for (Map<ShapeOwnerSW*,int>::Element *E=owners.front();E;E=E->next()) { - ShapeOwnerSW* co=(ShapeOwnerSW*)E->key(); +void ShapeSW::configure(const Rect3 &p_aabb) { + aabb = p_aabb; + configured = true; + for (Map<ShapeOwnerSW *, int>::Element *E = owners.front(); E; E = E->next()) { + ShapeOwnerSW *co = (ShapeOwnerSW *)E->key(); co->_shape_changed(); } } - -Vector3 ShapeSW::get_support(const Vector3& p_normal) const { +Vector3 ShapeSW::get_support(const Vector3 &p_normal) const { Vector3 res; int amnt; - get_supports(p_normal,1,&res,amnt); + get_supports(p_normal, 1, &res, amnt); return res; } void ShapeSW::add_owner(ShapeOwnerSW *p_owner) { - Map<ShapeOwnerSW*,int>::Element *E=owners.find(p_owner); + Map<ShapeOwnerSW *, int>::Element *E = owners.find(p_owner); if (E) { E->get()++; } else { - owners[p_owner]=1; + owners[p_owner] = 1; } } -void ShapeSW::remove_owner(ShapeOwnerSW *p_owner){ +void ShapeSW::remove_owner(ShapeOwnerSW *p_owner) { - Map<ShapeOwnerSW*,int>::Element *E=owners.find(p_owner); + Map<ShapeOwnerSW *, 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 ShapeSW::is_owner(ShapeOwnerSW *p_owner) const{ +bool ShapeSW::is_owner(ShapeOwnerSW *p_owner) const { return owners.has(p_owner); - } -const Map<ShapeOwnerSW*,int>& ShapeSW::get_owners() const{ +const Map<ShapeOwnerSW *, int> &ShapeSW::get_owners() const { return owners; } - ShapeSW::ShapeSW() { - custom_bias=0; - configured=false; + custom_bias = 0; + configured = false; } - ShapeSW::~ShapeSW() { ERR_FAIL_COND(owners.size()); } - - Plane PlaneShapeSW::get_plane() const { return plane; } -void PlaneShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const { +void PlaneShapeSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { // gibberish, a plane is infinity - r_min=-1e7; - r_max=1e7; + r_min = -1e7; + r_max = 1e7; } -Vector3 PlaneShapeSW::get_support(const Vector3& p_normal) const { +Vector3 PlaneShapeSW::get_support(const Vector3 &p_normal) const { - return p_normal*1e15; + return p_normal * 1e15; } +bool PlaneShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { -bool PlaneShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const { - - bool inters=plane.intersects_segment(p_begin,p_end,&r_result); - if(inters) - r_normal=plane.normal; + bool inters = plane.intersects_segment(p_begin, p_end, &r_result); + if (inters) + r_normal = plane.normal; return inters; } @@ -130,16 +121,15 @@ Vector3 PlaneShapeSW::get_moment_of_inertia(real_t p_mass) const { return Vector3(); //wtf } -void PlaneShapeSW::_setup(const Plane& p_plane) { +void PlaneShapeSW::_setup(const Plane &p_plane) { - plane=p_plane; - configure(Rect3(Vector3(-1e4,-1e4,-1e4),Vector3(1e4*2,1e4*2,1e4*2))); + plane = p_plane; + configure(Rect3(Vector3(-1e4, -1e4, -1e4), Vector3(1e4 * 2, 1e4 * 2, 1e4 * 2))); } -void PlaneShapeSW::set_data(const Variant& p_data) { +void PlaneShapeSW::set_data(const Variant &p_data) { _setup(p_data); - } Variant PlaneShapeSW::get_data() const { @@ -147,9 +137,7 @@ Variant PlaneShapeSW::get_data() const { return plane; } -PlaneShapeSW::PlaneShapeSW() { - - +PlaneShapeSW::PlaneShapeSW() { } // @@ -159,39 +147,38 @@ real_t RayShapeSW::get_length() const { return length; } -void RayShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const { +void RayShapeSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { // don't think this will be even used - r_min=0; - r_max=1; + r_min = 0; + r_max = 1; } -Vector3 RayShapeSW::get_support(const Vector3& p_normal) const { +Vector3 RayShapeSW::get_support(const Vector3 &p_normal) const { - if (p_normal.z>0) - return Vector3(0,0,length); + if (p_normal.z > 0) + return Vector3(0, 0, length); else - return Vector3(0,0,0); + return Vector3(0, 0, 0); } -void RayShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const { +void RayShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { if (Math::abs(p_normal.z) < _EDGE_IS_VALID_SUPPORT_TRESHOLD) { - r_amount=2; - r_supports[0]=Vector3(0,0,0); - r_supports[1]=Vector3(0,0,length); - } else if (p_normal.z>0) { - r_amount=1; - *r_supports=Vector3(0,0,length); + r_amount = 2; + r_supports[0] = Vector3(0, 0, 0); + r_supports[1] = Vector3(0, 0, length); + } else if (p_normal.z > 0) { + r_amount = 1; + *r_supports = Vector3(0, 0, length); } else { - r_amount=1; - *r_supports=Vector3(0,0,0); + r_amount = 1; + *r_supports = Vector3(0, 0, 0); } } - -bool RayShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const { +bool RayShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { return false; //simply not possible } @@ -201,16 +188,15 @@ Vector3 RayShapeSW::get_moment_of_inertia(real_t p_mass) const { return Vector3(); } -void RayShapeSW::_setup(real_t p_length) { +void RayShapeSW::_setup(real_t p_length) { - length=p_length; - configure(Rect3(Vector3(0,0,0),Vector3(0.1,0.1,length))); + length = p_length; + configure(Rect3(Vector3(0, 0, 0), Vector3(0.1, 0.1, length))); } -void RayShapeSW::set_data(const Variant& p_data) { +void RayShapeSW::set_data(const Variant &p_data) { _setup(p_data); - } Variant RayShapeSW::get_data() const { @@ -218,13 +204,11 @@ Variant RayShapeSW::get_data() const { return length; } -RayShapeSW::RayShapeSW() { +RayShapeSW::RayShapeSW() { - length=1; + length = 1; } - - /********** SPHERE *************/ real_t SphereShapeSW::get_radius() const { @@ -232,50 +216,47 @@ real_t SphereShapeSW::get_radius() const { return radius; } -void SphereShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const { +void SphereShapeSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { - real_t d = p_normal.dot( p_transform.origin ); + real_t d = p_normal.dot(p_transform.origin); // figure out scale at point Vector3 local_normal = p_transform.basis.xform_inv(p_normal); real_t scale = local_normal.length(); - r_min = d - (radius) * scale; - r_max = d + (radius) * scale; - + r_min = d - (radius)*scale; + r_max = d + (radius)*scale; } -Vector3 SphereShapeSW::get_support(const Vector3& p_normal) const { +Vector3 SphereShapeSW::get_support(const Vector3 &p_normal) const { - return p_normal*radius; + return p_normal * radius; } -void SphereShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const { +void SphereShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { - *r_supports=p_normal*radius; - r_amount=1; + *r_supports = p_normal * radius; + r_amount = 1; } -bool SphereShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const { +bool SphereShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { - return Geometry::segment_intersects_sphere(p_begin,p_end,Vector3(),radius,&r_result,&r_normal); + return Geometry::segment_intersects_sphere(p_begin, p_end, Vector3(), radius, &r_result, &r_normal); } Vector3 SphereShapeSW::get_moment_of_inertia(real_t p_mass) const { real_t s = 0.4 * p_mass * radius * radius; - return Vector3(s,s,s); + return Vector3(s, s, s); } void SphereShapeSW::_setup(real_t p_radius) { - - radius=p_radius; - configure(Rect3( Vector3(-radius,-radius,-radius), Vector3(radius*2.0,radius*2.0,radius*2.0))); - + radius = p_radius; + configure(Rect3(Vector3(-radius, -radius, -radius), Vector3(radius * 2.0, radius * 2.0, radius * 2.0))); } -void SphereShapeSW::set_data(const Variant& p_data) { +void SphereShapeSW::set_data(const Variant &p_data) { _setup(p_data); } @@ -287,113 +268,105 @@ Variant SphereShapeSW::get_data() const { SphereShapeSW::SphereShapeSW() { - radius=0; + radius = 0; } - /********** BOX *************/ - -void BoxShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const { +void BoxShapeSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { // no matter the angle, the box is mirrored anyway - Vector3 local_normal=p_transform.basis.xform_inv(p_normal); + Vector3 local_normal = p_transform.basis.xform_inv(p_normal); real_t length = local_normal.abs().dot(half_extents); - real_t distance = p_normal.dot( p_transform.origin ); + real_t distance = p_normal.dot(p_transform.origin); r_min = distance - length; r_max = distance + length; - - } -Vector3 BoxShapeSW::get_support(const Vector3& p_normal) const { - +Vector3 BoxShapeSW::get_support(const Vector3 &p_normal) const { Vector3 point( - (p_normal.x<0) ? -half_extents.x : half_extents.x, - (p_normal.y<0) ? -half_extents.y : half_extents.y, - (p_normal.z<0) ? -half_extents.z : half_extents.z - ); + (p_normal.x < 0) ? -half_extents.x : half_extents.x, + (p_normal.y < 0) ? -half_extents.y : half_extents.y, + (p_normal.z < 0) ? -half_extents.z : half_extents.z); return point; } -void BoxShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const { +void BoxShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { - static const int next[3]={1,2,0}; - static const int next2[3]={2,0,1}; + static const int next[3] = { 1, 2, 0 }; + static const int next2[3] = { 2, 0, 1 }; - for (int i=0;i<3;i++) { + for (int i = 0; i < 3; i++) { Vector3 axis; - axis[i]=1.0; - real_t dot = p_normal.dot( axis ); - if ( Math::abs( dot ) > _FACE_IS_VALID_SUPPORT_TRESHOLD ) { + axis[i] = 1.0; + real_t dot = p_normal.dot(axis); + if (Math::abs(dot) > _FACE_IS_VALID_SUPPORT_TRESHOLD) { //Vector3 axis_b; - bool neg = dot<0; + bool neg = dot < 0; r_amount = 4; Vector3 point; - point[i]=half_extents[i]; + point[i] = half_extents[i]; - int i_n=next[i]; - int i_n2=next2[i]; + int i_n = next[i]; + int i_n2 = next2[i]; - static const real_t sign[4][2]={ + static const real_t sign[4][2] = { - {-1.0, 1.0}, - { 1.0, 1.0}, - { 1.0,-1.0}, - {-1.0,-1.0}, + { -1.0, 1.0 }, + { 1.0, 1.0 }, + { 1.0, -1.0 }, + { -1.0, -1.0 }, }; - for (int j=0;j<4;j++) { - - point[i_n]=sign[j][0]*half_extents[i_n]; - point[i_n2]=sign[j][1]*half_extents[i_n2]; - r_supports[j]=neg?-point:point; + for (int j = 0; j < 4; j++) { + point[i_n] = sign[j][0] * half_extents[i_n]; + point[i_n2] = sign[j][1] * half_extents[i_n2]; + r_supports[j] = neg ? -point : point; } if (neg) { - SWAP( r_supports[1], r_supports[2] ); - SWAP( r_supports[0], r_supports[3] ); + SWAP(r_supports[1], r_supports[2]); + SWAP(r_supports[0], r_supports[3]); } return; } - r_amount=0; - + r_amount = 0; } - for (int i=0;i<3;i++) { + for (int i = 0; i < 3; i++) { Vector3 axis; - axis[i]=1.0; + axis[i] = 1.0; - if (Math::abs(p_normal.dot(axis))<_EDGE_IS_VALID_SUPPORT_TRESHOLD) { + if (Math::abs(p_normal.dot(axis)) < _EDGE_IS_VALID_SUPPORT_TRESHOLD) { - r_amount= 2; + r_amount = 2; - int i_n=next[i]; - int i_n2=next2[i]; + int i_n = next[i]; + int i_n2 = next2[i]; - Vector3 point=half_extents; + Vector3 point = half_extents; - if (p_normal[i_n]<0) { - point[i_n]=-point[i_n]; + if (p_normal[i_n] < 0) { + point[i_n] = -point[i_n]; } - if (p_normal[i_n2]<0) { - point[i_n2]=-point[i_n2]; + if (p_normal[i_n2] < 0) { + point[i_n2] = -point[i_n2]; } r_supports[0] = point; - point[i]=-point[i]; + point[i] = -point[i]; r_supports[1] = point; return; } @@ -401,44 +374,38 @@ void BoxShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_suppo /* USE POINT */ Vector3 point( - (p_normal.x<0) ? -half_extents.x : half_extents.x, - (p_normal.y<0) ? -half_extents.y : half_extents.y, - (p_normal.z<0) ? -half_extents.z : half_extents.z - ); + (p_normal.x < 0) ? -half_extents.x : half_extents.x, + (p_normal.y < 0) ? -half_extents.y : half_extents.y, + (p_normal.z < 0) ? -half_extents.z : half_extents.z); - r_amount=1; - r_supports[0]=point; + r_amount = 1; + r_supports[0] = point; } -bool BoxShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const { - - Rect3 aabb(-half_extents,half_extents*2.0); +bool BoxShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { - return aabb.intersects_segment(p_begin,p_end,&r_result,&r_normal); + Rect3 aabb(-half_extents, half_extents * 2.0); + return aabb.intersects_segment(p_begin, p_end, &r_result, &r_normal); } Vector3 BoxShapeSW::get_moment_of_inertia(real_t p_mass) const { - real_t lx=half_extents.x; - real_t ly=half_extents.y; - real_t lz=half_extents.z; - - return Vector3( (p_mass/3.0) * (ly*ly + lz*lz), (p_mass/3.0) * (lx*lx + lz*lz), (p_mass/3.0) * (lx*lx + ly*ly) ); + real_t lx = half_extents.x; + real_t ly = half_extents.y; + real_t lz = half_extents.z; + return Vector3((p_mass / 3.0) * (ly * ly + lz * lz), (p_mass / 3.0) * (lx * lx + lz * lz), (p_mass / 3.0) * (lx * lx + ly * ly)); } -void BoxShapeSW::_setup(const Vector3& p_half_extents) { - - half_extents=p_half_extents.abs(); - - configure(Rect3(-half_extents,half_extents*2)); +void BoxShapeSW::_setup(const Vector3 &p_half_extents) { + half_extents = p_half_extents.abs(); + configure(Rect3(-half_extents, half_extents * 2)); } -void BoxShapeSW::set_data(const Variant& p_data) { - +void BoxShapeSW::set_data(const Variant &p_data) { _setup(p_data); } @@ -448,138 +415,128 @@ Variant BoxShapeSW::get_data() const { return half_extents; } -BoxShapeSW::BoxShapeSW() { - - +BoxShapeSW::BoxShapeSW() { } - /********** CAPSULE *************/ +void CapsuleShapeSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { -void CapsuleShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const { - - Vector3 n=p_transform.basis.xform_inv(p_normal).normalized(); + Vector3 n = p_transform.basis.xform_inv(p_normal).normalized(); real_t h = (n.z > 0) ? height : -height; n *= radius; n.z += h * 0.5; - r_max=p_normal.dot(p_transform.xform(n)); - r_min=p_normal.dot(p_transform.xform(-n)); + r_max = p_normal.dot(p_transform.xform(n)); + r_min = p_normal.dot(p_transform.xform(-n)); return; n = p_transform.basis.xform(n); - real_t distance = p_normal.dot( p_transform.origin ); + real_t distance = p_normal.dot(p_transform.origin); real_t length = Math::abs(p_normal.dot(n)); r_min = distance - length; r_max = distance + length; - ERR_FAIL_COND( r_max < r_min ); - + ERR_FAIL_COND(r_max < r_min); } -Vector3 CapsuleShapeSW::get_support(const Vector3& p_normal) const { +Vector3 CapsuleShapeSW::get_support(const Vector3 &p_normal) const { - Vector3 n=p_normal; + Vector3 n = p_normal; real_t h = (n.z > 0) ? height : -height; - n*=radius; - n.z += h*0.5; + n *= radius; + n.z += h * 0.5; return n; } -void CapsuleShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const { - +void CapsuleShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { - Vector3 n=p_normal; + Vector3 n = p_normal; real_t d = n.z; - if (Math::abs( d )<_EDGE_IS_VALID_SUPPORT_TRESHOLD ) { + if (Math::abs(d) < _EDGE_IS_VALID_SUPPORT_TRESHOLD) { // make it flat - n.z=0.0; + n.z = 0.0; n.normalize(); - n*=radius; + n *= radius; - r_amount=2; - r_supports[0]=n; - r_supports[0].z+=height*0.5; - r_supports[1]=n; - r_supports[1].z-=height*0.5; + r_amount = 2; + r_supports[0] = n; + r_supports[0].z += height * 0.5; + r_supports[1] = n; + r_supports[1].z -= height * 0.5; } else { real_t h = (d > 0) ? height : -height; - n*=radius; - n.z += h*0.5; - r_amount=1; - *r_supports=n; - + n *= radius; + n.z += h * 0.5; + r_amount = 1; + *r_supports = n; } - } +bool CapsuleShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { -bool CapsuleShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const { + Vector3 norm = (p_end - p_begin).normalized(); + real_t min_d = 1e20; - Vector3 norm=(p_end-p_begin).normalized(); - real_t min_d=1e20; + Vector3 res, n; + bool collision = false; - - Vector3 res,n; - bool collision=false; - - Vector3 auxres,auxn; + Vector3 auxres, auxn; bool collided; // test against cylinder and spheres :-| - collided = Geometry::segment_intersects_cylinder(p_begin,p_end,height,radius,&auxres,&auxn); + collided = Geometry::segment_intersects_cylinder(p_begin, p_end, height, radius, &auxres, &auxn); if (collided) { - real_t d=norm.dot(auxres); - if (d<min_d) { - min_d=d; - res=auxres; - n=auxn; - collision=true; + real_t d = norm.dot(auxres); + if (d < min_d) { + min_d = d; + res = auxres; + n = auxn; + collision = true; } } - collided = Geometry::segment_intersects_sphere(p_begin,p_end,Vector3(0,0,height*0.5),radius,&auxres,&auxn); + collided = Geometry::segment_intersects_sphere(p_begin, p_end, Vector3(0, 0, height * 0.5), radius, &auxres, &auxn); if (collided) { - real_t d=norm.dot(auxres); - if (d<min_d) { - min_d=d; - res=auxres; - n=auxn; - collision=true; + real_t d = norm.dot(auxres); + if (d < min_d) { + min_d = d; + res = auxres; + n = auxn; + collision = true; } } - collided = Geometry::segment_intersects_sphere(p_begin,p_end,Vector3(0,0,height*-0.5),radius,&auxres,&auxn); + collided = Geometry::segment_intersects_sphere(p_begin, p_end, Vector3(0, 0, height * -0.5), radius, &auxres, &auxn); if (collided) { - real_t d=norm.dot(auxres); + real_t d = norm.dot(auxres); - if (d<min_d) { - min_d=d; - res=auxres; - n=auxn; - collision=true; + if (d < min_d) { + min_d = d; + res = auxres; + n = auxn; + collision = true; } } if (collision) { - r_result=res; - r_normal=n; + r_result = res; + r_normal = n; } return collision; } @@ -587,105 +544,90 @@ bool CapsuleShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_e Vector3 CapsuleShapeSW::get_moment_of_inertia(real_t p_mass) const { // use crappy AABB approximation - Vector3 extents=get_aabb().size*0.5; + Vector3 extents = get_aabb().size * 0.5; return Vector3( - (p_mass/3.0) * (extents.y*extents.y + extents.z*extents.z), - (p_mass/3.0) * (extents.x*extents.x + extents.z*extents.z), - (p_mass/3.0) * (extents.y*extents.y + extents.y*extents.y) - ); - + (p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z), + (p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z), + (p_mass / 3.0) * (extents.y * extents.y + extents.y * extents.y)); } +void CapsuleShapeSW::_setup(real_t p_height, real_t p_radius) { - - -void CapsuleShapeSW::_setup(real_t p_height,real_t p_radius) { - - height=p_height; - radius=p_radius; - configure(Rect3(Vector3(-radius,-radius,-height*0.5-radius),Vector3(radius*2,radius*2,height+radius*2.0))); - + height = p_height; + radius = p_radius; + configure(Rect3(Vector3(-radius, -radius, -height * 0.5 - radius), Vector3(radius * 2, radius * 2, height + radius * 2.0))); } -void CapsuleShapeSW::set_data(const Variant& p_data) { +void CapsuleShapeSW::set_data(const Variant &p_data) { Dictionary d = p_data; ERR_FAIL_COND(!d.has("radius")); ERR_FAIL_COND(!d.has("height")); - _setup(d["height"],d["radius"]); - + _setup(d["height"], d["radius"]); } Variant CapsuleShapeSW::get_data() const { Dictionary d; - d["radius"]=radius; - d["height"]=height; + d["radius"] = radius; + d["height"] = height; return d; - } +CapsuleShapeSW::CapsuleShapeSW() { -CapsuleShapeSW::CapsuleShapeSW() { - - height=radius=0; - + height = radius = 0; } /********** CONVEX POLYGON *************/ +void ConvexPolygonShapeSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { -void ConvexPolygonShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const { - - - int vertex_count=mesh.vertices.size(); - if (vertex_count==0) + int vertex_count = mesh.vertices.size(); + if (vertex_count == 0) return; - const Vector3 *vrts=&mesh.vertices[0]; + const Vector3 *vrts = &mesh.vertices[0]; - for (int i=0;i<vertex_count;i++) { + for (int i = 0; i < vertex_count; i++) { - real_t d=p_normal.dot( p_transform.xform( vrts[i] ) ); + real_t d = p_normal.dot(p_transform.xform(vrts[i])); - if (i==0 || d > r_max) - r_max=d; - if (i==0 || d < r_min) - r_min=d; + if (i == 0 || d > r_max) + r_max = d; + if (i == 0 || d < r_min) + r_min = d; } } -Vector3 ConvexPolygonShapeSW::get_support(const Vector3& p_normal) const { +Vector3 ConvexPolygonShapeSW::get_support(const Vector3 &p_normal) const { - Vector3 n=p_normal; + Vector3 n = p_normal; - int vert_support_idx=-1; + int vert_support_idx = -1; real_t support_max; - int vertex_count=mesh.vertices.size(); - if (vertex_count==0) + int vertex_count = mesh.vertices.size(); + if (vertex_count == 0) return Vector3(); - const Vector3 *vrts=&mesh.vertices[0]; + const Vector3 *vrts = &mesh.vertices[0]; - for (int i=0;i<vertex_count;i++) { + for (int i = 0; i < vertex_count; i++) { - real_t d=n.dot(vrts[i]); + real_t d = n.dot(vrts[i]); - if (i==0 || d > support_max) { - support_max=d; - vert_support_idx=i; + if (i == 0 || d > support_max) { + support_max = d; + vert_support_idx = i; } } - return vrts[vert_support_idx]; - + return vrts[vert_support_idx]; } - - -void ConvexPolygonShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const { +void ConvexPolygonShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { const Geometry::MeshData::Face *faces = mesh.faces.ptr(); int fc = mesh.faces.size(); @@ -700,28 +642,27 @@ void ConvexPolygonShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector real_t max; int vtx; - for (int i=0;i<vc;i++) { + for (int i = 0; i < vc; i++) { - real_t d=p_normal.dot(vertices[i]); + real_t d = p_normal.dot(vertices[i]); - if (i==0 || d > max) { - max=d; - vtx=i; + if (i == 0 || d > max) { + max = d; + vtx = i; } } + for (int i = 0; i < fc; i++) { - for(int i=0;i<fc;i++) { - - if (faces[i].plane.normal.dot(p_normal)>_FACE_IS_VALID_SUPPORT_TRESHOLD) { + if (faces[i].plane.normal.dot(p_normal) > _FACE_IS_VALID_SUPPORT_TRESHOLD) { int ic = faces[i].indices.size(); - const int *ind=faces[i].indices.ptr(); + const int *ind = faces[i].indices.ptr(); - bool valid=false; - for(int j=0;j<ic;j++) { - if (ind[j]==vtx) { - valid=true; + bool valid = false; + for (int j = 0; j < ic; j++) { + if (ind[j] == vtx) { + valid = true; break; } } @@ -729,114 +670,103 @@ void ConvexPolygonShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector if (!valid) continue; - int m = MIN(p_max,ic); - for(int j=0;j<m;j++) { + int m = MIN(p_max, ic); + for (int j = 0; j < m; j++) { - r_supports[j]=vertices[ind[j]]; + r_supports[j] = vertices[ind[j]]; } - r_amount=m; + r_amount = m; return; } } - for(int i=0;i<ec;i++) { + for (int i = 0; i < ec; i++) { + real_t dot = (vertices[edges[i].a] - vertices[edges[i].b]).normalized().dot(p_normal); + dot = ABS(dot); + if (dot < _EDGE_IS_VALID_SUPPORT_TRESHOLD && (edges[i].a == vtx || edges[i].b == vtx)) { - real_t dot=(vertices[edges[i].a]-vertices[edges[i].b]).normalized().dot(p_normal); - dot=ABS(dot); - if (dot < _EDGE_IS_VALID_SUPPORT_TRESHOLD && (edges[i].a==vtx || edges[i].b==vtx)) { - - r_amount=2; - r_supports[0]=vertices[edges[i].a]; - r_supports[1]=vertices[edges[i].b]; + r_amount = 2; + r_supports[0] = vertices[edges[i].a]; + r_supports[1] = vertices[edges[i].b]; return; } } - - r_supports[0]=vertices[vtx]; - r_amount=1; + r_supports[0] = vertices[vtx]; + r_amount = 1; } -bool ConvexPolygonShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const { - - +bool ConvexPolygonShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { const Geometry::MeshData::Face *faces = mesh.faces.ptr(); int fc = mesh.faces.size(); const Vector3 *vertices = mesh.vertices.ptr(); - Vector3 n = p_end-p_begin; + Vector3 n = p_end - p_begin; real_t min = 1e20; - bool col=false; + bool col = false; - for(int i=0;i<fc;i++) { + for (int i = 0; i < fc; i++) { if (faces[i].plane.normal.dot(n) > 0) continue; //opposing face int ic = faces[i].indices.size(); - const int *ind=faces[i].indices.ptr(); + const int *ind = faces[i].indices.ptr(); - for(int j=1;j<ic-1;j++) { + for (int j = 1; j < ic - 1; j++) { - Face3 f(vertices[ind[0]],vertices[ind[j]],vertices[ind[j+1]]); + Face3 f(vertices[ind[0]], vertices[ind[j]], vertices[ind[j + 1]]); Vector3 result; - if (f.intersects_segment(p_begin,p_end,&result)) { + if (f.intersects_segment(p_begin, p_end, &result)) { real_t d = n.dot(result); - if (d<min) { - min=d; - r_result=result; - r_normal=faces[i].plane.normal; - col=true; + if (d < min) { + min = d; + r_result = result; + r_normal = faces[i].plane.normal; + col = true; } break; } - } } return col; - } Vector3 ConvexPolygonShapeSW::get_moment_of_inertia(real_t p_mass) const { // use crappy AABB approximation - Vector3 extents=get_aabb().size*0.5; - - return Vector3( - (p_mass/3.0) * (extents.y*extents.y + extents.z*extents.z), - (p_mass/3.0) * (extents.x*extents.x + extents.z*extents.z), - (p_mass/3.0) * (extents.y*extents.y + extents.y*extents.y) - ); + Vector3 extents = get_aabb().size * 0.5; + return Vector3( + (p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z), + (p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z), + (p_mass / 3.0) * (extents.y * extents.y + extents.y * extents.y)); } -void ConvexPolygonShapeSW::_setup(const Vector<Vector3>& p_vertices) { +void ConvexPolygonShapeSW::_setup(const Vector<Vector3> &p_vertices) { - Error err = QuickHull::build(p_vertices,mesh); + Error err = QuickHull::build(p_vertices, mesh); Rect3 _aabb; - for(int i=0;i<mesh.vertices.size();i++) { + for (int i = 0; i < mesh.vertices.size(); i++) { - if (i==0) - _aabb.pos=mesh.vertices[i]; + if (i == 0) + _aabb.pos = mesh.vertices[i]; else _aabb.expand_to(mesh.vertices[i]); } configure(_aabb); - - } -void ConvexPolygonShapeSW::set_data(const Variant& p_data) { +void ConvexPolygonShapeSW::set_data(const Variant &p_data) { _setup(p_data); - } Variant ConvexPolygonShapeSW::get_data() const { @@ -844,113 +774,105 @@ Variant ConvexPolygonShapeSW::get_data() const { return mesh.vertices; } - -ConvexPolygonShapeSW::ConvexPolygonShapeSW() { - - +ConvexPolygonShapeSW::ConvexPolygonShapeSW() { } - /********** FACE POLYGON *************/ +void FaceShapeSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { -void FaceShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const { + for (int i = 0; i < 3; i++) { - for (int i=0;i<3;i++) { + Vector3 v = p_transform.xform(vertex[i]); + real_t d = p_normal.dot(v); - Vector3 v=p_transform.xform(vertex[i]); - real_t d=p_normal.dot(v); + if (i == 0 || d > r_max) + r_max = d; - if (i==0 || d > r_max) - r_max=d; - - if (i==0 || d < r_min) - r_min=d; + if (i == 0 || d < r_min) + r_min = d; } } -Vector3 FaceShapeSW::get_support(const Vector3& p_normal) const { - +Vector3 FaceShapeSW::get_support(const Vector3 &p_normal) const { - int vert_support_idx=-1; + int vert_support_idx = -1; real_t support_max; - for (int i=0;i<3;i++) { + for (int i = 0; i < 3; i++) { - real_t d=p_normal.dot(vertex[i]); + real_t d = p_normal.dot(vertex[i]); - if (i==0 || d > support_max) { - support_max=d; - vert_support_idx=i; + if (i == 0 || d > support_max) { + support_max = d; + vert_support_idx = i; } } return vertex[vert_support_idx]; } -void FaceShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const { +void FaceShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { - Vector3 n=p_normal; + Vector3 n = p_normal; /** TEST FACE AS SUPPORT **/ if (normal.dot(n) > _FACE_IS_VALID_SUPPORT_TRESHOLD) { - r_amount=3; - for (int i=0;i<3;i++) { + r_amount = 3; + for (int i = 0; i < 3; i++) { - r_supports[i]=vertex[i]; + r_supports[i] = vertex[i]; } return; - } /** FIND SUPPORT VERTEX **/ - int vert_support_idx=-1; + int vert_support_idx = -1; real_t support_max; - for (int i=0;i<3;i++) { + for (int i = 0; i < 3; i++) { - real_t d=n.dot(vertex[i]); + real_t d = n.dot(vertex[i]); - if (i==0 || d > support_max) { - support_max=d; - vert_support_idx=i; + if (i == 0 || d > support_max) { + support_max = d; + vert_support_idx = i; } } /** TEST EDGES AS SUPPORT **/ - for (int i=0;i<3;i++) { + for (int i = 0; i < 3; i++) { - int nx=(i+1)%3; - if (i!=vert_support_idx && nx!=vert_support_idx) + int nx = (i + 1) % 3; + if (i != vert_support_idx && nx != vert_support_idx) continue; - // check if edge is valid as a support - real_t dot=(vertex[i]-vertex[nx]).normalized().dot(n); - dot=ABS(dot); + // check if edge is valid as a support + real_t dot = (vertex[i] - vertex[nx]).normalized().dot(n); + dot = ABS(dot); if (dot < _EDGE_IS_VALID_SUPPORT_TRESHOLD) { - r_amount=2; - r_supports[0]=vertex[i]; - r_supports[1]=vertex[nx]; + r_amount = 2; + r_supports[0] = vertex[i]; + r_supports[1] = vertex[nx]; return; } } - r_amount=1; - r_supports[0]=vertex[vert_support_idx]; + r_amount = 1; + r_supports[0] = vertex[vert_support_idx]; } -bool FaceShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const { +bool FaceShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { - - bool c=Geometry::segment_intersects_triangle(p_begin,p_end,vertex[0],vertex[1],vertex[2],&r_result); + bool c = Geometry::segment_intersects_triangle(p_begin, p_end, vertex[0], vertex[1], vertex[2], &r_result); if (c) { - r_normal=Plane(vertex[0],vertex[1],vertex[2]).normal; - if (r_normal.dot(p_end-p_begin)>0) { - r_normal=-r_normal; + r_normal = Plane(vertex[0], vertex[1], vertex[2]).normal; + if (r_normal.dot(p_end - p_begin) > 0) { + r_normal = -r_normal; } } @@ -960,183 +882,162 @@ bool FaceShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end, Vector3 FaceShapeSW::get_moment_of_inertia(real_t p_mass) const { return Vector3(); // Sorry, but i don't think anyone cares, FaceShape! - } -FaceShapeSW::FaceShapeSW() { +FaceShapeSW::FaceShapeSW() { configure(Rect3()); - } - - PoolVector<Vector3> ConcavePolygonShapeSW::get_faces() const { - PoolVector<Vector3> rfaces; - rfaces.resize(faces.size()*3); + rfaces.resize(faces.size() * 3); - for(int i=0;i<faces.size();i++) { + for (int i = 0; i < faces.size(); i++) { - Face f=faces.get(i); + Face f = faces.get(i); - for(int j=0;j<3;j++) { + for (int j = 0; j < 3; j++) { - rfaces.set(i*3+j, vertices.get( f.indices[j] ) ); + rfaces.set(i * 3 + j, vertices.get(f.indices[j])); } } return rfaces; } -void ConcavePolygonShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const { +void ConcavePolygonShapeSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { - int count=vertices.size(); - if (count==0) { - r_min=0; - r_max=0; + int count = vertices.size(); + if (count == 0) { + r_min = 0; + r_max = 0; return; } - PoolVector<Vector3>::Read r=vertices.read(); - const Vector3 *vptr=r.ptr(); + PoolVector<Vector3>::Read r = vertices.read(); + const Vector3 *vptr = r.ptr(); - for (int i=0;i<count;i++) { + for (int i = 0; i < count; i++) { - real_t d=p_normal.dot( p_transform.xform( vptr[i] ) ); - - if (i==0 || d > r_max) - r_max=d; - if (i==0 || d < r_min) - r_min=d; + real_t d = p_normal.dot(p_transform.xform(vptr[i])); + if (i == 0 || d > r_max) + r_max = d; + if (i == 0 || d < r_min) + r_min = d; } } -Vector3 ConcavePolygonShapeSW::get_support(const Vector3& p_normal) const { - +Vector3 ConcavePolygonShapeSW::get_support(const Vector3 &p_normal) const { - int count=vertices.size(); - if (count==0) + int count = vertices.size(); + if (count == 0) return Vector3(); - PoolVector<Vector3>::Read r=vertices.read(); - const Vector3 *vptr=r.ptr(); + PoolVector<Vector3>::Read r = vertices.read(); + const Vector3 *vptr = r.ptr(); - Vector3 n=p_normal; + Vector3 n = p_normal; - int vert_support_idx=-1; + int vert_support_idx = -1; real_t support_max; - for (int i=0;i<count;i++) { + for (int i = 0; i < count; i++) { - real_t d=n.dot(vptr[i]); + real_t d = n.dot(vptr[i]); - if (i==0 || d > support_max) { - support_max=d; - vert_support_idx=i; + if (i == 0 || d > support_max) { + support_max = d; + vert_support_idx = i; } } - return vptr[vert_support_idx]; - } -void ConcavePolygonShapeSW::_cull_segment(int p_idx,_SegmentCullParams *p_params) const { - - const BVH *bvh=&p_params->bvh[p_idx]; +void ConcavePolygonShapeSW::_cull_segment(int p_idx, _SegmentCullParams *p_params) const { + const BVH *bvh = &p_params->bvh[p_idx]; /* if (p_params->dir.dot(bvh->aabb.get_support(-p_params->dir))>p_params->min_d) return; //test against whole AABB, which isn't very costly */ - //printf("addr: %p\n",bvh); - if (!bvh->aabb.intersects_segment(p_params->from,p_params->to)) { + if (!bvh->aabb.intersects_segment(p_params->from, p_params->to)) { return; } - - if (bvh->face_index>=0) { - + if (bvh->face_index >= 0) { Vector3 res; - Vector3 vertices[3]={ - p_params->vertices[ p_params->faces[ bvh->face_index ].indices[0] ], - p_params->vertices[ p_params->faces[ bvh->face_index ].indices[1] ], - p_params->vertices[ p_params->faces[ bvh->face_index ].indices[2] ] + Vector3 vertices[3] = { + p_params->vertices[p_params->faces[bvh->face_index].indices[0]], + p_params->vertices[p_params->faces[bvh->face_index].indices[1]], + p_params->vertices[p_params->faces[bvh->face_index].indices[2]] }; if (Geometry::segment_intersects_triangle( - p_params->from, - p_params->to, - vertices[0], - vertices[1], - vertices[2], - &res)) { + p_params->from, + p_params->to, + vertices[0], + vertices[1], + vertices[2], + &res)) { - - real_t d=p_params->dir.dot(res) - p_params->dir.dot(p_params->from); + real_t d = p_params->dir.dot(res) - p_params->dir.dot(p_params->from); //TODO, seems segmen/triangle intersection is broken :( - if (d>0 && d<p_params->min_d) { + if (d > 0 && d < p_params->min_d) { - p_params->min_d=d; - p_params->result=res; - p_params->normal=Plane(vertices[0],vertices[1],vertices[2]).normal; - if (p_params->normal.dot(p_params->dir)>0) - p_params->normal=-p_params->normal; + p_params->min_d = d; + p_params->result = res; + p_params->normal = Plane(vertices[0], vertices[1], vertices[2]).normal; + if (p_params->normal.dot(p_params->dir) > 0) + p_params->normal = -p_params->normal; p_params->collisions++; } - } - - } else { - if (bvh->left>=0) - _cull_segment(bvh->left,p_params); - if (bvh->right>=0) - _cull_segment(bvh->right,p_params); - - + if (bvh->left >= 0) + _cull_segment(bvh->left, p_params); + if (bvh->right >= 0) + _cull_segment(bvh->right, p_params); } } -bool ConcavePolygonShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const { +bool ConcavePolygonShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { - if (faces.size()==0) + if (faces.size() == 0) return false; // unlock data - PoolVector<Face>::Read fr=faces.read(); - PoolVector<Vector3>::Read vr=vertices.read(); - PoolVector<BVH>::Read br=bvh.read(); - + PoolVector<Face>::Read fr = faces.read(); + PoolVector<Vector3>::Read vr = vertices.read(); + PoolVector<BVH>::Read br = bvh.read(); _SegmentCullParams params; - params.from=p_begin; - params.to=p_end; - params.collisions=0; - params.dir=(p_end-p_begin).normalized(); + params.from = p_begin; + params.to = p_end; + params.collisions = 0; + params.dir = (p_end - p_begin).normalized(); - params.faces=fr.ptr(); - params.vertices=vr.ptr(); - params.bvh=br.ptr(); + params.faces = fr.ptr(); + params.vertices = vr.ptr(); + params.bvh = br.ptr(); - params.min_d=1e20; + params.min_d = 1e20; // cull - _cull_segment(0,¶ms); + _cull_segment(0, ¶ms); - if (params.collisions>0) { + if (params.collisions > 0) { - - r_result=params.result; - r_normal=params.normal; + r_result = params.result; + r_normal = params.normal; return true; } else { @@ -1144,81 +1045,76 @@ bool ConcavePolygonShapeSW::intersect_segment(const Vector3& p_begin,const Vecto } } -void ConcavePolygonShapeSW::_cull(int p_idx,_CullParams *p_params) const { +void ConcavePolygonShapeSW::_cull(int p_idx, _CullParams *p_params) const { - const BVH* bvh=&p_params->bvh[p_idx]; + const BVH *bvh = &p_params->bvh[p_idx]; - if (!p_params->aabb.intersects( bvh->aabb )) + if (!p_params->aabb.intersects(bvh->aabb)) return; - if (bvh->face_index>=0) { + if (bvh->face_index >= 0) { - const Face *f=&p_params->faces[ bvh->face_index ]; - FaceShapeSW *face=p_params->face; - face->normal=f->normal; - face->vertex[0]=p_params->vertices[f->indices[0]]; - face->vertex[1]=p_params->vertices[f->indices[1]]; - face->vertex[2]=p_params->vertices[f->indices[2]]; - p_params->callback(p_params->userdata,face); + const Face *f = &p_params->faces[bvh->face_index]; + FaceShapeSW *face = p_params->face; + face->normal = f->normal; + face->vertex[0] = p_params->vertices[f->indices[0]]; + face->vertex[1] = p_params->vertices[f->indices[1]]; + face->vertex[2] = p_params->vertices[f->indices[2]]; + p_params->callback(p_params->userdata, face); } else { - if (bvh->left>=0) { - - _cull(bvh->left,p_params); + if (bvh->left >= 0) { + _cull(bvh->left, p_params); } - if (bvh->right>=0) { + if (bvh->right >= 0) { - _cull(bvh->right,p_params); + _cull(bvh->right, p_params); } - } } -void ConcavePolygonShapeSW::cull(const Rect3& p_local_aabb,Callback p_callback,void* p_userdata) const { +void ConcavePolygonShapeSW::cull(const Rect3 &p_local_aabb, Callback p_callback, void *p_userdata) const { // make matrix local to concave - if (faces.size()==0) + if (faces.size() == 0) return; - Rect3 local_aabb=p_local_aabb; + Rect3 local_aabb = p_local_aabb; // unlock data - PoolVector<Face>::Read fr=faces.read(); - PoolVector<Vector3>::Read vr=vertices.read(); - PoolVector<BVH>::Read br=bvh.read(); + PoolVector<Face>::Read fr = faces.read(); + PoolVector<Vector3>::Read vr = vertices.read(); + PoolVector<BVH>::Read br = bvh.read(); FaceShapeSW face; // use this to send in the callback _CullParams params; - params.aabb=local_aabb; - params.face=&face; - params.faces=fr.ptr(); - params.vertices=vr.ptr(); - params.bvh=br.ptr(); - params.callback=p_callback; - params.userdata=p_userdata; + params.aabb = local_aabb; + params.face = &face; + params.faces = fr.ptr(); + params.vertices = vr.ptr(); + params.bvh = br.ptr(); + params.callback = p_callback; + params.userdata = p_userdata; // cull - _cull(0,¶ms); - + _cull(0, ¶ms); } Vector3 ConcavePolygonShapeSW::get_moment_of_inertia(real_t p_mass) const { // use crappy AABB approximation - Vector3 extents=get_aabb().size*0.5; + Vector3 extents = get_aabb().size * 0.5; return Vector3( - (p_mass/3.0) * (extents.y*extents.y + extents.z*extents.z), - (p_mass/3.0) * (extents.x*extents.x + extents.z*extents.z), - (p_mass/3.0) * (extents.y*extents.y + extents.y*extents.y) - ); + (p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z), + (p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z), + (p_mass / 3.0) * (extents.y * extents.y + extents.y * extents.y)); } - struct _VolumeSW_BVH_Element { Rect3 aabb; @@ -1228,26 +1124,25 @@ struct _VolumeSW_BVH_Element { struct _VolumeSW_BVH_CompareX { - _FORCE_INLINE_ bool operator ()(const _VolumeSW_BVH_Element& a, const _VolumeSW_BVH_Element& b) const { + _FORCE_INLINE_ bool operator()(const _VolumeSW_BVH_Element &a, const _VolumeSW_BVH_Element &b) const { - return a.center.x<b.center.x; + return a.center.x < b.center.x; } }; - struct _VolumeSW_BVH_CompareY { - _FORCE_INLINE_ bool operator ()(const _VolumeSW_BVH_Element& a, const _VolumeSW_BVH_Element& b) const { + _FORCE_INLINE_ bool operator()(const _VolumeSW_BVH_Element &a, const _VolumeSW_BVH_Element &b) const { - return a.center.y<b.center.y; + return a.center.y < b.center.y; } }; struct _VolumeSW_BVH_CompareZ { - _FORCE_INLINE_ bool operator ()(const _VolumeSW_BVH_Element& a, const _VolumeSW_BVH_Element& b) const { + _FORCE_INLINE_ bool operator()(const _VolumeSW_BVH_Element &a, const _VolumeSW_BVH_Element &b) const { - return a.center.z<b.center.z; + return a.center.z < b.center.z; } }; @@ -1260,107 +1155,102 @@ struct _VolumeSW_BVH { int face_index; }; +_VolumeSW_BVH *_volume_sw_build_bvh(_VolumeSW_BVH_Element *p_elements, int p_size, int &count) { -_VolumeSW_BVH* _volume_sw_build_bvh(_VolumeSW_BVH_Element *p_elements,int p_size,int &count) { - - _VolumeSW_BVH* bvh = memnew( _VolumeSW_BVH ); + _VolumeSW_BVH *bvh = memnew(_VolumeSW_BVH); - if (p_size==1) { + if (p_size == 1) { //leaf - bvh->aabb=p_elements[0].aabb; - bvh->left=NULL; - bvh->right=NULL; - bvh->face_index=p_elements->face_index; + bvh->aabb = p_elements[0].aabb; + bvh->left = NULL; + bvh->right = NULL; + bvh->face_index = p_elements->face_index; count++; return bvh; } else { - bvh->face_index=-1; + bvh->face_index = -1; } Rect3 aabb; - for(int i=0;i<p_size;i++) { + for (int i = 0; i < p_size; i++) { - if (i==0) - aabb=p_elements[i].aabb; + if (i == 0) + aabb = p_elements[i].aabb; else aabb.merge_with(p_elements[i].aabb); } - bvh->aabb=aabb; - switch(aabb.get_longest_axis_index()) { + bvh->aabb = aabb; + switch (aabb.get_longest_axis_index()) { case 0: { - SortArray<_VolumeSW_BVH_Element,_VolumeSW_BVH_CompareX> sort_x; - sort_x.sort(p_elements,p_size); + SortArray<_VolumeSW_BVH_Element, _VolumeSW_BVH_CompareX> sort_x; + sort_x.sort(p_elements, p_size); } break; case 1: { - SortArray<_VolumeSW_BVH_Element,_VolumeSW_BVH_CompareY> sort_y; - sort_y.sort(p_elements,p_size); + SortArray<_VolumeSW_BVH_Element, _VolumeSW_BVH_CompareY> sort_y; + sort_y.sort(p_elements, p_size); } break; case 2: { - SortArray<_VolumeSW_BVH_Element,_VolumeSW_BVH_CompareZ> sort_z; - sort_z.sort(p_elements,p_size); + SortArray<_VolumeSW_BVH_Element, _VolumeSW_BVH_CompareZ> sort_z; + sort_z.sort(p_elements, p_size); } break; } - int split=p_size/2; - bvh->left=_volume_sw_build_bvh(p_elements,split,count); - bvh->right=_volume_sw_build_bvh(&p_elements[split],p_size-split,count); + int split = p_size / 2; + bvh->left = _volume_sw_build_bvh(p_elements, split, count); + bvh->right = _volume_sw_build_bvh(&p_elements[split], p_size - split, count); //printf("branch at %p - %i: %i\n",bvh,count,bvh->face_index); count++; return bvh; } +void ConcavePolygonShapeSW::_fill_bvh(_VolumeSW_BVH *p_bvh_tree, BVH *p_bvh_array, int &p_idx) { -void ConcavePolygonShapeSW::_fill_bvh(_VolumeSW_BVH* p_bvh_tree,BVH* p_bvh_array,int& p_idx) { - - int idx=p_idx; + int idx = p_idx; - - p_bvh_array[idx].aabb=p_bvh_tree->aabb; - p_bvh_array[idx].face_index=p_bvh_tree->face_index; + p_bvh_array[idx].aabb = p_bvh_tree->aabb; + p_bvh_array[idx].face_index = p_bvh_tree->face_index; //printf("%p - %i: %i(%p) -- %p:%p\n",%p_bvh_array[idx],p_idx,p_bvh_array[i]->face_index,&p_bvh_tree->face_index,p_bvh_tree->left,p_bvh_tree->right); - if (p_bvh_tree->left) { - p_bvh_array[idx].left=++p_idx; - _fill_bvh(p_bvh_tree->left,p_bvh_array,p_idx); + p_bvh_array[idx].left = ++p_idx; + _fill_bvh(p_bvh_tree->left, p_bvh_array, p_idx); } else { - p_bvh_array[p_idx].left=-1; + p_bvh_array[p_idx].left = -1; } if (p_bvh_tree->right) { - p_bvh_array[idx].right=++p_idx; - _fill_bvh(p_bvh_tree->right,p_bvh_array,p_idx); + p_bvh_array[idx].right = ++p_idx; + _fill_bvh(p_bvh_tree->right, p_bvh_array, p_idx); } else { - p_bvh_array[p_idx].right=-1; + p_bvh_array[p_idx].right = -1; } memdelete(p_bvh_tree); - } void ConcavePolygonShapeSW::_setup(PoolVector<Vector3> p_faces) { - int src_face_count=p_faces.size(); - if (src_face_count==0) { + int src_face_count = p_faces.size(); + if (src_face_count == 0) { configure(Rect3()); return; } - ERR_FAIL_COND(src_face_count%3); - src_face_count/=3; + ERR_FAIL_COND(src_face_count % 3); + src_face_count /= 3; PoolVector<Vector3>::Read r = p_faces.read(); - const Vector3 * facesr= r.ptr(); + const Vector3 *facesr = r.ptr(); #if 0 Map<Vector3,int> point_map; @@ -1476,67 +1366,62 @@ void ConcavePolygonShapeSW::_setup(PoolVector<Vector3> p_faces) { #else PoolVector<_VolumeSW_BVH_Element> bvh_array; - bvh_array.resize( src_face_count ); + bvh_array.resize(src_face_count); PoolVector<_VolumeSW_BVH_Element>::Write bvhw = bvh_array.write(); - _VolumeSW_BVH_Element *bvh_arrayw=bvhw.ptr(); + _VolumeSW_BVH_Element *bvh_arrayw = bvhw.ptr(); faces.resize(src_face_count); PoolVector<Face>::Write w = faces.write(); - Face *facesw=w.ptr(); + Face *facesw = w.ptr(); - vertices.resize( src_face_count*3 ); + vertices.resize(src_face_count * 3); PoolVector<Vector3>::Write vw = vertices.write(); - Vector3 *verticesw=vw.ptr(); + Vector3 *verticesw = vw.ptr(); Rect3 _aabb; + for (int i = 0; i < src_face_count; i++) { - for(int i=0;i<src_face_count;i++) { - - Face3 face( facesr[i*3+0], facesr[i*3+1], facesr[i*3+2] ); + Face3 face(facesr[i * 3 + 0], facesr[i * 3 + 1], facesr[i * 3 + 2]); - bvh_arrayw[i].aabb=face.get_aabb(); + bvh_arrayw[i].aabb = face.get_aabb(); bvh_arrayw[i].center = bvh_arrayw[i].aabb.pos + bvh_arrayw[i].aabb.size * 0.5; - bvh_arrayw[i].face_index=i; - facesw[i].indices[0]=i*3+0; - facesw[i].indices[1]=i*3+1; - facesw[i].indices[2]=i*3+2; - facesw[i].normal=face.get_plane().normal; - verticesw[i*3+0]=face.vertex[0]; - verticesw[i*3+1]=face.vertex[1]; - verticesw[i*3+2]=face.vertex[2]; - if (i==0) - _aabb=bvh_arrayw[i].aabb; + bvh_arrayw[i].face_index = i; + facesw[i].indices[0] = i * 3 + 0; + facesw[i].indices[1] = i * 3 + 1; + facesw[i].indices[2] = i * 3 + 2; + facesw[i].normal = face.get_plane().normal; + verticesw[i * 3 + 0] = face.vertex[0]; + verticesw[i * 3 + 1] = face.vertex[1]; + verticesw[i * 3 + 2] = face.vertex[2]; + if (i == 0) + _aabb = bvh_arrayw[i].aabb; else _aabb.merge_with(bvh_arrayw[i].aabb); - } - w=PoolVector<Face>::Write(); - vw=PoolVector<Vector3>::Write(); + w = PoolVector<Face>::Write(); + vw = PoolVector<Vector3>::Write(); - int count=0; - _VolumeSW_BVH *bvh_tree=_volume_sw_build_bvh(bvh_arrayw,src_face_count,count); + int count = 0; + _VolumeSW_BVH *bvh_tree = _volume_sw_build_bvh(bvh_arrayw, src_face_count, count); - bvh.resize( count+1 ); + bvh.resize(count + 1); PoolVector<BVH>::Write bvhw2 = bvh.write(); - BVH*bvh_arrayw2=bvhw2.ptr(); + BVH *bvh_arrayw2 = bvhw2.ptr(); - int idx=0; - _fill_bvh(bvh_tree,bvh_arrayw2,idx); + int idx = 0; + _fill_bvh(bvh_tree, bvh_arrayw2, idx); configure(_aabb); // this type of shape has no margin - #endif } - -void ConcavePolygonShapeSW::set_data(const Variant& p_data) { - +void ConcavePolygonShapeSW::set_data(const Variant &p_data) { _setup(p_data); } @@ -1547,12 +1432,8 @@ Variant ConcavePolygonShapeSW::get_data() const { } ConcavePolygonShapeSW::ConcavePolygonShapeSW() { - - } - - /* HEIGHT MAP SHAPE */ PoolVector<real_t> HeightMapShapeSW::get_heights() const { @@ -1572,114 +1453,94 @@ real_t HeightMapShapeSW::get_cell_size() const { return cell_size; } - -void HeightMapShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const { +void HeightMapShapeSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { //not very useful, but not very used either - p_transform.xform(get_aabb()).project_range_in_plane( Plane(p_normal,0),r_min,r_max ); - + p_transform.xform(get_aabb()).project_range_in_plane(Plane(p_normal, 0), r_min, r_max); } -Vector3 HeightMapShapeSW::get_support(const Vector3& p_normal) const { - +Vector3 HeightMapShapeSW::get_support(const Vector3 &p_normal) const { //not very useful, but not very used either return get_aabb().get_support(p_normal); - } -bool HeightMapShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_point, Vector3 &r_normal) const { - +bool HeightMapShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal) const { return false; } - -void HeightMapShapeSW::cull(const Rect3& p_local_aabb,Callback p_callback,void* p_userdata) const { - - - +void HeightMapShapeSW::cull(const Rect3 &p_local_aabb, Callback p_callback, void *p_userdata) const { } - Vector3 HeightMapShapeSW::get_moment_of_inertia(real_t p_mass) const { - // use crappy AABB approximation - Vector3 extents=get_aabb().size*0.5; + Vector3 extents = get_aabb().size * 0.5; return Vector3( - (p_mass/3.0) * (extents.y*extents.y + extents.z*extents.z), - (p_mass/3.0) * (extents.x*extents.x + extents.z*extents.z), - (p_mass/3.0) * (extents.y*extents.y + extents.y*extents.y) - ); + (p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z), + (p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z), + (p_mass / 3.0) * (extents.y * extents.y + extents.y * extents.y)); } +void HeightMapShapeSW::_setup(PoolVector<real_t> p_heights, int p_width, int p_depth, real_t p_cell_size) { -void HeightMapShapeSW::_setup(PoolVector<real_t> p_heights,int p_width,int p_depth,real_t p_cell_size) { + heights = p_heights; + width = p_width; + depth = p_depth; + cell_size = p_cell_size; - heights=p_heights; - width=p_width; - depth=p_depth; - cell_size=p_cell_size; - - PoolVector<real_t>::Read r = heights. read(); + PoolVector<real_t>::Read r = heights.read(); Rect3 aabb; - for(int i=0;i<depth;i++) { + for (int i = 0; i < depth; i++) { - for(int j=0;j<width;j++) { + for (int j = 0; j < width; j++) { - real_t h = r[i*width+j]; + real_t h = r[i * width + j]; - Vector3 pos( j*cell_size, h, i*cell_size ); - if (i==0 || j==0) - aabb.pos=pos; + Vector3 pos(j * cell_size, h, i * cell_size); + if (i == 0 || j == 0) + aabb.pos = pos; else aabb.expand_to(pos); - } } - configure(aabb); } -void HeightMapShapeSW::set_data(const Variant& p_data) { +void HeightMapShapeSW::set_data(const Variant &p_data) { - ERR_FAIL_COND( p_data.get_type()!=Variant::DICTIONARY ); - Dictionary d=p_data; - ERR_FAIL_COND( !d.has("width") ); - ERR_FAIL_COND( !d.has("depth") ); - ERR_FAIL_COND( !d.has("cell_size") ); - ERR_FAIL_COND( !d.has("heights") ); - - int width=d["width"]; - int depth=d["depth"]; - real_t cell_size=d["cell_size"]; - PoolVector<real_t> heights=d["heights"]; + ERR_FAIL_COND(p_data.get_type() != Variant::DICTIONARY); + Dictionary d = p_data; + ERR_FAIL_COND(!d.has("width")); + ERR_FAIL_COND(!d.has("depth")); + ERR_FAIL_COND(!d.has("cell_size")); + ERR_FAIL_COND(!d.has("heights")); - ERR_FAIL_COND( width<= 0); - ERR_FAIL_COND( depth<= 0); - ERR_FAIL_COND( cell_size<= CMP_EPSILON); - ERR_FAIL_COND( heights.size() != (width*depth) ); - _setup(heights, width, depth, cell_size ); + int width = d["width"]; + int depth = d["depth"]; + real_t cell_size = d["cell_size"]; + PoolVector<real_t> heights = d["heights"]; + ERR_FAIL_COND(width <= 0); + ERR_FAIL_COND(depth <= 0); + ERR_FAIL_COND(cell_size <= CMP_EPSILON); + ERR_FAIL_COND(heights.size() != (width * depth)); + _setup(heights, width, depth, cell_size); } Variant HeightMapShapeSW::get_data() const { ERR_FAIL_V(Variant()); - } HeightMapShapeSW::HeightMapShapeSW() { - width=0; - depth=0; - cell_size=0; + width = 0; + depth = 0; + cell_size = 0; } - - - diff --git a/servers/physics/shape_sw.h b/servers/physics/shape_sw.h index 55daa5856..442cbc39e 100644 --- a/servers/physics/shape_sw.h +++ b/servers/physics/shape_sw.h @@ -29,9 +29,9 @@ #ifndef SHAPE_SW_H #define SHAPE_SW_H -#include "servers/physics_server.h" #include "bsp_tree.h" #include "geometry.h" +#include "servers/physics_server.h" /* SHAPE_LINE, ///< plane:"plane" @@ -46,16 +46,14 @@ SHAPE_CUSTOM, ///< Server-Implementation based custom shape, calling shape_creat class ShapeSW; -class ShapeOwnerSW : public RID_Data { +class ShapeOwnerSW : public RID_Data { public: - - virtual void _shape_changed()=0; - virtual void remove_shape(ShapeSW *p_shape)=0; + virtual void _shape_changed() = 0; + virtual void remove_shape(ShapeSW *p_shape) = 0; virtual ~ShapeOwnerSW() {} }; - class ShapeSW : public RID_Data { RID self; @@ -63,60 +61,58 @@ class ShapeSW : public RID_Data { bool configured; real_t custom_bias; - Map<ShapeOwnerSW*,int> owners; + Map<ShapeOwnerSW *, int> owners; + protected: + void configure(const Rect3 &p_aabb); - void configure(const Rect3& p_aabb); public: - enum { - MAX_SUPPORTS=8 + MAX_SUPPORTS = 8 }; - virtual real_t get_area() const { return aabb.get_area();} + virtual real_t get_area() const { return aabb.get_area(); } - _FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; } - _FORCE_INLINE_ RID get_self() const {return self; } + _FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; } + _FORCE_INLINE_ RID get_self() const { return self; } - virtual PhysicsServer::ShapeType get_type() const=0; + virtual PhysicsServer::ShapeType get_type() const = 0; _FORCE_INLINE_ Rect3 get_aabb() const { return aabb; } _FORCE_INLINE_ bool is_configured() const { return configured; } virtual bool is_concave() const { return false; } - virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const=0; - virtual Vector3 get_support(const Vector3& p_normal) const; - virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const=0; + virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const = 0; + virtual Vector3 get_support(const Vector3 &p_normal) const; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const = 0; - virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_point, Vector3 &r_normal) const=0; - virtual Vector3 get_moment_of_inertia(real_t p_mass) const=0; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal) const = 0; + virtual Vector3 get_moment_of_inertia(real_t p_mass) const = 0; - virtual void set_data(const Variant& p_data)=0; - virtual Variant get_data() const=0; + 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(ShapeOwnerSW *p_owner); void remove_owner(ShapeOwnerSW *p_owner); bool is_owner(ShapeOwnerSW *p_owner) const; - const Map<ShapeOwnerSW*,int>& get_owners() const; + const Map<ShapeOwnerSW *, int> &get_owners() const; ShapeSW(); virtual ~ShapeSW(); }; - class ConcaveShapeSW : public ShapeSW { public: - virtual bool is_concave() const { return true; } - typedef void (*Callback)(void* p_userdata,ShapeSW *p_convex); - virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const { r_amount=0; } + typedef void (*Callback)(void *p_userdata, ShapeSW *p_convex); + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { r_amount = 0; } - virtual void cull(const Rect3& p_local_aabb,Callback p_callback,void* p_userdata) const=0; + virtual void cull(const Rect3 &p_local_aabb, Callback p_callback, void *p_userdata) const = 0; ConcaveShapeSW() {} }; @@ -125,22 +121,22 @@ class PlaneShapeSW : public ShapeSW { Plane plane; - void _setup(const Plane& p_plane); -public: + void _setup(const Plane &p_plane); +public: Plane get_plane() const; virtual real_t get_area() const { return Math_INF; } virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_PLANE; } - virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const; - virtual Vector3 get_support(const Vector3& p_normal) const; - virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const { r_amount=0; } + virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; + virtual Vector3 get_support(const Vector3 &p_normal) const; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { r_amount = 0; } - virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; virtual Vector3 get_moment_of_inertia(real_t p_mass) const; - virtual void set_data(const Variant& p_data); + virtual void set_data(const Variant &p_data); virtual Variant get_data() const; PlaneShapeSW(); @@ -151,21 +147,21 @@ class RayShapeSW : public ShapeSW { real_t length; void _setup(real_t p_length); -public: +public: real_t get_length() const; virtual real_t get_area() const { return 0.0; } virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_RAY; } - virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const; - virtual Vector3 get_support(const Vector3& p_normal) const; - virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const; + virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; + virtual Vector3 get_support(const Vector3 &p_normal) const; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const; - virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; virtual Vector3 get_moment_of_inertia(real_t p_mass) const; - virtual void set_data(const Variant& p_data); + virtual void set_data(const Variant &p_data); virtual Variant get_data() const; RayShapeSW(); @@ -176,22 +172,22 @@ class SphereShapeSW : public ShapeSW { real_t radius; void _setup(real_t p_radius); -public: +public: real_t get_radius() const; - virtual real_t get_area() const { return 4.0/3.0 * Math_PI * radius * radius * radius; } + virtual real_t get_area() const { return 4.0 / 3.0 * Math_PI * radius * radius * radius; } virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_SPHERE; } - virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const; - virtual Vector3 get_support(const Vector3& p_normal) const; - virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const; - virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const; + virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; + virtual Vector3 get_support(const Vector3 &p_normal) const; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; virtual Vector3 get_moment_of_inertia(real_t p_mass) const; - virtual void set_data(const Variant& p_data); + virtual void set_data(const Variant &p_data); virtual Variant get_data() const; SphereShapeSW(); @@ -200,22 +196,22 @@ public: class BoxShapeSW : public ShapeSW { Vector3 half_extents; - void _setup(const Vector3& p_half_extents); -public: + void _setup(const Vector3 &p_half_extents); +public: _FORCE_INLINE_ Vector3 get_half_extents() const { return half_extents; } - virtual real_t get_area() const { return 8 * half_extents.x * half_extents.y * half_extents.z; } + virtual real_t get_area() const { return 8 * half_extents.x * half_extents.y * half_extents.z; } virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_BOX; } - virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const; - virtual Vector3 get_support(const Vector3& p_normal) const; - virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const; - virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const; + virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; + virtual Vector3 get_support(const Vector3 &p_normal) const; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; virtual Vector3 get_moment_of_inertia(real_t p_mass) const; - virtual void set_data(const Variant& p_data); + virtual void set_data(const Variant &p_data); virtual Variant get_data() const; BoxShapeSW(); @@ -226,25 +222,24 @@ class CapsuleShapeSW : public ShapeSW { real_t height; real_t radius; + void _setup(real_t p_height, real_t p_radius); - void _setup(real_t p_height,real_t p_radius); public: - _FORCE_INLINE_ real_t get_height() const { return height; } _FORCE_INLINE_ real_t get_radius() const { return radius; } - virtual real_t get_area() { return 4.0/3.0 * Math_PI * radius * radius * radius + height * Math_PI * radius * radius; } + virtual real_t get_area() { return 4.0 / 3.0 * Math_PI * radius * radius * radius + height * Math_PI * radius * radius; } virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CAPSULE; } - virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const; - virtual Vector3 get_support(const Vector3& p_normal) const; - virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const; - virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const; + virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; + virtual Vector3 get_support(const Vector3 &p_normal) const; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; virtual Vector3 get_moment_of_inertia(real_t p_mass) const; - virtual void set_data(const Variant& p_data); + virtual void set_data(const Variant &p_data); virtual Variant get_data() const; CapsuleShapeSW(); @@ -254,28 +249,26 @@ struct ConvexPolygonShapeSW : public ShapeSW { Geometry::MeshData mesh; - void _setup(const Vector<Vector3>& p_vertices); -public: + void _setup(const Vector<Vector3> &p_vertices); - const Geometry::MeshData& get_mesh() const { return mesh; } +public: + const Geometry::MeshData &get_mesh() const { return mesh; } virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CONVEX_POLYGON; } - virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const; - virtual Vector3 get_support(const Vector3& p_normal) const; - virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const; - virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const; + virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; + virtual Vector3 get_support(const Vector3 &p_normal) const; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; virtual Vector3 get_moment_of_inertia(real_t p_mass) const; - virtual void set_data(const Variant& p_data); + virtual void set_data(const Variant &p_data); virtual Variant get_data() const; ConvexPolygonShapeSW(); - }; - struct _VolumeSW_BVH; struct FaceShapeSW; @@ -326,39 +319,35 @@ struct ConcavePolygonShapeSW : public ConcaveShapeSW { Vector3 normal; real_t min_d; int collisions; - }; - void _cull_segment(int p_idx,_SegmentCullParams *p_params) const; - void _cull(int p_idx,_CullParams *p_params) const; - - void _fill_bvh(_VolumeSW_BVH* p_bvh_tree,BVH* p_bvh_array,int& p_idx); + void _cull_segment(int p_idx, _SegmentCullParams *p_params) const; + void _cull(int p_idx, _CullParams *p_params) const; + void _fill_bvh(_VolumeSW_BVH *p_bvh_tree, BVH *p_bvh_array, int &p_idx); void _setup(PoolVector<Vector3> p_faces); -public: +public: PoolVector<Vector3> get_faces() const; virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CONCAVE_POLYGON; } - virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const; - virtual Vector3 get_support(const Vector3& p_normal) const; + virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; + virtual Vector3 get_support(const Vector3 &p_normal) const; - virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; - virtual void cull(const Rect3& p_local_aabb,Callback p_callback,void* p_userdata) const; + virtual void cull(const Rect3 &p_local_aabb, Callback p_callback, void *p_userdata) const; virtual Vector3 get_moment_of_inertia(real_t p_mass) const; - virtual void set_data(const Variant& p_data); + virtual void set_data(const Variant &p_data); virtual Variant get_data() const; ConcavePolygonShapeSW(); - }; - struct HeightMapShapeSW : public ConcaveShapeSW { PoolVector<real_t> heights; @@ -369,9 +358,9 @@ struct HeightMapShapeSW : public ConcaveShapeSW { //void _cull_segment(int p_idx,_SegmentCullParams *p_params) const; //void _cull(int p_idx,_CullParams *p_params) const; - void _setup(PoolVector<real_t> p_heights,int p_width,int p_depth,real_t p_cell_size); -public: + void _setup(PoolVector<real_t> p_heights, int p_width, int p_depth, real_t p_cell_size); +public: PoolVector<real_t> get_heights() const; int get_width() const; int get_depth() const; @@ -379,19 +368,18 @@ public: virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_HEIGHTMAP; } - virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const; - virtual Vector3 get_support(const Vector3& p_normal) const; - virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const; + virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; + virtual Vector3 get_support(const Vector3 &p_normal) const; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; - virtual void cull(const Rect3& p_local_aabb,Callback p_callback,void* p_userdata) const; + virtual void cull(const Rect3 &p_local_aabb, Callback p_callback, void *p_userdata) const; virtual Vector3 get_moment_of_inertia(real_t p_mass) const; - virtual void set_data(const Variant& p_data); + virtual void set_data(const Variant &p_data); virtual Variant get_data() const; HeightMapShapeSW(); - }; //used internally @@ -402,22 +390,21 @@ struct FaceShapeSW : public ShapeSW { virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CONCAVE_POLYGON; } - const Vector3& get_vertex(int p_idx) const { return vertex[p_idx]; } + const Vector3 &get_vertex(int p_idx) const { return vertex[p_idx]; } - void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const; - Vector3 get_support(const Vector3& p_normal) const; - virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const; - bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const; + void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; + Vector3 get_support(const Vector3 &p_normal) const; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const; + bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; Vector3 get_moment_of_inertia(real_t p_mass) const; - virtual void set_data(const Variant& p_data) {} + virtual void set_data(const Variant &p_data) {} virtual Variant get_data() const { return Variant(); } FaceShapeSW(); }; - struct MotionShapeSW : public ShapeSW { ShapeSW *shape; @@ -425,56 +412,48 @@ struct MotionShapeSW : public ShapeSW { virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CONVEX_POLYGON; } - - void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const { + void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { Vector3 cast = p_transform.basis.xform(motion); - real_t mina,maxa; - real_t minb,maxb; + real_t mina, maxa; + real_t minb, maxb; Transform ofsb = p_transform; - ofsb.origin+=cast; - shape->project_range(p_normal,p_transform,mina,maxa); - shape->project_range(p_normal,ofsb,minb,maxb); - r_min=MIN(mina,minb); - r_max=MAX(maxa,maxb); + ofsb.origin += cast; + shape->project_range(p_normal, p_transform, mina, maxa); + shape->project_range(p_normal, ofsb, minb, maxb); + r_min = MIN(mina, minb); + r_max = MAX(maxa, maxb); } - Vector3 get_support(const Vector3& p_normal) const { + Vector3 get_support(const Vector3 &p_normal) const { Vector3 support = shape->get_support(p_normal); - if (p_normal.dot(motion)>0) { - support+=motion; + if (p_normal.dot(motion) > 0) { + support += motion; } return support; } - virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const { r_amount=0; } - bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const { return false; } + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { r_amount = 0; } + bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { return false; } Vector3 get_moment_of_inertia(real_t p_mass) const { return Vector3(); } - virtual void set_data(const Variant& p_data) {} + virtual void set_data(const Variant &p_data) {} virtual Variant get_data() const { return Variant(); } - MotionShapeSW() { configure(Rect3()); } + MotionShapeSW() { configure(Rect3()); } }; - - - struct _ShapeTestConvexBSPSW { const BSP_Tree *bsp; const ShapeSW *shape; Transform transform; - _FORCE_INLINE_ void project_range(const Vector3& p_normal, real_t& r_min, real_t& r_max) const { + _FORCE_INLINE_ void project_range(const Vector3 &p_normal, real_t &r_min, real_t &r_max) const { - shape->project_range(p_normal,transform,r_min,r_max); + shape->project_range(p_normal, transform, r_min, r_max); } - }; - - - #endif // SHAPESW_H diff --git a/servers/physics/space_sw.cpp b/servers/physics/space_sw.cpp index 0bc11041d..603c6fa3c 100644 --- a/servers/physics/space_sw.cpp +++ b/servers/physics/space_sw.cpp @@ -26,66 +26,58 @@ /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ -#include "global_config.h" #include "space_sw.h" #include "collision_solver_sw.h" +#include "global_config.h" #include "physics_server_sw.h" - _FORCE_INLINE_ static bool _match_object_type_query(CollisionObjectSW *p_object, uint32_t p_layer_mask, uint32_t p_type_mask) { - if (p_object->get_type()==CollisionObjectSW::TYPE_AREA) - return p_type_mask&PhysicsDirectSpaceState::TYPE_MASK_AREA; + if (p_object->get_type() == CollisionObjectSW::TYPE_AREA) + return p_type_mask & PhysicsDirectSpaceState::TYPE_MASK_AREA; - if ((p_object->get_layer_mask()&p_layer_mask)==0) + if ((p_object->get_layer_mask() & p_layer_mask) == 0) return false; - BodySW *body = static_cast<BodySW*>(p_object); - - return (1<<body->get_mode())&p_type_mask; + BodySW *body = static_cast<BodySW *>(p_object); + return (1 << body->get_mode()) & p_type_mask; } +bool PhysicsDirectSpaceStateSW::intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const Set<RID> &p_exclude, uint32_t p_layer_mask, uint32_t p_object_type_mask, bool p_pick_ray) { -bool PhysicsDirectSpaceStateSW::intersect_ray(const Vector3& p_from, const Vector3& p_to, RayResult &r_result, const Set<RID>& p_exclude, uint32_t p_layer_mask, uint32_t p_object_type_mask, bool p_pick_ray) { - + ERR_FAIL_COND_V(space->locked, false); - ERR_FAIL_COND_V(space->locked,false); - - Vector3 begin,end; + Vector3 begin, end; Vector3 normal; - begin=p_from; - end=p_to; - normal=(end-begin).normalized(); - - - int amount = space->broadphase->cull_segment(begin,end,space->intersection_query_results,SpaceSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results); + begin = p_from; + end = p_to; + normal = (end - begin).normalized(); + int amount = space->broadphase->cull_segment(begin, end, space->intersection_query_results, SpaceSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); //todo, create another array tha references results, compute AABBs and check closest point to ray origin, sort, and stop evaluating results when beyond first collision - bool collided=false; - Vector3 res_point,res_normal; + bool collided = false; + Vector3 res_point, res_normal; int res_shape; const CollisionObjectSW *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_pick_ray && !(static_cast<CollisionObjectSW*>(space->intersection_query_results[i])->is_ray_pickable())) + if (p_pick_ray && !(static_cast<CollisionObjectSW *>(space->intersection_query_results[i])->is_ray_pickable())) 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 CollisionObjectSW *col_obj=space->intersection_query_results[i]; + const CollisionObjectSW *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]; Transform inv_xform = col_obj->get_shape_inv_transform(shape_idx) * col_obj->get_inv_transform(); Vector3 local_from = inv_xform.xform(begin); @@ -93,293 +85,268 @@ bool PhysicsDirectSpaceStateSW::intersect_ray(const Vector3& p_from, const Vecto const ShapeSW *shape = col_obj->get_shape(shape_idx); - Vector3 shape_point,shape_normal; - - - if (shape->intersect_segment(local_from,local_to,shape_point,shape_normal)) { - + Vector3 shape_point, shape_normal; + if (shape->intersect_segment(local_from, local_to, shape_point, shape_normal)) { Transform xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); - shape_point=xform.xform(shape_point); + 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.collider_id = res_obj->get_instance_id(); + if (r_result.collider_id != 0) + r_result.collider = ObjectDB::get_instance(r_result.collider_id); else - r_result.collider=NULL; - r_result.normal=res_normal; - r_result.position=res_point; - r_result.rid=res_obj->get_self(); - r_result.shape=res_shape; + r_result.collider = NULL; + r_result.normal = res_normal; + r_result.position = res_point; + r_result.rid = res_obj->get_self(); + r_result.shape = res_shape; return true; - } +int PhysicsDirectSpaceStateSW::intersect_shape(const RID &p_shape, const Transform &p_xform, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_layer_mask, uint32_t p_object_type_mask) { -int PhysicsDirectSpaceStateSW::intersect_shape(const RID& p_shape, const Transform& p_xform,real_t p_margin,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude,uint32_t p_layer_mask,uint32_t p_object_type_mask) { - - if (p_result_max<=0) + if (p_result_max <= 0) return 0; - ShapeSW *shape = static_cast<PhysicsServerSW*>(PhysicsServer::get_singleton())->shape_owner.get(p_shape); - ERR_FAIL_COND_V(!shape,0); + ShapeSW *shape = static_cast<PhysicsServerSW *>(PhysicsServer::get_singleton())->shape_owner.get(p_shape); + ERR_FAIL_COND_V(!shape, 0); Rect3 aabb = p_xform.xform(shape->get_aabb()); - int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,SpaceSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results); + int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, SpaceSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); - int cc=0; + int cc = 0; //Transform ai = p_xform.affine_inverse(); - for(int i=0;i<amount;i++) { + for (int i = 0; i < amount; i++) { - if (cc>=p_result_max) + if (cc >= p_result_max) break; - 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; //area cant be picked by ray (default) - if (p_exclude.has( space->intersection_query_results[i]->get_self())) + if (p_exclude.has(space->intersection_query_results[i]->get_self())) continue; + const CollisionObjectSW *col_obj = space->intersection_query_results[i]; + int shape_idx = space->intersection_query_subindex_results[i]; - const CollisionObjectSW *col_obj=space->intersection_query_results[i]; - int shape_idx=space->intersection_query_subindex_results[i]; - - if (!CollisionSolverSW::solve_static(shape,p_xform,col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), NULL,NULL,NULL,p_margin,0)) + if (!CollisionSolverSW::solve_static(shape, p_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), NULL, NULL, NULL, p_margin, 0)) continue; if (r_results) { - r_results[cc].collider_id=col_obj->get_instance_id(); - if (r_results[cc].collider_id!=0) - r_results[cc].collider=ObjectDB::get_instance(r_results[cc].collider_id); + 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); else - r_results[cc].collider=NULL; - r_results[cc].rid=col_obj->get_self(); - r_results[cc].shape=shape_idx; + r_results[cc].collider = NULL; + r_results[cc].rid = col_obj->get_self(); + r_results[cc].shape = shape_idx; } cc++; - } return cc; - } +bool PhysicsDirectSpaceStateSW::cast_motion(const RID &p_shape, const Transform &p_xform, const Vector3 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set<RID> &p_exclude, uint32_t p_layer_mask, uint32_t p_object_type_mask, ShapeRestInfo *r_info) { -bool PhysicsDirectSpaceStateSW::cast_motion(const RID& p_shape, const Transform& p_xform,const Vector3& p_motion,real_t p_margin,real_t &p_closest_safe,real_t &p_closest_unsafe, const Set<RID>& p_exclude,uint32_t p_layer_mask,uint32_t p_object_type_mask,ShapeRestInfo *r_info) { - - - - ShapeSW *shape = static_cast<PhysicsServerSW*>(PhysicsServer::get_singleton())->shape_owner.get(p_shape); - ERR_FAIL_COND_V(!shape,false); + ShapeSW *shape = static_cast<PhysicsServerSW *>(PhysicsServer::get_singleton())->shape_owner.get(p_shape); + ERR_FAIL_COND_V(!shape, false); Rect3 aabb = p_xform.xform(shape->get_aabb()); - aabb=aabb.merge(Rect3(aabb.pos+p_motion,aabb.size)); //motion - aabb=aabb.grow(p_margin); + aabb = aabb.merge(Rect3(aabb.pos + p_motion, aabb.size)); //motion + aabb = aabb.grow(p_margin); /* if (p_motion!=Vector3()) print_line(p_motion); */ - int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,SpaceSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results); + int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, SpaceSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); - real_t best_safe=1; - real_t best_unsafe=1; + real_t best_safe = 1; + real_t best_unsafe = 1; Transform xform_inv = p_xform.affine_inverse(); MotionShapeSW mshape; - mshape.shape=shape; - mshape.motion=xform_inv.basis.xform(p_motion); + mshape.shape = shape; + mshape.motion = xform_inv.basis.xform(p_motion); - bool best_first=true; + bool best_first = true; - Vector3 closest_A,closest_B; + Vector3 closest_A, closest_B; - 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; //ignore excluded + const CollisionObjectSW *col_obj = space->intersection_query_results[i]; + int shape_idx = space->intersection_query_subindex_results[i]; - const CollisionObjectSW *col_obj=space->intersection_query_results[i]; - int shape_idx=space->intersection_query_subindex_results[i]; - - Vector3 point_A,point_B; - Vector3 sep_axis=p_motion.normalized(); + Vector3 point_A, point_B; + Vector3 sep_axis = p_motion.normalized(); Transform col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); //test initial overlap, does it collide if going all the way? - if (CollisionSolverSW::solve_distance(&mshape,p_xform,col_obj->get_shape(shape_idx),col_obj_xform,point_A,point_B,aabb,&sep_axis)) { + if (CollisionSolverSW::solve_distance(&mshape, p_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, aabb, &sep_axis)) { //print_line("failed motion cast (no collision)"); continue; } - - //test initial overlap +//test initial overlap #if 0 if (CollisionSolverSW::solve_static(shape,p_xform,col_obj->get_shape(shape_idx),col_obj_xform,NULL,NULL,&sep_axis)) { print_line("failed initial cast (collision at begining)"); return false; } #else - sep_axis=p_motion.normalized(); + sep_axis = p_motion.normalized(); - if (!CollisionSolverSW::solve_distance(shape,p_xform,col_obj->get_shape(shape_idx),col_obj_xform,point_A,point_B,aabb,&sep_axis)) { + if (!CollisionSolverSW::solve_distance(shape, p_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, aabb, &sep_axis)) { //print_line("failed motion cast (no collision)"); return false; } #endif - //just do kinematic solving - real_t low=0; - real_t hi=1; - Vector3 mnormal=p_motion.normalized(); + real_t low = 0; + real_t hi = 1; + Vector3 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; - Vector3 sep=mnormal; //important optimization for this to work fast enough + Vector3 sep = mnormal; //important optimization for this to work fast enough - mshape.motion=xform_inv.basis.xform(p_motion*ofs); + mshape.motion = xform_inv.basis.xform(p_motion * ofs); - Vector3 lA,lB; + Vector3 lA, lB; - bool collided = !CollisionSolverSW::solve_distance(&mshape,p_xform,col_obj->get_shape(shape_idx),col_obj_xform,lA,lB,aabb,&sep); + bool collided = !CollisionSolverSW::solve_distance(&mshape, p_xform, col_obj->get_shape(shape_idx), col_obj_xform, lA, lB, aabb, &sep); if (collided) { //print_line(itos(i)+": "+rtos(ofs)); - hi=ofs; + hi = ofs; } else { - point_A=lA; - point_B=lB; - low=ofs; + point_A = lA; + point_B = lB; + low = ofs; } } - if (low<best_safe) { - best_first=true; //force reset - best_safe=low; - best_unsafe=hi; + if (low < best_safe) { + best_first = true; //force reset + best_safe = low; + best_unsafe = hi; } - if (r_info && (best_first || (point_A.distance_squared_to(point_B) < closest_A.distance_squared_to(closest_B) && low<=best_safe))) { - closest_A=point_A; - closest_B=point_B; - r_info->collider_id=col_obj->get_instance_id(); - r_info->rid=col_obj->get_self(); - r_info->shape=shape_idx; - r_info->point=closest_B; - r_info->normal=(closest_A-closest_B).normalized(); - best_first=false; - if (col_obj->get_type()==CollisionObjectSW::TYPE_BODY) { - const BodySW *body=static_cast<const BodySW*>(col_obj); - r_info->linear_velocity= body->get_linear_velocity() + (body->get_angular_velocity()).cross(body->get_transform().origin - closest_B); + if (r_info && (best_first || (point_A.distance_squared_to(point_B) < closest_A.distance_squared_to(closest_B) && low <= best_safe))) { + closest_A = point_A; + closest_B = point_B; + r_info->collider_id = col_obj->get_instance_id(); + r_info->rid = col_obj->get_self(); + r_info->shape = shape_idx; + r_info->point = closest_B; + r_info->normal = (closest_A - closest_B).normalized(); + best_first = false; + if (col_obj->get_type() == CollisionObjectSW::TYPE_BODY) { + const BodySW *body = static_cast<const BodySW *>(col_obj); + r_info->linear_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(body->get_transform().origin - closest_B); } - } - - } - p_closest_safe=best_safe; - p_closest_unsafe=best_unsafe; + p_closest_safe = best_safe; + p_closest_unsafe = best_unsafe; return true; } -bool PhysicsDirectSpaceStateSW::collide_shape(RID p_shape, const Transform& p_shape_xform,real_t p_margin,Vector3 *r_results,int p_result_max,int &r_result_count, const Set<RID>& p_exclude,uint32_t p_layer_mask,uint32_t p_object_type_mask){ +bool PhysicsDirectSpaceStateSW::collide_shape(RID p_shape, const Transform &p_shape_xform, real_t p_margin, Vector3 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude, uint32_t p_layer_mask, uint32_t p_object_type_mask) { - if (p_result_max<=0) + if (p_result_max <= 0) return 0; - ShapeSW *shape = static_cast<PhysicsServerSW*>(PhysicsServer::get_singleton())->shape_owner.get(p_shape); - ERR_FAIL_COND_V(!shape,0); + ShapeSW *shape = static_cast<PhysicsServerSW *>(PhysicsServer::get_singleton())->shape_owner.get(p_shape); + ERR_FAIL_COND_V(!shape, 0); Rect3 aabb = p_shape_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,SpaceSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results); + int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, SpaceSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); - bool collided=false; - r_result_count=0; + bool collided = false; + r_result_count = 0; PhysicsServerSW::CollCbkData cbk; - cbk.max=p_result_max; - cbk.amount=0; - cbk.ptr=r_results; - CollisionSolverSW::CallbackResult cbkres=NULL; + cbk.max = p_result_max; + cbk.amount = 0; + cbk.ptr = r_results; + CollisionSolverSW::CallbackResult cbkres = NULL; - PhysicsServerSW::CollCbkData *cbkptr=NULL; - if (p_result_max>0) { - cbkptr=&cbk; - cbkres=PhysicsServerSW::_shape_col_cbk; + PhysicsServerSW::CollCbkData *cbkptr = NULL; + if (p_result_max > 0) { + cbkptr = &cbk; + cbkres = PhysicsServerSW::_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 CollisionObjectSW *col_obj=space->intersection_query_results[i]; - int shape_idx=space->intersection_query_subindex_results[i]; + const CollisionObjectSW *col_obj = space->intersection_query_results[i]; + int shape_idx = space->intersection_query_subindex_results[i]; - if (p_exclude.has( col_obj->get_self() )) { + if (p_exclude.has(col_obj->get_self())) { continue; } //print_line("AGAINST: "+itos(col_obj->get_self().get_id())+":"+itos(shape_idx)); //print_line("THE ABBB: "+(col_obj->get_transform() * col_obj->get_shape_transform(shape_idx)).xform(col_obj->get_shape(shape_idx)->get_aabb())); - if (CollisionSolverSW::solve_static(shape,p_shape_xform,col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx),cbkres,cbkptr,NULL,p_margin)) { - collided=true; + if (CollisionSolverSW::solve_static(shape, p_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, NULL, p_margin)) { + collided = true; } - } - r_result_count=cbk.amount; + r_result_count = cbk.amount; return collided; - } - struct _RestCallbackData { const CollisionObjectSW *object; @@ -391,173 +358,147 @@ struct _RestCallbackData { real_t best_len; }; -static void _rest_cbk_result(const Vector3& p_point_A,const Vector3& p_point_B,void *p_userdata) { - +static void _rest_cbk_result(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata) { - _RestCallbackData *rd=(_RestCallbackData*)p_userdata; + _RestCallbackData *rd = (_RestCallbackData *)p_userdata; Vector3 contact_rel = p_point_B - p_point_A; real_t len = contact_rel.length(); if (len <= rd->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 PhysicsDirectSpaceStateSW::rest_info(RID p_shape, const Transform& p_shape_xform,real_t p_margin,ShapeRestInfo *r_info, const Set<RID>& p_exclude,uint32_t p_layer_mask,uint32_t p_object_type_mask) { - +bool PhysicsDirectSpaceStateSW::rest_info(RID p_shape, const Transform &p_shape_xform, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude, uint32_t p_layer_mask, uint32_t p_object_type_mask) { - ShapeSW *shape = static_cast<PhysicsServerSW*>(PhysicsServer::get_singleton())->shape_owner.get(p_shape); - ERR_FAIL_COND_V(!shape,0); + ShapeSW *shape = static_cast<PhysicsServerSW *>(PhysicsServer::get_singleton())->shape_owner.get(p_shape); + ERR_FAIL_COND_V(!shape, 0); Rect3 aabb = p_shape_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,SpaceSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results); + int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, SpaceSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); _RestCallbackData rcd; - rcd.best_len=0; - rcd.best_object=NULL; - rcd.best_shape=0; + rcd.best_len = 0; + rcd.best_object = NULL; + rcd.best_shape = 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; - const CollisionObjectSW *col_obj=space->intersection_query_results[i]; - int shape_idx=space->intersection_query_subindex_results[i]; + const CollisionObjectSW *col_obj = space->intersection_query_results[i]; + int shape_idx = space->intersection_query_subindex_results[i]; - if (p_exclude.has( col_obj->get_self() )) + if (p_exclude.has(col_obj->get_self())) continue; - rcd.object=col_obj; - rcd.shape=shape_idx; - bool sc = CollisionSolverSW::solve_static(shape,p_shape_xform,col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx),_rest_cbk_result,&rcd,NULL,p_margin); + rcd.object = col_obj; + rcd.shape = shape_idx; + bool sc = CollisionSolverSW::solve_static(shape, p_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), _rest_cbk_result, &rcd, NULL, p_margin); if (!sc) continue; - - } - if (rcd.best_len==0) + 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(); - if (rcd.best_object->get_type()==CollisionObjectSW::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(); + if (rcd.best_object->get_type() == CollisionObjectSW::TYPE_BODY) { - const BodySW *body = static_cast<const BodySW*>(rcd.best_object); + const BodySW *body = static_cast<const BodySW *>(rcd.best_object); r_info->linear_velocity = body->get_linear_velocity() + - (body->get_angular_velocity()).cross(body->get_transform().origin-rcd.best_contact);// * mPos); - + (body->get_angular_velocity()).cross(body->get_transform().origin - rcd.best_contact); // * mPos); } else { - r_info->linear_velocity=Vector3(); + r_info->linear_velocity = Vector3(); } return true; } - PhysicsDirectSpaceStateSW::PhysicsDirectSpaceStateSW() { - - space=NULL; + space = NULL; } - //////////////////////////////////////////////////////////////////////////////////////////////////////////// +void *SpaceSW::_broadphase_pair(CollisionObjectSW *A, int p_subindex_A, CollisionObjectSW *B, int p_subindex_B, void *p_self) { + CollisionObjectSW::Type type_A = A->get_type(); + CollisionObjectSW::Type type_B = B->get_type(); + if (type_A > type_B) { - - - - - - - -void* SpaceSW::_broadphase_pair(CollisionObjectSW *A,int p_subindex_A,CollisionObjectSW *B,int p_subindex_B,void *p_self) { - - CollisionObjectSW::Type type_A=A->get_type(); - CollisionObjectSW::Type type_B=B->get_type(); - if (type_A>type_B) { - - SWAP(A,B); - SWAP(p_subindex_A,p_subindex_B); - SWAP(type_A,type_B); + SWAP(A, B); + SWAP(p_subindex_A, p_subindex_B); + SWAP(type_A, type_B); } - SpaceSW *self = (SpaceSW*)p_self; + SpaceSW *self = (SpaceSW *)p_self; self->collision_pairs++; - if (type_A==CollisionObjectSW::TYPE_AREA) { + if (type_A == CollisionObjectSW::TYPE_AREA) { - AreaSW *area=static_cast<AreaSW*>(A); - if (type_B==CollisionObjectSW::TYPE_AREA) { + AreaSW *area = static_cast<AreaSW *>(A); + if (type_B == CollisionObjectSW::TYPE_AREA) { - AreaSW *area_b=static_cast<AreaSW*>(B); - Area2PairSW *area2_pair = memnew(Area2PairSW(area_b,p_subindex_B,area,p_subindex_A) ); + AreaSW *area_b = static_cast<AreaSW *>(B); + Area2PairSW *area2_pair = memnew(Area2PairSW(area_b, p_subindex_B, area, p_subindex_A)); return area2_pair; } else { - BodySW *body=static_cast<BodySW*>(B); - AreaPairSW *area_pair = memnew(AreaPairSW(body,p_subindex_B,area,p_subindex_A) ); + BodySW *body = static_cast<BodySW *>(B); + AreaPairSW *area_pair = memnew(AreaPairSW(body, p_subindex_B, area, p_subindex_A)); return area_pair; } } else { - - BodyPairSW *b = memnew( BodyPairSW((BodySW*)A,p_subindex_A,(BodySW*)B,p_subindex_B) ); + BodyPairSW *b = memnew(BodyPairSW((BodySW *)A, p_subindex_A, (BodySW *)B, p_subindex_B)); return b; - } return NULL; } -void SpaceSW::_broadphase_unpair(CollisionObjectSW *A,int p_subindex_A,CollisionObjectSW *B,int p_subindex_B,void *p_data,void *p_self) { - +void SpaceSW::_broadphase_unpair(CollisionObjectSW *A, int p_subindex_A, CollisionObjectSW *B, int p_subindex_B, void *p_data, void *p_self) { - - SpaceSW *self = (SpaceSW*)p_self; + SpaceSW *self = (SpaceSW *)p_self; self->collision_pairs--; - ConstraintSW *c = (ConstraintSW*)p_data; + ConstraintSW *c = (ConstraintSW *)p_data; memdelete(c); } - -const SelfList<BodySW>::List& SpaceSW::get_active_body_list() const { +const SelfList<BodySW>::List &SpaceSW::get_active_body_list() const { return active_list; } -void SpaceSW::body_add_to_active_list(SelfList<BodySW>* p_body) { +void SpaceSW::body_add_to_active_list(SelfList<BodySW> *p_body) { active_list.add(p_body); } -void SpaceSW::body_remove_from_active_list(SelfList<BodySW>* p_body) { +void SpaceSW::body_remove_from_active_list(SelfList<BodySW> *p_body) { active_list.remove(p_body); - } -void SpaceSW::body_add_to_inertia_update_list(SelfList<BodySW>* p_body) { - +void SpaceSW::body_add_to_inertia_update_list(SelfList<BodySW> *p_body) { inertia_update_list.add(p_body); } -void SpaceSW::body_remove_from_inertia_update_list(SelfList<BodySW>* p_body) { +void SpaceSW::body_remove_from_inertia_update_list(SelfList<BodySW> *p_body) { inertia_update_list.remove(p_body); } @@ -569,112 +510,103 @@ BroadPhaseSW *SpaceSW::get_broadphase() { void SpaceSW::add_object(CollisionObjectSW *p_object) { - ERR_FAIL_COND( objects.has(p_object) ); + ERR_FAIL_COND(objects.has(p_object)); objects.insert(p_object); } void SpaceSW::remove_object(CollisionObjectSW *p_object) { - ERR_FAIL_COND( !objects.has(p_object) ); + ERR_FAIL_COND(!objects.has(p_object)); objects.erase(p_object); } -const Set<CollisionObjectSW*> &SpaceSW::get_objects() const { +const Set<CollisionObjectSW *> &SpaceSW::get_objects() const { return objects; } -void SpaceSW::body_add_to_state_query_list(SelfList<BodySW>* p_body) { +void SpaceSW::body_add_to_state_query_list(SelfList<BodySW> *p_body) { state_query_list.add(p_body); } -void SpaceSW::body_remove_from_state_query_list(SelfList<BodySW>* p_body) { +void SpaceSW::body_remove_from_state_query_list(SelfList<BodySW> *p_body) { state_query_list.remove(p_body); } -void SpaceSW::area_add_to_monitor_query_list(SelfList<AreaSW>* p_area) { +void SpaceSW::area_add_to_monitor_query_list(SelfList<AreaSW> *p_area) { monitor_query_list.add(p_area); } -void SpaceSW::area_remove_from_monitor_query_list(SelfList<AreaSW>* p_area) { +void SpaceSW::area_remove_from_monitor_query_list(SelfList<AreaSW> *p_area) { monitor_query_list.remove(p_area); } -void SpaceSW::area_add_to_moved_list(SelfList<AreaSW>* p_area) { +void SpaceSW::area_add_to_moved_list(SelfList<AreaSW> *p_area) { area_moved_list.add(p_area); } -void SpaceSW::area_remove_from_moved_list(SelfList<AreaSW>* p_area) { +void SpaceSW::area_remove_from_moved_list(SelfList<AreaSW> *p_area) { area_moved_list.remove(p_area); } -const SelfList<AreaSW>::List& SpaceSW::get_moved_area_list() const { +const SelfList<AreaSW>::List &SpaceSW::get_moved_area_list() const { return area_moved_list; } - - - void SpaceSW::call_queries() { - while(state_query_list.first()) { + while (state_query_list.first()) { - BodySW * b = state_query_list.first()->self(); + BodySW *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()) { - AreaSW * a = monitor_query_list.first()->self(); + AreaSW *a = monitor_query_list.first()->self(); a->call_queries(); monitor_query_list.remove(monitor_query_list.first()); } - } void SpaceSW::setup() { - contact_debug_count=0; - while(inertia_update_list.first()) { + contact_debug_count = 0; + while (inertia_update_list.first()) { inertia_update_list.first()->self()->update_inertias(); inertia_update_list.remove(inertia_update_list.first()); } - - } void SpaceSW::update() { - broadphase->update(); - } - void SpaceSW::set_param(PhysicsServer::SpaceParameter p_param, real_t p_value) { - switch(p_param) { + switch (p_param) { - case PhysicsServer::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: contact_recycle_radius=p_value; break; - case PhysicsServer::SPACE_PARAM_CONTACT_MAX_SEPARATION: contact_max_separation=p_value; break; - case PhysicsServer::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: contact_max_allowed_penetration=p_value; break; - case PhysicsServer::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_TRESHOLD: body_linear_velocity_sleep_threshold=p_value; break; - case PhysicsServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_TRESHOLD: body_angular_velocity_sleep_threshold=p_value; break; - case PhysicsServer::SPACE_PARAM_BODY_TIME_TO_SLEEP: body_time_to_sleep=p_value; break; - case PhysicsServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO: body_angular_velocity_damp_ratio=p_value; break; - case PhysicsServer::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: constraint_bias=p_value; break; + case PhysicsServer::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: contact_recycle_radius = p_value; break; + case PhysicsServer::SPACE_PARAM_CONTACT_MAX_SEPARATION: contact_max_separation = p_value; break; + case PhysicsServer::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: contact_max_allowed_penetration = p_value; break; + case PhysicsServer::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_TRESHOLD: body_linear_velocity_sleep_threshold = p_value; break; + case PhysicsServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_TRESHOLD: body_angular_velocity_sleep_threshold = p_value; break; + case PhysicsServer::SPACE_PARAM_BODY_TIME_TO_SLEEP: body_time_to_sleep = p_value; break; + case PhysicsServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO: body_angular_velocity_damp_ratio = p_value; break; + case PhysicsServer::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: constraint_bias = p_value; break; } } real_t SpaceSW::get_param(PhysicsServer::SpaceParameter p_param) const { - switch(p_param) { + switch (p_param) { case PhysicsServer::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: return contact_recycle_radius; case PhysicsServer::SPACE_PARAM_CONTACT_MAX_SEPARATION: return contact_max_separation; @@ -690,12 +622,12 @@ real_t SpaceSW::get_param(PhysicsServer::SpaceParameter p_param) const { void SpaceSW::lock() { - locked=true; + locked = true; } void SpaceSW::unlock() { - locked=false; + locked = false; } bool SpaceSW::is_locked() const { @@ -710,41 +642,36 @@ PhysicsDirectSpaceStateSW *SpaceSW::get_direct_state() { SpaceSW::SpaceSW() { - collision_pairs=0; - active_objects=0; - island_count=0; - contact_debug_count=0; + collision_pairs = 0; + active_objects = 0; + island_count = 0; + contact_debug_count = 0; - locked=false; - contact_recycle_radius=0.01; - contact_max_separation=0.05; - contact_max_allowed_penetration= 0.01; + locked = false; + contact_recycle_radius = 0.01; + contact_max_separation = 0.05; + contact_max_allowed_penetration = 0.01; constraint_bias = 0.01; - body_linear_velocity_sleep_threshold=GLOBAL_DEF("physics/3d/sleep_threshold_linear",0.1); - body_angular_velocity_sleep_threshold=GLOBAL_DEF("physics/3d/sleep_threshold_angular", (8.0 / 180.0 * Math_PI) ); - body_time_to_sleep=GLOBAL_DEF("physics/3d/time_before_sleep",0.5); - body_angular_velocity_damp_ratio=10; - + body_linear_velocity_sleep_threshold = GLOBAL_DEF("physics/3d/sleep_threshold_linear", 0.1); + body_angular_velocity_sleep_threshold = GLOBAL_DEF("physics/3d/sleep_threshold_angular", (8.0 / 180.0 * Math_PI)); + body_time_to_sleep = GLOBAL_DEF("physics/3d/time_before_sleep", 0.5); + body_angular_velocity_damp_ratio = 10; broadphase = BroadPhaseSW::create_func(); - broadphase->set_pair_callback(_broadphase_pair,this); - broadphase->set_unpair_callback(_broadphase_unpair,this); - area=NULL; + broadphase->set_pair_callback(_broadphase_pair, this); + broadphase->set_unpair_callback(_broadphase_unpair, this); + area = NULL; - direct_access = memnew( PhysicsDirectSpaceStateSW ); - direct_access->space=this; - - for(int i=0;i<ELAPSED_TIME_MAX;i++) - elapsed_time[i]=0; + direct_access = memnew(PhysicsDirectSpaceStateSW); + direct_access->space = this; + for (int i = 0; i < ELAPSED_TIME_MAX; i++) + elapsed_time[i] = 0; } SpaceSW::~SpaceSW() { memdelete(broadphase); - memdelete( direct_access ); + memdelete(direct_access); } - - - diff --git a/servers/physics/space_sw.h b/servers/physics/space_sw.h index 208831914..06538265b 100644 --- a/servers/physics/space_sw.h +++ b/servers/physics/space_sw.h @@ -29,39 +29,35 @@ #ifndef SPACE_SW_H #define SPACE_SW_H -#include "typedefs.h" -#include "hash_map.h" -#include "body_sw.h" +#include "area_pair_sw.h" #include "area_sw.h" #include "body_pair_sw.h" -#include "area_pair_sw.h" +#include "body_sw.h" #include "broad_phase_sw.h" #include "collision_object_sw.h" #include "global_config.h" - +#include "hash_map.h" +#include "typedefs.h" class PhysicsDirectSpaceStateSW : public PhysicsDirectSpaceState { - GDCLASS( PhysicsDirectSpaceStateSW, PhysicsDirectSpaceState ); -public: + GDCLASS(PhysicsDirectSpaceStateSW, PhysicsDirectSpaceState); +public: SpaceSW *space; - virtual bool intersect_ray(const Vector3& p_from, const Vector3& p_to,RayResult &r_result,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION,bool p_pick_ray=false); - virtual int intersect_shape(const RID& p_shape, const Transform& p_xform,real_t p_margin,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION); - virtual bool cast_motion(const RID& p_shape, const Transform& p_xform,const Vector3& p_motion,real_t p_margin,real_t &p_closest_safe,real_t &p_closest_unsafe, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION,ShapeRestInfo *r_info=NULL); - virtual bool collide_shape(RID p_shape, const Transform& p_shape_xform,real_t p_margin,Vector3 *r_results,int p_result_max,int &r_result_count, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION); - virtual bool rest_info(RID p_shape, const Transform& p_shape_xform,real_t p_margin,ShapeRestInfo *r_info, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION); + virtual bool intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_layer_mask = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION, bool p_pick_ray = false); + virtual int intersect_shape(const RID &p_shape, const Transform &p_xform, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_layer_mask = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION); + virtual bool cast_motion(const RID &p_shape, const Transform &p_xform, const Vector3 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_layer_mask = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION, ShapeRestInfo *r_info = NULL); + virtual bool collide_shape(RID p_shape, const Transform &p_shape_xform, real_t p_margin, Vector3 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_layer_mask = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION); + virtual bool rest_info(RID p_shape, const Transform &p_shape_xform, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_layer_mask = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION); PhysicsDirectSpaceStateSW(); }; - - class SpaceSW : public RID_Data { public: - enum ElapsedTime { ELAPSED_TIME_INTEGRATE_FORCES, ELAPSED_TIME_GENERATE_ISLANDS, @@ -71,8 +67,8 @@ public: ELAPSED_TIME_MAX }; -private: +private: uint64_t elapsed_time[ELAPSED_TIME_MAX]; PhysicsDirectSpaceStateSW *direct_access; @@ -85,10 +81,10 @@ private: SelfList<AreaSW>::List monitor_query_list; SelfList<AreaSW>::List area_moved_list; - static void* _broadphase_pair(CollisionObjectSW *A,int p_subindex_A,CollisionObjectSW *B,int p_subindex_B,void *p_self); - static void _broadphase_unpair(CollisionObjectSW *A,int p_subindex_A,CollisionObjectSW *B,int p_subindex_B,void *p_data,void *p_self); + static void *_broadphase_pair(CollisionObjectSW *A, int p_subindex_A, CollisionObjectSW *B, int p_subindex_B, void *p_self); + static void _broadphase_unpair(CollisionObjectSW *A, int p_subindex_A, CollisionObjectSW *B, int p_subindex_B, void *p_data, void *p_self); - Set<CollisionObjectSW*> objects; + Set<CollisionObjectSW *> objects; AreaSW *area; @@ -99,7 +95,7 @@ private: enum { - INTERSECTION_QUERY_MAX=2048 + INTERSECTION_QUERY_MAX = 2048 }; CollisionObjectSW *intersection_query_results[INTERSECTION_QUERY_MAX]; @@ -121,36 +117,35 @@ private: Vector<Vector3> contact_debug; int contact_debug_count; -friend class PhysicsDirectSpaceStateSW; + friend class PhysicsDirectSpaceStateSW; 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(AreaSW *p_area) { area=p_area; } + void set_default_area(AreaSW *p_area) { area = p_area; } AreaSW *get_default_area() const { return area; } - const SelfList<BodySW>::List& get_active_body_list() const; - void body_add_to_active_list(SelfList<BodySW>* p_body); - void body_remove_from_active_list(SelfList<BodySW>* p_body); - void body_add_to_inertia_update_list(SelfList<BodySW>* p_body); - void body_remove_from_inertia_update_list(SelfList<BodySW>* p_body); + const SelfList<BodySW>::List &get_active_body_list() const; + void body_add_to_active_list(SelfList<BodySW> *p_body); + void body_remove_from_active_list(SelfList<BodySW> *p_body); + void body_add_to_inertia_update_list(SelfList<BodySW> *p_body); + void body_remove_from_inertia_update_list(SelfList<BodySW> *p_body); - void body_add_to_state_query_list(SelfList<BodySW>* p_body); - void body_remove_from_state_query_list(SelfList<BodySW>* p_body); + void body_add_to_state_query_list(SelfList<BodySW> *p_body); + void body_remove_from_state_query_list(SelfList<BodySW> *p_body); - void area_add_to_monitor_query_list(SelfList<AreaSW>* p_area); - void area_remove_from_monitor_query_list(SelfList<AreaSW>* p_area); - void area_add_to_moved_list(SelfList<AreaSW>* p_area); - void area_remove_from_moved_list(SelfList<AreaSW>* p_area); - const SelfList<AreaSW>::List& get_moved_area_list() const; + void area_add_to_monitor_query_list(SelfList<AreaSW> *p_area); + void area_remove_from_monitor_query_list(SelfList<AreaSW> *p_area); + void area_add_to_moved_list(SelfList<AreaSW> *p_area); + void area_remove_from_moved_list(SelfList<AreaSW> *p_area); + const SelfList<AreaSW>::List &get_moved_area_list() const; BroadPhaseSW *get_broadphase(); void add_object(CollisionObjectSW *p_object); void remove_object(CollisionObjectSW *p_object); - const Set<CollisionObjectSW*> &get_objects() const; + const Set<CollisionObjectSW *> &get_objects() const; _FORCE_INLINE_ real_t get_contact_recycle_radius() const { return contact_recycle_radius; } _FORCE_INLINE_ real_t get_contact_max_separation() const { return contact_max_separation; } @@ -161,12 +156,10 @@ public: _FORCE_INLINE_ real_t get_body_time_to_sleep() const { return body_time_to_sleep; } _FORCE_INLINE_ real_t get_body_angular_velocity_damp_ratio() const { return body_angular_velocity_damp_ratio; } - void update(); void setup(); void call_queries(); - bool is_locked() const; void lock(); void unlock(); @@ -174,10 +167,10 @@ public: void set_param(PhysicsServer::SpaceParameter p_param, real_t p_value); real_t get_param(PhysicsServer::SpaceParameter p_param) const; - void set_island_count(int p_island_count) { island_count=p_island_count; } + 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; } @@ -186,19 +179,20 @@ public: void set_debug_contacts(int p_amount) { contact_debug.resize(p_amount); } _FORCE_INLINE_ bool is_debugging_contacts() const { return !contact_debug.empty(); } - _FORCE_INLINE_ void add_debug_contact(const Vector3& p_contact) { if (contact_debug_count<contact_debug.size()) contact_debug[contact_debug_count++]=p_contact; } + _FORCE_INLINE_ void add_debug_contact(const Vector3 &p_contact) { + if (contact_debug_count < contact_debug.size()) contact_debug[contact_debug_count++] = p_contact; + } _FORCE_INLINE_ Vector<Vector3> get_debug_contacts() { return contact_debug; } _FORCE_INLINE_ int get_debug_contact_count() { return contact_debug_count; } - void set_static_global_body(RID p_body) { static_global_body=p_body; } + void set_static_global_body(RID p_body) { static_global_body = p_body; } RID get_static_global_body() { return static_global_body; } - void set_elapsed_time(ElapsedTime p_time,uint64_t p_msec) { elapsed_time[p_time]=p_msec; } + 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]; } SpaceSW(); ~SpaceSW(); }; - #endif // SPACE__SW_H diff --git a/servers/physics/step_sw.cpp b/servers/physics/step_sw.cpp index 0bd5a874e..c7b1be7a9 100644 --- a/servers/physics/step_sw.cpp +++ b/servers/physics/step_sw.cpp @@ -31,279 +31,268 @@ #include "os/os.h" -void StepSW::_populate_island(BodySW* p_body,BodySW** p_island,ConstraintSW **p_constraint_island) { +void StepSW::_populate_island(BodySW *p_body, BodySW **p_island, ConstraintSW **p_constraint_island) { p_body->set_island_step(_step); p_body->set_island_next(*p_island); - *p_island=p_body; + *p_island = p_body; - for(Map<ConstraintSW*,int>::Element *E=p_body->get_constraint_map().front();E;E=E->next()) { + for (Map<ConstraintSW *, int>::Element *E = p_body->get_constraint_map().front(); E; E = E->next()) { - ConstraintSW *c=(ConstraintSW*)E->key(); - if (c->get_island_step()==_step) + ConstraintSW *c = (ConstraintSW *)E->key(); + if (c->get_island_step() == _step) continue; //already processed c->set_island_step(_step); c->set_island_next(*p_constraint_island); - *p_constraint_island=c; + *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; BodySW *b = c->get_body_ptr()[i]; - if (b->get_island_step()==_step || b->get_mode()==PhysicsServer::BODY_MODE_STATIC || b->get_mode()==PhysicsServer::BODY_MODE_KINEMATIC) + if (b->get_island_step() == _step || b->get_mode() == PhysicsServer::BODY_MODE_STATIC || b->get_mode() == PhysicsServer::BODY_MODE_KINEMATIC) continue; //no go - _populate_island(c->get_body_ptr()[i],p_island,p_constraint_island); + _populate_island(c->get_body_ptr()[i], p_island, p_constraint_island); } } } -void StepSW::_setup_island(ConstraintSW *p_island,real_t p_delta) { +void StepSW::_setup_island(ConstraintSW *p_island, real_t p_delta) { - ConstraintSW *ci=p_island; - while(ci) { + ConstraintSW *ci = p_island; + while (ci) { bool process = ci->setup(p_delta); //todo remove from island if process fails - ci=ci->get_island_next(); + ci = ci->get_island_next(); } } -void StepSW::_solve_island(ConstraintSW *p_island,int p_iterations,real_t p_delta){ +void StepSW::_solve_island(ConstraintSW *p_island, int p_iterations, real_t p_delta) { - int at_priority=1; + int at_priority = 1; - while(p_island) { + while (p_island) { - for(int i=0;i<p_iterations;i++) { + for (int i = 0; i < p_iterations; i++) { - ConstraintSW *ci=p_island; - while(ci) { + ConstraintSW *ci = p_island; + while (ci) { ci->solve(p_delta); - ci=ci->get_island_next(); + ci = ci->get_island_next(); } } at_priority++; { - ConstraintSW *ci=p_island; - ConstraintSW *prev=NULL; - while(ci) { - if (ci->get_priority()<at_priority) { + ConstraintSW *ci = p_island; + ConstraintSW *prev = NULL; + while (ci) { + if (ci->get_priority() < at_priority) { if (prev) { prev->set_island_next(ci->get_island_next()); //remove } else { - p_island=ci->get_island_next(); + p_island = ci->get_island_next(); } } else { - prev=ci; + prev = ci; } - ci=ci->get_island_next(); + ci = ci->get_island_next(); } } } - } -void StepSW::_check_suspend(BodySW *p_island,real_t p_delta) { - +void StepSW::_check_suspend(BodySW *p_island, real_t p_delta) { - bool can_sleep=true; + bool can_sleep = true; BodySW *b = p_island; - while(b) { + while (b) { - if (b->get_mode()==PhysicsServer::BODY_MODE_STATIC || b->get_mode()==PhysicsServer::BODY_MODE_KINEMATIC) { - b=b->get_island_next(); + if (b->get_mode() == PhysicsServer::BODY_MODE_STATIC || b->get_mode() == PhysicsServer::BODY_MODE_KINEMATIC) { + b = b->get_island_next(); continue; //ignore for static } if (!b->sleep_test(p_delta)) - can_sleep=false; + 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()==PhysicsServer::BODY_MODE_STATIC || b->get_mode()==PhysicsServer::BODY_MODE_KINEMATIC) { - b=b->get_island_next(); + if (b->get_mode() == PhysicsServer::BODY_MODE_STATIC || b->get_mode() == PhysicsServer::BODY_MODE_KINEMATIC) { + b = b->get_island_next(); continue; //ignore for static } bool active = b->is_active(); - if (active==can_sleep) + if (active == can_sleep) b->set_active(!can_sleep); - b=b->get_island_next(); + b = b->get_island_next(); } } -void StepSW::step(SpaceSW* p_space,real_t p_delta,int p_iterations) { +void StepSW::step(SpaceSW *p_space, real_t p_delta, int p_iterations) { p_space->lock(); // can't access space during this p_space->setup(); //update inertias, etc - const SelfList<BodySW>::List * body_list = &p_space->get_active_body_list(); + const SelfList<BodySW>::List *body_list = &p_space->get_active_body_list(); /* INTEGRATE FORCES */ uint64_t profile_begtime = OS::get_singleton()->get_ticks_usec(); - uint64_t profile_endtime=0; + uint64_t profile_endtime = 0; - int active_count=0; + int active_count = 0; - const SelfList<BodySW>*b = body_list->first(); - while(b) { + const SelfList<BodySW> *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(SpaceSW::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(SpaceSW::ELAPSED_TIME_INTEGRATE_FORCES, profile_endtime - profile_begtime); + profile_begtime = profile_endtime; } /* GENERATE CONSTRAINT ISLANDS */ - BodySW *island_list=NULL; - ConstraintSW *constraint_island_list=NULL; + BodySW *island_list = NULL; + ConstraintSW *constraint_island_list = NULL; b = body_list->first(); - int island_count=0; + int island_count = 0; - while(b) { + while (b) { BodySW *body = b->self(); + if (body->get_island_step() != _step) { - if (body->get_island_step()!=_step) { - - BodySW *island=NULL; - ConstraintSW *constraint_island=NULL; - _populate_island(body,&island,&constraint_island); + BodySW *island = NULL; + ConstraintSW *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<AreaSW>::List &aml = p_space->get_moved_area_list(); - while(aml.first()) { - for(const Set<ConstraintSW*>::Element *E=aml.first()->self()->get_constraints().front();E;E=E->next()) { + while (aml.first()) { + for (const Set<ConstraintSW *>::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) { - ConstraintSW*c=E->get(); - if (c->get_island_step()==_step) + ConstraintSW *c = E->get(); + if (c->get_island_step() == _step) continue; c->set_island_step(_step); c->set_island_next(NULL); c->set_island_list_next(constraint_island_list); - constraint_island_list=c; + constraint_island_list = c; } - p_space->area_remove_from_moved_list((SelfList<AreaSW>*)aml.first()); //faster to remove here + p_space->area_remove_from_moved_list((SelfList<AreaSW> *)aml.first()); //faster to remove here } { //profile - profile_endtime=OS::get_singleton()->get_ticks_usec(); - p_space->set_elapsed_time(SpaceSW::ELAPSED_TIME_GENERATE_ISLANDS,profile_endtime-profile_begtime); - profile_begtime=profile_endtime; + profile_endtime = OS::get_singleton()->get_ticks_usec(); + p_space->set_elapsed_time(SpaceSW::ELAPSED_TIME_GENERATE_ISLANDS, profile_endtime - profile_begtime); + profile_begtime = profile_endtime; } - //print_line("island count: "+itos(island_count)+" active count: "+itos(active_count)); /* SETUP CONSTRAINT ISLANDS */ { - ConstraintSW *ci=constraint_island_list; - while(ci) { + ConstraintSW *ci = constraint_island_list; + while (ci) { - _setup_island(ci,p_delta); - ci=ci->get_island_list_next(); + _setup_island(ci, p_delta); + ci = ci->get_island_list_next(); } } { //profile - profile_endtime=OS::get_singleton()->get_ticks_usec(); - p_space->set_elapsed_time(SpaceSW::ELAPSED_TIME_SETUP_CONSTRAINTS,profile_endtime-profile_begtime); - profile_begtime=profile_endtime; + profile_endtime = OS::get_singleton()->get_ticks_usec(); + p_space->set_elapsed_time(SpaceSW::ELAPSED_TIME_SETUP_CONSTRAINTS, profile_endtime - profile_begtime); + profile_begtime = profile_endtime; } /* SOLVE CONSTRAINT ISLANDS */ { - ConstraintSW *ci=constraint_island_list; - while(ci) { + ConstraintSW *ci = constraint_island_list; + while (ci) { //iterating each island separatedly improves cache efficiency - _solve_island(ci,p_iterations,p_delta); - ci=ci->get_island_list_next(); + _solve_island(ci, p_iterations, p_delta); + ci = ci->get_island_list_next(); } } - { //profile - profile_endtime=OS::get_singleton()->get_ticks_usec(); - p_space->set_elapsed_time(SpaceSW::ELAPSED_TIME_SOLVE_CONSTRAINTS,profile_endtime-profile_begtime); - profile_begtime=profile_endtime; + profile_endtime = OS::get_singleton()->get_ticks_usec(); + p_space->set_elapsed_time(SpaceSW::ELAPSED_TIME_SOLVE_CONSTRAINTS, profile_endtime - profile_begtime); + profile_begtime = profile_endtime; } /* INTEGRATE VELOCITIES */ b = body_list->first(); - while(b) { - const SelfList<BodySW>*n=b->next(); + while (b) { + const SelfList<BodySW> *n = b->next(); b->self()->integrate_velocities(p_delta); - b=n; + b = n; } /* SLEEP / WAKE UP ISLANDS */ { - BodySW *bi=island_list; - while(bi) { + BodySW *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(SpaceSW::ELAPSED_TIME_INTEGRATE_VELOCITIES,profile_endtime-profile_begtime); - profile_begtime=profile_endtime; + profile_endtime = OS::get_singleton()->get_ticks_usec(); + p_space->set_elapsed_time(SpaceSW::ELAPSED_TIME_INTEGRATE_VELOCITIES, profile_endtime - profile_begtime); + profile_begtime = profile_endtime; } p_space->update(); p_space->unlock(); _step++; - - - } StepSW::StepSW() { - _step=1; + _step = 1; } diff --git a/servers/physics/step_sw.h b/servers/physics/step_sw.h index 7048a7693..54f5fe985 100644 --- a/servers/physics/step_sw.h +++ b/servers/physics/step_sw.h @@ -35,13 +35,13 @@ class StepSW { uint64_t _step; - void _populate_island(BodySW* p_body,BodySW** p_island,ConstraintSW **p_constraint_island); - void _setup_island(ConstraintSW *p_island,real_t p_delta); - void _solve_island(ConstraintSW *p_island,int p_iterations,real_t p_delta); - void _check_suspend(BodySW *p_island,real_t p_delta); -public: + void _populate_island(BodySW *p_body, BodySW **p_island, ConstraintSW **p_constraint_island); + void _setup_island(ConstraintSW *p_island, real_t p_delta); + void _solve_island(ConstraintSW *p_island, int p_iterations, real_t p_delta); + void _check_suspend(BodySW *p_island, real_t p_delta); - void step(SpaceSW* p_space,real_t p_delta,int p_iterations); +public: + void step(SpaceSW *p_space, real_t p_delta, int p_iterations); StepSW(); }; diff --git a/servers/physics_2d/area_2d_sw.cpp b/servers/physics_2d/area_2d_sw.cpp index 262550271..885ede7cb 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) { +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; + 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 02a65962e..8c52c9652 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 cb91caf62..769db8eb3 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 219ca30c4..78be9572b 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 0dab534ef..d49dd35ad 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) { - +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; + 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 3fb01959a..7e4fef8df 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 c05d61b65..ee94a7ace 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) { -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; - + 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 7a4771782..023c182c3 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 e1f6f4f92..c282c0364 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 82e91118c..7c5d799f9 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 06eead64c..74c01e122 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 857053ccf..f30f72d6d 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 4347155c2..8cde46217 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 b9ec434ae..a255cc6ba 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 9ae0e4041..30bb9f6b5 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 0f77e9b42..e6eec05f3 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 4c116cee3..f72a7ef0e 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, + }, { - _generate_contacts_point_point, - _generate_contacts_point_edge, - },{ - 0, - _generate_contacts_edge_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) { - - Vector2 axis=p_axis; + _FORCE_INLINE_ bool test_axis(const Vector2 &p_axis) { + Vector2 axis = p_axis; - if ( Math::abs(axis.x)<CMP_EPSILON && - Math::abs(axis.y)<CMP_EPSILON) { + 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) { +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); - 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) { - - const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW*>(p_a); - const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_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); - 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) { - - const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW*>(p_a); - const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_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); - 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) { +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); + 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) { +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); + 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) { +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); + 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); + 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[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; + 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; + 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 6b698a09f..a13c3dd14 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 3fdecdf41..6218af72a 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.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.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 2a5fc9fe1..886cb90ac 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 cce668405..659b5b321 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 76adf0642..027758649 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 1be15e4ed..8fe38f365 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 920742ea0..0a56683c5 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 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 = 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 e33e2d78c..b49c37ab2 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 34ba149d2..ef1a5b333 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 f8b533080..ff30b2df0 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 b6eb42713..32d632453 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; + 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); + 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 c04cdfa45..00d86da7f 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 9bced5006..93df0a00f 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(); + 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) { + 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; + 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; + 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(); + 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) { + 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); + 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(); + 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 071aa7bdf..103f328ed 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 355cc25a6..55bc62975 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 0896e1016..c743358b6 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(); }; diff --git a/servers/physics_2d_server.cpp b/servers/physics_2d_server.cpp index bdbe85612..f922b8f7a 100644 --- a/servers/physics_2d_server.cpp +++ b/servers/physics_2d_server.cpp @@ -28,103 +28,97 @@ /*************************************************************************/ #include "physics_2d_server.h" #include "print_string.h" -Physics2DServer * Physics2DServer::singleton=NULL; - +Physics2DServer *Physics2DServer::singleton = NULL; void Physics2DDirectBodyState::integrate_forces() { real_t step = get_step(); Vector2 lv = get_linear_velocity(); - lv+=get_total_gravity() * step; + lv += get_total_gravity() * step; real_t av = get_angular_velocity(); float damp = 1.0 - step * get_total_linear_damp(); - if (damp<0) // reached zero in the given time - damp=0; + if (damp < 0) // reached zero in the given time + damp = 0; - lv*=damp; + lv *= damp; damp = 1.0 - step * get_total_angular_damp(); - if (damp<0) // reached zero in the given time - damp=0; + if (damp < 0) // reached zero in the given time + damp = 0; - av*=damp; + av *= damp; set_linear_velocity(lv); set_angular_velocity(av); - - } -Object* Physics2DDirectBodyState::get_contact_collider_object(int p_contact_idx) const { +Object *Physics2DDirectBodyState::get_contact_collider_object(int p_contact_idx) const { ObjectID objid = get_contact_collider_id(p_contact_idx); - Object *obj = ObjectDB::get_instance( objid ); + Object *obj = ObjectDB::get_instance(objid); return obj; } -Physics2DServer * Physics2DServer::get_singleton() { +Physics2DServer *Physics2DServer::get_singleton() { return singleton; } void Physics2DDirectBodyState::_bind_methods() { - ClassDB::bind_method(D_METHOD("get_total_gravity"),&Physics2DDirectBodyState::get_total_gravity); - ClassDB::bind_method(D_METHOD("get_total_linear_damp"),&Physics2DDirectBodyState::get_total_linear_damp); - ClassDB::bind_method(D_METHOD("get_total_angular_damp"),&Physics2DDirectBodyState::get_total_angular_damp); + ClassDB::bind_method(D_METHOD("get_total_gravity"), &Physics2DDirectBodyState::get_total_gravity); + ClassDB::bind_method(D_METHOD("get_total_linear_damp"), &Physics2DDirectBodyState::get_total_linear_damp); + ClassDB::bind_method(D_METHOD("get_total_angular_damp"), &Physics2DDirectBodyState::get_total_angular_damp); - ClassDB::bind_method(D_METHOD("get_inverse_mass"),&Physics2DDirectBodyState::get_inverse_mass); - ClassDB::bind_method(D_METHOD("get_inverse_inertia"),&Physics2DDirectBodyState::get_inverse_inertia); + ClassDB::bind_method(D_METHOD("get_inverse_mass"), &Physics2DDirectBodyState::get_inverse_mass); + ClassDB::bind_method(D_METHOD("get_inverse_inertia"), &Physics2DDirectBodyState::get_inverse_inertia); - ClassDB::bind_method(D_METHOD("set_linear_velocity","velocity"),&Physics2DDirectBodyState::set_linear_velocity); - ClassDB::bind_method(D_METHOD("get_linear_velocity"),&Physics2DDirectBodyState::get_linear_velocity); + ClassDB::bind_method(D_METHOD("set_linear_velocity", "velocity"), &Physics2DDirectBodyState::set_linear_velocity); + ClassDB::bind_method(D_METHOD("get_linear_velocity"), &Physics2DDirectBodyState::get_linear_velocity); - ClassDB::bind_method(D_METHOD("set_angular_velocity","velocity"),&Physics2DDirectBodyState::set_angular_velocity); - ClassDB::bind_method(D_METHOD("get_angular_velocity"),&Physics2DDirectBodyState::get_angular_velocity); + ClassDB::bind_method(D_METHOD("set_angular_velocity", "velocity"), &Physics2DDirectBodyState::set_angular_velocity); + ClassDB::bind_method(D_METHOD("get_angular_velocity"), &Physics2DDirectBodyState::get_angular_velocity); - ClassDB::bind_method(D_METHOD("set_transform","transform"),&Physics2DDirectBodyState::set_transform); - ClassDB::bind_method(D_METHOD("get_transform"),&Physics2DDirectBodyState::get_transform); + ClassDB::bind_method(D_METHOD("set_transform", "transform"), &Physics2DDirectBodyState::set_transform); + ClassDB::bind_method(D_METHOD("get_transform"), &Physics2DDirectBodyState::get_transform); - ClassDB::bind_method(D_METHOD("set_sleep_state","enabled"),&Physics2DDirectBodyState::set_sleep_state); - ClassDB::bind_method(D_METHOD("is_sleeping"),&Physics2DDirectBodyState::is_sleeping); + ClassDB::bind_method(D_METHOD("set_sleep_state", "enabled"), &Physics2DDirectBodyState::set_sleep_state); + ClassDB::bind_method(D_METHOD("is_sleeping"), &Physics2DDirectBodyState::is_sleeping); - ClassDB::bind_method(D_METHOD("get_contact_count"),&Physics2DDirectBodyState::get_contact_count); - - ClassDB::bind_method(D_METHOD("get_contact_local_pos","contact_idx"),&Physics2DDirectBodyState::get_contact_local_pos); - ClassDB::bind_method(D_METHOD("get_contact_local_normal","contact_idx"),&Physics2DDirectBodyState::get_contact_local_normal); - ClassDB::bind_method(D_METHOD("get_contact_local_shape","contact_idx"),&Physics2DDirectBodyState::get_contact_local_shape); - ClassDB::bind_method(D_METHOD("get_contact_collider","contact_idx"),&Physics2DDirectBodyState::get_contact_collider); - ClassDB::bind_method(D_METHOD("get_contact_collider_pos","contact_idx"),&Physics2DDirectBodyState::get_contact_collider_pos); - ClassDB::bind_method(D_METHOD("get_contact_collider_id","contact_idx"),&Physics2DDirectBodyState::get_contact_collider_id); - ClassDB::bind_method(D_METHOD("get_contact_collider_object","contact_idx"),&Physics2DDirectBodyState::get_contact_collider_object); - ClassDB::bind_method(D_METHOD("get_contact_collider_shape","contact_idx"),&Physics2DDirectBodyState::get_contact_collider_shape); - ClassDB::bind_method(D_METHOD("get_contact_collider_shape_metadata:Variant","contact_idx"),&Physics2DDirectBodyState::get_contact_collider_shape_metadata); - ClassDB::bind_method(D_METHOD("get_contact_collider_velocity_at_pos","contact_idx"),&Physics2DDirectBodyState::get_contact_collider_velocity_at_pos); - ClassDB::bind_method(D_METHOD("get_step"),&Physics2DDirectBodyState::get_step); - ClassDB::bind_method(D_METHOD("integrate_forces"),&Physics2DDirectBodyState::integrate_forces); - ClassDB::bind_method(D_METHOD("get_space_state:Physics2DDirectSpaceState"),&Physics2DDirectBodyState::get_space_state); + ClassDB::bind_method(D_METHOD("get_contact_count"), &Physics2DDirectBodyState::get_contact_count); + ClassDB::bind_method(D_METHOD("get_contact_local_pos", "contact_idx"), &Physics2DDirectBodyState::get_contact_local_pos); + ClassDB::bind_method(D_METHOD("get_contact_local_normal", "contact_idx"), &Physics2DDirectBodyState::get_contact_local_normal); + ClassDB::bind_method(D_METHOD("get_contact_local_shape", "contact_idx"), &Physics2DDirectBodyState::get_contact_local_shape); + ClassDB::bind_method(D_METHOD("get_contact_collider", "contact_idx"), &Physics2DDirectBodyState::get_contact_collider); + ClassDB::bind_method(D_METHOD("get_contact_collider_pos", "contact_idx"), &Physics2DDirectBodyState::get_contact_collider_pos); + ClassDB::bind_method(D_METHOD("get_contact_collider_id", "contact_idx"), &Physics2DDirectBodyState::get_contact_collider_id); + ClassDB::bind_method(D_METHOD("get_contact_collider_object", "contact_idx"), &Physics2DDirectBodyState::get_contact_collider_object); + ClassDB::bind_method(D_METHOD("get_contact_collider_shape", "contact_idx"), &Physics2DDirectBodyState::get_contact_collider_shape); + ClassDB::bind_method(D_METHOD("get_contact_collider_shape_metadata:Variant", "contact_idx"), &Physics2DDirectBodyState::get_contact_collider_shape_metadata); + ClassDB::bind_method(D_METHOD("get_contact_collider_velocity_at_pos", "contact_idx"), &Physics2DDirectBodyState::get_contact_collider_velocity_at_pos); + ClassDB::bind_method(D_METHOD("get_step"), &Physics2DDirectBodyState::get_step); + ClassDB::bind_method(D_METHOD("integrate_forces"), &Physics2DDirectBodyState::integrate_forces); + ClassDB::bind_method(D_METHOD("get_space_state:Physics2DDirectSpaceState"), &Physics2DDirectBodyState::get_space_state); } Physics2DDirectBodyState::Physics2DDirectBodyState() {} /////////////////////////////////////////////////////// - - void Physics2DShapeQueryParameters::set_shape(const RES &p_shape) { ERR_FAIL_COND(p_shape.is_null()); - shape=p_shape->get_rid(); + shape = p_shape->get_rid(); } -void Physics2DShapeQueryParameters::set_shape_rid(const RID& p_shape) { +void Physics2DShapeQueryParameters::set_shape_rid(const RID &p_shape) { - shape=p_shape; + shape = p_shape; } RID Physics2DShapeQueryParameters::get_shape_rid() const { @@ -132,125 +126,120 @@ RID Physics2DShapeQueryParameters::get_shape_rid() const { return shape; } -void Physics2DShapeQueryParameters::set_transform(const Transform2D& p_transform){ +void Physics2DShapeQueryParameters::set_transform(const Transform2D &p_transform) { - transform=p_transform; + transform = p_transform; } -Transform2D Physics2DShapeQueryParameters::get_transform() const{ +Transform2D Physics2DShapeQueryParameters::get_transform() const { return transform; } -void Physics2DShapeQueryParameters::set_motion(const Vector2& p_motion){ +void Physics2DShapeQueryParameters::set_motion(const Vector2 &p_motion) { - motion=p_motion; + motion = p_motion; } -Vector2 Physics2DShapeQueryParameters::get_motion() const{ +Vector2 Physics2DShapeQueryParameters::get_motion() const { return motion; } -void Physics2DShapeQueryParameters::set_margin(float p_margin){ +void Physics2DShapeQueryParameters::set_margin(float p_margin) { - margin=p_margin; + margin = p_margin; } -float Physics2DShapeQueryParameters::get_margin() const{ +float Physics2DShapeQueryParameters::get_margin() const { return margin; } -void Physics2DShapeQueryParameters::set_layer_mask(int p_layer_mask){ +void Physics2DShapeQueryParameters::set_layer_mask(int p_layer_mask) { - layer_mask=p_layer_mask; + layer_mask = p_layer_mask; } -int Physics2DShapeQueryParameters::get_layer_mask() const{ +int Physics2DShapeQueryParameters::get_layer_mask() const { return layer_mask; } +void Physics2DShapeQueryParameters::set_object_type_mask(int p_object_type_mask) { -void Physics2DShapeQueryParameters::set_object_type_mask(int p_object_type_mask){ - - object_type_mask=p_object_type_mask; + object_type_mask = p_object_type_mask; } -int Physics2DShapeQueryParameters::get_object_type_mask() const{ +int Physics2DShapeQueryParameters::get_object_type_mask() const { return object_type_mask; } -void Physics2DShapeQueryParameters::set_exclude(const Vector<RID>& p_exclude) { +void Physics2DShapeQueryParameters::set_exclude(const Vector<RID> &p_exclude) { exclude.clear(); - for(int i=0;i<p_exclude.size();i++) + for (int i = 0; i < p_exclude.size(); i++) exclude.insert(p_exclude[i]); - } -Vector<RID> Physics2DShapeQueryParameters::get_exclude() const{ +Vector<RID> Physics2DShapeQueryParameters::get_exclude() const { Vector<RID> ret; ret.resize(exclude.size()); - int idx=0; - for(Set<RID>::Element *E=exclude.front();E;E=E->next()) { - ret[idx]=E->get(); + int idx = 0; + for (Set<RID>::Element *E = exclude.front(); E; E = E->next()) { + ret[idx] = E->get(); } return ret; } void Physics2DShapeQueryParameters::_bind_methods() { - ClassDB::bind_method(D_METHOD("set_shape","shape:Shape2D"),&Physics2DShapeQueryParameters::set_shape); - ClassDB::bind_method(D_METHOD("set_shape_rid","shape"),&Physics2DShapeQueryParameters::set_shape_rid); - ClassDB::bind_method(D_METHOD("get_shape_rid"),&Physics2DShapeQueryParameters::get_shape_rid); - - ClassDB::bind_method(D_METHOD("set_transform","transform"),&Physics2DShapeQueryParameters::set_transform); - ClassDB::bind_method(D_METHOD("get_transform"),&Physics2DShapeQueryParameters::get_transform); + ClassDB::bind_method(D_METHOD("set_shape", "shape:Shape2D"), &Physics2DShapeQueryParameters::set_shape); + ClassDB::bind_method(D_METHOD("set_shape_rid", "shape"), &Physics2DShapeQueryParameters::set_shape_rid); + ClassDB::bind_method(D_METHOD("get_shape_rid"), &Physics2DShapeQueryParameters::get_shape_rid); - ClassDB::bind_method(D_METHOD("set_motion","motion"),&Physics2DShapeQueryParameters::set_motion); - ClassDB::bind_method(D_METHOD("get_motion"),&Physics2DShapeQueryParameters::get_motion); + ClassDB::bind_method(D_METHOD("set_transform", "transform"), &Physics2DShapeQueryParameters::set_transform); + ClassDB::bind_method(D_METHOD("get_transform"), &Physics2DShapeQueryParameters::get_transform); - ClassDB::bind_method(D_METHOD("set_margin","margin"),&Physics2DShapeQueryParameters::set_margin); - ClassDB::bind_method(D_METHOD("get_margin"),&Physics2DShapeQueryParameters::get_margin); + ClassDB::bind_method(D_METHOD("set_motion", "motion"), &Physics2DShapeQueryParameters::set_motion); + ClassDB::bind_method(D_METHOD("get_motion"), &Physics2DShapeQueryParameters::get_motion); - ClassDB::bind_method(D_METHOD("set_layer_mask","layer_mask"),&Physics2DShapeQueryParameters::set_layer_mask); - ClassDB::bind_method(D_METHOD("get_layer_mask"),&Physics2DShapeQueryParameters::get_layer_mask); + ClassDB::bind_method(D_METHOD("set_margin", "margin"), &Physics2DShapeQueryParameters::set_margin); + ClassDB::bind_method(D_METHOD("get_margin"), &Physics2DShapeQueryParameters::get_margin); - ClassDB::bind_method(D_METHOD("set_object_type_mask","object_type_mask"),&Physics2DShapeQueryParameters::set_object_type_mask); - ClassDB::bind_method(D_METHOD("get_object_type_mask"),&Physics2DShapeQueryParameters::get_object_type_mask); - - ClassDB::bind_method(D_METHOD("set_exclude","exclude"),&Physics2DShapeQueryParameters::set_exclude); - ClassDB::bind_method(D_METHOD("get_exclude"),&Physics2DShapeQueryParameters::get_exclude); + ClassDB::bind_method(D_METHOD("set_layer_mask", "layer_mask"), &Physics2DShapeQueryParameters::set_layer_mask); + ClassDB::bind_method(D_METHOD("get_layer_mask"), &Physics2DShapeQueryParameters::get_layer_mask); + ClassDB::bind_method(D_METHOD("set_object_type_mask", "object_type_mask"), &Physics2DShapeQueryParameters::set_object_type_mask); + ClassDB::bind_method(D_METHOD("get_object_type_mask"), &Physics2DShapeQueryParameters::get_object_type_mask); + ClassDB::bind_method(D_METHOD("set_exclude", "exclude"), &Physics2DShapeQueryParameters::set_exclude); + ClassDB::bind_method(D_METHOD("get_exclude"), &Physics2DShapeQueryParameters::get_exclude); } Physics2DShapeQueryParameters::Physics2DShapeQueryParameters() { - margin=0; - layer_mask=0x7FFFFFFF; - object_type_mask=Physics2DDirectSpaceState::TYPE_MASK_COLLISION; + margin = 0; + layer_mask = 0x7FFFFFFF; + object_type_mask = Physics2DDirectSpaceState::TYPE_MASK_COLLISION; } - -Dictionary Physics2DDirectSpaceState::_intersect_ray(const Vector2& p_from, const Vector2& p_to,const Vector<RID>& p_exclude,uint32_t p_layers,uint32_t p_object_type_mask) { +Dictionary Physics2DDirectSpaceState::_intersect_ray(const Vector2 &p_from, const Vector2 &p_to, const Vector<RID> &p_exclude, uint32_t p_layers, uint32_t p_object_type_mask) { RayResult inters; Set<RID> exclude; - for(int i=0;i<p_exclude.size();i++) + for (int i = 0; i < p_exclude.size(); i++) exclude.insert(p_exclude[i]); - bool res = intersect_ray(p_from,p_to,inters,exclude,p_layers,p_object_type_mask); + bool res = intersect_ray(p_from, p_to, inters, exclude, p_layers, p_object_type_mask); if (!res) return Dictionary(); Dictionary d; - d["position"]=inters.position; - d["normal"]=inters.normal; - d["collider_id"]=inters.collider_id; - d["collider"]=inters.collider; - d["shape"]=inters.shape; - d["rid"]=inters.rid; - d["metadata"]=inters.metadata; + d["position"] = inters.position; + d["normal"] = inters.normal; + d["collider_id"] = inters.collider_id; + d["collider"] = inters.collider; + d["shape"] = inters.shape; + d["rid"] = inters.rid; + d["metadata"] = inters.metadata; return d; } @@ -259,131 +248,119 @@ Array Physics2DDirectSpaceState::_intersect_shape(const Ref<Physics2DShapeQueryP Vector<ShapeResult> sr; sr.resize(p_max_results); - int rc = intersect_shape(psq->shape,psq->transform,psq->motion,psq->margin,sr.ptr(),sr.size(),psq->exclude,psq->layer_mask,psq->object_type_mask); + int rc = intersect_shape(psq->shape, psq->transform, psq->motion, psq->margin, sr.ptr(), sr.size(), psq->exclude, psq->layer_mask, psq->object_type_mask); Array ret; ret.resize(rc); - for(int i=0;i<rc;i++) { + for (int i = 0; i < rc; i++) { Dictionary d; - d["rid"]=sr[i].rid; - d["collider_id"]=sr[i].collider_id; - d["collider"]=sr[i].collider; - d["shape"]=sr[i].shape; - d["metadata"]=sr[i].metadata; - ret[i]=d; + d["rid"] = sr[i].rid; + d["collider_id"] = sr[i].collider_id; + d["collider"] = sr[i].collider; + d["shape"] = sr[i].shape; + d["metadata"] = sr[i].metadata; + ret[i] = d; } return ret; } -Array Physics2DDirectSpaceState::_cast_motion(const Ref<Physics2DShapeQueryParameters> &psq){ +Array Physics2DDirectSpaceState::_cast_motion(const Ref<Physics2DShapeQueryParameters> &psq) { - float closest_safe,closest_unsafe; - bool res = cast_motion(psq->shape,psq->transform,psq->motion,psq->margin,closest_safe,closest_unsafe,psq->exclude,psq->layer_mask,psq->object_type_mask); + float closest_safe, closest_unsafe; + bool res = cast_motion(psq->shape, psq->transform, psq->motion, psq->margin, closest_safe, closest_unsafe, psq->exclude, psq->layer_mask, psq->object_type_mask); if (!res) return Array(); Array ret; ret.resize(2); - ret[0]=closest_safe; - ret[1]=closest_unsafe; + ret[0] = closest_safe; + ret[1] = closest_unsafe; return ret; - } -Array Physics2DDirectSpaceState::_intersect_point(const Vector2& p_point,int p_max_results,const Vector<RID>& p_exclude,uint32_t p_layers,uint32_t p_object_type_mask) { +Array Physics2DDirectSpaceState::_intersect_point(const Vector2 &p_point, int p_max_results, const Vector<RID> &p_exclude, uint32_t p_layers, uint32_t p_object_type_mask) { Set<RID> exclude; - for(int i=0;i<p_exclude.size();i++) + for (int i = 0; i < p_exclude.size(); i++) exclude.insert(p_exclude[i]); Vector<ShapeResult> ret; ret.resize(p_max_results); - int rc = intersect_point(p_point,ret.ptr(),ret.size(),exclude,p_layers,p_object_type_mask); - if (rc==0) + int rc = intersect_point(p_point, ret.ptr(), ret.size(), exclude, p_layers, p_object_type_mask); + if (rc == 0) return Array(); Array r; r.resize(rc); - for(int i=0;i<rc;i++) { + for (int i = 0; i < rc; i++) { Dictionary d; - d["rid"]=ret[i].rid; - d["collider_id"]=ret[i].collider_id; - d["collider"]=ret[i].collider; - d["shape"]=ret[i].shape; - d["metadata"]=ret[i].metadata; - r[i]=d; + d["rid"] = ret[i].rid; + d["collider_id"] = ret[i].collider_id; + d["collider"] = ret[i].collider; + d["shape"] = ret[i].shape; + d["metadata"] = ret[i].metadata; + r[i] = d; } return r; - } -Array Physics2DDirectSpaceState::_collide_shape(const Ref<Physics2DShapeQueryParameters> &psq, int p_max_results){ +Array Physics2DDirectSpaceState::_collide_shape(const Ref<Physics2DShapeQueryParameters> &psq, int p_max_results) { Vector<Vector2> ret; - ret.resize(p_max_results*2); - int rc=0; - bool res = collide_shape(psq->shape,psq->transform,psq->motion,psq->margin,ret.ptr(),p_max_results,rc,psq->exclude,psq->layer_mask,psq->object_type_mask); + ret.resize(p_max_results * 2); + int rc = 0; + bool res = collide_shape(psq->shape, psq->transform, psq->motion, psq->margin, ret.ptr(), p_max_results, rc, psq->exclude, psq->layer_mask, psq->object_type_mask); if (!res) return Array(); Array r; - r.resize(rc*2); - for(int i=0;i<rc*2;i++) - r[i]=ret[i]; + r.resize(rc * 2); + for (int i = 0; i < rc * 2; i++) + r[i] = ret[i]; return r; - } -Dictionary Physics2DDirectSpaceState::_get_rest_info(const Ref<Physics2DShapeQueryParameters> &psq){ +Dictionary Physics2DDirectSpaceState::_get_rest_info(const Ref<Physics2DShapeQueryParameters> &psq) { ShapeRestInfo sri; - bool res = rest_info(psq->shape,psq->transform,psq->motion,psq->margin,&sri,psq->exclude,psq->layer_mask,psq->object_type_mask); + bool res = rest_info(psq->shape, psq->transform, psq->motion, psq->margin, &sri, psq->exclude, psq->layer_mask, psq->object_type_mask); Dictionary r; if (!res) return r; - r["point"]=sri.point; - r["normal"]=sri.normal; - r["rid"]=sri.rid; - r["collider_id"]=sri.collider_id; - r["shape"]=sri.shape; - r["linear_velocity"]=sri.linear_velocity; - r["metadata"]=sri.metadata; + r["point"] = sri.point; + r["normal"] = sri.normal; + r["rid"] = sri.rid; + r["collider_id"] = sri.collider_id; + r["shape"] = sri.shape; + r["linear_velocity"] = sri.linear_velocity; + r["metadata"] = sri.metadata; return r; } - - Physics2DDirectSpaceState::Physics2DDirectSpaceState() { - - - } - void Physics2DDirectSpaceState::_bind_methods() { - - ClassDB::bind_method(D_METHOD("intersect_point","point","max_results","exclude","layer_mask","type_mask"),&Physics2DDirectSpaceState::_intersect_point,DEFVAL(32),DEFVAL(Array()),DEFVAL(0x7FFFFFFF),DEFVAL(TYPE_MASK_COLLISION)); - ClassDB::bind_method(D_METHOD("intersect_ray:Dictionary","from","to","exclude","layer_mask","type_mask"),&Physics2DDirectSpaceState::_intersect_ray,DEFVAL(Array()),DEFVAL(0x7FFFFFFF),DEFVAL(TYPE_MASK_COLLISION)); - ClassDB::bind_method(D_METHOD("intersect_shape","shape:Physics2DShapeQueryParameters","max_results"),&Physics2DDirectSpaceState::_intersect_shape,DEFVAL(32)); - ClassDB::bind_method(D_METHOD("cast_motion","shape:Physics2DShapeQueryParameters"),&Physics2DDirectSpaceState::_cast_motion); - ClassDB::bind_method(D_METHOD("collide_shape","shape:Physics2DShapeQueryParameters","max_results"),&Physics2DDirectSpaceState::_collide_shape,DEFVAL(32)); - ClassDB::bind_method(D_METHOD("get_rest_info","shape:Physics2DShapeQueryParameters"),&Physics2DDirectSpaceState::_get_rest_info); + ClassDB::bind_method(D_METHOD("intersect_point", "point", "max_results", "exclude", "layer_mask", "type_mask"), &Physics2DDirectSpaceState::_intersect_point, DEFVAL(32), DEFVAL(Array()), DEFVAL(0x7FFFFFFF), DEFVAL(TYPE_MASK_COLLISION)); + ClassDB::bind_method(D_METHOD("intersect_ray:Dictionary", "from", "to", "exclude", "layer_mask", "type_mask"), &Physics2DDirectSpaceState::_intersect_ray, DEFVAL(Array()), DEFVAL(0x7FFFFFFF), DEFVAL(TYPE_MASK_COLLISION)); + ClassDB::bind_method(D_METHOD("intersect_shape", "shape:Physics2DShapeQueryParameters", "max_results"), &Physics2DDirectSpaceState::_intersect_shape, DEFVAL(32)); + ClassDB::bind_method(D_METHOD("cast_motion", "shape:Physics2DShapeQueryParameters"), &Physics2DDirectSpaceState::_cast_motion); + ClassDB::bind_method(D_METHOD("collide_shape", "shape:Physics2DShapeQueryParameters", "max_results"), &Physics2DDirectSpaceState::_collide_shape, DEFVAL(32)); + ClassDB::bind_method(D_METHOD("get_rest_info", "shape:Physics2DShapeQueryParameters"), &Physics2DDirectSpaceState::_get_rest_info); //ClassDB::bind_method(D_METHOD("cast_motion","shape","xform","motion","exclude","umask"),&Physics2DDirectSpaceState::_intersect_shape,DEFVAL(Array()),DEFVAL(0)); - BIND_CONSTANT( TYPE_MASK_STATIC_BODY ); - BIND_CONSTANT( TYPE_MASK_KINEMATIC_BODY ); - BIND_CONSTANT( TYPE_MASK_RIGID_BODY ); - BIND_CONSTANT( TYPE_MASK_CHARACTER_BODY ); - BIND_CONSTANT( TYPE_MASK_AREA ); - BIND_CONSTANT( TYPE_MASK_COLLISION ); - + BIND_CONSTANT(TYPE_MASK_STATIC_BODY); + BIND_CONSTANT(TYPE_MASK_KINEMATIC_BODY); + BIND_CONSTANT(TYPE_MASK_RIGID_BODY); + BIND_CONSTANT(TYPE_MASK_CHARACTER_BODY); + BIND_CONSTANT(TYPE_MASK_AREA); + BIND_CONSTANT(TYPE_MASK_COLLISION); } - int Physics2DShapeQueryResult::get_result_count() const { return result.size(); @@ -396,7 +373,7 @@ ObjectID Physics2DShapeQueryResult::get_result_object_id(int p_idx) const { return result[p_idx].collider_id; } -Object* Physics2DShapeQueryResult::get_result_object(int p_idx) const { +Object *Physics2DShapeQueryResult::get_result_object(int p_idx) const { return result[p_idx].collider; } @@ -406,19 +383,15 @@ int Physics2DShapeQueryResult::get_result_object_shape(int p_idx) const { } Physics2DShapeQueryResult::Physics2DShapeQueryResult() { - - } void Physics2DShapeQueryResult::_bind_methods() { - ClassDB::bind_method(D_METHOD("get_result_count"),&Physics2DShapeQueryResult::get_result_count); - ClassDB::bind_method(D_METHOD("get_result_rid","idx"),&Physics2DShapeQueryResult::get_result_rid); - ClassDB::bind_method(D_METHOD("get_result_object_id","idx"),&Physics2DShapeQueryResult::get_result_object_id); - ClassDB::bind_method(D_METHOD("get_result_object","idx"),&Physics2DShapeQueryResult::get_result_object); - ClassDB::bind_method(D_METHOD("get_result_object_shape","idx"),&Physics2DShapeQueryResult::get_result_object_shape); - - + ClassDB::bind_method(D_METHOD("get_result_count"), &Physics2DShapeQueryResult::get_result_count); + ClassDB::bind_method(D_METHOD("get_result_rid", "idx"), &Physics2DShapeQueryResult::get_result_rid); + ClassDB::bind_method(D_METHOD("get_result_object_id", "idx"), &Physics2DShapeQueryResult::get_result_object_id); + ClassDB::bind_method(D_METHOD("get_result_object", "idx"), &Physics2DShapeQueryResult::get_result_object); + ClassDB::bind_method(D_METHOD("get_result_object_shape", "idx"), &Physics2DShapeQueryResult::get_result_object_shape); } /////////////////////////////// @@ -427,41 +400,41 @@ void Physics2DShapeQueryResult::_bind_methods() { return colliding; }*/ -Vector2 Physics2DTestMotionResult::get_motion() const{ +Vector2 Physics2DTestMotionResult::get_motion() const { return result.motion; } -Vector2 Physics2DTestMotionResult::get_motion_remainder() const{ +Vector2 Physics2DTestMotionResult::get_motion_remainder() const { return result.remainder; } -Vector2 Physics2DTestMotionResult::get_collision_point() const{ +Vector2 Physics2DTestMotionResult::get_collision_point() const { return result.collision_point; } -Vector2 Physics2DTestMotionResult::get_collision_normal() const{ +Vector2 Physics2DTestMotionResult::get_collision_normal() const { return result.collision_normal; } -Vector2 Physics2DTestMotionResult::get_collider_velocity() const{ +Vector2 Physics2DTestMotionResult::get_collider_velocity() const { return result.collider_velocity; } -ObjectID Physics2DTestMotionResult::get_collider_id() const{ +ObjectID Physics2DTestMotionResult::get_collider_id() const { return result.collider_id; } -RID Physics2DTestMotionResult::get_collider_rid() const{ +RID Physics2DTestMotionResult::get_collider_rid() const { return result.collider; } -Object* Physics2DTestMotionResult::get_collider() const { +Object *Physics2DTestMotionResult::get_collider() const { return ObjectDB::get_instance(result.collider_id); } -int Physics2DTestMotionResult::get_collider_shape() const{ +int Physics2DTestMotionResult::get_collider_shape() const { return result.collider_shape; } @@ -469,269 +442,254 @@ int Physics2DTestMotionResult::get_collider_shape() const{ void Physics2DTestMotionResult::_bind_methods() { //ClassDB::bind_method(D_METHOD("is_colliding"),&Physics2DTestMotionResult::is_colliding); - ClassDB::bind_method(D_METHOD("get_motion"),&Physics2DTestMotionResult::get_motion); - ClassDB::bind_method(D_METHOD("get_motion_remainder"),&Physics2DTestMotionResult::get_motion_remainder); - ClassDB::bind_method(D_METHOD("get_collision_point"),&Physics2DTestMotionResult::get_collision_point); - ClassDB::bind_method(D_METHOD("get_collision_normal"),&Physics2DTestMotionResult::get_collision_normal); - ClassDB::bind_method(D_METHOD("get_collider_velocity"),&Physics2DTestMotionResult::get_collider_velocity); - ClassDB::bind_method(D_METHOD("get_collider_id"),&Physics2DTestMotionResult::get_collider_id); - ClassDB::bind_method(D_METHOD("get_collider_rid"),&Physics2DTestMotionResult::get_collider_rid); - ClassDB::bind_method(D_METHOD("get_collider"),&Physics2DTestMotionResult::get_collider); - ClassDB::bind_method(D_METHOD("get_collider_shape"),&Physics2DTestMotionResult::get_collider_shape); - + ClassDB::bind_method(D_METHOD("get_motion"), &Physics2DTestMotionResult::get_motion); + ClassDB::bind_method(D_METHOD("get_motion_remainder"), &Physics2DTestMotionResult::get_motion_remainder); + ClassDB::bind_method(D_METHOD("get_collision_point"), &Physics2DTestMotionResult::get_collision_point); + ClassDB::bind_method(D_METHOD("get_collision_normal"), &Physics2DTestMotionResult::get_collision_normal); + ClassDB::bind_method(D_METHOD("get_collider_velocity"), &Physics2DTestMotionResult::get_collider_velocity); + ClassDB::bind_method(D_METHOD("get_collider_id"), &Physics2DTestMotionResult::get_collider_id); + ClassDB::bind_method(D_METHOD("get_collider_rid"), &Physics2DTestMotionResult::get_collider_rid); + ClassDB::bind_method(D_METHOD("get_collider"), &Physics2DTestMotionResult::get_collider); + ClassDB::bind_method(D_METHOD("get_collider_shape"), &Physics2DTestMotionResult::get_collider_shape); } -Physics2DTestMotionResult::Physics2DTestMotionResult(){ +Physics2DTestMotionResult::Physics2DTestMotionResult() { - colliding=false; - result.collider_id=0; - result.collider_shape=0; + colliding = false; + result.collider_id = 0; + result.collider_shape = 0; } - /////////////////////////////////////// +bool Physics2DServer::_body_test_motion(RID p_body, const Transform2D &p_from, const Vector2 &p_motion, float p_margin, const Ref<Physics2DTestMotionResult> &p_result) { - -bool Physics2DServer::_body_test_motion(RID p_body,const Transform2D& p_from,const Vector2& p_motion,float p_margin,const Ref<Physics2DTestMotionResult>& p_result) { - - MotionResult *r=NULL; + MotionResult *r = NULL; if (p_result.is_valid()) - r=p_result->get_result_ptr(); - return body_test_motion(p_body,p_from,p_motion,p_margin,r); + r = p_result->get_result_ptr(); + return body_test_motion(p_body, p_from, p_motion, p_margin, r); } void Physics2DServer::_bind_methods() { + ClassDB::bind_method(D_METHOD("shape_create", "type"), &Physics2DServer::shape_create); + ClassDB::bind_method(D_METHOD("shape_set_data", "shape", "data"), &Physics2DServer::shape_set_data); - ClassDB::bind_method(D_METHOD("shape_create","type"),&Physics2DServer::shape_create); - ClassDB::bind_method(D_METHOD("shape_set_data","shape","data"),&Physics2DServer::shape_set_data); - - ClassDB::bind_method(D_METHOD("shape_get_type","shape"),&Physics2DServer::shape_get_type); - ClassDB::bind_method(D_METHOD("shape_get_data","shape"),&Physics2DServer::shape_get_data); + ClassDB::bind_method(D_METHOD("shape_get_type", "shape"), &Physics2DServer::shape_get_type); + ClassDB::bind_method(D_METHOD("shape_get_data", "shape"), &Physics2DServer::shape_get_data); + ClassDB::bind_method(D_METHOD("space_create"), &Physics2DServer::space_create); + ClassDB::bind_method(D_METHOD("space_set_active", "space", "active"), &Physics2DServer::space_set_active); + ClassDB::bind_method(D_METHOD("space_is_active", "space"), &Physics2DServer::space_is_active); + ClassDB::bind_method(D_METHOD("space_set_param", "space", "param", "value"), &Physics2DServer::space_set_param); + ClassDB::bind_method(D_METHOD("space_get_param", "space", "param"), &Physics2DServer::space_get_param); + ClassDB::bind_method(D_METHOD("space_get_direct_state:Physics2DDirectSpaceState", "space"), &Physics2DServer::space_get_direct_state); - ClassDB::bind_method(D_METHOD("space_create"),&Physics2DServer::space_create); - ClassDB::bind_method(D_METHOD("space_set_active","space","active"),&Physics2DServer::space_set_active); - ClassDB::bind_method(D_METHOD("space_is_active","space"),&Physics2DServer::space_is_active); - ClassDB::bind_method(D_METHOD("space_set_param","space","param","value"),&Physics2DServer::space_set_param); - ClassDB::bind_method(D_METHOD("space_get_param","space","param"),&Physics2DServer::space_get_param); - ClassDB::bind_method(D_METHOD("space_get_direct_state:Physics2DDirectSpaceState","space"),&Physics2DServer::space_get_direct_state); + ClassDB::bind_method(D_METHOD("area_create"), &Physics2DServer::area_create); + ClassDB::bind_method(D_METHOD("area_set_space", "area", "space"), &Physics2DServer::area_set_space); + ClassDB::bind_method(D_METHOD("area_get_space", "area"), &Physics2DServer::area_get_space); - ClassDB::bind_method(D_METHOD("area_create"),&Physics2DServer::area_create); - ClassDB::bind_method(D_METHOD("area_set_space","area","space"),&Physics2DServer::area_set_space); - ClassDB::bind_method(D_METHOD("area_get_space","area"),&Physics2DServer::area_get_space); + ClassDB::bind_method(D_METHOD("area_set_space_override_mode", "area", "mode"), &Physics2DServer::area_set_space_override_mode); + ClassDB::bind_method(D_METHOD("area_get_space_override_mode", "area"), &Physics2DServer::area_get_space_override_mode); - ClassDB::bind_method(D_METHOD("area_set_space_override_mode","area","mode"),&Physics2DServer::area_set_space_override_mode); - ClassDB::bind_method(D_METHOD("area_get_space_override_mode","area"),&Physics2DServer::area_get_space_override_mode); + ClassDB::bind_method(D_METHOD("area_add_shape", "area", "shape", "transform"), &Physics2DServer::area_add_shape, DEFVAL(Transform2D())); + ClassDB::bind_method(D_METHOD("area_set_shape", "area", "shape_idx", "shape"), &Physics2DServer::area_set_shape); + ClassDB::bind_method(D_METHOD("area_set_shape_transform", "area", "shape_idx", "transform"), &Physics2DServer::area_set_shape_transform); - ClassDB::bind_method(D_METHOD("area_add_shape","area","shape","transform"),&Physics2DServer::area_add_shape,DEFVAL(Transform2D())); - ClassDB::bind_method(D_METHOD("area_set_shape","area","shape_idx","shape"),&Physics2DServer::area_set_shape); - ClassDB::bind_method(D_METHOD("area_set_shape_transform","area","shape_idx","transform"),&Physics2DServer::area_set_shape_transform); + ClassDB::bind_method(D_METHOD("area_get_shape_count", "area"), &Physics2DServer::area_get_shape_count); + ClassDB::bind_method(D_METHOD("area_get_shape", "area", "shape_idx"), &Physics2DServer::area_get_shape); + ClassDB::bind_method(D_METHOD("area_get_shape_transform", "area", "shape_idx"), &Physics2DServer::area_get_shape_transform); - ClassDB::bind_method(D_METHOD("area_get_shape_count","area"),&Physics2DServer::area_get_shape_count); - ClassDB::bind_method(D_METHOD("area_get_shape","area","shape_idx"),&Physics2DServer::area_get_shape); - ClassDB::bind_method(D_METHOD("area_get_shape_transform","area","shape_idx"),&Physics2DServer::area_get_shape_transform); + ClassDB::bind_method(D_METHOD("area_remove_shape", "area", "shape_idx"), &Physics2DServer::area_remove_shape); + ClassDB::bind_method(D_METHOD("area_clear_shapes", "area"), &Physics2DServer::area_clear_shapes); - ClassDB::bind_method(D_METHOD("area_remove_shape","area","shape_idx"),&Physics2DServer::area_remove_shape); - ClassDB::bind_method(D_METHOD("area_clear_shapes","area"),&Physics2DServer::area_clear_shapes); + ClassDB::bind_method(D_METHOD("area_set_layer_mask", "area", "mask"), &Physics2DServer::area_set_layer_mask); + ClassDB::bind_method(D_METHOD("area_set_collision_mask", "area", "mask"), &Physics2DServer::area_set_collision_mask); - ClassDB::bind_method(D_METHOD("area_set_layer_mask","area","mask"),&Physics2DServer::area_set_layer_mask); - ClassDB::bind_method(D_METHOD("area_set_collision_mask","area","mask"),&Physics2DServer::area_set_collision_mask); + ClassDB::bind_method(D_METHOD("area_set_param", "area", "param", "value"), &Physics2DServer::area_set_param); + ClassDB::bind_method(D_METHOD("area_set_transform", "area", "transform"), &Physics2DServer::area_set_transform); - ClassDB::bind_method(D_METHOD("area_set_param","area","param","value"),&Physics2DServer::area_set_param); - ClassDB::bind_method(D_METHOD("area_set_transform","area","transform"),&Physics2DServer::area_set_transform); + ClassDB::bind_method(D_METHOD("area_get_param", "area", "param"), &Physics2DServer::area_get_param); + ClassDB::bind_method(D_METHOD("area_get_transform", "area"), &Physics2DServer::area_get_transform); - ClassDB::bind_method(D_METHOD("area_get_param","area","param"),&Physics2DServer::area_get_param); - ClassDB::bind_method(D_METHOD("area_get_transform","area"),&Physics2DServer::area_get_transform); + ClassDB::bind_method(D_METHOD("area_attach_object_instance_ID", "area", "id"), &Physics2DServer::area_attach_object_instance_ID); + ClassDB::bind_method(D_METHOD("area_get_object_instance_ID", "area"), &Physics2DServer::area_get_object_instance_ID); - ClassDB::bind_method(D_METHOD("area_attach_object_instance_ID","area","id"),&Physics2DServer::area_attach_object_instance_ID); - ClassDB::bind_method(D_METHOD("area_get_object_instance_ID","area"),&Physics2DServer::area_get_object_instance_ID); + ClassDB::bind_method(D_METHOD("area_set_monitor_callback", "area", "receiver", "method"), &Physics2DServer::area_set_monitor_callback); - ClassDB::bind_method(D_METHOD("area_set_monitor_callback","area","receiver","method"),&Physics2DServer::area_set_monitor_callback); + ClassDB::bind_method(D_METHOD("body_create", "mode", "init_sleeping"), &Physics2DServer::body_create, DEFVAL(BODY_MODE_RIGID), DEFVAL(false)); - ClassDB::bind_method(D_METHOD("body_create","mode","init_sleeping"),&Physics2DServer::body_create,DEFVAL(BODY_MODE_RIGID),DEFVAL(false)); + ClassDB::bind_method(D_METHOD("body_set_space", "body", "space"), &Physics2DServer::body_set_space); + ClassDB::bind_method(D_METHOD("body_get_space", "body"), &Physics2DServer::body_get_space); - ClassDB::bind_method(D_METHOD("body_set_space","body","space"),&Physics2DServer::body_set_space); - ClassDB::bind_method(D_METHOD("body_get_space","body"),&Physics2DServer::body_get_space); + ClassDB::bind_method(D_METHOD("body_set_mode", "body", "mode"), &Physics2DServer::body_set_mode); + ClassDB::bind_method(D_METHOD("body_get_mode", "body"), &Physics2DServer::body_get_mode); - ClassDB::bind_method(D_METHOD("body_set_mode","body","mode"),&Physics2DServer::body_set_mode); - ClassDB::bind_method(D_METHOD("body_get_mode","body"),&Physics2DServer::body_get_mode); + ClassDB::bind_method(D_METHOD("body_add_shape", "body", "shape", "transform"), &Physics2DServer::body_add_shape, DEFVAL(Transform2D())); + ClassDB::bind_method(D_METHOD("body_set_shape", "body", "shape_idx", "shape"), &Physics2DServer::body_set_shape); + ClassDB::bind_method(D_METHOD("body_set_shape_transform", "body", "shape_idx", "transform"), &Physics2DServer::body_set_shape_transform); + ClassDB::bind_method(D_METHOD("body_set_shape_metadata", "body", "shape_idx", "metadata"), &Physics2DServer::body_set_shape_metadata); - ClassDB::bind_method(D_METHOD("body_add_shape","body","shape","transform"),&Physics2DServer::body_add_shape,DEFVAL(Transform2D())); - ClassDB::bind_method(D_METHOD("body_set_shape","body","shape_idx","shape"),&Physics2DServer::body_set_shape); - ClassDB::bind_method(D_METHOD("body_set_shape_transform","body","shape_idx","transform"),&Physics2DServer::body_set_shape_transform); - ClassDB::bind_method(D_METHOD("body_set_shape_metadata","body","shape_idx","metadata"),&Physics2DServer::body_set_shape_metadata); + ClassDB::bind_method(D_METHOD("body_get_shape_count", "body"), &Physics2DServer::body_get_shape_count); + ClassDB::bind_method(D_METHOD("body_get_shape", "body", "shape_idx"), &Physics2DServer::body_get_shape); + ClassDB::bind_method(D_METHOD("body_get_shape_transform", "body", "shape_idx"), &Physics2DServer::body_get_shape_transform); + ClassDB::bind_method(D_METHOD("body_get_shape_metadata", "body", "shape_idx"), &Physics2DServer::body_get_shape_metadata); - ClassDB::bind_method(D_METHOD("body_get_shape_count","body"),&Physics2DServer::body_get_shape_count); - ClassDB::bind_method(D_METHOD("body_get_shape","body","shape_idx"),&Physics2DServer::body_get_shape); - ClassDB::bind_method(D_METHOD("body_get_shape_transform","body","shape_idx"),&Physics2DServer::body_get_shape_transform); - ClassDB::bind_method(D_METHOD("body_get_shape_metadata","body","shape_idx"),&Physics2DServer::body_get_shape_metadata); + ClassDB::bind_method(D_METHOD("body_remove_shape", "body", "shape_idx"), &Physics2DServer::body_remove_shape); + ClassDB::bind_method(D_METHOD("body_clear_shapes", "body"), &Physics2DServer::body_clear_shapes); - ClassDB::bind_method(D_METHOD("body_remove_shape","body","shape_idx"),&Physics2DServer::body_remove_shape); - ClassDB::bind_method(D_METHOD("body_clear_shapes","body"),&Physics2DServer::body_clear_shapes); + ClassDB::bind_method(D_METHOD("body_set_shape_as_trigger", "body", "shape_idx", "enable"), &Physics2DServer::body_set_shape_as_trigger); + ClassDB::bind_method(D_METHOD("body_is_shape_set_as_trigger", "body", "shape_idx"), &Physics2DServer::body_is_shape_set_as_trigger); + ClassDB::bind_method(D_METHOD("body_attach_object_instance_ID", "body", "id"), &Physics2DServer::body_attach_object_instance_ID); + ClassDB::bind_method(D_METHOD("body_get_object_instance_ID", "body"), &Physics2DServer::body_get_object_instance_ID); - ClassDB::bind_method(D_METHOD("body_set_shape_as_trigger","body","shape_idx","enable"),&Physics2DServer::body_set_shape_as_trigger); - ClassDB::bind_method(D_METHOD("body_is_shape_set_as_trigger","body","shape_idx"),&Physics2DServer::body_is_shape_set_as_trigger); + ClassDB::bind_method(D_METHOD("body_set_continuous_collision_detection_mode", "body", "mode"), &Physics2DServer::body_set_continuous_collision_detection_mode); + ClassDB::bind_method(D_METHOD("body_get_continuous_collision_detection_mode", "body"), &Physics2DServer::body_get_continuous_collision_detection_mode); - ClassDB::bind_method(D_METHOD("body_attach_object_instance_ID","body","id"),&Physics2DServer::body_attach_object_instance_ID); - ClassDB::bind_method(D_METHOD("body_get_object_instance_ID","body"),&Physics2DServer::body_get_object_instance_ID); + ClassDB::bind_method(D_METHOD("body_set_layer_mask", "body", "mask"), &Physics2DServer::body_set_layer_mask); + ClassDB::bind_method(D_METHOD("body_get_layer_mask", "body"), &Physics2DServer::body_get_layer_mask); + ClassDB::bind_method(D_METHOD("body_set_collision_mask", "body", "mask"), &Physics2DServer::body_set_collision_mask); + ClassDB::bind_method(D_METHOD("body_get_collision_mask", "body"), &Physics2DServer::body_get_collision_mask); - ClassDB::bind_method(D_METHOD("body_set_continuous_collision_detection_mode","body","mode"),&Physics2DServer::body_set_continuous_collision_detection_mode); - ClassDB::bind_method(D_METHOD("body_get_continuous_collision_detection_mode","body"),&Physics2DServer::body_get_continuous_collision_detection_mode); + ClassDB::bind_method(D_METHOD("body_set_param", "body", "param", "value"), &Physics2DServer::body_set_param); + ClassDB::bind_method(D_METHOD("body_get_param", "body", "param"), &Physics2DServer::body_get_param); + ClassDB::bind_method(D_METHOD("body_set_state", "body", "state", "value"), &Physics2DServer::body_set_state); + ClassDB::bind_method(D_METHOD("body_get_state", "body", "state"), &Physics2DServer::body_get_state); - ClassDB::bind_method(D_METHOD("body_set_layer_mask","body","mask"),&Physics2DServer::body_set_layer_mask); - ClassDB::bind_method(D_METHOD("body_get_layer_mask","body"),&Physics2DServer::body_get_layer_mask); + ClassDB::bind_method(D_METHOD("body_apply_impulse", "body", "pos", "impulse"), &Physics2DServer::body_apply_impulse); + ClassDB::bind_method(D_METHOD("body_add_force", "body", "offset", "force"), &Physics2DServer::body_add_force); + ClassDB::bind_method(D_METHOD("body_set_axis_velocity", "body", "axis_velocity"), &Physics2DServer::body_set_axis_velocity); - ClassDB::bind_method(D_METHOD("body_set_collision_mask","body","mask"),&Physics2DServer::body_set_collision_mask); - ClassDB::bind_method(D_METHOD("body_get_collision_mask","body"),&Physics2DServer::body_get_collision_mask); - - - ClassDB::bind_method(D_METHOD("body_set_param","body","param","value"),&Physics2DServer::body_set_param); - ClassDB::bind_method(D_METHOD("body_get_param","body","param"),&Physics2DServer::body_get_param); - - ClassDB::bind_method(D_METHOD("body_set_state","body","state","value"),&Physics2DServer::body_set_state); - ClassDB::bind_method(D_METHOD("body_get_state","body","state"),&Physics2DServer::body_get_state); - - ClassDB::bind_method(D_METHOD("body_apply_impulse","body","pos","impulse"),&Physics2DServer::body_apply_impulse); - ClassDB::bind_method(D_METHOD("body_add_force","body","offset","force"),&Physics2DServer::body_add_force); - ClassDB::bind_method(D_METHOD("body_set_axis_velocity","body","axis_velocity"),&Physics2DServer::body_set_axis_velocity); - - ClassDB::bind_method(D_METHOD("body_add_collision_exception","body","excepted_body"),&Physics2DServer::body_add_collision_exception); - ClassDB::bind_method(D_METHOD("body_remove_collision_exception","body","excepted_body"),&Physics2DServer::body_remove_collision_exception); + ClassDB::bind_method(D_METHOD("body_add_collision_exception", "body", "excepted_body"), &Physics2DServer::body_add_collision_exception); + ClassDB::bind_method(D_METHOD("body_remove_collision_exception", "body", "excepted_body"), &Physics2DServer::body_remove_collision_exception); //virtual void body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions)=0; - ClassDB::bind_method(D_METHOD("body_set_max_contacts_reported","body","amount"),&Physics2DServer::body_set_max_contacts_reported); - ClassDB::bind_method(D_METHOD("body_get_max_contacts_reported","body"),&Physics2DServer::body_get_max_contacts_reported); + ClassDB::bind_method(D_METHOD("body_set_max_contacts_reported", "body", "amount"), &Physics2DServer::body_set_max_contacts_reported); + ClassDB::bind_method(D_METHOD("body_get_max_contacts_reported", "body"), &Physics2DServer::body_get_max_contacts_reported); - ClassDB::bind_method(D_METHOD("body_set_one_way_collision_direction","body","normal"),&Physics2DServer::body_set_one_way_collision_direction); - ClassDB::bind_method(D_METHOD("body_get_one_way_collision_direction","body"),&Physics2DServer::body_get_one_way_collision_direction); + ClassDB::bind_method(D_METHOD("body_set_one_way_collision_direction", "body", "normal"), &Physics2DServer::body_set_one_way_collision_direction); + ClassDB::bind_method(D_METHOD("body_get_one_way_collision_direction", "body"), &Physics2DServer::body_get_one_way_collision_direction); - ClassDB::bind_method(D_METHOD("body_set_one_way_collision_max_depth","body","depth"),&Physics2DServer::body_set_one_way_collision_max_depth); - ClassDB::bind_method(D_METHOD("body_get_one_way_collision_max_depth","body"),&Physics2DServer::body_get_one_way_collision_max_depth); + ClassDB::bind_method(D_METHOD("body_set_one_way_collision_max_depth", "body", "depth"), &Physics2DServer::body_set_one_way_collision_max_depth); + ClassDB::bind_method(D_METHOD("body_get_one_way_collision_max_depth", "body"), &Physics2DServer::body_get_one_way_collision_max_depth); + ClassDB::bind_method(D_METHOD("body_set_omit_force_integration", "body", "enable"), &Physics2DServer::body_set_omit_force_integration); + ClassDB::bind_method(D_METHOD("body_is_omitting_force_integration", "body"), &Physics2DServer::body_is_omitting_force_integration); - ClassDB::bind_method(D_METHOD("body_set_omit_force_integration","body","enable"),&Physics2DServer::body_set_omit_force_integration); - ClassDB::bind_method(D_METHOD("body_is_omitting_force_integration","body"),&Physics2DServer::body_is_omitting_force_integration); + ClassDB::bind_method(D_METHOD("body_set_force_integration_callback", "body", "receiver", "method", "userdata"), &Physics2DServer::body_set_force_integration_callback, DEFVAL(Variant())); - ClassDB::bind_method(D_METHOD("body_set_force_integration_callback","body","receiver","method","userdata"),&Physics2DServer::body_set_force_integration_callback,DEFVAL(Variant())); - - ClassDB::bind_method(D_METHOD("body_test_motion","body","from","motion","margin","result:Physics2DTestMotionResult"),&Physics2DServer::_body_test_motion,DEFVAL(0.08),DEFVAL(Variant())); + ClassDB::bind_method(D_METHOD("body_test_motion", "body", "from", "motion", "margin", "result:Physics2DTestMotionResult"), &Physics2DServer::_body_test_motion, DEFVAL(0.08), DEFVAL(Variant())); /* JOINT API */ - ClassDB::bind_method(D_METHOD("joint_set_param","joint","param","value"),&Physics2DServer::joint_set_param); - ClassDB::bind_method(D_METHOD("joint_get_param","joint","param"),&Physics2DServer::joint_get_param); + ClassDB::bind_method(D_METHOD("joint_set_param", "joint", "param", "value"), &Physics2DServer::joint_set_param); + ClassDB::bind_method(D_METHOD("joint_get_param", "joint", "param"), &Physics2DServer::joint_get_param); - ClassDB::bind_method(D_METHOD("pin_joint_create","anchor","body_a","body_b"),&Physics2DServer::pin_joint_create,DEFVAL(RID())); - ClassDB::bind_method(D_METHOD("groove_joint_create","groove1_a","groove2_a","anchor_b","body_a","body_b"),&Physics2DServer::groove_joint_create,DEFVAL(RID()),DEFVAL(RID())); - ClassDB::bind_method(D_METHOD("damped_spring_joint_create","anchor_a","anchor_b","body_a","body_b"),&Physics2DServer::damped_spring_joint_create,DEFVAL(RID())); + ClassDB::bind_method(D_METHOD("pin_joint_create", "anchor", "body_a", "body_b"), &Physics2DServer::pin_joint_create, DEFVAL(RID())); + ClassDB::bind_method(D_METHOD("groove_joint_create", "groove1_a", "groove2_a", "anchor_b", "body_a", "body_b"), &Physics2DServer::groove_joint_create, DEFVAL(RID()), DEFVAL(RID())); + ClassDB::bind_method(D_METHOD("damped_spring_joint_create", "anchor_a", "anchor_b", "body_a", "body_b"), &Physics2DServer::damped_spring_joint_create, DEFVAL(RID())); - ClassDB::bind_method(D_METHOD("damped_string_joint_set_param","joint","param","value"),&Physics2DServer::damped_string_joint_set_param); - ClassDB::bind_method(D_METHOD("damped_string_joint_get_param","joint","param"),&Physics2DServer::damped_string_joint_get_param); + ClassDB::bind_method(D_METHOD("damped_string_joint_set_param", "joint", "param", "value"), &Physics2DServer::damped_string_joint_set_param); + ClassDB::bind_method(D_METHOD("damped_string_joint_get_param", "joint", "param"), &Physics2DServer::damped_string_joint_get_param); - ClassDB::bind_method(D_METHOD("joint_get_type","joint"),&Physics2DServer::joint_get_type); + ClassDB::bind_method(D_METHOD("joint_get_type", "joint"), &Physics2DServer::joint_get_type); - ClassDB::bind_method(D_METHOD("free_rid","rid"),&Physics2DServer::free); + ClassDB::bind_method(D_METHOD("free_rid", "rid"), &Physics2DServer::free); - ClassDB::bind_method(D_METHOD("set_active","active"),&Physics2DServer::set_active); + ClassDB::bind_method(D_METHOD("set_active", "active"), &Physics2DServer::set_active); - ClassDB::bind_method(D_METHOD("get_process_info","process_info"),&Physics2DServer::get_process_info); + ClassDB::bind_method(D_METHOD("get_process_info", "process_info"), &Physics2DServer::get_process_info); //ClassDB::bind_method(D_METHOD("init"),&Physics2DServer::init); //ClassDB::bind_method(D_METHOD("step"),&Physics2DServer::step); //ClassDB::bind_method(D_METHOD("sync"),&Physics2DServer::sync); //ClassDB::bind_method(D_METHOD("flush_queries"),&Physics2DServer::flush_queries); - BIND_CONSTANT( SPACE_PARAM_CONTACT_RECYCLE_RADIUS ); - BIND_CONSTANT( SPACE_PARAM_CONTACT_MAX_SEPARATION ); - BIND_CONSTANT( SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION ); - BIND_CONSTANT( SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_TRESHOLD ); - BIND_CONSTANT( SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_TRESHOLD ); - BIND_CONSTANT( SPACE_PARAM_BODY_TIME_TO_SLEEP ); - BIND_CONSTANT( SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS ); + BIND_CONSTANT(SPACE_PARAM_CONTACT_RECYCLE_RADIUS); + BIND_CONSTANT(SPACE_PARAM_CONTACT_MAX_SEPARATION); + BIND_CONSTANT(SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION); + BIND_CONSTANT(SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_TRESHOLD); + BIND_CONSTANT(SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_TRESHOLD); + BIND_CONSTANT(SPACE_PARAM_BODY_TIME_TO_SLEEP); + BIND_CONSTANT(SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS); - BIND_CONSTANT( SHAPE_LINE ); - BIND_CONSTANT( SHAPE_SEGMENT ); - BIND_CONSTANT( SHAPE_CIRCLE ); - BIND_CONSTANT( SHAPE_RECTANGLE ); - BIND_CONSTANT( SHAPE_CAPSULE ); - BIND_CONSTANT( SHAPE_CONVEX_POLYGON ); - BIND_CONSTANT( SHAPE_CONCAVE_POLYGON ); - BIND_CONSTANT( SHAPE_CUSTOM ); + BIND_CONSTANT(SHAPE_LINE); + BIND_CONSTANT(SHAPE_SEGMENT); + BIND_CONSTANT(SHAPE_CIRCLE); + BIND_CONSTANT(SHAPE_RECTANGLE); + BIND_CONSTANT(SHAPE_CAPSULE); + BIND_CONSTANT(SHAPE_CONVEX_POLYGON); + BIND_CONSTANT(SHAPE_CONCAVE_POLYGON); + BIND_CONSTANT(SHAPE_CUSTOM); - BIND_CONSTANT( AREA_PARAM_GRAVITY ); - BIND_CONSTANT( AREA_PARAM_GRAVITY_VECTOR ); - BIND_CONSTANT( AREA_PARAM_GRAVITY_IS_POINT ); - BIND_CONSTANT( AREA_PARAM_GRAVITY_DISTANCE_SCALE ); - BIND_CONSTANT( AREA_PARAM_GRAVITY_POINT_ATTENUATION ); - BIND_CONSTANT( AREA_PARAM_LINEAR_DAMP); - BIND_CONSTANT( AREA_PARAM_ANGULAR_DAMP); - BIND_CONSTANT( AREA_PARAM_PRIORITY ); + BIND_CONSTANT(AREA_PARAM_GRAVITY); + BIND_CONSTANT(AREA_PARAM_GRAVITY_VECTOR); + BIND_CONSTANT(AREA_PARAM_GRAVITY_IS_POINT); + BIND_CONSTANT(AREA_PARAM_GRAVITY_DISTANCE_SCALE); + BIND_CONSTANT(AREA_PARAM_GRAVITY_POINT_ATTENUATION); + BIND_CONSTANT(AREA_PARAM_LINEAR_DAMP); + BIND_CONSTANT(AREA_PARAM_ANGULAR_DAMP); + BIND_CONSTANT(AREA_PARAM_PRIORITY); - BIND_CONSTANT( AREA_SPACE_OVERRIDE_DISABLED ); - BIND_CONSTANT( AREA_SPACE_OVERRIDE_COMBINE ); - BIND_CONSTANT( AREA_SPACE_OVERRIDE_COMBINE_REPLACE ); - BIND_CONSTANT( AREA_SPACE_OVERRIDE_REPLACE ); - BIND_CONSTANT( AREA_SPACE_OVERRIDE_REPLACE_COMBINE ); + BIND_CONSTANT(AREA_SPACE_OVERRIDE_DISABLED); + BIND_CONSTANT(AREA_SPACE_OVERRIDE_COMBINE); + BIND_CONSTANT(AREA_SPACE_OVERRIDE_COMBINE_REPLACE); + BIND_CONSTANT(AREA_SPACE_OVERRIDE_REPLACE); + BIND_CONSTANT(AREA_SPACE_OVERRIDE_REPLACE_COMBINE); - BIND_CONSTANT( BODY_MODE_STATIC ); - BIND_CONSTANT( BODY_MODE_KINEMATIC ); - BIND_CONSTANT( BODY_MODE_RIGID ); - BIND_CONSTANT( BODY_MODE_CHARACTER ); + BIND_CONSTANT(BODY_MODE_STATIC); + BIND_CONSTANT(BODY_MODE_KINEMATIC); + BIND_CONSTANT(BODY_MODE_RIGID); + BIND_CONSTANT(BODY_MODE_CHARACTER); - BIND_CONSTANT( BODY_PARAM_BOUNCE ); - BIND_CONSTANT( BODY_PARAM_FRICTION ); - BIND_CONSTANT( BODY_PARAM_MASS ); - BIND_CONSTANT( BODY_PARAM_INERTIA ); - BIND_CONSTANT( BODY_PARAM_GRAVITY_SCALE ); - BIND_CONSTANT( BODY_PARAM_LINEAR_DAMP); - BIND_CONSTANT( BODY_PARAM_ANGULAR_DAMP); - BIND_CONSTANT( BODY_PARAM_MAX ); + BIND_CONSTANT(BODY_PARAM_BOUNCE); + BIND_CONSTANT(BODY_PARAM_FRICTION); + BIND_CONSTANT(BODY_PARAM_MASS); + BIND_CONSTANT(BODY_PARAM_INERTIA); + BIND_CONSTANT(BODY_PARAM_GRAVITY_SCALE); + BIND_CONSTANT(BODY_PARAM_LINEAR_DAMP); + BIND_CONSTANT(BODY_PARAM_ANGULAR_DAMP); + BIND_CONSTANT(BODY_PARAM_MAX); - BIND_CONSTANT( BODY_STATE_TRANSFORM ); - BIND_CONSTANT( BODY_STATE_LINEAR_VELOCITY ); - BIND_CONSTANT( BODY_STATE_ANGULAR_VELOCITY ); - BIND_CONSTANT( BODY_STATE_SLEEPING ); - BIND_CONSTANT( BODY_STATE_CAN_SLEEP ); + BIND_CONSTANT(BODY_STATE_TRANSFORM); + BIND_CONSTANT(BODY_STATE_LINEAR_VELOCITY); + BIND_CONSTANT(BODY_STATE_ANGULAR_VELOCITY); + BIND_CONSTANT(BODY_STATE_SLEEPING); + BIND_CONSTANT(BODY_STATE_CAN_SLEEP); - BIND_CONSTANT( JOINT_PIN ); - BIND_CONSTANT( JOINT_GROOVE ); - BIND_CONSTANT( JOINT_DAMPED_SPRING ); + BIND_CONSTANT(JOINT_PIN); + BIND_CONSTANT(JOINT_GROOVE); + BIND_CONSTANT(JOINT_DAMPED_SPRING); - BIND_CONSTANT( DAMPED_STRING_REST_LENGTH ); - BIND_CONSTANT( DAMPED_STRING_STIFFNESS ); - BIND_CONSTANT( DAMPED_STRING_DAMPING ); + BIND_CONSTANT(DAMPED_STRING_REST_LENGTH); + BIND_CONSTANT(DAMPED_STRING_STIFFNESS); + BIND_CONSTANT(DAMPED_STRING_DAMPING); - BIND_CONSTANT( CCD_MODE_DISABLED ); - BIND_CONSTANT( CCD_MODE_CAST_RAY ); - BIND_CONSTANT( CCD_MODE_CAST_SHAPE ); + BIND_CONSTANT(CCD_MODE_DISABLED); + BIND_CONSTANT(CCD_MODE_CAST_RAY); + BIND_CONSTANT(CCD_MODE_CAST_SHAPE); //BIND_CONSTANT( TYPE_BODY ); //BIND_CONSTANT( TYPE_AREA ); - BIND_CONSTANT( AREA_BODY_ADDED ); - BIND_CONSTANT( AREA_BODY_REMOVED ); - - BIND_CONSTANT( INFO_ACTIVE_OBJECTS ); - BIND_CONSTANT( INFO_COLLISION_PAIRS ); - BIND_CONSTANT( INFO_ISLAND_COUNT ); - + BIND_CONSTANT(AREA_BODY_ADDED); + BIND_CONSTANT(AREA_BODY_REMOVED); + BIND_CONSTANT(INFO_ACTIVE_OBJECTS); + BIND_CONSTANT(INFO_COLLISION_PAIRS); + BIND_CONSTANT(INFO_ISLAND_COUNT); } - Physics2DServer::Physics2DServer() { //ERR_FAIL_COND( singleton!=NULL ); - singleton=this; + singleton = this; } Physics2DServer::~Physics2DServer() { - singleton=NULL; + singleton = NULL; } - diff --git a/servers/physics_2d_server.h b/servers/physics_2d_server.h index 424d2fa7c..3c64d3df6 100644 --- a/servers/physics_2d_server.h +++ b/servers/physics_2d_server.h @@ -37,61 +37,60 @@ class Physics2DDirectSpaceState; class Physics2DDirectBodyState : public Object { - GDCLASS( Physics2DDirectBodyState, Object ); + GDCLASS(Physics2DDirectBodyState, Object); + protected: static void _bind_methods(); -public: - virtual Vector2 get_total_gravity() const=0; // get gravity vector working on this body space/area - virtual float get_total_linear_damp() const=0; // get density of this body space/area - virtual float get_total_angular_damp() const=0; // get density of this body space/area +public: + virtual Vector2 get_total_gravity() const = 0; // get gravity vector working on this body space/area + virtual float get_total_linear_damp() const = 0; // get density of this body space/area + virtual float get_total_angular_damp() const = 0; // get density of this body space/area - virtual float get_inverse_mass() const=0; // get the mass - virtual real_t get_inverse_inertia() const=0; // get density of this body space + virtual float get_inverse_mass() const = 0; // get the mass + virtual real_t get_inverse_inertia() const = 0; // get density of this body space - virtual void set_linear_velocity(const Vector2& p_velocity)=0; - virtual Vector2 get_linear_velocity() const=0; + virtual void set_linear_velocity(const Vector2 &p_velocity) = 0; + virtual Vector2 get_linear_velocity() const = 0; - virtual void set_angular_velocity(real_t p_velocity)=0; - virtual real_t get_angular_velocity() const=0; + virtual void set_angular_velocity(real_t p_velocity) = 0; + virtual real_t get_angular_velocity() const = 0; - virtual void set_transform(const Transform2D& p_transform)=0; - virtual Transform2D get_transform() const=0; + virtual void set_transform(const Transform2D &p_transform) = 0; + virtual Transform2D get_transform() const = 0; - virtual void set_sleep_state(bool p_enable)=0; - virtual bool is_sleeping() const=0; + virtual void set_sleep_state(bool p_enable) = 0; + virtual bool is_sleeping() const = 0; - virtual int get_contact_count() const=0; + virtual int get_contact_count() const = 0; - virtual Vector2 get_contact_local_pos(int p_contact_idx) const=0; - virtual Vector2 get_contact_local_normal(int p_contact_idx) const=0; - virtual int get_contact_local_shape(int p_contact_idx) const=0; + virtual Vector2 get_contact_local_pos(int p_contact_idx) const = 0; + virtual Vector2 get_contact_local_normal(int p_contact_idx) const = 0; + virtual int get_contact_local_shape(int p_contact_idx) const = 0; - virtual RID get_contact_collider(int p_contact_idx) const=0; - virtual Vector2 get_contact_collider_pos(int p_contact_idx) const=0; - virtual ObjectID get_contact_collider_id(int p_contact_idx) const=0; - virtual Object* get_contact_collider_object(int p_contact_idx) const; - virtual int get_contact_collider_shape(int p_contact_idx) const=0; - virtual Variant get_contact_collider_shape_metadata(int p_contact_idx) const=0; - virtual Vector2 get_contact_collider_velocity_at_pos(int p_contact_idx) const=0; + virtual RID get_contact_collider(int p_contact_idx) const = 0; + virtual Vector2 get_contact_collider_pos(int p_contact_idx) const = 0; + virtual ObjectID get_contact_collider_id(int p_contact_idx) const = 0; + virtual Object *get_contact_collider_object(int p_contact_idx) const; + virtual int get_contact_collider_shape(int p_contact_idx) const = 0; + virtual Variant get_contact_collider_shape_metadata(int p_contact_idx) const = 0; + virtual Vector2 get_contact_collider_velocity_at_pos(int p_contact_idx) const = 0; - virtual real_t get_step() const=0; + virtual real_t get_step() const = 0; virtual void integrate_forces(); - virtual Physics2DDirectSpaceState* get_space_state()=0; + virtual Physics2DDirectSpaceState *get_space_state() = 0; Physics2DDirectBodyState(); }; - class Physics2DShapeQueryResult; - //used for script class Physics2DShapeQueryParameters : public Reference { GDCLASS(Physics2DShapeQueryParameters, Reference); -friend class Physics2DDirectSpaceState; + friend class Physics2DDirectSpaceState; RID shape; Transform2D transform; Vector2 motion; @@ -99,19 +98,19 @@ friend class Physics2DDirectSpaceState; Set<RID> exclude; uint32_t layer_mask; uint32_t object_type_mask; + protected: static void _bind_methods(); -public: - - void set_shape(const RES& p_shape); - void set_shape_rid(const RID& p_shape); +public: + void set_shape(const RES &p_shape); + void set_shape_rid(const RID &p_shape); RID get_shape_rid() const; - void set_transform(const Transform2D& p_transform); + void set_transform(const Transform2D &p_transform); Transform2D get_transform() const; - void set_motion(const Vector2& p_motion); + void set_motion(const Vector2 &p_motion); Vector2 get_motion() const; void set_margin(float p_margin); @@ -123,38 +122,35 @@ public: void set_object_type_mask(int p_object_type_mask); int get_object_type_mask() const; - void set_exclude(const Vector<RID>& p_exclude); + void set_exclude(const Vector<RID> &p_exclude); Vector<RID> get_exclude() const; Physics2DShapeQueryParameters(); - }; - class Physics2DDirectSpaceState : public Object { - GDCLASS( Physics2DDirectSpaceState, Object ); + GDCLASS(Physics2DDirectSpaceState, Object); - Dictionary _intersect_ray(const Vector2& p_from, const Vector2& p_to,const Vector<RID>& p_exclude=Vector<RID>(),uint32_t p_layers=0,uint32_t p_object_type_mask=TYPE_MASK_COLLISION); + Dictionary _intersect_ray(const Vector2 &p_from, const Vector2 &p_to, const Vector<RID> &p_exclude = Vector<RID>(), uint32_t p_layers = 0, uint32_t p_object_type_mask = TYPE_MASK_COLLISION); - Array _intersect_point(const Vector2& p_point,int p_max_results=32,const Vector<RID>& p_exclude=Vector<RID>(),uint32_t p_layers=0,uint32_t p_object_type_mask=TYPE_MASK_COLLISION); - Array _intersect_shape(const Ref<Physics2DShapeQueryParameters> &p_shape_query,int p_max_results=32); + Array _intersect_point(const Vector2 &p_point, int p_max_results = 32, const Vector<RID> &p_exclude = Vector<RID>(), uint32_t p_layers = 0, uint32_t p_object_type_mask = TYPE_MASK_COLLISION); + Array _intersect_shape(const Ref<Physics2DShapeQueryParameters> &p_shape_query, int p_max_results = 32); Array _cast_motion(const Ref<Physics2DShapeQueryParameters> &p_shape_query); - Array _collide_shape(const Ref<Physics2DShapeQueryParameters> &p_shape_query,int p_max_results=32); + Array _collide_shape(const Ref<Physics2DShapeQueryParameters> &p_shape_query, int p_max_results = 32); Dictionary _get_rest_info(const Ref<Physics2DShapeQueryParameters> &p_shape_query); protected: static void _bind_methods(); public: - enum ObjectTypeMask { - TYPE_MASK_STATIC_BODY=1<<0, - TYPE_MASK_KINEMATIC_BODY=1<<1, - TYPE_MASK_RIGID_BODY=1<<2, - TYPE_MASK_CHARACTER_BODY=1<<3, - TYPE_MASK_AREA=1<<4, - TYPE_MASK_COLLISION=TYPE_MASK_STATIC_BODY|TYPE_MASK_CHARACTER_BODY|TYPE_MASK_KINEMATIC_BODY|TYPE_MASK_RIGID_BODY + TYPE_MASK_STATIC_BODY = 1 << 0, + TYPE_MASK_KINEMATIC_BODY = 1 << 1, + TYPE_MASK_RIGID_BODY = 1 << 2, + TYPE_MASK_CHARACTER_BODY = 1 << 3, + TYPE_MASK_AREA = 1 << 4, + TYPE_MASK_COLLISION = TYPE_MASK_STATIC_BODY | TYPE_MASK_CHARACTER_BODY | TYPE_MASK_KINEMATIC_BODY | TYPE_MASK_RIGID_BODY }; @@ -169,7 +165,7 @@ public: Variant metadata; }; - 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)=0; + 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) = 0; struct ShapeResult { @@ -178,17 +174,15 @@ public: Object *collider; int shape; Variant metadata; - - }; - 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)=0; + 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) = 0; - virtual int intersect_shape(const RID& p_shape, const Transform2D& p_xform,const Vector2& p_motion,float 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)=0; + virtual int intersect_shape(const RID &p_shape, const Transform2D &p_xform, const Vector2 &p_motion, float 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) = 0; - virtual bool cast_motion(const RID& p_shape, const Transform2D& p_xform,const Vector2& p_motion,float p_margin,float &p_closest_safe,float &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)=0; + virtual bool cast_motion(const RID &p_shape, const Transform2D &p_xform, const Vector2 &p_motion, float p_margin, float &p_closest_safe, float &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) = 0; - virtual bool collide_shape(RID p_shape, const Transform2D& p_shape_xform,const Vector2& p_motion,float 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)=0; + virtual bool collide_shape(RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, float 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) = 0; struct ShapeRestInfo { @@ -199,32 +193,29 @@ public: int shape; Vector2 linear_velocity; //velocity at contact point Variant metadata; - }; - virtual bool rest_info(RID p_shape, const Transform2D& p_shape_xform,const Vector2& p_motion,float 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)=0; - + virtual bool rest_info(RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, float 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) = 0; Physics2DDirectSpaceState(); }; - class Physics2DShapeQueryResult : public Reference { - GDCLASS( Physics2DShapeQueryResult, Reference ); + GDCLASS(Physics2DShapeQueryResult, Reference); Vector<Physics2DDirectSpaceState::ShapeResult> result; -friend class Physics2DDirectSpaceState; + friend class Physics2DDirectSpaceState; protected: static void _bind_methods(); -public: +public: int get_result_count() const; RID get_result_rid(int p_idx) const; ObjectID get_result_object_id(int p_idx) const; - Object* get_result_object(int p_idx) const; + Object *get_result_object(int p_idx) const; int get_result_object_shape(int p_idx) const; Physics2DShapeQueryResult(); @@ -234,18 +225,17 @@ class Physics2DTestMotionResult; class Physics2DServer : public Object { - GDCLASS( Physics2DServer, Object ); + GDCLASS(Physics2DServer, Object); - static Physics2DServer * singleton; + static Physics2DServer *singleton; - virtual bool _body_test_motion(RID p_body, const Transform2D &p_from, const Vector2& p_motion, float p_margin=0.08, const Ref<Physics2DTestMotionResult>& p_result=Ref<Physics2DTestMotionResult>()); + virtual bool _body_test_motion(RID p_body, const Transform2D &p_from, const Vector2 &p_motion, float p_margin = 0.08, const Ref<Physics2DTestMotionResult> &p_result = Ref<Physics2DTestMotionResult>()); protected: static void _bind_methods(); public: - - static Physics2DServer * get_singleton(); + static Physics2DServer *get_singleton(); enum ShapeType { SHAPE_LINE, ///< plane:"plane" @@ -259,22 +249,22 @@ public: SHAPE_CUSTOM, ///< Server-Implementation based custom shape, calling shape_create() with this value will result in an error }; - virtual RID shape_create(ShapeType p_shape)=0; - virtual void shape_set_data(RID p_shape, const Variant& p_data)=0; - virtual void shape_set_custom_solver_bias(RID p_shape, real_t p_bias)=0; + virtual RID shape_create(ShapeType p_shape) = 0; + virtual void shape_set_data(RID p_shape, const Variant &p_data) = 0; + virtual void shape_set_custom_solver_bias(RID p_shape, real_t p_bias) = 0; - virtual ShapeType shape_get_type(RID p_shape) const=0; - virtual Variant shape_get_data(RID p_shape) const=0; - virtual real_t shape_get_custom_solver_bias(RID p_shape) const=0; + virtual ShapeType shape_get_type(RID p_shape) const = 0; + virtual Variant shape_get_data(RID p_shape) const = 0; + virtual real_t shape_get_custom_solver_bias(RID p_shape) const = 0; //these work well, but should be used from the main thread only - 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)=0; + 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) = 0; /* SPACE API */ - virtual RID space_create()=0; - virtual void space_set_active(RID p_space,bool p_active)=0; - virtual bool space_is_active(RID p_space) const=0; + virtual RID space_create() = 0; + virtual void space_set_active(RID p_space, bool p_active) = 0; + virtual bool space_is_active(RID p_space) const = 0; enum SpaceParameter { @@ -287,15 +277,15 @@ public: SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS, }; - virtual void space_set_param(RID p_space,SpaceParameter p_param, real_t p_value)=0; - virtual real_t space_get_param(RID p_space,SpaceParameter p_param) const=0; + virtual void space_set_param(RID p_space, SpaceParameter p_param, real_t p_value) = 0; + virtual real_t space_get_param(RID p_space, SpaceParameter p_param) const = 0; // this function only works on fixed process, errors and returns null otherwise - virtual Physics2DDirectSpaceState* space_get_direct_state(RID p_space)=0; + virtual Physics2DDirectSpaceState *space_get_direct_state(RID p_space) = 0; - virtual void space_set_debug_contacts(RID p_space,int p_max_contacts)=0; - virtual Vector<Vector2> space_get_contacts(RID p_space) const=0; - virtual int space_get_contact_count(RID p_space) const=0; + virtual void space_set_debug_contacts(RID p_space, int p_max_contacts) = 0; + virtual Vector<Vector2> space_get_contacts(RID p_space) const = 0; + virtual int space_get_contact_count(RID p_space) const = 0; //missing space parameters @@ -303,8 +293,6 @@ public: //missing attenuation? missing better override? - - enum AreaParameter { AREA_PARAM_GRAVITY, AREA_PARAM_GRAVITY_VECTOR, @@ -316,11 +304,10 @@ public: AREA_PARAM_PRIORITY }; - virtual RID area_create()=0; - - virtual void area_set_space(RID p_area, RID p_space)=0; - virtual RID area_get_space(RID p_area) const=0; + virtual RID area_create() = 0; + virtual void area_set_space(RID p_area, RID p_space) = 0; + virtual RID area_get_space(RID p_area) const = 0; enum AreaSpaceOverrideMode { AREA_SPACE_OVERRIDE_DISABLED, @@ -330,37 +317,37 @@ public: AREA_SPACE_OVERRIDE_REPLACE_COMBINE // Discards all previous calculations, then keeps combining }; - virtual void area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode)=0; - virtual AreaSpaceOverrideMode area_get_space_override_mode(RID p_area) const=0; + virtual void area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode) = 0; + virtual AreaSpaceOverrideMode area_get_space_override_mode(RID p_area) const = 0; - virtual void area_add_shape(RID p_area, RID p_shape, const Transform2D& p_transform=Transform2D())=0; - virtual void area_set_shape(RID p_area, int p_shape_idx,RID p_shape)=0; - virtual void area_set_shape_transform(RID p_area, int p_shape_idx, const Transform2D& p_transform)=0; + virtual void area_add_shape(RID p_area, RID p_shape, const Transform2D &p_transform = Transform2D()) = 0; + virtual void area_set_shape(RID p_area, int p_shape_idx, RID p_shape) = 0; + virtual void area_set_shape_transform(RID p_area, int p_shape_idx, const Transform2D &p_transform) = 0; - virtual int area_get_shape_count(RID p_area) const=0; - virtual RID area_get_shape(RID p_area, int p_shape_idx) const=0; - virtual Transform2D area_get_shape_transform(RID p_area, int p_shape_idx) const=0; + virtual int area_get_shape_count(RID p_area) const = 0; + virtual RID area_get_shape(RID p_area, int p_shape_idx) const = 0; + virtual Transform2D area_get_shape_transform(RID p_area, int p_shape_idx) const = 0; - virtual void area_remove_shape(RID p_area, int p_shape_idx)=0; - virtual void area_clear_shapes(RID p_area)=0; + virtual void area_remove_shape(RID p_area, int p_shape_idx) = 0; + virtual void area_clear_shapes(RID p_area) = 0; - virtual void area_attach_object_instance_ID(RID p_area,ObjectID p_ID)=0; - virtual ObjectID area_get_object_instance_ID(RID p_area) const=0; + virtual void area_attach_object_instance_ID(RID p_area, ObjectID p_ID) = 0; + virtual ObjectID area_get_object_instance_ID(RID p_area) const = 0; - virtual void area_set_param(RID p_area,AreaParameter p_param,const Variant& p_value)=0; - virtual void area_set_transform(RID p_area, const Transform2D& p_transform)=0; + virtual void area_set_param(RID p_area, AreaParameter p_param, const Variant &p_value) = 0; + virtual void area_set_transform(RID p_area, const Transform2D &p_transform) = 0; - virtual Variant area_get_param(RID p_parea,AreaParameter p_param) const=0; - virtual Transform2D area_get_transform(RID p_area) const=0; + virtual Variant area_get_param(RID p_parea, AreaParameter p_param) const = 0; + virtual Transform2D area_get_transform(RID p_area) const = 0; - virtual void area_set_collision_mask(RID p_area,uint32_t p_mask)=0; - virtual void area_set_layer_mask(RID p_area,uint32_t p_mask)=0; + virtual void area_set_collision_mask(RID p_area, uint32_t p_mask) = 0; + virtual void area_set_layer_mask(RID p_area, uint32_t p_mask) = 0; - virtual void area_set_monitorable(RID p_area,bool p_monitorable)=0; - virtual void area_set_pickable(RID p_area,bool p_pickable)=0; + virtual void area_set_monitorable(RID p_area, bool p_monitorable) = 0; + virtual void area_set_pickable(RID p_area, bool p_pickable) = 0; - virtual void area_set_monitor_callback(RID p_area,Object *p_receiver,const StringName& p_method)=0; - virtual void area_set_area_monitor_callback(RID p_area,Object *p_receiver,const StringName& p_method)=0; + virtual void area_set_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) = 0; + virtual void area_set_area_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) = 0; /* BODY API */ @@ -374,32 +361,32 @@ public: //BODY_MODE_SOFT ?? }; - virtual RID body_create(BodyMode p_mode=BODY_MODE_RIGID,bool p_init_sleeping=false)=0; + virtual RID body_create(BodyMode p_mode = BODY_MODE_RIGID, bool p_init_sleeping = false) = 0; - virtual void body_set_space(RID p_body, RID p_space)=0; - virtual RID body_get_space(RID p_body) const=0; + virtual void body_set_space(RID p_body, RID p_space) = 0; + virtual RID body_get_space(RID p_body) const = 0; - virtual void body_set_mode(RID p_body, BodyMode p_mode)=0; - virtual BodyMode body_get_mode(RID p_body) const=0; + virtual void body_set_mode(RID p_body, BodyMode p_mode) = 0; + virtual BodyMode body_get_mode(RID p_body) const = 0; - virtual void body_add_shape(RID p_body, RID p_shape, const Transform2D& p_transform=Transform2D())=0; - virtual void body_set_shape(RID p_body, int p_shape_idx,RID p_shape)=0; - virtual void body_set_shape_transform(RID p_body, int p_shape_idx, const Transform2D& p_transform)=0; - virtual void body_set_shape_metadata(RID p_body, int p_shape_idx, const Variant& p_metadata)=0; + virtual void body_add_shape(RID p_body, RID p_shape, const Transform2D &p_transform = Transform2D()) = 0; + virtual void body_set_shape(RID p_body, int p_shape_idx, RID p_shape) = 0; + virtual void body_set_shape_transform(RID p_body, int p_shape_idx, const Transform2D &p_transform) = 0; + virtual void body_set_shape_metadata(RID p_body, int p_shape_idx, const Variant &p_metadata) = 0; - virtual int body_get_shape_count(RID p_body) const=0; - virtual RID body_get_shape(RID p_body, int p_shape_idx) const=0; - virtual Transform2D body_get_shape_transform(RID p_body, int p_shape_idx) const=0; - virtual Variant body_get_shape_metadata(RID p_body, int p_shape_idx) const=0; + virtual int body_get_shape_count(RID p_body) const = 0; + virtual RID body_get_shape(RID p_body, int p_shape_idx) const = 0; + virtual Transform2D body_get_shape_transform(RID p_body, int p_shape_idx) const = 0; + virtual Variant body_get_shape_metadata(RID p_body, int p_shape_idx) const = 0; - virtual void body_set_shape_as_trigger(RID p_body, int p_shape_idx,bool p_enable)=0; - virtual bool body_is_shape_set_as_trigger(RID p_body, int p_shape_idx) const=0; + virtual void body_set_shape_as_trigger(RID p_body, int p_shape_idx, bool p_enable) = 0; + virtual bool body_is_shape_set_as_trigger(RID p_body, int p_shape_idx) const = 0; - virtual void body_remove_shape(RID p_body, int p_shape_idx)=0; - virtual void body_clear_shapes(RID p_body)=0; + virtual void body_remove_shape(RID p_body, int p_shape_idx) = 0; + virtual void body_clear_shapes(RID p_body) = 0; - virtual void body_attach_object_instance_ID(RID p_body,uint32_t p_ID)=0; - virtual uint32_t body_get_object_instance_ID(RID p_body) const=0; + virtual void body_attach_object_instance_ID(RID p_body, uint32_t p_ID) = 0; + virtual uint32_t body_get_object_instance_ID(RID p_body) const = 0; enum CCDMode { CCD_MODE_DISABLED, @@ -407,30 +394,29 @@ public: CCD_MODE_CAST_SHAPE, }; - virtual void body_set_continuous_collision_detection_mode(RID p_body,CCDMode p_mode)=0; - virtual CCDMode body_get_continuous_collision_detection_mode(RID p_body) const=0; + virtual void body_set_continuous_collision_detection_mode(RID p_body, CCDMode p_mode) = 0; + virtual CCDMode body_get_continuous_collision_detection_mode(RID p_body) const = 0; - virtual void body_set_layer_mask(RID p_body, uint32_t p_mask)=0; - virtual uint32_t body_get_layer_mask(RID p_body) const=0; + virtual void body_set_layer_mask(RID p_body, uint32_t p_mask) = 0; + virtual uint32_t body_get_layer_mask(RID p_body) const = 0; - virtual void body_set_collision_mask(RID p_body, uint32_t p_mask)=0; - virtual uint32_t body_get_collision_mask(RID p_body) const=0; + virtual void body_set_collision_mask(RID p_body, uint32_t p_mask) = 0; + virtual uint32_t body_get_collision_mask(RID p_body) const = 0; // common body variables enum BodyParameter { BODY_PARAM_BOUNCE, BODY_PARAM_FRICTION, BODY_PARAM_MASS, ///< unused for static, always infinite - BODY_PARAM_INERTIA, // read-only: computed from mass & shapes + BODY_PARAM_INERTIA, // read-only: computed from mass & shapes BODY_PARAM_GRAVITY_SCALE, BODY_PARAM_LINEAR_DAMP, BODY_PARAM_ANGULAR_DAMP, BODY_PARAM_MAX, }; - virtual void body_set_param(RID p_body, BodyParameter p_param, float p_value)=0; - virtual float body_get_param(RID p_body, BodyParameter p_param) const=0; - + virtual void body_set_param(RID p_body, BodyParameter p_param, float p_value) = 0; + virtual float body_get_param(RID p_body, BodyParameter p_param) const = 0; //state enum BodyState { @@ -441,47 +427,47 @@ public: BODY_STATE_CAN_SLEEP, }; - virtual void body_set_state(RID p_body, BodyState p_state, const Variant& p_variant)=0; - virtual Variant body_get_state(RID p_body, BodyState p_state) const=0; + virtual void body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) = 0; + virtual Variant body_get_state(RID p_body, BodyState p_state) const = 0; //do something about it - virtual void body_set_applied_force(RID p_body, const Vector2& p_force)=0; - virtual Vector2 body_get_applied_force(RID p_body) const=0; + virtual void body_set_applied_force(RID p_body, const Vector2 &p_force) = 0; + virtual Vector2 body_get_applied_force(RID p_body) const = 0; - virtual void body_set_applied_torque(RID p_body, float p_torque)=0; - virtual float body_get_applied_torque(RID p_body) const=0; + virtual void body_set_applied_torque(RID p_body, float p_torque) = 0; + virtual float body_get_applied_torque(RID p_body) const = 0; - virtual void body_add_force(RID p_body, const Vector2& p_offset, const Vector2& p_force)=0; + virtual void body_add_force(RID p_body, const Vector2 &p_offset, const Vector2 &p_force) = 0; - virtual void body_apply_impulse(RID p_body, const Vector2& p_offset, const Vector2& p_impulse)=0; - virtual void body_set_axis_velocity(RID p_body, const Vector2& p_axis_velocity)=0; + virtual void body_apply_impulse(RID p_body, const Vector2 &p_offset, const Vector2 &p_impulse) = 0; + virtual void body_set_axis_velocity(RID p_body, const Vector2 &p_axis_velocity) = 0; //fix - virtual void body_add_collision_exception(RID p_body, RID p_body_b)=0; - virtual void body_remove_collision_exception(RID p_body, RID p_body_b)=0; - virtual void body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions)=0; + virtual void body_add_collision_exception(RID p_body, RID p_body_b) = 0; + virtual void body_remove_collision_exception(RID p_body, RID p_body_b) = 0; + virtual void body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) = 0; - virtual void body_set_max_contacts_reported(RID p_body, int p_contacts)=0; - virtual int body_get_max_contacts_reported(RID p_body) const=0; + virtual void body_set_max_contacts_reported(RID p_body, int p_contacts) = 0; + virtual int body_get_max_contacts_reported(RID p_body) const = 0; - virtual void body_set_one_way_collision_direction(RID p_body,const Vector2& p_direction)=0; - virtual Vector2 body_get_one_way_collision_direction(RID p_body) const=0; + virtual void body_set_one_way_collision_direction(RID p_body, const Vector2 &p_direction) = 0; + virtual Vector2 body_get_one_way_collision_direction(RID p_body) const = 0; - virtual void body_set_one_way_collision_max_depth(RID p_body,float p_max_depth)=0; - virtual float body_get_one_way_collision_max_depth(RID p_body) const=0; + virtual void body_set_one_way_collision_max_depth(RID p_body, float p_max_depth) = 0; + virtual float body_get_one_way_collision_max_depth(RID p_body) const = 0; //missing remove - virtual void body_set_contacts_reported_depth_treshold(RID p_body, float p_treshold)=0; - virtual float body_get_contacts_reported_depth_treshold(RID p_body) const=0; + virtual void body_set_contacts_reported_depth_treshold(RID p_body, float p_treshold) = 0; + virtual float body_get_contacts_reported_depth_treshold(RID p_body) const = 0; - virtual void body_set_omit_force_integration(RID p_body,bool p_omit)=0; - virtual bool body_is_omitting_force_integration(RID p_body) const=0; + virtual void body_set_omit_force_integration(RID p_body, bool p_omit) = 0; + virtual bool body_is_omitting_force_integration(RID p_body) const = 0; - virtual void body_set_force_integration_callback(RID p_body,Object *p_receiver,const StringName& p_method,const Variant& p_udata=Variant())=0; + virtual void body_set_force_integration_callback(RID p_body, Object *p_receiver, const StringName &p_method, const Variant &p_udata = Variant()) = 0; - 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)=0; + 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) = 0; - virtual void body_set_pickable(RID p_body,bool p_pickable)=0; + virtual void body_set_pickable(RID p_body, bool p_pickable) = 0; struct MotionResult { @@ -497,7 +483,7 @@ public: Variant collider_metadata; }; - virtual bool body_test_motion(RID p_body,const Transform2D& p_from,const Vector2& p_motion,float p_margin=0.001,MotionResult *r_result=NULL)=0; + virtual bool body_test_motion(RID p_body, const Transform2D &p_from, const Vector2 &p_motion, float p_margin = 0.001, MotionResult *r_result = NULL) = 0; /* JOINT API */ @@ -514,29 +500,29 @@ public: JOINT_PARAM_MAX_FORCE, }; - virtual void joint_set_param(RID p_joint, JointParam p_param, real_t p_value)=0; - virtual real_t joint_get_param(RID p_joint,JointParam p_param) const=0; + virtual void joint_set_param(RID p_joint, JointParam p_param, real_t p_value) = 0; + virtual real_t joint_get_param(RID p_joint, JointParam p_param) const = 0; - virtual RID pin_joint_create(const Vector2& p_anchor,RID p_body_a,RID p_body_b=RID())=0; - 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)=0; - 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())=0; + virtual RID pin_joint_create(const Vector2 &p_anchor, RID p_body_a, RID p_body_b = RID()) = 0; + 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) = 0; + 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()) = 0; enum PinJointParam { PIN_JOINT_SOFTNESS }; - virtual void pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value)=0; - virtual real_t pin_joint_get_param(RID p_joint, PinJointParam p_param) const=0; + virtual void pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value) = 0; + virtual real_t pin_joint_get_param(RID p_joint, PinJointParam p_param) const = 0; enum DampedStringParam { DAMPED_STRING_REST_LENGTH, DAMPED_STRING_STIFFNESS, DAMPED_STRING_DAMPING }; - virtual void damped_string_joint_set_param(RID p_joint, DampedStringParam p_param, real_t p_value)=0; - virtual real_t damped_string_joint_get_param(RID p_joint, DampedStringParam p_param) const=0; + virtual void damped_string_joint_set_param(RID p_joint, DampedStringParam p_param, real_t p_value) = 0; + virtual real_t damped_string_joint_get_param(RID p_joint, DampedStringParam p_param) const = 0; - virtual JointType joint_get_type(RID p_joint) const=0; + virtual JointType joint_get_type(RID p_joint) const = 0; /* QUERY API */ @@ -545,18 +531,17 @@ public: AREA_BODY_REMOVED }; - /* MISC */ - virtual void free(RID p_rid)=0; + virtual void free(RID p_rid) = 0; - virtual void set_active(bool p_active)=0; - virtual void init()=0; - virtual void step(float p_step)=0; - virtual void sync()=0; - virtual void flush_queries()=0; - virtual void end_sync()=0; - virtual void finish()=0; + virtual void set_active(bool p_active) = 0; + virtual void init() = 0; + virtual void step(float p_step) = 0; + virtual void sync() = 0; + virtual void flush_queries() = 0; + virtual void end_sync() = 0; + virtual void finish() = 0; enum ProcessInfo { @@ -567,26 +552,25 @@ public: INFO_BROAD_PHASE_TIME }; - virtual int get_process_info(ProcessInfo p_info)=0; + virtual int get_process_info(ProcessInfo p_info) = 0; Physics2DServer(); ~Physics2DServer(); }; - class Physics2DTestMotionResult : public Reference { - GDCLASS( Physics2DTestMotionResult, Reference ); + GDCLASS(Physics2DTestMotionResult, Reference); Physics2DServer::MotionResult result; bool colliding; -friend class Physics2DServer; + friend class Physics2DServer; protected: static void _bind_methods(); -public: - Physics2DServer::MotionResult* get_result_ptr() const { return const_cast<Physics2DServer::MotionResult*>(&result); } +public: + Physics2DServer::MotionResult *get_result_ptr() const { return const_cast<Physics2DServer::MotionResult *>(&result); } //bool is_colliding() const; Vector2 get_motion() const; @@ -597,26 +581,25 @@ public: Vector2 get_collider_velocity() const; ObjectID get_collider_id() const; RID get_collider_rid() const; - Object* get_collider() const; + Object *get_collider() const; int get_collider_shape() const; Physics2DTestMotionResult(); }; - -VARIANT_ENUM_CAST( Physics2DServer::ShapeType ); -VARIANT_ENUM_CAST( Physics2DServer::SpaceParameter ); -VARIANT_ENUM_CAST( Physics2DServer::AreaParameter ); -VARIANT_ENUM_CAST( Physics2DServer::AreaSpaceOverrideMode ); -VARIANT_ENUM_CAST( Physics2DServer::BodyMode ); -VARIANT_ENUM_CAST( Physics2DServer::BodyParameter ); -VARIANT_ENUM_CAST( Physics2DServer::BodyState ); -VARIANT_ENUM_CAST( Physics2DServer::CCDMode ); -VARIANT_ENUM_CAST( Physics2DServer::JointParam ); -VARIANT_ENUM_CAST( Physics2DServer::JointType ); -VARIANT_ENUM_CAST( Physics2DServer::DampedStringParam ); +VARIANT_ENUM_CAST(Physics2DServer::ShapeType); +VARIANT_ENUM_CAST(Physics2DServer::SpaceParameter); +VARIANT_ENUM_CAST(Physics2DServer::AreaParameter); +VARIANT_ENUM_CAST(Physics2DServer::AreaSpaceOverrideMode); +VARIANT_ENUM_CAST(Physics2DServer::BodyMode); +VARIANT_ENUM_CAST(Physics2DServer::BodyParameter); +VARIANT_ENUM_CAST(Physics2DServer::BodyState); +VARIANT_ENUM_CAST(Physics2DServer::CCDMode); +VARIANT_ENUM_CAST(Physics2DServer::JointParam); +VARIANT_ENUM_CAST(Physics2DServer::JointType); +VARIANT_ENUM_CAST(Physics2DServer::DampedStringParam); //VARIANT_ENUM_CAST( Physics2DServer::ObjectType ); -VARIANT_ENUM_CAST( Physics2DServer::AreaBodyStatus ); -VARIANT_ENUM_CAST( Physics2DServer::ProcessInfo ); +VARIANT_ENUM_CAST(Physics2DServer::AreaBodyStatus); +VARIANT_ENUM_CAST(Physics2DServer::ProcessInfo); #endif diff --git a/servers/physics_server.cpp b/servers/physics_server.cpp index e76bfd113..d8f77fbe0 100644 --- a/servers/physics_server.cpp +++ b/servers/physics_server.cpp @@ -28,110 +28,102 @@ /*************************************************************************/ #include "physics_server.h" #include "print_string.h" -PhysicsServer * PhysicsServer::singleton=NULL; - +PhysicsServer *PhysicsServer::singleton = NULL; void PhysicsDirectBodyState::integrate_forces() { real_t step = get_step(); Vector3 lv = get_linear_velocity(); - lv+=get_total_gravity() * step; + lv += get_total_gravity() * step; Vector3 av = get_angular_velocity(); float linear_damp = 1.0 - step * get_total_linear_damp(); - if (linear_damp<0) // reached zero in the given time - linear_damp=0; + if (linear_damp < 0) // reached zero in the given time + linear_damp = 0; float angular_damp = 1.0 - step * get_total_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; - lv*=linear_damp; - av*=angular_damp; + lv *= linear_damp; + av *= angular_damp; set_linear_velocity(lv); set_angular_velocity(av); - - - - } -Object* PhysicsDirectBodyState::get_contact_collider_object(int p_contact_idx) const { +Object *PhysicsDirectBodyState::get_contact_collider_object(int p_contact_idx) const { ObjectID objid = get_contact_collider_id(p_contact_idx); - Object *obj = ObjectDB::get_instance( objid ); + Object *obj = ObjectDB::get_instance(objid); return obj; } -PhysicsServer * PhysicsServer::get_singleton() { +PhysicsServer *PhysicsServer::get_singleton() { return singleton; } void PhysicsDirectBodyState::_bind_methods() { - ClassDB::bind_method(D_METHOD("get_total_gravity"),&PhysicsDirectBodyState::get_total_gravity); - ClassDB::bind_method(D_METHOD("get_total_linear_damp"),&PhysicsDirectBodyState::get_total_linear_damp); - ClassDB::bind_method(D_METHOD("get_total_angular_damp"),&PhysicsDirectBodyState::get_total_angular_damp); - - ClassDB::bind_method(D_METHOD("get_center_of_mass"),&PhysicsDirectBodyState::get_center_of_mass); - ClassDB::bind_method(D_METHOD("get_principal_inetria_axes"),&PhysicsDirectBodyState::get_principal_inertia_axes); + ClassDB::bind_method(D_METHOD("get_total_gravity"), &PhysicsDirectBodyState::get_total_gravity); + ClassDB::bind_method(D_METHOD("get_total_linear_damp"), &PhysicsDirectBodyState::get_total_linear_damp); + ClassDB::bind_method(D_METHOD("get_total_angular_damp"), &PhysicsDirectBodyState::get_total_angular_damp); - ClassDB::bind_method(D_METHOD("get_inverse_mass"),&PhysicsDirectBodyState::get_inverse_mass); - ClassDB::bind_method(D_METHOD("get_inverse_inertia"),&PhysicsDirectBodyState::get_inverse_inertia); + ClassDB::bind_method(D_METHOD("get_center_of_mass"), &PhysicsDirectBodyState::get_center_of_mass); + ClassDB::bind_method(D_METHOD("get_principal_inetria_axes"), &PhysicsDirectBodyState::get_principal_inertia_axes); - ClassDB::bind_method(D_METHOD("set_linear_velocity","velocity"),&PhysicsDirectBodyState::set_linear_velocity); - ClassDB::bind_method(D_METHOD("get_linear_velocity"),&PhysicsDirectBodyState::get_linear_velocity); + ClassDB::bind_method(D_METHOD("get_inverse_mass"), &PhysicsDirectBodyState::get_inverse_mass); + ClassDB::bind_method(D_METHOD("get_inverse_inertia"), &PhysicsDirectBodyState::get_inverse_inertia); - ClassDB::bind_method(D_METHOD("set_angular_velocity","velocity"),&PhysicsDirectBodyState::set_angular_velocity); - ClassDB::bind_method(D_METHOD("get_angular_velocity"),&PhysicsDirectBodyState::get_angular_velocity); + ClassDB::bind_method(D_METHOD("set_linear_velocity", "velocity"), &PhysicsDirectBodyState::set_linear_velocity); + ClassDB::bind_method(D_METHOD("get_linear_velocity"), &PhysicsDirectBodyState::get_linear_velocity); - ClassDB::bind_method(D_METHOD("set_transform","transform"),&PhysicsDirectBodyState::set_transform); - ClassDB::bind_method(D_METHOD("get_transform"),&PhysicsDirectBodyState::get_transform); + ClassDB::bind_method(D_METHOD("set_angular_velocity", "velocity"), &PhysicsDirectBodyState::set_angular_velocity); + ClassDB::bind_method(D_METHOD("get_angular_velocity"), &PhysicsDirectBodyState::get_angular_velocity); - ClassDB::bind_method(D_METHOD("add_force","force","pos"),&PhysicsDirectBodyState::add_force); - ClassDB::bind_method(D_METHOD("apply_impulse","pos","j"),&PhysicsDirectBodyState::apply_impulse); - ClassDB::bind_method(D_METHOD("apply_torqe_impulse","j"),&PhysicsDirectBodyState::apply_torque_impulse); + ClassDB::bind_method(D_METHOD("set_transform", "transform"), &PhysicsDirectBodyState::set_transform); + ClassDB::bind_method(D_METHOD("get_transform"), &PhysicsDirectBodyState::get_transform); - ClassDB::bind_method(D_METHOD("set_sleep_state","enabled"),&PhysicsDirectBodyState::set_sleep_state); - ClassDB::bind_method(D_METHOD("is_sleeping"),&PhysicsDirectBodyState::is_sleeping); + ClassDB::bind_method(D_METHOD("add_force", "force", "pos"), &PhysicsDirectBodyState::add_force); + ClassDB::bind_method(D_METHOD("apply_impulse", "pos", "j"), &PhysicsDirectBodyState::apply_impulse); + ClassDB::bind_method(D_METHOD("apply_torqe_impulse", "j"), &PhysicsDirectBodyState::apply_torque_impulse); - ClassDB::bind_method(D_METHOD("get_contact_count"),&PhysicsDirectBodyState::get_contact_count); + ClassDB::bind_method(D_METHOD("set_sleep_state", "enabled"), &PhysicsDirectBodyState::set_sleep_state); + ClassDB::bind_method(D_METHOD("is_sleeping"), &PhysicsDirectBodyState::is_sleeping); - ClassDB::bind_method(D_METHOD("get_contact_local_pos","contact_idx"),&PhysicsDirectBodyState::get_contact_local_pos); - ClassDB::bind_method(D_METHOD("get_contact_local_normal","contact_idx"),&PhysicsDirectBodyState::get_contact_local_normal); - ClassDB::bind_method(D_METHOD("get_contact_local_shape","contact_idx"),&PhysicsDirectBodyState::get_contact_local_shape); - ClassDB::bind_method(D_METHOD("get_contact_collider","contact_idx"),&PhysicsDirectBodyState::get_contact_collider); - ClassDB::bind_method(D_METHOD("get_contact_collider_pos","contact_idx"),&PhysicsDirectBodyState::get_contact_collider_pos); - ClassDB::bind_method(D_METHOD("get_contact_collider_id","contact_idx"),&PhysicsDirectBodyState::get_contact_collider_id); - ClassDB::bind_method(D_METHOD("get_contact_collider_object","contact_idx"),&PhysicsDirectBodyState::get_contact_collider_object); - ClassDB::bind_method(D_METHOD("get_contact_collider_shape","contact_idx"),&PhysicsDirectBodyState::get_contact_collider_shape); - ClassDB::bind_method(D_METHOD("get_contact_collider_velocity_at_pos","contact_idx"),&PhysicsDirectBodyState::get_contact_collider_velocity_at_pos); - ClassDB::bind_method(D_METHOD("get_step"),&PhysicsDirectBodyState::get_step); - ClassDB::bind_method(D_METHOD("integrate_forces"),&PhysicsDirectBodyState::integrate_forces); - ClassDB::bind_method(D_METHOD("get_space_state:PhysicsDirectSpaceState"),&PhysicsDirectBodyState::get_space_state); + ClassDB::bind_method(D_METHOD("get_contact_count"), &PhysicsDirectBodyState::get_contact_count); + ClassDB::bind_method(D_METHOD("get_contact_local_pos", "contact_idx"), &PhysicsDirectBodyState::get_contact_local_pos); + ClassDB::bind_method(D_METHOD("get_contact_local_normal", "contact_idx"), &PhysicsDirectBodyState::get_contact_local_normal); + ClassDB::bind_method(D_METHOD("get_contact_local_shape", "contact_idx"), &PhysicsDirectBodyState::get_contact_local_shape); + ClassDB::bind_method(D_METHOD("get_contact_collider", "contact_idx"), &PhysicsDirectBodyState::get_contact_collider); + ClassDB::bind_method(D_METHOD("get_contact_collider_pos", "contact_idx"), &PhysicsDirectBodyState::get_contact_collider_pos); + ClassDB::bind_method(D_METHOD("get_contact_collider_id", "contact_idx"), &PhysicsDirectBodyState::get_contact_collider_id); + ClassDB::bind_method(D_METHOD("get_contact_collider_object", "contact_idx"), &PhysicsDirectBodyState::get_contact_collider_object); + ClassDB::bind_method(D_METHOD("get_contact_collider_shape", "contact_idx"), &PhysicsDirectBodyState::get_contact_collider_shape); + ClassDB::bind_method(D_METHOD("get_contact_collider_velocity_at_pos", "contact_idx"), &PhysicsDirectBodyState::get_contact_collider_velocity_at_pos); + ClassDB::bind_method(D_METHOD("get_step"), &PhysicsDirectBodyState::get_step); + ClassDB::bind_method(D_METHOD("integrate_forces"), &PhysicsDirectBodyState::integrate_forces); + ClassDB::bind_method(D_METHOD("get_space_state:PhysicsDirectSpaceState"), &PhysicsDirectBodyState::get_space_state); } PhysicsDirectBodyState::PhysicsDirectBodyState() {} /////////////////////////////////////////////////////// - - void PhysicsShapeQueryParameters::set_shape(const RES &p_shape) { ERR_FAIL_COND(p_shape.is_null()); - shape=p_shape->get_rid(); + shape = p_shape->get_rid(); } -void PhysicsShapeQueryParameters::set_shape_rid(const RID& p_shape) { +void PhysicsShapeQueryParameters::set_shape_rid(const RID &p_shape) { - shape=p_shape; + shape = p_shape; } RID PhysicsShapeQueryParameters::get_shape_rid() const { @@ -139,95 +131,89 @@ RID PhysicsShapeQueryParameters::get_shape_rid() const { return shape; } -void PhysicsShapeQueryParameters::set_transform(const Transform& p_transform){ +void PhysicsShapeQueryParameters::set_transform(const Transform &p_transform) { - transform=p_transform; + transform = p_transform; } -Transform PhysicsShapeQueryParameters::get_transform() const{ +Transform PhysicsShapeQueryParameters::get_transform() const { return transform; } -void PhysicsShapeQueryParameters::set_margin(float p_margin){ +void PhysicsShapeQueryParameters::set_margin(float p_margin) { - margin=p_margin; + margin = p_margin; } -float PhysicsShapeQueryParameters::get_margin() const{ +float PhysicsShapeQueryParameters::get_margin() const { return margin; } -void PhysicsShapeQueryParameters::set_layer_mask(int p_layer_mask){ +void PhysicsShapeQueryParameters::set_layer_mask(int p_layer_mask) { - layer_mask=p_layer_mask; + layer_mask = p_layer_mask; } -int PhysicsShapeQueryParameters::get_layer_mask() const{ +int PhysicsShapeQueryParameters::get_layer_mask() const { return layer_mask; } +void PhysicsShapeQueryParameters::set_object_type_mask(int p_object_type_mask) { -void PhysicsShapeQueryParameters::set_object_type_mask(int p_object_type_mask){ - - object_type_mask=p_object_type_mask; + object_type_mask = p_object_type_mask; } -int PhysicsShapeQueryParameters::get_object_type_mask() const{ +int PhysicsShapeQueryParameters::get_object_type_mask() const { return object_type_mask; } -void PhysicsShapeQueryParameters::set_exclude(const Vector<RID>& p_exclude) { +void PhysicsShapeQueryParameters::set_exclude(const Vector<RID> &p_exclude) { exclude.clear(); - for(int i=0;i<p_exclude.size();i++) + for (int i = 0; i < p_exclude.size(); i++) exclude.insert(p_exclude[i]); - } -Vector<RID> PhysicsShapeQueryParameters::get_exclude() const{ +Vector<RID> PhysicsShapeQueryParameters::get_exclude() const { Vector<RID> ret; ret.resize(exclude.size()); - int idx=0; - for(Set<RID>::Element *E=exclude.front();E;E=E->next()) { - ret[idx]=E->get(); + int idx = 0; + for (Set<RID>::Element *E = exclude.front(); E; E = E->next()) { + ret[idx] = E->get(); } return ret; } void PhysicsShapeQueryParameters::_bind_methods() { - ClassDB::bind_method(D_METHOD("set_shape","shape:Shape"),&PhysicsShapeQueryParameters::set_shape); - ClassDB::bind_method(D_METHOD("set_shape_rid","shape"),&PhysicsShapeQueryParameters::set_shape_rid); - ClassDB::bind_method(D_METHOD("get_shape_rid"),&PhysicsShapeQueryParameters::get_shape_rid); - - ClassDB::bind_method(D_METHOD("set_transform","transform"),&PhysicsShapeQueryParameters::set_transform); - ClassDB::bind_method(D_METHOD("get_transform"),&PhysicsShapeQueryParameters::get_transform); + ClassDB::bind_method(D_METHOD("set_shape", "shape:Shape"), &PhysicsShapeQueryParameters::set_shape); + ClassDB::bind_method(D_METHOD("set_shape_rid", "shape"), &PhysicsShapeQueryParameters::set_shape_rid); + ClassDB::bind_method(D_METHOD("get_shape_rid"), &PhysicsShapeQueryParameters::get_shape_rid); - ClassDB::bind_method(D_METHOD("set_margin","margin"),&PhysicsShapeQueryParameters::set_margin); - ClassDB::bind_method(D_METHOD("get_margin"),&PhysicsShapeQueryParameters::get_margin); + ClassDB::bind_method(D_METHOD("set_transform", "transform"), &PhysicsShapeQueryParameters::set_transform); + ClassDB::bind_method(D_METHOD("get_transform"), &PhysicsShapeQueryParameters::get_transform); - ClassDB::bind_method(D_METHOD("set_layer_mask","layer_mask"),&PhysicsShapeQueryParameters::set_layer_mask); - ClassDB::bind_method(D_METHOD("get_layer_mask"),&PhysicsShapeQueryParameters::get_layer_mask); + ClassDB::bind_method(D_METHOD("set_margin", "margin"), &PhysicsShapeQueryParameters::set_margin); + ClassDB::bind_method(D_METHOD("get_margin"), &PhysicsShapeQueryParameters::get_margin); - ClassDB::bind_method(D_METHOD("set_object_type_mask","object_type_mask"),&PhysicsShapeQueryParameters::set_object_type_mask); - ClassDB::bind_method(D_METHOD("get_object_type_mask"),&PhysicsShapeQueryParameters::get_object_type_mask); - - ClassDB::bind_method(D_METHOD("set_exclude","exclude"),&PhysicsShapeQueryParameters::set_exclude); - ClassDB::bind_method(D_METHOD("get_exclude"),&PhysicsShapeQueryParameters::get_exclude); + ClassDB::bind_method(D_METHOD("set_layer_mask", "layer_mask"), &PhysicsShapeQueryParameters::set_layer_mask); + ClassDB::bind_method(D_METHOD("get_layer_mask"), &PhysicsShapeQueryParameters::get_layer_mask); + ClassDB::bind_method(D_METHOD("set_object_type_mask", "object_type_mask"), &PhysicsShapeQueryParameters::set_object_type_mask); + ClassDB::bind_method(D_METHOD("get_object_type_mask"), &PhysicsShapeQueryParameters::get_object_type_mask); + ClassDB::bind_method(D_METHOD("set_exclude", "exclude"), &PhysicsShapeQueryParameters::set_exclude); + ClassDB::bind_method(D_METHOD("get_exclude"), &PhysicsShapeQueryParameters::get_exclude); } PhysicsShapeQueryParameters::PhysicsShapeQueryParameters() { - margin=0; - layer_mask=0x7FFFFFFF; - object_type_mask=PhysicsDirectSpaceState::TYPE_MASK_COLLISION; + margin = 0; + layer_mask = 0x7FFFFFFF; + object_type_mask = PhysicsDirectSpaceState::TYPE_MASK_COLLISION; } - - ///////////////////////////////////// /* @@ -260,26 +246,25 @@ Variant PhysicsDirectSpaceState::_intersect_shape(const RID& p_shape, const Tran } */ - -Dictionary PhysicsDirectSpaceState::_intersect_ray(const Vector3& p_from, const Vector3& p_to,const Vector<RID>& p_exclude,uint32_t p_layers,uint32_t p_object_type_mask) { +Dictionary PhysicsDirectSpaceState::_intersect_ray(const Vector3 &p_from, const Vector3 &p_to, const Vector<RID> &p_exclude, uint32_t p_layers, uint32_t p_object_type_mask) { RayResult inters; Set<RID> exclude; - for(int i=0;i<p_exclude.size();i++) + for (int i = 0; i < p_exclude.size(); i++) exclude.insert(p_exclude[i]); - bool res = intersect_ray(p_from,p_to,inters,exclude,p_layers,p_object_type_mask); + bool res = intersect_ray(p_from, p_to, inters, exclude, p_layers, p_object_type_mask); if (!res) return Dictionary(); Dictionary d; - d["position"]=inters.position; - d["normal"]=inters.normal; - d["collider_id"]=inters.collider_id; - d["collider"]=inters.collider; - d["shape"]=inters.shape; - d["rid"]=inters.rid; + d["position"] = inters.position; + d["normal"] = inters.normal; + d["collider_id"] = inters.collider_id; + d["collider"] = inters.collider; + d["shape"] = inters.shape; + d["rid"] = inters.rid; return d; } @@ -288,101 +273,89 @@ Array PhysicsDirectSpaceState::_intersect_shape(const Ref<PhysicsShapeQueryParam Vector<ShapeResult> sr; sr.resize(p_max_results); - int rc = intersect_shape(psq->shape,psq->transform,psq->margin,sr.ptr(),sr.size(),psq->exclude,psq->layer_mask,psq->object_type_mask); + int rc = intersect_shape(psq->shape, psq->transform, psq->margin, sr.ptr(), sr.size(), psq->exclude, psq->layer_mask, psq->object_type_mask); Array ret; ret.resize(rc); - for(int i=0;i<rc;i++) { + for (int i = 0; i < rc; i++) { Dictionary d; - d["rid"]=sr[i].rid; - d["collider_id"]=sr[i].collider_id; - d["collider"]=sr[i].collider; - d["shape"]=sr[i].shape; - ret[i]=d; + d["rid"] = sr[i].rid; + d["collider_id"] = sr[i].collider_id; + d["collider"] = sr[i].collider; + d["shape"] = sr[i].shape; + ret[i] = d; } return ret; } -Array PhysicsDirectSpaceState::_cast_motion(const Ref<PhysicsShapeQueryParameters> &psq,const Vector3& p_motion){ +Array PhysicsDirectSpaceState::_cast_motion(const Ref<PhysicsShapeQueryParameters> &psq, const Vector3 &p_motion) { - float closest_safe,closest_unsafe; - bool res = cast_motion(psq->shape,psq->transform,p_motion,psq->margin,closest_safe,closest_unsafe,psq->exclude,psq->layer_mask,psq->object_type_mask); + float closest_safe, closest_unsafe; + bool res = cast_motion(psq->shape, psq->transform, p_motion, psq->margin, closest_safe, closest_unsafe, psq->exclude, psq->layer_mask, psq->object_type_mask); if (!res) return Array(); Array ret; ret.resize(2); - ret[0]=closest_safe; - ret[1]=closest_unsafe; + ret[0] = closest_safe; + ret[1] = closest_unsafe; return ret; - } -Array PhysicsDirectSpaceState::_collide_shape(const Ref<PhysicsShapeQueryParameters> &psq, int p_max_results){ +Array PhysicsDirectSpaceState::_collide_shape(const Ref<PhysicsShapeQueryParameters> &psq, int p_max_results) { Vector<Vector3> ret; - ret.resize(p_max_results*2); - int rc=0; - bool res = collide_shape(psq->shape,psq->transform,psq->margin,ret.ptr(),p_max_results,rc,psq->exclude,psq->layer_mask,psq->object_type_mask); + ret.resize(p_max_results * 2); + int rc = 0; + bool res = collide_shape(psq->shape, psq->transform, psq->margin, ret.ptr(), p_max_results, rc, psq->exclude, psq->layer_mask, psq->object_type_mask); if (!res) return Array(); Array r; - r.resize(rc*2); - for(int i=0;i<rc*2;i++) - r[i]=ret[i]; + r.resize(rc * 2); + for (int i = 0; i < rc * 2; i++) + r[i] = ret[i]; return r; - } -Dictionary PhysicsDirectSpaceState::_get_rest_info(const Ref<PhysicsShapeQueryParameters> &psq){ +Dictionary PhysicsDirectSpaceState::_get_rest_info(const Ref<PhysicsShapeQueryParameters> &psq) { ShapeRestInfo sri; - bool res = rest_info(psq->shape,psq->transform,psq->margin,&sri,psq->exclude,psq->layer_mask,psq->object_type_mask); + bool res = rest_info(psq->shape, psq->transform, psq->margin, &sri, psq->exclude, psq->layer_mask, psq->object_type_mask); Dictionary r; if (!res) return r; - r["point"]=sri.point; - r["normal"]=sri.normal; - r["rid"]=sri.rid; - r["collider_id"]=sri.collider_id; - r["shape"]=sri.shape; - r["linear_velocity"]=sri.linear_velocity; + r["point"] = sri.point; + r["normal"] = sri.normal; + r["rid"] = sri.rid; + r["collider_id"] = sri.collider_id; + r["shape"] = sri.shape; + r["linear_velocity"] = sri.linear_velocity; return r; } - - PhysicsDirectSpaceState::PhysicsDirectSpaceState() { - - - } - void PhysicsDirectSpaceState::_bind_methods() { - //ClassDB::bind_method(D_METHOD("intersect_ray","from","to","exclude","umask"),&PhysicsDirectSpaceState::_intersect_ray,DEFVAL(Array()),DEFVAL(0)); //ClassDB::bind_method(D_METHOD("intersect_shape:PhysicsShapeQueryResult","shape","xform","result_max","exclude","umask"),&PhysicsDirectSpaceState::_intersect_shape,DEFVAL(Array()),DEFVAL(0)); - ClassDB::bind_method(D_METHOD("intersect_ray:Dictionary","from","to","exclude","layer_mask","type_mask"),&PhysicsDirectSpaceState::_intersect_ray,DEFVAL(Array()),DEFVAL(0x7FFFFFFF),DEFVAL(TYPE_MASK_COLLISION)); - ClassDB::bind_method(D_METHOD("intersect_shape","shape:PhysicsShapeQueryParameters","max_results"),&PhysicsDirectSpaceState::_intersect_shape,DEFVAL(32)); - ClassDB::bind_method(D_METHOD("cast_motion","shape:PhysicsShapeQueryParameters","motion"),&PhysicsDirectSpaceState::_cast_motion); - ClassDB::bind_method(D_METHOD("collide_shape","shape:PhysicsShapeQueryParameters","max_results"),&PhysicsDirectSpaceState::_collide_shape,DEFVAL(32)); - ClassDB::bind_method(D_METHOD("get_rest_info","shape:PhysicsShapeQueryParameters"),&PhysicsDirectSpaceState::_get_rest_info); - - - BIND_CONSTANT( TYPE_MASK_STATIC_BODY ); - BIND_CONSTANT( TYPE_MASK_KINEMATIC_BODY ); - BIND_CONSTANT( TYPE_MASK_RIGID_BODY ); - BIND_CONSTANT( TYPE_MASK_CHARACTER_BODY ); - BIND_CONSTANT( TYPE_MASK_AREA ); - BIND_CONSTANT( TYPE_MASK_COLLISION ); + ClassDB::bind_method(D_METHOD("intersect_ray:Dictionary", "from", "to", "exclude", "layer_mask", "type_mask"), &PhysicsDirectSpaceState::_intersect_ray, DEFVAL(Array()), DEFVAL(0x7FFFFFFF), DEFVAL(TYPE_MASK_COLLISION)); + ClassDB::bind_method(D_METHOD("intersect_shape", "shape:PhysicsShapeQueryParameters", "max_results"), &PhysicsDirectSpaceState::_intersect_shape, DEFVAL(32)); + ClassDB::bind_method(D_METHOD("cast_motion", "shape:PhysicsShapeQueryParameters", "motion"), &PhysicsDirectSpaceState::_cast_motion); + ClassDB::bind_method(D_METHOD("collide_shape", "shape:PhysicsShapeQueryParameters", "max_results"), &PhysicsDirectSpaceState::_collide_shape, DEFVAL(32)); + ClassDB::bind_method(D_METHOD("get_rest_info", "shape:PhysicsShapeQueryParameters"), &PhysicsDirectSpaceState::_get_rest_info); + BIND_CONSTANT(TYPE_MASK_STATIC_BODY); + BIND_CONSTANT(TYPE_MASK_KINEMATIC_BODY); + BIND_CONSTANT(TYPE_MASK_RIGID_BODY); + BIND_CONSTANT(TYPE_MASK_CHARACTER_BODY); + BIND_CONSTANT(TYPE_MASK_AREA); + BIND_CONSTANT(TYPE_MASK_COLLISION); } - int PhysicsShapeQueryResult::get_result_count() const { return result.size(); @@ -395,7 +368,7 @@ ObjectID PhysicsShapeQueryResult::get_result_object_id(int p_idx) const { return result[p_idx].collider_id; } -Object* PhysicsShapeQueryResult::get_result_object(int p_idx) const { +Object *PhysicsShapeQueryResult::get_result_object(int p_idx) const { return result[p_idx].collider; } @@ -405,165 +378,152 @@ int PhysicsShapeQueryResult::get_result_object_shape(int p_idx) const { } PhysicsShapeQueryResult::PhysicsShapeQueryResult() { - - } void PhysicsShapeQueryResult::_bind_methods() { - ClassDB::bind_method(D_METHOD("get_result_count"),&PhysicsShapeQueryResult::get_result_count); - ClassDB::bind_method(D_METHOD("get_result_rid","idx"),&PhysicsShapeQueryResult::get_result_rid); - ClassDB::bind_method(D_METHOD("get_result_object_id","idx"),&PhysicsShapeQueryResult::get_result_object_id); - ClassDB::bind_method(D_METHOD("get_result_object","idx"),&PhysicsShapeQueryResult::get_result_object); - ClassDB::bind_method(D_METHOD("get_result_object_shape","idx"),&PhysicsShapeQueryResult::get_result_object_shape); - - + ClassDB::bind_method(D_METHOD("get_result_count"), &PhysicsShapeQueryResult::get_result_count); + ClassDB::bind_method(D_METHOD("get_result_rid", "idx"), &PhysicsShapeQueryResult::get_result_rid); + ClassDB::bind_method(D_METHOD("get_result_object_id", "idx"), &PhysicsShapeQueryResult::get_result_object_id); + ClassDB::bind_method(D_METHOD("get_result_object", "idx"), &PhysicsShapeQueryResult::get_result_object); + ClassDB::bind_method(D_METHOD("get_result_object_shape", "idx"), &PhysicsShapeQueryResult::get_result_object_shape); } - - - - /////////////////////////////////////// void PhysicsServer::_bind_methods() { + ClassDB::bind_method(D_METHOD("shape_create", "type"), &PhysicsServer::shape_create); + ClassDB::bind_method(D_METHOD("shape_set_data", "shape", "data"), &PhysicsServer::shape_set_data); - ClassDB::bind_method(D_METHOD("shape_create","type"),&PhysicsServer::shape_create); - ClassDB::bind_method(D_METHOD("shape_set_data","shape","data"),&PhysicsServer::shape_set_data); - - ClassDB::bind_method(D_METHOD("shape_get_type","shape"),&PhysicsServer::shape_get_type); - ClassDB::bind_method(D_METHOD("shape_get_data","shape"),&PhysicsServer::shape_get_data); - - - ClassDB::bind_method(D_METHOD("space_create"),&PhysicsServer::space_create); - ClassDB::bind_method(D_METHOD("space_set_active","space","active"),&PhysicsServer::space_set_active); - ClassDB::bind_method(D_METHOD("space_is_active","space"),&PhysicsServer::space_is_active); - ClassDB::bind_method(D_METHOD("space_set_param","space","param","value"),&PhysicsServer::space_set_param); - ClassDB::bind_method(D_METHOD("space_get_param","space","param"),&PhysicsServer::space_get_param); - ClassDB::bind_method(D_METHOD("space_get_direct_state:PhysicsDirectSpaceState","space"),&PhysicsServer::space_get_direct_state); + ClassDB::bind_method(D_METHOD("shape_get_type", "shape"), &PhysicsServer::shape_get_type); + ClassDB::bind_method(D_METHOD("shape_get_data", "shape"), &PhysicsServer::shape_get_data); - ClassDB::bind_method(D_METHOD("area_create"),&PhysicsServer::area_create); - ClassDB::bind_method(D_METHOD("area_set_space","area","space"),&PhysicsServer::area_set_space); - ClassDB::bind_method(D_METHOD("area_get_space","area"),&PhysicsServer::area_get_space); + ClassDB::bind_method(D_METHOD("space_create"), &PhysicsServer::space_create); + ClassDB::bind_method(D_METHOD("space_set_active", "space", "active"), &PhysicsServer::space_set_active); + ClassDB::bind_method(D_METHOD("space_is_active", "space"), &PhysicsServer::space_is_active); + ClassDB::bind_method(D_METHOD("space_set_param", "space", "param", "value"), &PhysicsServer::space_set_param); + ClassDB::bind_method(D_METHOD("space_get_param", "space", "param"), &PhysicsServer::space_get_param); + ClassDB::bind_method(D_METHOD("space_get_direct_state:PhysicsDirectSpaceState", "space"), &PhysicsServer::space_get_direct_state); - ClassDB::bind_method(D_METHOD("area_set_space_override_mode","area","mode"),&PhysicsServer::area_set_space_override_mode); - ClassDB::bind_method(D_METHOD("area_get_space_override_mode","area"),&PhysicsServer::area_get_space_override_mode); + ClassDB::bind_method(D_METHOD("area_create"), &PhysicsServer::area_create); + ClassDB::bind_method(D_METHOD("area_set_space", "area", "space"), &PhysicsServer::area_set_space); + ClassDB::bind_method(D_METHOD("area_get_space", "area"), &PhysicsServer::area_get_space); - ClassDB::bind_method(D_METHOD("area_add_shape","area","shape","transform"),&PhysicsServer::area_add_shape,DEFVAL(Transform())); - ClassDB::bind_method(D_METHOD("area_set_shape","area","shape_idx","shape"),&PhysicsServer::area_set_shape); - ClassDB::bind_method(D_METHOD("area_set_shape_transform","area","shape_idx","transform"),&PhysicsServer::area_set_shape_transform); + ClassDB::bind_method(D_METHOD("area_set_space_override_mode", "area", "mode"), &PhysicsServer::area_set_space_override_mode); + ClassDB::bind_method(D_METHOD("area_get_space_override_mode", "area"), &PhysicsServer::area_get_space_override_mode); - ClassDB::bind_method(D_METHOD("area_get_shape_count","area"),&PhysicsServer::area_get_shape_count); - ClassDB::bind_method(D_METHOD("area_get_shape","area","shape_idx"),&PhysicsServer::area_get_shape); - ClassDB::bind_method(D_METHOD("area_get_shape_transform","area","shape_idx"),&PhysicsServer::area_get_shape_transform); + ClassDB::bind_method(D_METHOD("area_add_shape", "area", "shape", "transform"), &PhysicsServer::area_add_shape, DEFVAL(Transform())); + ClassDB::bind_method(D_METHOD("area_set_shape", "area", "shape_idx", "shape"), &PhysicsServer::area_set_shape); + ClassDB::bind_method(D_METHOD("area_set_shape_transform", "area", "shape_idx", "transform"), &PhysicsServer::area_set_shape_transform); - ClassDB::bind_method(D_METHOD("area_remove_shape","area","shape_idx"),&PhysicsServer::area_remove_shape); - ClassDB::bind_method(D_METHOD("area_clear_shapes","area"),&PhysicsServer::area_clear_shapes); + ClassDB::bind_method(D_METHOD("area_get_shape_count", "area"), &PhysicsServer::area_get_shape_count); + ClassDB::bind_method(D_METHOD("area_get_shape", "area", "shape_idx"), &PhysicsServer::area_get_shape); + ClassDB::bind_method(D_METHOD("area_get_shape_transform", "area", "shape_idx"), &PhysicsServer::area_get_shape_transform); - ClassDB::bind_method(D_METHOD("area_set_layer_mask","area","mask"),&PhysicsServer::area_set_layer_mask); - ClassDB::bind_method(D_METHOD("area_set_collision_mask","area","mask"),&PhysicsServer::area_set_collision_mask); + ClassDB::bind_method(D_METHOD("area_remove_shape", "area", "shape_idx"), &PhysicsServer::area_remove_shape); + ClassDB::bind_method(D_METHOD("area_clear_shapes", "area"), &PhysicsServer::area_clear_shapes); - ClassDB::bind_method(D_METHOD("area_set_param","area","param","value"),&PhysicsServer::area_set_param); - ClassDB::bind_method(D_METHOD("area_set_transform","area","transform"),&PhysicsServer::area_set_transform); + ClassDB::bind_method(D_METHOD("area_set_layer_mask", "area", "mask"), &PhysicsServer::area_set_layer_mask); + ClassDB::bind_method(D_METHOD("area_set_collision_mask", "area", "mask"), &PhysicsServer::area_set_collision_mask); - ClassDB::bind_method(D_METHOD("area_get_param","area","param"),&PhysicsServer::area_get_param); - ClassDB::bind_method(D_METHOD("area_get_transform","area"),&PhysicsServer::area_get_transform); + ClassDB::bind_method(D_METHOD("area_set_param", "area", "param", "value"), &PhysicsServer::area_set_param); + ClassDB::bind_method(D_METHOD("area_set_transform", "area", "transform"), &PhysicsServer::area_set_transform); - ClassDB::bind_method(D_METHOD("area_attach_object_instance_ID","area","id"),&PhysicsServer::area_attach_object_instance_ID); - ClassDB::bind_method(D_METHOD("area_get_object_instance_ID","area"),&PhysicsServer::area_get_object_instance_ID); + ClassDB::bind_method(D_METHOD("area_get_param", "area", "param"), &PhysicsServer::area_get_param); + ClassDB::bind_method(D_METHOD("area_get_transform", "area"), &PhysicsServer::area_get_transform); - ClassDB::bind_method(D_METHOD("area_set_monitor_callback","area","receiver","method"),&PhysicsServer::area_set_monitor_callback); + ClassDB::bind_method(D_METHOD("area_attach_object_instance_ID", "area", "id"), &PhysicsServer::area_attach_object_instance_ID); + ClassDB::bind_method(D_METHOD("area_get_object_instance_ID", "area"), &PhysicsServer::area_get_object_instance_ID); - ClassDB::bind_method(D_METHOD("area_set_ray_pickable","area","enable"),&PhysicsServer::area_set_ray_pickable); - ClassDB::bind_method(D_METHOD("area_is_ray_pickable","area"),&PhysicsServer::area_is_ray_pickable); + ClassDB::bind_method(D_METHOD("area_set_monitor_callback", "area", "receiver", "method"), &PhysicsServer::area_set_monitor_callback); - ClassDB::bind_method(D_METHOD("body_create","mode","init_sleeping"),&PhysicsServer::body_create,DEFVAL(BODY_MODE_RIGID),DEFVAL(false)); + ClassDB::bind_method(D_METHOD("area_set_ray_pickable", "area", "enable"), &PhysicsServer::area_set_ray_pickable); + ClassDB::bind_method(D_METHOD("area_is_ray_pickable", "area"), &PhysicsServer::area_is_ray_pickable); - ClassDB::bind_method(D_METHOD("body_set_space","body","space"),&PhysicsServer::body_set_space); - ClassDB::bind_method(D_METHOD("body_get_space","body"),&PhysicsServer::body_get_space); + ClassDB::bind_method(D_METHOD("body_create", "mode", "init_sleeping"), &PhysicsServer::body_create, DEFVAL(BODY_MODE_RIGID), DEFVAL(false)); - ClassDB::bind_method(D_METHOD("body_set_mode","body","mode"),&PhysicsServer::body_set_mode); - ClassDB::bind_method(D_METHOD("body_get_mode","body"),&PhysicsServer::body_get_mode); + ClassDB::bind_method(D_METHOD("body_set_space", "body", "space"), &PhysicsServer::body_set_space); + ClassDB::bind_method(D_METHOD("body_get_space", "body"), &PhysicsServer::body_get_space); - ClassDB::bind_method(D_METHOD("body_set_layer_mask","body","mask"),&PhysicsServer::body_set_layer_mask); - ClassDB::bind_method(D_METHOD("body_get_layer_mask","body"),&PhysicsServer::body_get_layer_mask); + ClassDB::bind_method(D_METHOD("body_set_mode", "body", "mode"), &PhysicsServer::body_set_mode); + ClassDB::bind_method(D_METHOD("body_get_mode", "body"), &PhysicsServer::body_get_mode); - ClassDB::bind_method(D_METHOD("body_set_collision_mask","body","mask"),&PhysicsServer::body_set_collision_mask); - ClassDB::bind_method(D_METHOD("body_get_collision_mask","body"),&PhysicsServer::body_get_collision_mask); + ClassDB::bind_method(D_METHOD("body_set_layer_mask", "body", "mask"), &PhysicsServer::body_set_layer_mask); + ClassDB::bind_method(D_METHOD("body_get_layer_mask", "body"), &PhysicsServer::body_get_layer_mask); - ClassDB::bind_method(D_METHOD("body_add_shape","body","shape","transform"),&PhysicsServer::body_add_shape,DEFVAL(Transform())); - ClassDB::bind_method(D_METHOD("body_set_shape","body","shape_idx","shape"),&PhysicsServer::body_set_shape); - ClassDB::bind_method(D_METHOD("body_set_shape_transform","body","shape_idx","transform"),&PhysicsServer::body_set_shape_transform); + ClassDB::bind_method(D_METHOD("body_set_collision_mask", "body", "mask"), &PhysicsServer::body_set_collision_mask); + ClassDB::bind_method(D_METHOD("body_get_collision_mask", "body"), &PhysicsServer::body_get_collision_mask); - ClassDB::bind_method(D_METHOD("body_get_shape_count","body"),&PhysicsServer::body_get_shape_count); - ClassDB::bind_method(D_METHOD("body_get_shape","body","shape_idx"),&PhysicsServer::body_get_shape); - ClassDB::bind_method(D_METHOD("body_get_shape_transform","body","shape_idx"),&PhysicsServer::body_get_shape_transform); + ClassDB::bind_method(D_METHOD("body_add_shape", "body", "shape", "transform"), &PhysicsServer::body_add_shape, DEFVAL(Transform())); + ClassDB::bind_method(D_METHOD("body_set_shape", "body", "shape_idx", "shape"), &PhysicsServer::body_set_shape); + ClassDB::bind_method(D_METHOD("body_set_shape_transform", "body", "shape_idx", "transform"), &PhysicsServer::body_set_shape_transform); - ClassDB::bind_method(D_METHOD("body_remove_shape","body","shape_idx"),&PhysicsServer::body_remove_shape); - ClassDB::bind_method(D_METHOD("body_clear_shapes","body"),&PhysicsServer::body_clear_shapes); + ClassDB::bind_method(D_METHOD("body_get_shape_count", "body"), &PhysicsServer::body_get_shape_count); + ClassDB::bind_method(D_METHOD("body_get_shape", "body", "shape_idx"), &PhysicsServer::body_get_shape); + ClassDB::bind_method(D_METHOD("body_get_shape_transform", "body", "shape_idx"), &PhysicsServer::body_get_shape_transform); - ClassDB::bind_method(D_METHOD("body_attach_object_instance_ID","body","id"),&PhysicsServer::body_attach_object_instance_ID); - ClassDB::bind_method(D_METHOD("body_get_object_instance_ID","body"),&PhysicsServer::body_get_object_instance_ID); + ClassDB::bind_method(D_METHOD("body_remove_shape", "body", "shape_idx"), &PhysicsServer::body_remove_shape); + ClassDB::bind_method(D_METHOD("body_clear_shapes", "body"), &PhysicsServer::body_clear_shapes); + ClassDB::bind_method(D_METHOD("body_attach_object_instance_ID", "body", "id"), &PhysicsServer::body_attach_object_instance_ID); + ClassDB::bind_method(D_METHOD("body_get_object_instance_ID", "body"), &PhysicsServer::body_get_object_instance_ID); - ClassDB::bind_method(D_METHOD("body_set_enable_continuous_collision_detection","body","enable"),&PhysicsServer::body_set_enable_continuous_collision_detection); - ClassDB::bind_method(D_METHOD("body_is_continuous_collision_detection_enabled","body"),&PhysicsServer::body_is_continuous_collision_detection_enabled); - + ClassDB::bind_method(D_METHOD("body_set_enable_continuous_collision_detection", "body", "enable"), &PhysicsServer::body_set_enable_continuous_collision_detection); + ClassDB::bind_method(D_METHOD("body_is_continuous_collision_detection_enabled", "body"), &PhysicsServer::body_is_continuous_collision_detection_enabled); //ClassDB::bind_method(D_METHOD("body_set_user_flags","flags""),&PhysicsServer::body_set_shape,DEFVAL(Transform)); //ClassDB::bind_method(D_METHOD("body_get_user_flags","body","shape_idx","shape"),&PhysicsServer::body_get_shape); - ClassDB::bind_method(D_METHOD("body_set_param","body","param","value"),&PhysicsServer::body_set_param); - ClassDB::bind_method(D_METHOD("body_get_param","body","param"),&PhysicsServer::body_get_param); + ClassDB::bind_method(D_METHOD("body_set_param", "body", "param", "value"), &PhysicsServer::body_set_param); + ClassDB::bind_method(D_METHOD("body_get_param", "body", "param"), &PhysicsServer::body_get_param); - ClassDB::bind_method(D_METHOD("body_set_state","body","state","value"),&PhysicsServer::body_set_state); - ClassDB::bind_method(D_METHOD("body_get_state","body","state"),&PhysicsServer::body_get_state); + ClassDB::bind_method(D_METHOD("body_set_state", "body", "state", "value"), &PhysicsServer::body_set_state); + ClassDB::bind_method(D_METHOD("body_get_state", "body", "state"), &PhysicsServer::body_get_state); - ClassDB::bind_method(D_METHOD("body_apply_impulse","body","pos","impulse"),&PhysicsServer::body_apply_impulse); - ClassDB::bind_method(D_METHOD("body_apply_torque_impulse","body","impulse"),&PhysicsServer::body_apply_torque_impulse); - ClassDB::bind_method(D_METHOD("body_set_axis_velocity","body","axis_velocity"),&PhysicsServer::body_set_axis_velocity); + ClassDB::bind_method(D_METHOD("body_apply_impulse", "body", "pos", "impulse"), &PhysicsServer::body_apply_impulse); + ClassDB::bind_method(D_METHOD("body_apply_torque_impulse", "body", "impulse"), &PhysicsServer::body_apply_torque_impulse); + ClassDB::bind_method(D_METHOD("body_set_axis_velocity", "body", "axis_velocity"), &PhysicsServer::body_set_axis_velocity); - ClassDB::bind_method(D_METHOD("body_set_axis_lock","body","axis"),&PhysicsServer::body_set_axis_lock); - ClassDB::bind_method(D_METHOD("body_get_axis_lock","body"),&PhysicsServer::body_get_axis_lock); + ClassDB::bind_method(D_METHOD("body_set_axis_lock", "body", "axis"), &PhysicsServer::body_set_axis_lock); + ClassDB::bind_method(D_METHOD("body_get_axis_lock", "body"), &PhysicsServer::body_get_axis_lock); - ClassDB::bind_method(D_METHOD("body_add_collision_exception","body","excepted_body"),&PhysicsServer::body_add_collision_exception); - ClassDB::bind_method(D_METHOD("body_remove_collision_exception","body","excepted_body"),&PhysicsServer::body_remove_collision_exception); + ClassDB::bind_method(D_METHOD("body_add_collision_exception", "body", "excepted_body"), &PhysicsServer::body_add_collision_exception); + ClassDB::bind_method(D_METHOD("body_remove_collision_exception", "body", "excepted_body"), &PhysicsServer::body_remove_collision_exception); //virtual void body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions)=0; - ClassDB::bind_method(D_METHOD("body_set_max_contacts_reported","body","amount"),&PhysicsServer::body_set_max_contacts_reported); - ClassDB::bind_method(D_METHOD("body_get_max_contacts_reported","body"),&PhysicsServer::body_get_max_contacts_reported); + ClassDB::bind_method(D_METHOD("body_set_max_contacts_reported", "body", "amount"), &PhysicsServer::body_set_max_contacts_reported); + ClassDB::bind_method(D_METHOD("body_get_max_contacts_reported", "body"), &PhysicsServer::body_get_max_contacts_reported); - ClassDB::bind_method(D_METHOD("body_set_omit_force_integration","body","enable"),&PhysicsServer::body_set_omit_force_integration); - ClassDB::bind_method(D_METHOD("body_is_omitting_force_integration","body"),&PhysicsServer::body_is_omitting_force_integration); + ClassDB::bind_method(D_METHOD("body_set_omit_force_integration", "body", "enable"), &PhysicsServer::body_set_omit_force_integration); + ClassDB::bind_method(D_METHOD("body_is_omitting_force_integration", "body"), &PhysicsServer::body_is_omitting_force_integration); - ClassDB::bind_method(D_METHOD("body_set_force_integration_callback","body","receiver","method","userdata"),&PhysicsServer::body_set_force_integration_callback,DEFVAL(Variant())); + ClassDB::bind_method(D_METHOD("body_set_force_integration_callback", "body", "receiver", "method", "userdata"), &PhysicsServer::body_set_force_integration_callback, DEFVAL(Variant())); - ClassDB::bind_method(D_METHOD("body_set_ray_pickable","body","enable"),&PhysicsServer::body_set_ray_pickable); - ClassDB::bind_method(D_METHOD("body_is_ray_pickable","body"),&PhysicsServer::body_is_ray_pickable); + ClassDB::bind_method(D_METHOD("body_set_ray_pickable", "body", "enable"), &PhysicsServer::body_set_ray_pickable); + ClassDB::bind_method(D_METHOD("body_is_ray_pickable", "body"), &PhysicsServer::body_is_ray_pickable); /* JOINT API */ - BIND_CONSTANT( JOINT_PIN ); - BIND_CONSTANT( JOINT_HINGE ); - BIND_CONSTANT( JOINT_SLIDER ); - BIND_CONSTANT( JOINT_CONE_TWIST ); - BIND_CONSTANT( JOINT_6DOF ); - - ClassDB::bind_method(D_METHOD("joint_create_pin","body_A","local_A","body_B","local_B"),&PhysicsServer::joint_create_pin); - ClassDB::bind_method(D_METHOD("pin_joint_set_param","joint","param","value"),&PhysicsServer::pin_joint_set_param); - ClassDB::bind_method(D_METHOD("pin_joint_get_param","joint","param"),&PhysicsServer::pin_joint_get_param); + BIND_CONSTANT(JOINT_PIN); + BIND_CONSTANT(JOINT_HINGE); + BIND_CONSTANT(JOINT_SLIDER); + BIND_CONSTANT(JOINT_CONE_TWIST); + BIND_CONSTANT(JOINT_6DOF); - ClassDB::bind_method(D_METHOD("pin_joint_set_local_A","joint","local_A"),&PhysicsServer::pin_joint_set_local_A); - ClassDB::bind_method(D_METHOD("pin_joint_get_local_A","joint"),&PhysicsServer::pin_joint_get_local_A); + ClassDB::bind_method(D_METHOD("joint_create_pin", "body_A", "local_A", "body_B", "local_B"), &PhysicsServer::joint_create_pin); + ClassDB::bind_method(D_METHOD("pin_joint_set_param", "joint", "param", "value"), &PhysicsServer::pin_joint_set_param); + ClassDB::bind_method(D_METHOD("pin_joint_get_param", "joint", "param"), &PhysicsServer::pin_joint_get_param); - ClassDB::bind_method(D_METHOD("pin_joint_set_local_B","joint","local_B"),&PhysicsServer::pin_joint_set_local_B); - ClassDB::bind_method(D_METHOD("pin_joint_get_local_B","joint"),&PhysicsServer::pin_joint_get_local_B); + ClassDB::bind_method(D_METHOD("pin_joint_set_local_A", "joint", "local_A"), &PhysicsServer::pin_joint_set_local_A); + ClassDB::bind_method(D_METHOD("pin_joint_get_local_A", "joint"), &PhysicsServer::pin_joint_get_local_A); - BIND_CONSTANT(PIN_JOINT_BIAS ); - BIND_CONSTANT(PIN_JOINT_DAMPING ); - BIND_CONSTANT(PIN_JOINT_IMPULSE_CLAMP ); + ClassDB::bind_method(D_METHOD("pin_joint_set_local_B", "joint", "local_B"), &PhysicsServer::pin_joint_set_local_B); + ClassDB::bind_method(D_METHOD("pin_joint_get_local_B", "joint"), &PhysicsServer::pin_joint_get_local_B); + BIND_CONSTANT(PIN_JOINT_BIAS); + BIND_CONSTANT(PIN_JOINT_DAMPING); + BIND_CONSTANT(PIN_JOINT_IMPULSE_CLAMP); BIND_CONSTANT(HINGE_JOINT_BIAS); BIND_CONSTANT(HINGE_JOINT_LIMIT_UPPER); @@ -576,92 +536,88 @@ void PhysicsServer::_bind_methods() { BIND_CONSTANT(HINGE_JOINT_FLAG_USE_LIMIT); BIND_CONSTANT(HINGE_JOINT_FLAG_ENABLE_MOTOR); - ClassDB::bind_method(D_METHOD("joint_create_hinge","body_A","hinge_A","body_B","hinge_B"),&PhysicsServer::joint_create_hinge); + ClassDB::bind_method(D_METHOD("joint_create_hinge", "body_A", "hinge_A", "body_B", "hinge_B"), &PhysicsServer::joint_create_hinge); - ClassDB::bind_method(D_METHOD("hinge_joint_set_param","joint","param","value"),&PhysicsServer::hinge_joint_set_param); - ClassDB::bind_method(D_METHOD("hinge_joint_get_param","joint","param"),&PhysicsServer::hinge_joint_get_param); + ClassDB::bind_method(D_METHOD("hinge_joint_set_param", "joint", "param", "value"), &PhysicsServer::hinge_joint_set_param); + ClassDB::bind_method(D_METHOD("hinge_joint_get_param", "joint", "param"), &PhysicsServer::hinge_joint_get_param); - ClassDB::bind_method(D_METHOD("hinge_joint_set_flag","joint","flag","enabled"),&PhysicsServer::hinge_joint_set_flag); - ClassDB::bind_method(D_METHOD("hinge_joint_get_flag","joint","flag"),&PhysicsServer::hinge_joint_get_flag); + ClassDB::bind_method(D_METHOD("hinge_joint_set_flag", "joint", "flag", "enabled"), &PhysicsServer::hinge_joint_set_flag); + ClassDB::bind_method(D_METHOD("hinge_joint_get_flag", "joint", "flag"), &PhysicsServer::hinge_joint_get_flag); - ClassDB::bind_method(D_METHOD("joint_create_slider","body_A","local_ref_A","body_B","local_ref_B"),&PhysicsServer::joint_create_slider); + ClassDB::bind_method(D_METHOD("joint_create_slider", "body_A", "local_ref_A", "body_B", "local_ref_B"), &PhysicsServer::joint_create_slider); - ClassDB::bind_method(D_METHOD("slider_joint_set_param","joint","param","value"),&PhysicsServer::slider_joint_set_param); - ClassDB::bind_method(D_METHOD("slider_joint_get_param","joint","param"),&PhysicsServer::slider_joint_get_param); + ClassDB::bind_method(D_METHOD("slider_joint_set_param", "joint", "param", "value"), &PhysicsServer::slider_joint_set_param); + ClassDB::bind_method(D_METHOD("slider_joint_get_param", "joint", "param"), &PhysicsServer::slider_joint_get_param); - BIND_CONSTANT( SLIDER_JOINT_LINEAR_LIMIT_UPPER ); - BIND_CONSTANT( SLIDER_JOINT_LINEAR_LIMIT_LOWER ); - BIND_CONSTANT( SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS ); - BIND_CONSTANT( SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION ); - BIND_CONSTANT( SLIDER_JOINT_LINEAR_LIMIT_DAMPING ); - BIND_CONSTANT( SLIDER_JOINT_LINEAR_MOTION_SOFTNESS ); - BIND_CONSTANT( SLIDER_JOINT_LINEAR_MOTION_RESTITUTION ); - BIND_CONSTANT( SLIDER_JOINT_LINEAR_MOTION_DAMPING ); - BIND_CONSTANT( SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS ); - BIND_CONSTANT( SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION ); - BIND_CONSTANT( SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING ); + BIND_CONSTANT(SLIDER_JOINT_LINEAR_LIMIT_UPPER); + BIND_CONSTANT(SLIDER_JOINT_LINEAR_LIMIT_LOWER); + BIND_CONSTANT(SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS); + BIND_CONSTANT(SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION); + BIND_CONSTANT(SLIDER_JOINT_LINEAR_LIMIT_DAMPING); + BIND_CONSTANT(SLIDER_JOINT_LINEAR_MOTION_SOFTNESS); + BIND_CONSTANT(SLIDER_JOINT_LINEAR_MOTION_RESTITUTION); + BIND_CONSTANT(SLIDER_JOINT_LINEAR_MOTION_DAMPING); + BIND_CONSTANT(SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS); + BIND_CONSTANT(SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION); + BIND_CONSTANT(SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING); - BIND_CONSTANT( SLIDER_JOINT_ANGULAR_LIMIT_UPPER ); - BIND_CONSTANT( SLIDER_JOINT_ANGULAR_LIMIT_LOWER ); - BIND_CONSTANT( SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS ); - BIND_CONSTANT( SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION ); - BIND_CONSTANT( SLIDER_JOINT_ANGULAR_LIMIT_DAMPING ); - BIND_CONSTANT( SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS ); - BIND_CONSTANT( SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION ); - BIND_CONSTANT( SLIDER_JOINT_ANGULAR_MOTION_DAMPING ); - BIND_CONSTANT( SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS ); - BIND_CONSTANT( SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION ); - BIND_CONSTANT( SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING ); - BIND_CONSTANT( SLIDER_JOINT_MAX ); + BIND_CONSTANT(SLIDER_JOINT_ANGULAR_LIMIT_UPPER); + BIND_CONSTANT(SLIDER_JOINT_ANGULAR_LIMIT_LOWER); + BIND_CONSTANT(SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS); + BIND_CONSTANT(SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION); + BIND_CONSTANT(SLIDER_JOINT_ANGULAR_LIMIT_DAMPING); + BIND_CONSTANT(SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS); + BIND_CONSTANT(SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION); + BIND_CONSTANT(SLIDER_JOINT_ANGULAR_MOTION_DAMPING); + BIND_CONSTANT(SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS); + BIND_CONSTANT(SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION); + BIND_CONSTANT(SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING); + BIND_CONSTANT(SLIDER_JOINT_MAX); + ClassDB::bind_method(D_METHOD("joint_create_cone_twist", "body_A", "local_ref_A", "body_B", "local_ref_B"), &PhysicsServer::joint_create_cone_twist); - ClassDB::bind_method(D_METHOD("joint_create_cone_twist","body_A","local_ref_A","body_B","local_ref_B"),&PhysicsServer::joint_create_cone_twist); + ClassDB::bind_method(D_METHOD("cone_twist_joint_set_param", "joint", "param", "value"), &PhysicsServer::cone_twist_joint_set_param); + ClassDB::bind_method(D_METHOD("cone_twist_joint_get_param", "joint", "param"), &PhysicsServer::cone_twist_joint_get_param); - ClassDB::bind_method(D_METHOD("cone_twist_joint_set_param","joint","param","value"),&PhysicsServer::cone_twist_joint_set_param); - ClassDB::bind_method(D_METHOD("cone_twist_joint_get_param","joint","param"),&PhysicsServer::cone_twist_joint_get_param); + BIND_CONSTANT(CONE_TWIST_JOINT_SWING_SPAN); + BIND_CONSTANT(CONE_TWIST_JOINT_TWIST_SPAN); + BIND_CONSTANT(CONE_TWIST_JOINT_BIAS); + BIND_CONSTANT(CONE_TWIST_JOINT_SOFTNESS); + BIND_CONSTANT(CONE_TWIST_JOINT_RELAXATION); - BIND_CONSTANT( CONE_TWIST_JOINT_SWING_SPAN ); - BIND_CONSTANT( CONE_TWIST_JOINT_TWIST_SPAN ); - BIND_CONSTANT( CONE_TWIST_JOINT_BIAS ); - BIND_CONSTANT( CONE_TWIST_JOINT_SOFTNESS ); - BIND_CONSTANT( CONE_TWIST_JOINT_RELAXATION ); + BIND_CONSTANT(G6DOF_JOINT_LINEAR_LOWER_LIMIT); + BIND_CONSTANT(G6DOF_JOINT_LINEAR_UPPER_LIMIT); + BIND_CONSTANT(G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS); + BIND_CONSTANT(G6DOF_JOINT_LINEAR_RESTITUTION); + BIND_CONSTANT(G6DOF_JOINT_LINEAR_DAMPING); + BIND_CONSTANT(G6DOF_JOINT_ANGULAR_LOWER_LIMIT); + BIND_CONSTANT(G6DOF_JOINT_ANGULAR_UPPER_LIMIT); + BIND_CONSTANT(G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS); + BIND_CONSTANT(G6DOF_JOINT_ANGULAR_DAMPING); + BIND_CONSTANT(G6DOF_JOINT_ANGULAR_RESTITUTION); + BIND_CONSTANT(G6DOF_JOINT_ANGULAR_FORCE_LIMIT); + BIND_CONSTANT(G6DOF_JOINT_ANGULAR_ERP); + BIND_CONSTANT(G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY); + BIND_CONSTANT(G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT); + BIND_CONSTANT(G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT); + BIND_CONSTANT(G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT); + BIND_CONSTANT(G6DOF_JOINT_FLAG_ENABLE_MOTOR); - BIND_CONSTANT( G6DOF_JOINT_LINEAR_LOWER_LIMIT ); - BIND_CONSTANT( G6DOF_JOINT_LINEAR_UPPER_LIMIT ); - BIND_CONSTANT( G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS ); - BIND_CONSTANT( G6DOF_JOINT_LINEAR_RESTITUTION ); - BIND_CONSTANT( G6DOF_JOINT_LINEAR_DAMPING ); - BIND_CONSTANT( G6DOF_JOINT_ANGULAR_LOWER_LIMIT ); - BIND_CONSTANT( G6DOF_JOINT_ANGULAR_UPPER_LIMIT ); - BIND_CONSTANT( G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS ); - BIND_CONSTANT( G6DOF_JOINT_ANGULAR_DAMPING ); - BIND_CONSTANT( G6DOF_JOINT_ANGULAR_RESTITUTION ); - BIND_CONSTANT( G6DOF_JOINT_ANGULAR_FORCE_LIMIT ); - BIND_CONSTANT( G6DOF_JOINT_ANGULAR_ERP ); - BIND_CONSTANT( G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY ); - BIND_CONSTANT( G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT ); + ClassDB::bind_method(D_METHOD("joint_get_type", "joint"), &PhysicsServer::joint_get_type); + ClassDB::bind_method(D_METHOD("joint_set_solver_priority", "joint", "priority"), &PhysicsServer::joint_set_solver_priority); + ClassDB::bind_method(D_METHOD("joint_get_solver_priority", "joint"), &PhysicsServer::joint_get_solver_priority); - BIND_CONSTANT( G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT ); - BIND_CONSTANT( G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT ); - BIND_CONSTANT( G6DOF_JOINT_FLAG_ENABLE_MOTOR ); + ClassDB::bind_method(D_METHOD("joint_create_generic_6dof", "body_A", "local_ref_A", "body_B", "local_ref_B"), &PhysicsServer::joint_create_generic_6dof); - ClassDB::bind_method(D_METHOD("joint_get_type","joint"),&PhysicsServer::joint_get_type); - - ClassDB::bind_method(D_METHOD("joint_set_solver_priority","joint","priority"),&PhysicsServer::joint_set_solver_priority); - ClassDB::bind_method(D_METHOD("joint_get_solver_priority","joint"),&PhysicsServer::joint_get_solver_priority); - - ClassDB::bind_method(D_METHOD("joint_create_generic_6dof","body_A","local_ref_A","body_B","local_ref_B"),&PhysicsServer::joint_create_generic_6dof); - - ClassDB::bind_method(D_METHOD("generic_6dof_joint_set_param","joint","axis","param","value"),&PhysicsServer::generic_6dof_joint_set_param); - ClassDB::bind_method(D_METHOD("generic_6dof_joint_get_param","joint","axis","param"),&PhysicsServer::generic_6dof_joint_get_param); + ClassDB::bind_method(D_METHOD("generic_6dof_joint_set_param", "joint", "axis", "param", "value"), &PhysicsServer::generic_6dof_joint_set_param); + ClassDB::bind_method(D_METHOD("generic_6dof_joint_get_param", "joint", "axis", "param"), &PhysicsServer::generic_6dof_joint_get_param); - ClassDB::bind_method(D_METHOD("generic_6dof_joint_set_flag","joint","axis","flag","enable"),&PhysicsServer::generic_6dof_joint_set_flag); - ClassDB::bind_method(D_METHOD("generic_6dof_joint_get_flag","joint","axis","flag"),&PhysicsServer::generic_6dof_joint_get_flag); + ClassDB::bind_method(D_METHOD("generic_6dof_joint_set_flag", "joint", "axis", "flag", "enable"), &PhysicsServer::generic_6dof_joint_set_flag); + ClassDB::bind_method(D_METHOD("generic_6dof_joint_get_flag", "joint", "axis", "flag"), &PhysicsServer::generic_6dof_joint_get_flag); - -/* + /* ClassDB::bind_method(D_METHOD("joint_set_param","joint","param","value"),&PhysicsServer::joint_set_param); ClassDB::bind_method(D_METHOD("joint_get_param","joint","param"),&PhysicsServer::joint_get_param); @@ -674,63 +630,61 @@ void PhysicsServer::_bind_methods() { ClassDB::bind_method(D_METHOD("joint_get_type","joint"),&PhysicsServer::joint_get_type); */ - ClassDB::bind_method(D_METHOD("free_rid","rid"),&PhysicsServer::free); + ClassDB::bind_method(D_METHOD("free_rid", "rid"), &PhysicsServer::free); - ClassDB::bind_method(D_METHOD("set_active","active"),&PhysicsServer::set_active); + ClassDB::bind_method(D_METHOD("set_active", "active"), &PhysicsServer::set_active); //ClassDB::bind_method(D_METHOD("init"),&PhysicsServer::init); //ClassDB::bind_method(D_METHOD("step"),&PhysicsServer::step); //ClassDB::bind_method(D_METHOD("sync"),&PhysicsServer::sync); //ClassDB::bind_method(D_METHOD("flush_queries"),&PhysicsServer::flush_queries); + ClassDB::bind_method(D_METHOD("get_process_info", "process_info"), &PhysicsServer::get_process_info); - ClassDB::bind_method(D_METHOD("get_process_info","process_info"),&PhysicsServer::get_process_info); - - BIND_CONSTANT( SHAPE_PLANE ); - BIND_CONSTANT( SHAPE_RAY ); - BIND_CONSTANT( SHAPE_SPHERE ); - BIND_CONSTANT( SHAPE_BOX ); - BIND_CONSTANT( SHAPE_CAPSULE ); - BIND_CONSTANT( SHAPE_CONVEX_POLYGON ); - BIND_CONSTANT( SHAPE_CONCAVE_POLYGON ); - BIND_CONSTANT( SHAPE_HEIGHTMAP ); - BIND_CONSTANT( SHAPE_CUSTOM ); + BIND_CONSTANT(SHAPE_PLANE); + BIND_CONSTANT(SHAPE_RAY); + BIND_CONSTANT(SHAPE_SPHERE); + BIND_CONSTANT(SHAPE_BOX); + BIND_CONSTANT(SHAPE_CAPSULE); + BIND_CONSTANT(SHAPE_CONVEX_POLYGON); + BIND_CONSTANT(SHAPE_CONCAVE_POLYGON); + BIND_CONSTANT(SHAPE_HEIGHTMAP); + BIND_CONSTANT(SHAPE_CUSTOM); + BIND_CONSTANT(AREA_PARAM_GRAVITY); + BIND_CONSTANT(AREA_PARAM_GRAVITY_VECTOR); + BIND_CONSTANT(AREA_PARAM_GRAVITY_IS_POINT); + BIND_CONSTANT(AREA_PARAM_GRAVITY_DISTANCE_SCALE); + BIND_CONSTANT(AREA_PARAM_GRAVITY_POINT_ATTENUATION); + BIND_CONSTANT(AREA_PARAM_LINEAR_DAMP); + BIND_CONSTANT(AREA_PARAM_ANGULAR_DAMP); + BIND_CONSTANT(AREA_PARAM_PRIORITY); - BIND_CONSTANT( AREA_PARAM_GRAVITY ); - BIND_CONSTANT( AREA_PARAM_GRAVITY_VECTOR ); - BIND_CONSTANT( AREA_PARAM_GRAVITY_IS_POINT ); - BIND_CONSTANT( AREA_PARAM_GRAVITY_DISTANCE_SCALE ); - BIND_CONSTANT( AREA_PARAM_GRAVITY_POINT_ATTENUATION ); - BIND_CONSTANT( AREA_PARAM_LINEAR_DAMP ); - BIND_CONSTANT( AREA_PARAM_ANGULAR_DAMP ); - BIND_CONSTANT( AREA_PARAM_PRIORITY ); + BIND_CONSTANT(AREA_SPACE_OVERRIDE_DISABLED); + BIND_CONSTANT(AREA_SPACE_OVERRIDE_COMBINE); + BIND_CONSTANT(AREA_SPACE_OVERRIDE_COMBINE_REPLACE); + BIND_CONSTANT(AREA_SPACE_OVERRIDE_REPLACE); + BIND_CONSTANT(AREA_SPACE_OVERRIDE_REPLACE_COMBINE); - BIND_CONSTANT( AREA_SPACE_OVERRIDE_DISABLED ); - BIND_CONSTANT( AREA_SPACE_OVERRIDE_COMBINE ); - BIND_CONSTANT( AREA_SPACE_OVERRIDE_COMBINE_REPLACE ); - BIND_CONSTANT( AREA_SPACE_OVERRIDE_REPLACE ); - BIND_CONSTANT( AREA_SPACE_OVERRIDE_REPLACE_COMBINE ); + BIND_CONSTANT(BODY_MODE_STATIC); + BIND_CONSTANT(BODY_MODE_KINEMATIC); + BIND_CONSTANT(BODY_MODE_RIGID); + BIND_CONSTANT(BODY_MODE_CHARACTER); - BIND_CONSTANT( BODY_MODE_STATIC ); - BIND_CONSTANT( BODY_MODE_KINEMATIC ); - BIND_CONSTANT( BODY_MODE_RIGID ); - BIND_CONSTANT( BODY_MODE_CHARACTER ); + BIND_CONSTANT(BODY_PARAM_BOUNCE); + BIND_CONSTANT(BODY_PARAM_FRICTION); + BIND_CONSTANT(BODY_PARAM_MASS); + BIND_CONSTANT(BODY_PARAM_GRAVITY_SCALE); + BIND_CONSTANT(BODY_PARAM_ANGULAR_DAMP); + BIND_CONSTANT(BODY_PARAM_LINEAR_DAMP); + BIND_CONSTANT(BODY_PARAM_MAX); - BIND_CONSTANT( BODY_PARAM_BOUNCE ); - BIND_CONSTANT( BODY_PARAM_FRICTION ); - BIND_CONSTANT( BODY_PARAM_MASS ); - BIND_CONSTANT( BODY_PARAM_GRAVITY_SCALE ); - BIND_CONSTANT( BODY_PARAM_ANGULAR_DAMP ); - BIND_CONSTANT( BODY_PARAM_LINEAR_DAMP ); - BIND_CONSTANT( BODY_PARAM_MAX ); - - BIND_CONSTANT( BODY_STATE_TRANSFORM ); - BIND_CONSTANT( BODY_STATE_LINEAR_VELOCITY ); - BIND_CONSTANT( BODY_STATE_ANGULAR_VELOCITY ); - BIND_CONSTANT( BODY_STATE_SLEEPING ); - BIND_CONSTANT( BODY_STATE_CAN_SLEEP ); -/* + BIND_CONSTANT(BODY_STATE_TRANSFORM); + BIND_CONSTANT(BODY_STATE_LINEAR_VELOCITY); + BIND_CONSTANT(BODY_STATE_ANGULAR_VELOCITY); + BIND_CONSTANT(BODY_STATE_SLEEPING); + BIND_CONSTANT(BODY_STATE_CAN_SLEEP); + /* BIND_CONSTANT( JOINT_PIN ); BIND_CONSTANT( JOINT_GROOVE ); BIND_CONSTANT( JOINT_DAMPED_SPRING ); @@ -742,26 +696,21 @@ void PhysicsServer::_bind_methods() { //BIND_CONSTANT( TYPE_BODY ); //BIND_CONSTANT( TYPE_AREA ); - BIND_CONSTANT( AREA_BODY_ADDED ); - BIND_CONSTANT( AREA_BODY_REMOVED ); - - BIND_CONSTANT( INFO_ACTIVE_OBJECTS ); - BIND_CONSTANT( INFO_COLLISION_PAIRS ); - BIND_CONSTANT( INFO_ISLAND_COUNT ); - - + BIND_CONSTANT(AREA_BODY_ADDED); + BIND_CONSTANT(AREA_BODY_REMOVED); + BIND_CONSTANT(INFO_ACTIVE_OBJECTS); + BIND_CONSTANT(INFO_COLLISION_PAIRS); + BIND_CONSTANT(INFO_ISLAND_COUNT); } - PhysicsServer::PhysicsServer() { - ERR_FAIL_COND( singleton!=NULL ); - singleton=this; + ERR_FAIL_COND(singleton != NULL); + singleton = this; } PhysicsServer::~PhysicsServer() { - singleton=NULL; + singleton = NULL; } - diff --git a/servers/physics_server.h b/servers/physics_server.h index 653959be4..dbd0c79f0 100644 --- a/servers/physics_server.h +++ b/servers/physics_server.h @@ -36,81 +36,81 @@ class PhysicsDirectSpaceState; class PhysicsDirectBodyState : public Object { - GDCLASS( PhysicsDirectBodyState, Object ); + GDCLASS(PhysicsDirectBodyState, Object); + protected: static void _bind_methods(); -public: - virtual Vector3 get_total_gravity() const=0; - virtual float get_total_angular_damp() const=0; - virtual float get_total_linear_damp() const=0; +public: + virtual Vector3 get_total_gravity() const = 0; + virtual float get_total_angular_damp() const = 0; + virtual float get_total_linear_damp() const = 0; - virtual Vector3 get_center_of_mass() const=0; - virtual Basis get_principal_inertia_axes() const=0; - virtual float get_inverse_mass() const=0; // get the mass - virtual Vector3 get_inverse_inertia() const=0; // get density of this body space - virtual Basis get_inverse_inertia_tensor() const=0; // get density of this body space + virtual Vector3 get_center_of_mass() const = 0; + virtual Basis get_principal_inertia_axes() const = 0; + virtual float get_inverse_mass() const = 0; // get the mass + virtual Vector3 get_inverse_inertia() const = 0; // get density of this body space + virtual Basis get_inverse_inertia_tensor() const = 0; // get density of this body space - virtual void set_linear_velocity(const Vector3& p_velocity)=0; - virtual Vector3 get_linear_velocity() const=0; + virtual void set_linear_velocity(const Vector3 &p_velocity) = 0; + virtual Vector3 get_linear_velocity() const = 0; - virtual void set_angular_velocity(const Vector3& p_velocity)=0; - virtual Vector3 get_angular_velocity() const=0; + virtual void set_angular_velocity(const Vector3 &p_velocity) = 0; + virtual Vector3 get_angular_velocity() const = 0; - virtual void set_transform(const Transform& p_transform)=0; - virtual Transform get_transform() const=0; + virtual void set_transform(const Transform &p_transform) = 0; + virtual Transform get_transform() const = 0; - virtual void add_force(const Vector3& p_force, const Vector3& p_pos)=0; - virtual void apply_impulse(const Vector3& p_pos, const Vector3& p_j)=0; - virtual void apply_torque_impulse(const Vector3& p_j)=0; + virtual void add_force(const Vector3 &p_force, const Vector3 &p_pos) = 0; + virtual void apply_impulse(const Vector3 &p_pos, const Vector3 &p_j) = 0; + virtual void apply_torque_impulse(const Vector3 &p_j) = 0; - virtual void set_sleep_state(bool p_enable)=0; - virtual bool is_sleeping() const=0; + virtual void set_sleep_state(bool p_enable) = 0; + virtual bool is_sleeping() const = 0; - virtual int get_contact_count() const=0; + virtual int get_contact_count() const = 0; - virtual Vector3 get_contact_local_pos(int p_contact_idx) const=0; - virtual Vector3 get_contact_local_normal(int p_contact_idx) const=0; - virtual int get_contact_local_shape(int p_contact_idx) const=0; + virtual Vector3 get_contact_local_pos(int p_contact_idx) const = 0; + virtual Vector3 get_contact_local_normal(int p_contact_idx) const = 0; + virtual int get_contact_local_shape(int p_contact_idx) const = 0; - virtual RID get_contact_collider(int p_contact_idx) const=0; - virtual Vector3 get_contact_collider_pos(int p_contact_idx) const=0; - virtual ObjectID get_contact_collider_id(int p_contact_idx) const=0; - virtual Object* get_contact_collider_object(int p_contact_idx) const; - virtual int get_contact_collider_shape(int p_contact_idx) const=0; - virtual Vector3 get_contact_collider_velocity_at_pos(int p_contact_idx) const=0; + virtual RID get_contact_collider(int p_contact_idx) const = 0; + virtual Vector3 get_contact_collider_pos(int p_contact_idx) const = 0; + virtual ObjectID get_contact_collider_id(int p_contact_idx) const = 0; + virtual Object *get_contact_collider_object(int p_contact_idx) const; + virtual int get_contact_collider_shape(int p_contact_idx) const = 0; + virtual Vector3 get_contact_collider_velocity_at_pos(int p_contact_idx) const = 0; - virtual real_t get_step() const=0; + virtual real_t get_step() const = 0; virtual void integrate_forces(); - virtual PhysicsDirectSpaceState* get_space_state()=0; + virtual PhysicsDirectSpaceState *get_space_state() = 0; PhysicsDirectBodyState(); }; - class PhysicsShapeQueryResult; class PhysicsShapeQueryParameters : public Reference { GDCLASS(PhysicsShapeQueryParameters, Reference); -friend class PhysicsDirectSpaceState; + friend class PhysicsDirectSpaceState; RID shape; Transform transform; float margin; Set<RID> exclude; uint32_t layer_mask; uint32_t object_type_mask; + protected: static void _bind_methods(); -public: - - void set_shape(const RES& p_shape); - void set_shape_rid(const RID& p_shape); +public: + void set_shape(const RES &p_shape); + void set_shape_rid(const RID &p_shape); RID get_shape_rid() const; - void set_transform(const Transform& p_transform); + void set_transform(const Transform &p_transform); Transform get_transform() const; void set_margin(float p_margin); @@ -122,47 +122,39 @@ public: void set_object_type_mask(int p_object_type_mask); int get_object_type_mask() const; - void set_exclude(const Vector<RID>& p_exclude); + void set_exclude(const Vector<RID> &p_exclude); Vector<RID> get_exclude() const; PhysicsShapeQueryParameters(); - }; - - class PhysicsDirectSpaceState : public Object { - GDCLASS( PhysicsDirectSpaceState, Object ); + GDCLASS(PhysicsDirectSpaceState, Object); //Variant _intersect_ray(const Vector3& p_from, const Vector3& p_to,const Vector<RID>& p_exclude=Vector<RID>(),uint32_t p_collision_mask=0); //Variant _intersect_shape(const RID& p_shape, const Transform& p_xform,int p_result_max=64,const Vector<RID>& p_exclude=Vector<RID>(),uint32_t p_collision_mask=0); public: - enum ObjectTypeMask { - TYPE_MASK_STATIC_BODY=1<<0, - TYPE_MASK_KINEMATIC_BODY=1<<1, - TYPE_MASK_RIGID_BODY=1<<2, - TYPE_MASK_CHARACTER_BODY=1<<3, - TYPE_MASK_AREA=1<<4, - TYPE_MASK_COLLISION=TYPE_MASK_STATIC_BODY|TYPE_MASK_CHARACTER_BODY|TYPE_MASK_KINEMATIC_BODY|TYPE_MASK_RIGID_BODY + TYPE_MASK_STATIC_BODY = 1 << 0, + TYPE_MASK_KINEMATIC_BODY = 1 << 1, + TYPE_MASK_RIGID_BODY = 1 << 2, + TYPE_MASK_CHARACTER_BODY = 1 << 3, + TYPE_MASK_AREA = 1 << 4, + TYPE_MASK_COLLISION = TYPE_MASK_STATIC_BODY | TYPE_MASK_CHARACTER_BODY | TYPE_MASK_KINEMATIC_BODY | TYPE_MASK_RIGID_BODY }; - private: - Dictionary _intersect_ray(const Vector3& p_from, const Vector3& p_to,const Vector<RID>& p_exclude=Vector<RID>(),uint32_t p_layers=0,uint32_t p_object_type_mask=TYPE_MASK_COLLISION); - Array _intersect_shape(const Ref<PhysicsShapeQueryParameters> &p_shape_query,int p_max_results=32); - Array _cast_motion(const Ref<PhysicsShapeQueryParameters> &p_shape_query,const Vector3& p_motion); - Array _collide_shape(const Ref<PhysicsShapeQueryParameters> &p_shape_query,int p_max_results=32); + Dictionary _intersect_ray(const Vector3 &p_from, const Vector3 &p_to, const Vector<RID> &p_exclude = Vector<RID>(), uint32_t p_layers = 0, uint32_t p_object_type_mask = TYPE_MASK_COLLISION); + Array _intersect_shape(const Ref<PhysicsShapeQueryParameters> &p_shape_query, int p_max_results = 32); + Array _cast_motion(const Ref<PhysicsShapeQueryParameters> &p_shape_query, const Vector3 &p_motion); + Array _collide_shape(const Ref<PhysicsShapeQueryParameters> &p_shape_query, int p_max_results = 32); Dictionary _get_rest_info(const Ref<PhysicsShapeQueryParameters> &p_shape_query); - protected: static void _bind_methods(); public: - - struct RayResult { Vector3 position; @@ -173,7 +165,7 @@ public: int shape; }; - virtual bool intersect_ray(const Vector3& p_from, const Vector3& p_to,RayResult &r_result,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION,bool p_pick_ray=false)=0; + virtual bool intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_layer_mask = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION, bool p_pick_ray = false) = 0; struct ShapeResult { @@ -181,10 +173,9 @@ public: ObjectID collider_id; Object *collider; int shape; - }; - virtual int intersect_shape(const RID& p_shape, const Transform& p_xform,float 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)=0; + virtual int intersect_shape(const RID &p_shape, const Transform &p_xform, float 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) = 0; struct ShapeRestInfo { @@ -194,55 +185,49 @@ public: ObjectID collider_id; int shape; Vector3 linear_velocity; //velocity at contact point - }; - virtual bool cast_motion(const RID& p_shape, const Transform& p_xform,const Vector3& p_motion,float p_margin,float &p_closest_safe,float &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,ShapeRestInfo *r_info=NULL)=0; - - virtual bool collide_shape(RID p_shape, const Transform& p_shape_xform,float p_margin,Vector3 *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)=0; + virtual bool cast_motion(const RID &p_shape, const Transform &p_xform, const Vector3 &p_motion, float p_margin, float &p_closest_safe, float &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, ShapeRestInfo *r_info = NULL) = 0; - virtual bool rest_info(RID p_shape, const Transform& p_shape_xform,float 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)=0; + virtual bool collide_shape(RID p_shape, const Transform &p_shape_xform, float p_margin, Vector3 *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) = 0; + virtual bool rest_info(RID p_shape, const Transform &p_shape_xform, float 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) = 0; PhysicsDirectSpaceState(); }; - class PhysicsShapeQueryResult : public Reference { - GDCLASS( PhysicsShapeQueryResult, Reference ); + GDCLASS(PhysicsShapeQueryResult, Reference); Vector<PhysicsDirectSpaceState::ShapeResult> result; -friend class PhysicsDirectSpaceState; + friend class PhysicsDirectSpaceState; protected: static void _bind_methods(); -public: +public: int get_result_count() const; RID get_result_rid(int p_idx) const; ObjectID get_result_object_id(int p_idx) const; - Object* get_result_object(int p_idx) const; + Object *get_result_object(int p_idx) const; int get_result_object_shape(int p_idx) const; PhysicsShapeQueryResult(); }; - class PhysicsServer : public Object { - GDCLASS( PhysicsServer, Object ); - - static PhysicsServer * singleton; + GDCLASS(PhysicsServer, Object); + static PhysicsServer *singleton; protected: static void _bind_methods(); public: - - static PhysicsServer * get_singleton(); + static PhysicsServer *get_singleton(); enum ShapeType { SHAPE_PLANE, ///< plane:"plane" @@ -256,20 +241,19 @@ public: SHAPE_CUSTOM, ///< Server-Implementation based custom shape, calling shape_create() with this value will result in an error }; - virtual RID shape_create(ShapeType p_shape)=0; - virtual void shape_set_data(RID p_shape, const Variant& p_data)=0; - virtual void shape_set_custom_solver_bias(RID p_shape, real_t p_bias)=0; - - virtual ShapeType shape_get_type(RID p_shape) const=0; - virtual Variant shape_get_data(RID p_shape) const=0; - virtual real_t shape_get_custom_solver_bias(RID p_shape) const=0; + virtual RID shape_create(ShapeType p_shape) = 0; + virtual void shape_set_data(RID p_shape, const Variant &p_data) = 0; + virtual void shape_set_custom_solver_bias(RID p_shape, real_t p_bias) = 0; + virtual ShapeType shape_get_type(RID p_shape) const = 0; + virtual Variant shape_get_data(RID p_shape) const = 0; + virtual real_t shape_get_custom_solver_bias(RID p_shape) const = 0; /* SPACE API */ - virtual RID space_create()=0; - virtual void space_set_active(RID p_space,bool p_active)=0; - virtual bool space_is_active(RID p_space) const=0; + virtual RID space_create() = 0; + virtual void space_set_active(RID p_space, bool p_active) = 0; + virtual bool space_is_active(RID p_space) const = 0; enum SpaceParameter { @@ -283,15 +267,15 @@ public: SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS, }; - virtual void space_set_param(RID p_space,SpaceParameter p_param, real_t p_value)=0; - virtual real_t space_get_param(RID p_space,SpaceParameter p_param) const=0; + virtual void space_set_param(RID p_space, SpaceParameter p_param, real_t p_value) = 0; + virtual real_t space_get_param(RID p_space, SpaceParameter p_param) const = 0; // this function only works on fixed process, errors and returns null otherwise - virtual PhysicsDirectSpaceState* space_get_direct_state(RID p_space)=0; + virtual PhysicsDirectSpaceState *space_get_direct_state(RID p_space) = 0; - virtual void space_set_debug_contacts(RID p_space,int p_max_contacts)=0; - virtual Vector<Vector3> space_get_contacts(RID p_space) const=0; - virtual int space_get_contact_count(RID p_space) const=0; + virtual void space_set_debug_contacts(RID p_space, int p_max_contacts) = 0; + virtual Vector<Vector3> space_get_contacts(RID p_space) const = 0; + virtual int space_get_contact_count(RID p_space) const = 0; //missing space parameters @@ -299,8 +283,6 @@ public: //missing attenuation? missing better override? - - enum AreaParameter { AREA_PARAM_GRAVITY, AREA_PARAM_GRAVITY_VECTOR, @@ -312,11 +294,10 @@ public: AREA_PARAM_PRIORITY }; - virtual RID area_create()=0; - - virtual void area_set_space(RID p_area, RID p_space)=0; - virtual RID area_get_space(RID p_area) const=0; + virtual RID area_create() = 0; + virtual void area_set_space(RID p_area, RID p_space) = 0; + virtual RID area_get_space(RID p_area) const = 0; enum AreaSpaceOverrideMode { AREA_SPACE_OVERRIDE_DISABLED, @@ -326,39 +307,39 @@ public: AREA_SPACE_OVERRIDE_REPLACE_COMBINE }; - virtual void area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode)=0; - virtual AreaSpaceOverrideMode area_get_space_override_mode(RID p_area) const=0; + virtual void area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode) = 0; + virtual AreaSpaceOverrideMode area_get_space_override_mode(RID p_area) const = 0; - virtual void area_add_shape(RID p_area, RID p_shape, const Transform& p_transform=Transform())=0; - virtual void area_set_shape(RID p_area, int p_shape_idx,RID p_shape)=0; - virtual void area_set_shape_transform(RID p_area, int p_shape_idx, const Transform& p_transform)=0; + virtual void area_add_shape(RID p_area, RID p_shape, const Transform &p_transform = Transform()) = 0; + virtual void area_set_shape(RID p_area, int p_shape_idx, RID p_shape) = 0; + virtual void area_set_shape_transform(RID p_area, int p_shape_idx, const Transform &p_transform) = 0; - virtual int area_get_shape_count(RID p_area) const=0; - virtual RID area_get_shape(RID p_area, int p_shape_idx) const=0; - virtual Transform area_get_shape_transform(RID p_area, int p_shape_idx) const=0; + virtual int area_get_shape_count(RID p_area) const = 0; + virtual RID area_get_shape(RID p_area, int p_shape_idx) const = 0; + virtual Transform area_get_shape_transform(RID p_area, int p_shape_idx) const = 0; - virtual void area_remove_shape(RID p_area, int p_shape_idx)=0; - virtual void area_clear_shapes(RID p_area)=0; + virtual void area_remove_shape(RID p_area, int p_shape_idx) = 0; + virtual void area_clear_shapes(RID p_area) = 0; - virtual void area_attach_object_instance_ID(RID p_area,ObjectID p_ID)=0; - virtual ObjectID area_get_object_instance_ID(RID p_area) const=0; + virtual void area_attach_object_instance_ID(RID p_area, ObjectID p_ID) = 0; + virtual ObjectID area_get_object_instance_ID(RID p_area) const = 0; - virtual void area_set_param(RID p_area,AreaParameter p_param,const Variant& p_value)=0; - virtual void area_set_transform(RID p_area, const Transform& p_transform)=0; + virtual void area_set_param(RID p_area, AreaParameter p_param, const Variant &p_value) = 0; + virtual void area_set_transform(RID p_area, const Transform &p_transform) = 0; - virtual Variant area_get_param(RID p_parea,AreaParameter p_param) const=0; - virtual Transform area_get_transform(RID p_area) const=0; + virtual Variant area_get_param(RID p_parea, AreaParameter p_param) const = 0; + virtual Transform area_get_transform(RID p_area) const = 0; - virtual void area_set_collision_mask(RID p_area,uint32_t p_mask)=0; - virtual void area_set_layer_mask(RID p_area,uint32_t p_mask)=0; + virtual void area_set_collision_mask(RID p_area, uint32_t p_mask) = 0; + virtual void area_set_layer_mask(RID p_area, uint32_t p_mask) = 0; - virtual void area_set_monitorable(RID p_area,bool p_monitorable)=0; + virtual void area_set_monitorable(RID p_area, bool p_monitorable) = 0; - virtual void area_set_monitor_callback(RID p_area,Object *p_receiver,const StringName& p_method)=0; - virtual void area_set_area_monitor_callback(RID p_area,Object *p_receiver,const StringName& p_method)=0; + virtual void area_set_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) = 0; + virtual void area_set_area_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) = 0; - virtual void area_set_ray_pickable(RID p_area,bool p_enable)=0; - virtual bool area_is_ray_pickable(RID p_area) const=0; + virtual void area_set_ray_pickable(RID p_area, bool p_enable) = 0; + virtual bool area_is_ray_pickable(RID p_area) const = 0; /* BODY API */ @@ -372,42 +353,42 @@ public: BODY_MODE_CHARACTER }; - virtual RID body_create(BodyMode p_mode=BODY_MODE_RIGID,bool p_init_sleeping=false)=0; + virtual RID body_create(BodyMode p_mode = BODY_MODE_RIGID, bool p_init_sleeping = false) = 0; - virtual void body_set_space(RID p_body, RID p_space)=0; - virtual RID body_get_space(RID p_body) const=0; + virtual void body_set_space(RID p_body, RID p_space) = 0; + virtual RID body_get_space(RID p_body) const = 0; - virtual void body_set_mode(RID p_body, BodyMode p_mode)=0; - virtual BodyMode body_get_mode(RID p_body) const=0; + virtual void body_set_mode(RID p_body, BodyMode p_mode) = 0; + virtual BodyMode body_get_mode(RID p_body) const = 0; - virtual void body_add_shape(RID p_body, RID p_shape, const Transform& p_transform=Transform())=0; - virtual void body_set_shape(RID p_body, int p_shape_idx,RID p_shape)=0; - virtual void body_set_shape_transform(RID p_body, int p_shape_idx, const Transform& p_transform)=0; + virtual void body_add_shape(RID p_body, RID p_shape, const Transform &p_transform = Transform()) = 0; + virtual void body_set_shape(RID p_body, int p_shape_idx, RID p_shape) = 0; + virtual void body_set_shape_transform(RID p_body, int p_shape_idx, const Transform &p_transform) = 0; - virtual int body_get_shape_count(RID p_body) const=0; - virtual RID body_get_shape(RID p_body, int p_shape_idx) const=0; - virtual Transform body_get_shape_transform(RID p_body, int p_shape_idx) const=0; + virtual int body_get_shape_count(RID p_body) const = 0; + virtual RID body_get_shape(RID p_body, int p_shape_idx) const = 0; + virtual Transform body_get_shape_transform(RID p_body, int p_shape_idx) const = 0; - virtual void body_set_shape_as_trigger(RID p_body, int p_shape_idx,bool p_enable)=0; - virtual bool body_is_shape_set_as_trigger(RID p_body, int p_shape_idx) const=0; + virtual void body_set_shape_as_trigger(RID p_body, int p_shape_idx, bool p_enable) = 0; + virtual bool body_is_shape_set_as_trigger(RID p_body, int p_shape_idx) const = 0; - virtual void body_remove_shape(RID p_body, int p_shape_idx)=0; - virtual void body_clear_shapes(RID p_body)=0; + virtual void body_remove_shape(RID p_body, int p_shape_idx) = 0; + virtual void body_clear_shapes(RID p_body) = 0; - virtual void body_attach_object_instance_ID(RID p_body,uint32_t p_ID)=0; - virtual uint32_t body_get_object_instance_ID(RID p_body) const=0; + virtual void body_attach_object_instance_ID(RID p_body, uint32_t p_ID) = 0; + virtual uint32_t body_get_object_instance_ID(RID p_body) const = 0; - virtual void body_set_enable_continuous_collision_detection(RID p_body,bool p_enable)=0; - virtual bool body_is_continuous_collision_detection_enabled(RID p_body) const=0; + virtual void body_set_enable_continuous_collision_detection(RID p_body, bool p_enable) = 0; + virtual bool body_is_continuous_collision_detection_enabled(RID p_body) const = 0; - virtual void body_set_layer_mask(RID p_body, uint32_t p_mask)=0; - virtual uint32_t body_get_layer_mask(RID p_body, uint32_t p_mask) const=0; + virtual void body_set_layer_mask(RID p_body, uint32_t p_mask) = 0; + virtual uint32_t body_get_layer_mask(RID p_body, uint32_t p_mask) const = 0; - virtual void body_set_collision_mask(RID p_body, uint32_t p_mask)=0; - virtual uint32_t body_get_collision_mask(RID p_body, uint32_t p_mask) const=0; + virtual void body_set_collision_mask(RID p_body, uint32_t p_mask) = 0; + virtual uint32_t body_get_collision_mask(RID p_body, uint32_t p_mask) const = 0; - virtual void body_set_user_flags(RID p_body, uint32_t p_flags)=0; - virtual uint32_t body_get_user_flags(RID p_body, uint32_t p_flags) const=0; + virtual void body_set_user_flags(RID p_body, uint32_t p_flags) = 0; + virtual uint32_t body_get_user_flags(RID p_body, uint32_t p_flags) const = 0; // common body variables enum BodyParameter { @@ -420,9 +401,8 @@ public: BODY_PARAM_MAX, }; - virtual void body_set_param(RID p_body, BodyParameter p_param, float p_value)=0; - virtual float body_get_param(RID p_body, BodyParameter p_param) const=0; - + virtual void body_set_param(RID p_body, BodyParameter p_param, float p_value) = 0; + virtual float body_get_param(RID p_body, BodyParameter p_param) const = 0; //state enum BodyState { @@ -433,19 +413,19 @@ public: BODY_STATE_CAN_SLEEP }; - virtual void body_set_state(RID p_body, BodyState p_state, const Variant& p_variant)=0; - virtual Variant body_get_state(RID p_body, BodyState p_state) const=0; + virtual void body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) = 0; + virtual Variant body_get_state(RID p_body, BodyState p_state) const = 0; //do something about it - virtual void body_set_applied_force(RID p_body, const Vector3& p_force)=0; - virtual Vector3 body_get_applied_force(RID p_body) const=0; + virtual void body_set_applied_force(RID p_body, const Vector3 &p_force) = 0; + virtual Vector3 body_get_applied_force(RID p_body) const = 0; - virtual void body_set_applied_torque(RID p_body, const Vector3& p_torque)=0; - virtual Vector3 body_get_applied_torque(RID p_body) const=0; + virtual void body_set_applied_torque(RID p_body, const Vector3 &p_torque) = 0; + virtual Vector3 body_get_applied_torque(RID p_body) const = 0; - virtual void body_apply_impulse(RID p_body, const Vector3& p_pos, const Vector3& p_impulse)=0; - virtual void body_apply_torque_impulse(RID p_body, const Vector3& p_impulse)=0; - virtual void body_set_axis_velocity(RID p_body, const Vector3& p_axis_velocity)=0; + virtual void body_apply_impulse(RID p_body, const Vector3 &p_pos, const Vector3 &p_impulse) = 0; + virtual void body_apply_torque_impulse(RID p_body, const Vector3 &p_impulse) = 0; + virtual void body_set_axis_velocity(RID p_body, const Vector3 &p_axis_velocity) = 0; enum BodyAxisLock { BODY_AXIS_LOCK_DISABLED, @@ -454,29 +434,28 @@ public: BODY_AXIS_LOCK_Z, }; - virtual void body_set_axis_lock(RID p_body,BodyAxisLock p_lock)=0; - virtual BodyAxisLock body_get_axis_lock(RID p_body) const=0; + virtual void body_set_axis_lock(RID p_body, BodyAxisLock p_lock) = 0; + virtual BodyAxisLock body_get_axis_lock(RID p_body) const = 0; //fix - virtual void body_add_collision_exception(RID p_body, RID p_body_b)=0; - virtual void body_remove_collision_exception(RID p_body, RID p_body_b)=0; - virtual void body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions)=0; + virtual void body_add_collision_exception(RID p_body, RID p_body_b) = 0; + virtual void body_remove_collision_exception(RID p_body, RID p_body_b) = 0; + virtual void body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) = 0; - virtual void body_set_max_contacts_reported(RID p_body, int p_contacts)=0; - virtual int body_get_max_contacts_reported(RID p_body) const=0; + virtual void body_set_max_contacts_reported(RID p_body, int p_contacts) = 0; + virtual int body_get_max_contacts_reported(RID p_body) const = 0; //missing remove - virtual void body_set_contacts_reported_depth_treshold(RID p_body, float p_treshold)=0; - virtual float body_get_contacts_reported_depth_treshold(RID p_body) const=0; - - virtual void body_set_omit_force_integration(RID p_body,bool p_omit)=0; - virtual bool body_is_omitting_force_integration(RID p_body) const=0; + virtual void body_set_contacts_reported_depth_treshold(RID p_body, float p_treshold) = 0; + virtual float body_get_contacts_reported_depth_treshold(RID p_body) const = 0; - virtual void body_set_force_integration_callback(RID p_body,Object *p_receiver,const StringName& p_method,const Variant& p_udata=Variant())=0; + virtual void body_set_omit_force_integration(RID p_body, bool p_omit) = 0; + virtual bool body_is_omitting_force_integration(RID p_body) const = 0; - virtual void body_set_ray_pickable(RID p_body,bool p_enable)=0; - virtual bool body_is_ray_pickable(RID p_body) const=0; + virtual void body_set_force_integration_callback(RID p_body, Object *p_receiver, const StringName &p_method, const Variant &p_udata = Variant()) = 0; + virtual void body_set_ray_pickable(RID p_body, bool p_enable) = 0; + virtual bool body_is_ray_pickable(RID p_body) const = 0; /* JOINT API */ @@ -490,13 +469,12 @@ public: }; - virtual JointType joint_get_type(RID p_joint) const=0; - - virtual void joint_set_solver_priority(RID p_joint,int p_priority)=0; - virtual int joint_get_solver_priority(RID p_joint) const=0; + virtual JointType joint_get_type(RID p_joint) const = 0; + virtual void joint_set_solver_priority(RID p_joint, int p_priority) = 0; + virtual int joint_get_solver_priority(RID p_joint) const = 0; - virtual RID joint_create_pin(RID p_body_A,const Vector3& p_local_A,RID p_body_B,const Vector3& p_local_B)=0; + virtual RID joint_create_pin(RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B) = 0; enum PinJointParam { PIN_JOINT_BIAS, @@ -504,14 +482,14 @@ public: PIN_JOINT_IMPULSE_CLAMP }; - virtual void pin_joint_set_param(RID p_joint,PinJointParam p_param, float p_value)=0; - virtual float pin_joint_get_param(RID p_joint,PinJointParam p_param) const=0; + virtual void pin_joint_set_param(RID p_joint, PinJointParam p_param, float p_value) = 0; + virtual float pin_joint_get_param(RID p_joint, PinJointParam p_param) const = 0; - virtual void pin_joint_set_local_A(RID p_joint, const Vector3& p_A)=0; - virtual Vector3 pin_joint_get_local_A(RID p_joint) const=0; + virtual void pin_joint_set_local_A(RID p_joint, const Vector3 &p_A) = 0; + virtual Vector3 pin_joint_get_local_A(RID p_joint) const = 0; - virtual void pin_joint_set_local_B(RID p_joint, const Vector3& p_B)=0; - virtual Vector3 pin_joint_get_local_B(RID p_joint) const=0; + virtual void pin_joint_set_local_B(RID p_joint, const Vector3 &p_B) = 0; + virtual Vector3 pin_joint_get_local_B(RID p_joint) const = 0; enum HingeJointParam { @@ -532,15 +510,14 @@ public: HINGE_JOINT_FLAG_MAX }; - virtual RID joint_create_hinge(RID p_body_A,const Transform& p_hinge_A,RID p_body_B,const Transform& p_hinge_B)=0; - virtual RID joint_create_hinge_simple(RID p_body_A,const Vector3& p_pivot_A,const Vector3& p_axis_A,RID p_body_B,const Vector3& p_pivot_B,const Vector3& p_axis_B)=0; - + virtual RID joint_create_hinge(RID p_body_A, const Transform &p_hinge_A, RID p_body_B, const Transform &p_hinge_B) = 0; + virtual RID joint_create_hinge_simple(RID p_body_A, const Vector3 &p_pivot_A, const Vector3 &p_axis_A, RID p_body_B, const Vector3 &p_pivot_B, const Vector3 &p_axis_B) = 0; - virtual void hinge_joint_set_param(RID p_joint,HingeJointParam p_param, float p_value)=0; - virtual float hinge_joint_get_param(RID p_joint,HingeJointParam p_param) const=0; + virtual void hinge_joint_set_param(RID p_joint, HingeJointParam p_param, float p_value) = 0; + virtual float hinge_joint_get_param(RID p_joint, HingeJointParam p_param) const = 0; - virtual void hinge_joint_set_flag(RID p_joint,HingeJointFlag p_flag, bool p_value)=0; - virtual bool hinge_joint_get_flag(RID p_joint,HingeJointFlag p_flag) const=0; + virtual void hinge_joint_set_flag(RID p_joint, HingeJointFlag p_flag, bool p_value) = 0; + virtual bool hinge_joint_get_flag(RID p_joint, HingeJointFlag p_flag) const = 0; enum SliderJointParam { SLIDER_JOINT_LINEAR_LIMIT_UPPER, @@ -568,13 +545,12 @@ public: SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING, SLIDER_JOINT_MAX - }; - virtual RID joint_create_slider(RID p_body_A,const Transform& p_local_frame_A,RID p_body_B,const Transform& p_local_frame_B)=0; //reference frame is A + virtual RID joint_create_slider(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) = 0; //reference frame is A - virtual void slider_joint_set_param(RID p_joint,SliderJointParam p_param, float p_value)=0; - virtual float slider_joint_get_param(RID p_joint,SliderJointParam p_param) const=0; + virtual void slider_joint_set_param(RID p_joint, SliderJointParam p_param, float p_value) = 0; + virtual float slider_joint_get_param(RID p_joint, SliderJointParam p_param) const = 0; enum ConeTwistJointParam { CONE_TWIST_JOINT_SWING_SPAN, @@ -585,12 +561,10 @@ public: CONE_TWIST_MAX }; + virtual RID joint_create_cone_twist(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) = 0; //reference frame is A - virtual RID joint_create_cone_twist(RID p_body_A,const Transform& p_local_frame_A,RID p_body_B,const Transform& p_local_frame_B)=0; //reference frame is A - - virtual void cone_twist_joint_set_param(RID p_joint,ConeTwistJointParam p_param, float p_value)=0; - virtual float cone_twist_joint_get_param(RID p_joint,ConeTwistJointParam p_param) const=0; - + virtual void cone_twist_joint_set_param(RID p_joint, ConeTwistJointParam p_param, float p_value) = 0; + virtual float cone_twist_joint_get_param(RID p_joint, ConeTwistJointParam p_param) const = 0; enum G6DOFJointAxisParam { G6DOF_JOINT_LINEAR_LOWER_LIMIT, @@ -618,16 +592,13 @@ public: G6DOF_JOINT_FLAG_MAX }; + virtual RID joint_create_generic_6dof(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) = 0; //reference frame is A + virtual void generic_6dof_joint_set_param(RID p_joint, Vector3::Axis, G6DOFJointAxisParam p_param, float p_value) = 0; + virtual float generic_6dof_joint_get_param(RID p_joint, Vector3::Axis, G6DOFJointAxisParam p_param) = 0; - virtual RID joint_create_generic_6dof(RID p_body_A,const Transform& p_local_frame_A,RID p_body_B,const Transform& p_local_frame_B)=0; //reference frame is A - - virtual void generic_6dof_joint_set_param(RID p_joint,Vector3::Axis,G6DOFJointAxisParam p_param, float p_value)=0; - virtual float generic_6dof_joint_get_param(RID p_joint,Vector3::Axis,G6DOFJointAxisParam p_param)=0; - - virtual void generic_6dof_joint_set_flag(RID p_joint,Vector3::Axis,G6DOFJointAxisFlag p_flag, bool p_enable)=0; - virtual bool generic_6dof_joint_get_flag(RID p_joint,Vector3::Axis,G6DOFJointAxisFlag p_flag)=0; - + virtual void generic_6dof_joint_set_flag(RID p_joint, Vector3::Axis, G6DOFJointAxisFlag p_flag, bool p_enable) = 0; + virtual bool generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis, G6DOFJointAxisFlag p_flag) = 0; #if 0 enum JointType { @@ -667,17 +638,16 @@ public: AREA_BODY_REMOVED }; - /* MISC */ - virtual void free(RID p_rid)=0; + virtual void free(RID p_rid) = 0; - virtual void set_active(bool p_active)=0; - virtual void init()=0; - virtual void step(float p_step)=0; - virtual void sync()=0; - virtual void flush_queries()=0; - virtual void finish()=0; + virtual void set_active(bool p_active) = 0; + virtual void init() = 0; + virtual void step(float p_step) = 0; + virtual void sync() = 0; + virtual void flush_queries() = 0; + virtual void finish() = 0; enum ProcessInfo { @@ -686,30 +656,30 @@ public: INFO_ISLAND_COUNT }; - virtual int get_process_info(ProcessInfo p_info)=0; + virtual int get_process_info(ProcessInfo p_info) = 0; PhysicsServer(); ~PhysicsServer(); }; -VARIANT_ENUM_CAST( PhysicsServer::ShapeType ); -VARIANT_ENUM_CAST( PhysicsServer::SpaceParameter ); -VARIANT_ENUM_CAST( PhysicsServer::AreaParameter ); -VARIANT_ENUM_CAST( PhysicsServer::AreaSpaceOverrideMode ); -VARIANT_ENUM_CAST( PhysicsServer::BodyMode ); -VARIANT_ENUM_CAST( PhysicsServer::BodyParameter ); -VARIANT_ENUM_CAST( PhysicsServer::BodyState ); -VARIANT_ENUM_CAST( PhysicsServer::BodyAxisLock ); -VARIANT_ENUM_CAST( PhysicsServer::PinJointParam ); -VARIANT_ENUM_CAST( PhysicsServer::JointType ); -VARIANT_ENUM_CAST( PhysicsServer::HingeJointParam ); -VARIANT_ENUM_CAST( PhysicsServer::HingeJointFlag ); -VARIANT_ENUM_CAST( PhysicsServer::SliderJointParam ); -VARIANT_ENUM_CAST( PhysicsServer::ConeTwistJointParam ); -VARIANT_ENUM_CAST( PhysicsServer::G6DOFJointAxisParam ); -VARIANT_ENUM_CAST( PhysicsServer::G6DOFJointAxisFlag); +VARIANT_ENUM_CAST(PhysicsServer::ShapeType); +VARIANT_ENUM_CAST(PhysicsServer::SpaceParameter); +VARIANT_ENUM_CAST(PhysicsServer::AreaParameter); +VARIANT_ENUM_CAST(PhysicsServer::AreaSpaceOverrideMode); +VARIANT_ENUM_CAST(PhysicsServer::BodyMode); +VARIANT_ENUM_CAST(PhysicsServer::BodyParameter); +VARIANT_ENUM_CAST(PhysicsServer::BodyState); +VARIANT_ENUM_CAST(PhysicsServer::BodyAxisLock); +VARIANT_ENUM_CAST(PhysicsServer::PinJointParam); +VARIANT_ENUM_CAST(PhysicsServer::JointType); +VARIANT_ENUM_CAST(PhysicsServer::HingeJointParam); +VARIANT_ENUM_CAST(PhysicsServer::HingeJointFlag); +VARIANT_ENUM_CAST(PhysicsServer::SliderJointParam); +VARIANT_ENUM_CAST(PhysicsServer::ConeTwistJointParam); +VARIANT_ENUM_CAST(PhysicsServer::G6DOFJointAxisParam); +VARIANT_ENUM_CAST(PhysicsServer::G6DOFJointAxisFlag); //VARIANT_ENUM_CAST( PhysicsServer::ObjectType ); -VARIANT_ENUM_CAST( PhysicsServer::AreaBodyStatus ); -VARIANT_ENUM_CAST( PhysicsServer::ProcessInfo ); +VARIANT_ENUM_CAST(PhysicsServer::AreaBodyStatus); +VARIANT_ENUM_CAST(PhysicsServer::ProcessInfo); #endif |