diff options
Diffstat (limited to 'servers/physics_2d')
34 files changed, 1220 insertions, 79 deletions
diff --git a/servers/physics_2d/area_2d_sw.cpp b/servers/physics_2d/area_2d_sw.cpp index 65f3b80dd3..1a41cda482 100644 --- a/servers/physics_2d/area_2d_sw.cpp +++ b/servers/physics_2d/area_2d_sw.cpp @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -120,6 +120,7 @@ void Area2DSW::set_param(Physics2DServer::AreaParameter p_param, const Variant& 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; @@ -136,6 +137,7 @@ Variant Area2DSW::get_param(Physics2DServer::AreaParameter p_param) const { 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_LINEAR_DAMP: return linear_damp; case Physics2DServer::AREA_PARAM_ANGULAR_DAMP: return angular_damp; @@ -248,6 +250,7 @@ Area2DSW::Area2DSW() : CollisionObject2DSW(TYPE_AREA), monitor_query_list(this), gravity=9.80665; gravity_vector=Vector2(0,-1); gravity_is_point=false; + gravity_distance_scale=0; point_attenuation=1; angular_damp=1.0; diff --git a/servers/physics_2d/area_2d_sw.h b/servers/physics_2d/area_2d_sw.h index 26b7b2516c..6d99764c68 100644 --- a/servers/physics_2d/area_2d_sw.h +++ b/servers/physics_2d/area_2d_sw.h @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -46,6 +46,7 @@ class Area2DSW : public CollisionObject2DSW{ float gravity; Vector2 gravity_vector; bool gravity_is_point; + float gravity_distance_scale; float point_attenuation; float linear_damp; float angular_damp; @@ -139,6 +140,9 @@ public: _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(float scale) { gravity_distance_scale=scale; } + _FORCE_INLINE_ float get_gravity_distance_scale() const { return gravity_distance_scale; } + _FORCE_INLINE_ void set_point_attenuation(float p_point_attenuation) { point_attenuation=p_point_attenuation; } _FORCE_INLINE_ float get_point_attenuation() const { return point_attenuation; } diff --git a/servers/physics_2d/area_pair_2d_sw.cpp b/servers/physics_2d/area_pair_2d_sw.cpp index ed2e34c972..3b1705bd56 100644 --- a/servers/physics_2d/area_pair_2d_sw.cpp +++ b/servers/physics_2d/area_pair_2d_sw.cpp @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -32,7 +32,7 @@ bool AreaPair2DSW::setup(float p_step) { - bool result = 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) { @@ -102,7 +102,7 @@ AreaPair2DSW::~AreaPair2DSW() { bool Area2Pair2DSW::setup(float p_step) { - bool result = 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) { diff --git a/servers/physics_2d/area_pair_2d_sw.h b/servers/physics_2d/area_pair_2d_sw.h index 575490b109..59113c9162 100644 --- a/servers/physics_2d/area_pair_2d_sw.h +++ b/servers/physics_2d/area_pair_2d_sw.h @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/servers/physics_2d/body_2d_sw.cpp b/servers/physics_2d/body_2d_sw.cpp index 06d466ace8..38835c9a82 100644 --- a/servers/physics_2d/body_2d_sw.cpp +++ b/servers/physics_2d/body_2d_sw.cpp @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -275,10 +275,13 @@ void Body2DSW::set_state(Physics2DServer::BodyState p_state, const Variant& p_va Matrix32 t = p_variant; t.orthonormalize(); 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(); } break; case Physics2DServer::BODY_STATE_LINEAR_VELOCITY: { @@ -286,12 +289,14 @@ void Body2DSW::set_state(Physics2DServer::BodyState p_state, const Variant& p_va //if (mode==Physics2DServer::BODY_MODE_STATIC) // break; linear_velocity=p_variant; + wakeup(); } break; case Physics2DServer::BODY_STATE_ANGULAR_VELOCITY: { //if (mode!=Physics2DServer::BODY_MODE_RIGID) // break; angular_velocity=p_variant; + wakeup(); } break; case Physics2DServer::BODY_STATE_SLEEPING: { @@ -378,14 +383,16 @@ void Body2DSW::set_space(Space2DSW *p_space){ void Body2DSW::_compute_area_gravity(const Area2DSW *p_area) { if (p_area->is_gravity_point()) { - - gravity = (p_area->get_transform().xform(p_area->get_gravity_vector()) - get_transform().get_origin()).normalized() * p_area->get_gravity(); - + 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) ); + } else { + gravity += (p_area->get_transform().xform(p_area->get_gravity_vector()) - get_transform().get_origin()).normalized() * p_area->get_gravity(); + } } else { - gravity = p_area->get_gravity_vector() * p_area->get_gravity(); + gravity += p_area->get_gravity_vector() * p_area->get_gravity(); } - gravity*=gravity_scale; } void Body2DSW::integrate_forces(real_t p_step) { @@ -393,32 +400,39 @@ void Body2DSW::integrate_forces(real_t p_step) { if (mode==Physics2DServer::BODY_MODE_STATIC) return; - Area2DSW *current_area = get_space()->get_default_area(); - ERR_FAIL_COND(!current_area); + Area2DSW *def_area = get_space()->get_default_area(); + Area2DSW *damp_area = def_area; + ERR_FAIL_COND(!def_area); - int prio = current_area->get_priority(); int ac = areas.size(); + bool replace = false; + gravity=Vector2(0,0); if (ac) { + areas.sort(); const AreaCMP *aa = &areas[0]; - for(int i=0;i<ac;i++) { - if (aa[i].area->get_priority() > prio) { - current_area=aa[i].area; - prio=current_area->get_priority(); + damp_area = aa[ac-1].area; + for(int i=ac-1;i>=0;i--) { + _compute_area_gravity(aa[i].area); + if (aa[i].area->get_space_override_mode() == Physics2DServer::AREA_SPACE_OVERRIDE_REPLACE) { + replace = true; + break; } } } - - _compute_area_gravity(current_area); + if( !replace ) { + _compute_area_gravity(def_area); + } + gravity*=gravity_scale; if (angular_damp>=0) area_angular_damp=angular_damp; else - area_angular_damp=current_area->get_angular_damp(); + area_angular_damp=damp_area->get_angular_damp(); if (linear_damp>=0) area_linear_damp=linear_damp; else - area_linear_damp=current_area->get_linear_damp(); + area_linear_damp=damp_area->get_linear_damp(); Vector2 motion; bool do_motion=false; @@ -482,7 +496,8 @@ void Body2DSW::integrate_forces(real_t p_step) { _update_shapes_with_motion(motion); } - current_area=NULL; // clear the area, so it is set in the next frame + 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; } @@ -647,6 +662,7 @@ Body2DSW::Body2DSW() : CollisionObject2DSW(TYPE_BODY), active_list(this), inerti area_linear_damp=0; contact_count=0; gravity_scale=1.0; + using_one_way_cache=false; one_way_collision_max_depth=0.1; still_time=0; diff --git a/servers/physics_2d/body_2d_sw.h b/servers/physics_2d/body_2d_sw.h index 3b87be2737..c86bb51f1d 100644 --- a/servers/physics_2d/body_2d_sw.h +++ b/servers/physics_2d/body_2d_sw.h @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -81,6 +81,7 @@ class Body2DSW : public CollisionObject2DSW { bool active; bool can_sleep; bool first_time_kinematic; + bool using_one_way_cache; void _update_inertia(); virtual void _shapes_changed(); Matrix32 new_transform; @@ -91,13 +92,14 @@ class Body2DSW : public CollisionObject2DSW { struct AreaCMP { Area2DSW *area; - _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_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;} }; - VSet<AreaCMP> areas; + Vector<AreaCMP> areas; struct Contact { @@ -140,7 +142,7 @@ public: void set_force_integration_callback(ObjectID p_id, const StringName& p_method, const Variant &p_udata=Variant()); - _FORCE_INLINE_ void add_area(Area2DSW *p_area) { areas.insert(AreaCMP(p_area)); } + _FORCE_INLINE_ void add_area(Area2DSW *p_area) { areas.ordered_insert(AreaCMP(p_area)); } _FORCE_INLINE_ void remove_area(Area2DSW *p_area) { areas.erase(AreaCMP(p_area)); } _FORCE_INLINE_ void set_max_contacts_reported(int p_size) { contacts.resize(p_size); contact_count=0; if (mode==Physics2DServer::BODY_MODE_KINEMATIC && p_size) set_active(true);} @@ -200,6 +202,15 @@ public: void set_active(bool p_active); _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) + return; + set_active(true); + } + + + + void set_param(Physics2DServer::BodyParameter p_param, float); float get_param(Physics2DServer::BodyParameter p_param) const; @@ -219,12 +230,17 @@ public: _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; } + 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(float p_depth) { one_way_collision_max_depth=p_depth; } float 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; } + void set_space(Space2DSW *p_space); void update_inertias(); diff --git a/servers/physics_2d/body_pair_2d_sw.cpp b/servers/physics_2d/body_pair_2d_sw.cpp index c4d6abe5ac..6bfed134e6 100644 --- a/servers/physics_2d/body_pair_2d_sw.cpp +++ b/servers/physics_2d/body_pair_2d_sw.cpp @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -234,7 +234,7 @@ bool BodyPair2DSW::setup(float p_step) { //cannot collide - if ((A->get_layer_mask()&B->get_layer_mask())==0 || 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)) { + 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; } @@ -265,6 +265,8 @@ bool BodyPair2DSW::setup(float p_step) { } //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); if (!collided) { @@ -280,9 +282,68 @@ bool BodyPair2DSW::setup(float p_step) { collided=true; } - if (!collided) + if (!collided) { + oneway_disabled=false; return false; + } + + } + + if (oneway_disabled) + return false; + + //if (!prev_collided) { + { + + if (A->is_using_one_way_collision()) { + Vector2 direction = A->get_one_way_collision_direction(); + bool valid=false; + for(int i=0;i<contact_count;i++) { + Contact& c = contacts[i]; + + if (c.normal.dot(direction)<0) + continue; + if (B->get_linear_velocity().dot(direction)<0) + continue; + + if (!c.reused) { + continue; + } + + valid=true; + } + + if (!valid) { + 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; + for(int i=0;i<contact_count;i++) { + + Contact& c = contacts[i]; + if (c.normal.dot(direction)<0) + continue; + if (A->get_linear_velocity().dot(direction)<0) + continue; + + if (!c.reused) { + continue; + } + + valid=true; + } + if (!valid) { + collided=false; + oneway_disabled=true; + return false; + } + } } real_t max_penetration = space->get_contact_max_allowed_penetration(); @@ -472,6 +533,7 @@ BodyPair2DSW::BodyPair2DSW(Body2DSW *p_A, int p_shape_A,Body2DSW *p_B, int p_sha B->add_constraint(this,1); contact_count=0; collided=false; + oneway_disabled=false; } diff --git a/servers/physics_2d/body_pair_2d_sw.h b/servers/physics_2d/body_pair_2d_sw.h index 15d7e62d3a..a7fa287be4 100644 --- a/servers/physics_2d/body_pair_2d_sw.h +++ b/servers/physics_2d/body_pair_2d_sw.h @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -76,6 +76,7 @@ class BodyPair2DSW : public Constraint2DSW { Contact contacts[MAX_CONTACTS]; int contact_count; bool collided; + bool oneway_disabled; int cc; diff --git a/servers/physics_2d/broad_phase_2d_basic.cpp b/servers/physics_2d/broad_phase_2d_basic.cpp index 9641a986e8..e61b4735b9 100644 --- a/servers/physics_2d/broad_phase_2d_basic.cpp +++ b/servers/physics_2d/broad_phase_2d_basic.cpp @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/servers/physics_2d/broad_phase_2d_basic.h b/servers/physics_2d/broad_phase_2d_basic.h index ce15752251..cdee77ffd7 100644 --- a/servers/physics_2d/broad_phase_2d_basic.h +++ b/servers/physics_2d/broad_phase_2d_basic.h @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/servers/physics_2d/broad_phase_2d_hash_grid.cpp b/servers/physics_2d/broad_phase_2d_hash_grid.cpp index 0f08f63937..4651d485c2 100644 --- a/servers/physics_2d/broad_phase_2d_hash_grid.cpp +++ b/servers/physics_2d/broad_phase_2d_hash_grid.cpp @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -467,9 +467,10 @@ int BroadPhase2DHashGrid::cull_segment(const Vector2& p_from, const Vector2& p_t delta.x=cell_size/delta.x; delta.y=cell_size/delta.y; - Point2i pos = p_from.floor() / cell_size; - Point2i end = p_to.floor() / cell_size; - Point2i step = Vector2( SGN(dir.x), SGN(dir.y)); + Point2i pos = (p_from/cell_size).floor(); + Point2i end = (p_to/cell_size).floor(); + + Point2i step = Vector2( SGN(dir.x), SGN(dir.y) ); Vector2 max; diff --git a/servers/physics_2d/broad_phase_2d_hash_grid.h b/servers/physics_2d/broad_phase_2d_hash_grid.h index d530b35d5d..a2eecf7bbf 100644 --- a/servers/physics_2d/broad_phase_2d_hash_grid.h +++ b/servers/physics_2d/broad_phase_2d_hash_grid.h @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/servers/physics_2d/broad_phase_2d_sw.cpp b/servers/physics_2d/broad_phase_2d_sw.cpp index 7ded6ed01e..fd8e7254b6 100644 --- a/servers/physics_2d/broad_phase_2d_sw.cpp +++ b/servers/physics_2d/broad_phase_2d_sw.cpp @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/servers/physics_2d/broad_phase_2d_sw.h b/servers/physics_2d/broad_phase_2d_sw.h index 510f7db112..283d6941e1 100644 --- a/servers/physics_2d/broad_phase_2d_sw.h +++ b/servers/physics_2d/broad_phase_2d_sw.h @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/servers/physics_2d/collision_object_2d_sw.cpp b/servers/physics_2d/collision_object_2d_sw.cpp index eefc598b39..7c8e223c57 100644 --- a/servers/physics_2d/collision_object_2d_sw.cpp +++ b/servers/physics_2d/collision_object_2d_sw.cpp @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -226,7 +226,7 @@ CollisionObject2DSW::CollisionObject2DSW(Type p_type) { type=p_type; space=NULL; instance_id=0; - user_mask=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 0c91237876..f3432060b9 100644 --- a/servers/physics_2d/collision_object_2d_sw.h +++ b/servers/physics_2d/collision_object_2d_sw.h @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -65,7 +65,7 @@ private: Space2DSW *space; Matrix32 transform; Matrix32 inv_transform; - uint32_t user_mask; + uint32_t collision_mask; uint32_t layer_mask; bool _static; @@ -117,8 +117,8 @@ public: _FORCE_INLINE_ bool is_shape_set_as_trigger(int p_idx) const { return shapes[p_idx].trigger; } - void set_user_mask(uint32_t p_mask) {user_mask=p_mask;} - _FORCE_INLINE_ uint32_t get_user_mask() const { return user_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;} _FORCE_INLINE_ uint32_t get_layer_mask() const { return layer_mask; } @@ -133,6 +133,11 @@ public: 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 { + + return layer_mask&p_other->collision_mask || p_other->layer_mask&collision_mask; + } + virtual ~CollisionObject2DSW() {} }; diff --git a/servers/physics_2d/collision_solver_2d_sat.cpp b/servers/physics_2d/collision_solver_2d_sat.cpp index 9ed594e0eb..2525c6e942 100644 --- a/servers/physics_2d/collision_solver_2d_sat.cpp +++ b/servers/physics_2d/collision_solver_2d_sat.cpp @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/servers/physics_2d/collision_solver_2d_sat.h b/servers/physics_2d/collision_solver_2d_sat.h index be5a3dc79f..554f756738 100644 --- a/servers/physics_2d/collision_solver_2d_sat.h +++ b/servers/physics_2d/collision_solver_2d_sat.h @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/servers/physics_2d/collision_solver_2d_sw.cpp b/servers/physics_2d/collision_solver_2d_sw.cpp index 4c7e68d643..134ad04222 100644 --- a/servers/physics_2d/collision_solver_2d_sw.cpp +++ b/servers/physics_2d/collision_solver_2d_sw.cpp @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/servers/physics_2d/collision_solver_2d_sw.h b/servers/physics_2d/collision_solver_2d_sw.h index 07141b0d09..0cdd54f8b6 100644 --- a/servers/physics_2d/collision_solver_2d_sw.h +++ b/servers/physics_2d/collision_solver_2d_sw.h @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/servers/physics_2d/constraint_2d_sw.cpp b/servers/physics_2d/constraint_2d_sw.cpp index e97b5d794e..24d56ce9fe 100644 --- a/servers/physics_2d/constraint_2d_sw.cpp +++ b/servers/physics_2d/constraint_2d_sw.cpp @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/servers/physics_2d/constraint_2d_sw.h b/servers/physics_2d/constraint_2d_sw.h index 7abe49f5b4..a9145c382f 100644 --- a/servers/physics_2d/constraint_2d_sw.h +++ b/servers/physics_2d/constraint_2d_sw.h @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/servers/physics_2d/joints_2d_sw.cpp b/servers/physics_2d/joints_2d_sw.cpp index fea58b6e8d..b4c149e7e0 100644 --- a/servers/physics_2d/joints_2d_sw.cpp +++ b/servers/physics_2d/joints_2d_sw.cpp @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/servers/physics_2d/joints_2d_sw.h b/servers/physics_2d/joints_2d_sw.h index 0a9bf34250..2093be88c9 100644 --- a/servers/physics_2d/joints_2d_sw.h +++ b/servers/physics_2d/joints_2d_sw.h @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/servers/physics_2d/physics_2d_server_sw.cpp b/servers/physics_2d/physics_2d_server_sw.cpp index 883acd0200..b446f4928a 100644 --- a/servers/physics_2d/physics_2d_server_sw.cpp +++ b/servers/physics_2d/physics_2d_server_sw.cpp @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -30,7 +30,7 @@ #include "broad_phase_2d_basic.h" #include "broad_phase_2d_hash_grid.h" #include "collision_solver_2d_sw.h" - +#include "globals.h" RID Physics2DServerSW::shape_create(ShapeType p_shape) { Shape2DSW *shape=NULL; @@ -261,7 +261,7 @@ Physics2DDirectSpaceState* Physics2DServerSW::space_get_direct_state(RID p_space Space2DSW *space = space_owner.get(p_space); ERR_FAIL_COND_V(!space,NULL); - if (/*doing_sync ||*/ space->is_locked()) { + if ((using_threads && !doing_sync) || space->is_locked()) { ERR_EXPLAIN("Space state is inaccesible right now, wait for iteration or fixed process notification."); ERR_FAIL_V(NULL); @@ -480,6 +480,22 @@ void Physics2DServerSW::area_set_monitorable(RID p_area,bool p_monitorable) { } +void Physics2DServerSW::area_set_collision_mask(RID p_area,uint32_t p_mask) { + + Area2DSW *area = area_owner.get(p_area); + ERR_FAIL_COND(!area); + + area->set_collision_mask(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); + + area->set_layer_mask(p_mask); +} + void Physics2DServerSW::area_set_monitor_callback(RID p_area,Object *p_receiver,const StringName& p_method) { @@ -717,7 +733,7 @@ void Physics2DServerSW::body_set_layer_mask(RID p_body, uint32_t p_flags) { }; -uint32_t Physics2DServerSW::body_get_layer_mask(RID p_body, uint32_t p_flags) const { +uint32_t Physics2DServerSW::body_get_layer_mask(RID p_body) const { Body2DSW *body = body_owner.get(p_body); ERR_FAIL_COND_V(!body,0); @@ -726,20 +742,20 @@ uint32_t Physics2DServerSW::body_get_layer_mask(RID p_body, uint32_t p_flags) co }; -void Physics2DServerSW::body_set_user_mask(RID p_body, uint32_t p_flags) { +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_user_mask(p_flags); + body->set_collision_mask(p_flags); }; -uint32_t Physics2DServerSW::body_get_user_mask(RID p_body, uint32_t p_flags) const { +uint32_t Physics2DServerSW::body_get_collision_mask(RID p_body) const { Body2DSW *body = body_owner.get(p_body); ERR_FAIL_COND_V(!body,0); - return body->get_user_mask(); + return body->get_collision_mask(); }; void Physics2DServerSW::body_set_param(RID p_body, BodyParameter p_param, float p_value) { @@ -783,6 +799,8 @@ void Physics2DServerSW::body_set_applied_force(RID p_body, const Vector2& p_forc ERR_FAIL_COND(!body); body->set_applied_force(p_force); + body->wakeup(); + }; Vector2 Physics2DServerSW::body_get_applied_force(RID p_body) const { @@ -798,6 +816,7 @@ void Physics2DServerSW::body_set_applied_torque(RID p_body, float p_torque) { ERR_FAIL_COND(!body); body->set_applied_torque(p_torque); + body->wakeup(); }; float Physics2DServerSW::body_get_applied_torque(RID p_body) const { @@ -814,6 +833,7 @@ void Physics2DServerSW::body_apply_impulse(RID p_body, const Vector2& p_pos, con ERR_FAIL_COND(!body); body->apply_impulse(p_pos,p_impulse); + body->wakeup(); }; void Physics2DServerSW::body_set_axis_velocity(RID p_body, const Vector2& p_axis_velocity) { @@ -826,7 +846,7 @@ void Physics2DServerSW::body_set_axis_velocity(RID p_body, const Vector2& p_axis v-=axis*axis.dot(v); v+=p_axis_velocity; body->set_linear_velocity(v); - + body->wakeup(); }; void Physics2DServerSW::body_add_collision_exception(RID p_body, RID p_body_b) { @@ -835,7 +855,7 @@ void Physics2DServerSW::body_add_collision_exception(RID p_body, RID p_body_b) { ERR_FAIL_COND(!body); body->add_exception(p_body_b); - + body->wakeup(); }; void Physics2DServerSW::body_remove_collision_exception(RID p_body, RID p_body_b) { @@ -844,6 +864,7 @@ void Physics2DServerSW::body_remove_collision_exception(RID p_body, RID p_body_b ERR_FAIL_COND(!body); body->remove_exception(p_body_b); + body->wakeup(); }; @@ -959,6 +980,18 @@ void Physics2DServerSW::body_set_pickable(RID p_body,bool p_pickable) { } +bool Physics2DServerSW::body_test_motion(RID p_body,const Vector2& p_motion,float 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_motion,p_margin,r_result); + +} + + /* JOINT API */ void Physics2DServerSW::joint_set_param(RID p_joint, JointParam p_param, real_t p_value) { @@ -1163,7 +1196,7 @@ void Physics2DServerSW::set_active(bool p_active) { void Physics2DServerSW::init() { - doing_sync=true; + doing_sync=false; last_step=0.001; iterations=8;// 8? stepper = memnew( Step2DSW ); @@ -1195,6 +1228,7 @@ void Physics2DServerSW::step(float p_step) { void Physics2DServerSW::sync() { + doing_sync=true; }; void Physics2DServerSW::flush_queries() { @@ -1202,7 +1236,7 @@ void Physics2DServerSW::flush_queries() { if (!active) return; - doing_sync=true; + for( Set<const Space2DSW*>::Element *E=active_spaces.front();E;E=E->next()) { Space2DSW *space=(Space2DSW *)E->get(); @@ -1211,6 +1245,10 @@ void Physics2DServerSW::flush_queries() { }; +void Physics2DServerSW::end_sync() { + doing_sync=false; +} + void Physics2DServerSW::finish() { @@ -1250,6 +1288,7 @@ Physics2DServerSW::Physics2DServerSW() { island_count=0; active_objects=0; collision_pairs=0; + using_threads=int(Globals::get_singleton()->get("physics_2d/thread_model"))==2; }; diff --git a/servers/physics_2d/physics_2d_server_sw.h b/servers/physics_2d/physics_2d_server_sw.h index 58fc4eeb33..6e875701b8 100644 --- a/servers/physics_2d/physics_2d_server_sw.h +++ b/servers/physics_2d/physics_2d_server_sw.h @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -51,6 +51,8 @@ friend class Physics2DDirectSpaceStateSW; int active_objects; int collision_pairs; + bool using_threads; + Step2DSW *stepper; Set<const Space2DSW*> active_spaces; @@ -134,6 +136,8 @@ public: virtual Variant area_get_param(RID p_parea,AreaParameter p_param) const; virtual Matrix32 area_get_transform(RID p_area) const; virtual void area_set_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); @@ -177,10 +181,10 @@ public: 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); - virtual uint32_t body_get_layer_mask(RID p_body, uint32_t p_mask) const; + virtual uint32_t body_get_layer_mask(RID p_body) const; - virtual void body_set_user_mask(RID p_body, uint32_t p_mask); - virtual uint32_t body_get_user_mask(RID p_body, uint32_t p_mask) const; + virtual void body_set_collision_mask(RID p_body, uint32_t p_mask); + virtual uint32_t body_get_collision_mask(RID p_) const; virtual void body_set_param(RID p_body, BodyParameter p_param, float p_value); virtual float body_get_param(RID p_body, BodyParameter p_param) const; @@ -223,6 +227,9 @@ public: virtual void body_set_pickable(RID p_body,bool p_pickable); + virtual bool body_test_motion(RID p_body,const Vector2& p_motion,float 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); @@ -243,8 +250,9 @@ public: virtual void set_active(bool p_active); virtual void init(); virtual void step(float p_step); - virtual void sync(); + virtual void sync(); virtual void flush_queries(); + virtual void end_sync(); virtual void finish(); int get_process_info(ProcessInfo p_info); diff --git a/servers/physics_2d/physics_2d_server_wrap_mt.cpp b/servers/physics_2d/physics_2d_server_wrap_mt.cpp new file mode 100644 index 0000000000..c5f023f162 --- /dev/null +++ b/servers/physics_2d/physics_2d_server_wrap_mt.cpp @@ -0,0 +1,169 @@ +#include "physics_2d_server_wrap_mt.h" + +#include "os/os.h" + +void Physics2DServerWrapMT::thread_exit() { + + exit=true; +} + +void Physics2DServerWrapMT::thread_step(float p_delta) { + + physics_2d_server->step(p_delta); + step_sem->post(); + +} + +void Physics2DServerWrapMT::_thread_callback(void *_instance) { + + Physics2DServerWrapMT *vsmt = reinterpret_cast<Physics2DServerWrapMT*>(_instance); + + + vsmt->thread_loop(); +} + +void Physics2DServerWrapMT::thread_loop() { + + server_thread=Thread::get_caller_ID(); + + OS::get_singleton()->make_rendering_thread(); + + physics_2d_server->init(); + + exit=false; + step_thread_up=true; + while(!exit) { + // flush commands one by one, until exit is requested + command_queue.wait_and_flush_one(); + } + + command_queue.flush_all(); // flush all + + physics_2d_server->finish(); + +} + + +/* EVENT QUEUING */ + + +void Physics2DServerWrapMT::step(float p_step) { + + if (create_thread) { + + command_queue.push( this, &Physics2DServerWrapMT::thread_step,p_step); + } else { + + command_queue.flush_all(); //flush all pending from other threads + physics_2d_server->step(p_step); + } +} + +void Physics2DServerWrapMT::sync() { + + if (step_sem) { + if (first_frame) + first_frame=false; + else + step_sem->wait(); //must not wait if a step was not issued + } + physics_2d_server->sync();; +} + +void Physics2DServerWrapMT::flush_queries(){ + + physics_2d_server->flush_queries(); +} + +void Physics2DServerWrapMT::end_sync() { + + physics_2d_server->end_sync();; +} + +void Physics2DServerWrapMT::init() { + + if (create_thread) { + + step_sem = Semaphore::create(); + print_line("CREATING PHYSICS 2D THREAD"); + //OS::get_singleton()->release_rendering_thread(); + if (create_thread) { + thread = Thread::create( _thread_callback, this ); + print_line("STARTING PHYISICS 2D THREAD"); + } + while(!step_thread_up) { + OS::get_singleton()->delay_usec(1000); + } + print_line("DONE PHYSICS 2D THREAD"); + } else { + + physics_2d_server->init(); + } + +} + +void Physics2DServerWrapMT::finish() { + + + if (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(); + pin_joint_free_cached_ids(); + groove_joint_free_cached_ids(); + damped_string_free_cached_ids(); +*/ + 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) { + + 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_DEF("core/thread_rid_pool_prealloc",20); + area_pool_max_size=GLOBAL_DEF("core/thread_rid_pool_prealloc",20); + body_pool_max_size=GLOBAL_DEF("core/thread_rid_pool_prealloc",20); + pin_joint_pool_max_size=GLOBAL_DEF("core/thread_rid_pool_prealloc",20); + groove_joint_pool_max_size=GLOBAL_DEF("core/thread_rid_pool_prealloc",20); + damped_spring_joint_pool_max_size=GLOBAL_DEF("core/thread_rid_pool_prealloc",20); + + if (!p_create_thread) { + server_thread=Thread::get_caller_ID(); + } else { + server_thread=0; + } + + main_thread = Thread::get_caller_ID(); + 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 new file mode 100644 index 0000000000..0ddc8f16ec --- /dev/null +++ b/servers/physics_2d/physics_2d_server_wrap_mt.h @@ -0,0 +1,297 @@ +#ifndef PHYSICS2DSERVERWRAPMT_H +#define PHYSICS2DSERVERWRAPMT_H + + +#include "servers/physics_2d_server.h" +#include "command_queue_mt.h" +#include "os/thread.h" +#include "globals.h" + +#ifdef DEBUG_SYNC +#define SYNC_DEBUG print_line("sync on: "+String(__FUNCTION__)); +#else +#define SYNC_DEBUG +#endif + + +class Physics2DServerWrapMT : public Physics2DServer { + + mutable Physics2DServer *physics_2d_server; + + mutable CommandQueueMT command_queue; + + static void _thread_callback(void *_instance); + void thread_loop(); + + Thread::ID server_thread; + Thread::ID main_thread; + volatile bool exit; + Thread *thread; + volatile bool step_thread_up; + bool create_thread; + + Semaphore *step_sem; + int step_pending; + void thread_step(float p_delta); + void thread_flush(); + + void thread_exit(); + + Mutex*alloc_mutex; + bool first_frame; + + int shape_pool_max_size; + List<RID> shape_id_pool; + int area_pool_max_size; + List<RID> area_id_pool; + int body_pool_max_size; + List<RID> body_id_pool; + int pin_joint_pool_max_size; + List<RID> pin_joint_id_pool; + int groove_joint_pool_max_size; + List<RID> groove_joint_id_pool; + 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); + + + //these work well, but should be used from the main thread only + bool shape_collide(RID p_shape_A, const Matrix32& p_xform_A,const Vector2& p_motion_A,RID p_shape_B, const Matrix32& 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); + } + + /* SPACE API */ + + 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); + + // this function only works on fixed process, errors and returns null otherwise + Physics2DDirectSpaceState* space_get_direct_state(RID p_space) { + + ERR_FAIL_COND_V(main_thread!=Thread::get_caller_ID(),NULL); + return physics_2d_server->space_get_direct_state(p_space); + } + + + /* AREA API */ + + //FUNC0RID(area); + FUNC0R(RID,area_create); + + FUNC2(area_set_space,RID,RID); + FUNC1RC(RID,area_get_space,RID); + + FUNC2(area_set_space_override_mode,RID,AreaSpaceOverrideMode); + FUNC1RC(AreaSpaceOverrideMode,area_get_space_override_mode,RID); + + FUNC3(area_add_shape,RID,RID,const Matrix32&); + FUNC3(area_set_shape,RID,int,RID); + FUNC3(area_set_shape_transform,RID,int,const Matrix32&); + + FUNC1RC(int,area_get_shape_count,RID); + FUNC2RC(RID,area_get_shape,RID,int); + FUNC2RC(Matrix32,area_get_shape_transform,RID,int); + FUNC2(area_remove_shape,RID,int); + FUNC1(area_clear_shapes,RID); + + FUNC2(area_attach_object_instance_ID,RID,ObjectID); + FUNC1RC(ObjectID,area_get_object_instance_ID,RID); + + FUNC3(area_set_param,RID,AreaParameter,const Variant&); + FUNC2(area_set_transform,RID,const Matrix32&); + + FUNC2RC(Variant,area_get_param,RID,AreaParameter); + FUNC1RC(Matrix32,area_get_transform,RID); + + FUNC2(area_set_collision_mask,RID,uint32_t); + FUNC2(area_set_layer_mask,RID,uint32_t); + + 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) + + FUNC2(body_set_space,RID,RID); + FUNC1RC(RID,body_get_space,RID); + + FUNC2(body_set_mode,RID,BodyMode); + FUNC1RC(BodyMode,body_get_mode,RID); + + + FUNC3(body_add_shape,RID,RID,const Matrix32&); + FUNC3(body_set_shape,RID,int,RID); + FUNC3(body_set_shape_transform,RID,int,const Matrix32&); + FUNC3(body_set_shape_metadata,RID,int,const Variant&); + + FUNC1RC(int,body_get_shape_count,RID); + FUNC2RC(Matrix32,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); + + 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_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_collision_mask,RID,uint32_t); + FUNC1RC(uint32_t,body_get_collision_mask,RID); + + + FUNC3(body_set_param,RID,BodyParameter,float); + FUNC2RC(float,body_get_param,RID,BodyParameter); + + + FUNC3(body_set_state,RID,BodyState,const Variant&); + FUNC2RC(Variant,body_get_state,RID,BodyState); + + FUNC2(body_set_applied_force,RID,const Vector2&); + FUNC1RC(Vector2,body_get_applied_force,RID); + + FUNC2(body_set_applied_torque,RID,float); + FUNC1RC(float,body_get_applied_torque,RID); + + FUNC3(body_apply_impulse,RID,const Vector2&,const Vector2&); + FUNC2(body_set_axis_velocity,RID,const Vector2&); + + 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_max_contacts_reported,RID,int); + FUNC1RC(int,body_get_max_contacts_reported,RID); + + FUNC2(body_set_one_way_collision_direction,RID,const Vector2&); + FUNC1RC(Vector2,body_get_one_way_collision_direction,RID); + + FUNC2(body_set_one_way_collision_max_depth,RID,float); + FUNC1RC(float,body_get_one_way_collision_max_depth,RID); + + + FUNC2(body_set_contacts_reported_depth_treshold,RID,float); + FUNC1RC(float,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 Matrix32& 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); + + bool body_test_motion(RID p_body,const Vector2& p_motion,float 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_motion,p_margin,r_result); + } + + /* JOINT API */ + + + 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); + + 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); + + virtual void init(); + virtual void step(float p_step); + virtual void sync(); + virtual void end_sync(); + virtual void flush_queries(); + virtual void finish(); + + int get_process_info(ProcessInfo p_info) { + return physics_2d_server->get_process_info(p_info); + } + + Physics2DServerWrapMT(Physics2DServer* p_contained,bool p_create_thread); + ~Physics2DServerWrapMT(); + + + 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 )); + else //single unsafe + return memnew( Physics2DServerWrapMT( memnew( T ), true )); + + + } + +#undef ServerNameWrapMT +#undef ServerName +#undef server_name + +}; + +#ifdef DEBUG_SYNC +#undef DEBUG_SYNC +#endif +#undef SYNC_DEBUG + +#endif // PHYSICS2DSERVERWRAPMT_H diff --git a/servers/physics_2d/shape_2d_sw.cpp b/servers/physics_2d/shape_2d_sw.cpp index 9a4b52d563..d3591ec744 100644 --- a/servers/physics_2d/shape_2d_sw.cpp +++ b/servers/physics_2d/shape_2d_sw.cpp @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/servers/physics_2d/shape_2d_sw.h b/servers/physics_2d/shape_2d_sw.h index 05ea5b21cd..6d5473aa17 100644 --- a/servers/physics_2d/shape_2d_sw.h +++ b/servers/physics_2d/shape_2d_sw.h @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp index 5aaf9a7613..767ad9038d 100644 --- a/servers/physics_2d/space_2d_sw.cpp +++ b/servers/physics_2d/space_2d_sw.cpp @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -555,8 +555,524 @@ Physics2DDirectSpaceStateSW::Physics2DDirectSpaceStateSW() { +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++) { + + 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 (!keep) { + + if (i<amount-1) { + SWAP(intersection_query_results[i],intersection_query_results[amount-1]); + SWAP(intersection_query_subindex_results[i],intersection_query_subindex_results[amount-1]); + + } + + amount--; + i--; + + } + } + + return amount; +} + +bool Space2DSW::test_body_motion(Body2DSW *p_body,const Vector2&p_motion,float p_margin,Physics2DServer::MotionResult *r_result) { + + //give me back regular physics engine logic + //this is madness + //and most people using this function will think + //what it does is simpler than using physics + //this took about a week to get right.. + //but is it right? who knows at this point.. + + Rect2 body_aabb; + + for(int i=0;i<p_body->get_shape_count();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.grow(p_margin); + + + Matrix32 body_transform = p_body->get_transform(); + + { + //STEP 1, FREE BODY IF STUCK + + const int max_results = 32; + 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; + + Physics2DServerSW::CollCbkData *cbkptr=NULL; + cbkptr=&cbk; + cbkres=Physics2DServerSW::_shape_col_cbk; + + bool collided=false; + + int amount = _cull_aabb_for_body(p_body,body_aabb); + + for(int j=0;j<p_body->get_shape_count();j++) { + if (p_body->is_shape_set_as_trigger(j)) + continue; + + Matrix32 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++) { + + const CollisionObject2DSW *col_obj=intersection_query_results[i]; + int shape_idx=intersection_query_subindex_results[i]; + + if (col_obj->get_type()==CollisionObject2DSW::TYPE_BODY) { + + const Body2DSW *body=static_cast<const Body2DSW*>(col_obj); + + Vector2 cdir = body->get_one_way_collision_direction(); + //if (cdir!=Vector2() && p_motion.dot(cdir)<0) + // continue; + + cbk.valid_dir=cdir; + cbk.valid_depth=body->get_one_way_collision_max_depth(); + } else { + 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 (!collided) + break; + + Vector2 recover_motion; + + for(int i=0;i<cbk.amount;i++) { + + Vector2 a = sr[i*2+0]; + Vector2 b = sr[i*2+1]; + + // float d = a.distance_to(b); + + //if (d<margin) + /// continue; + recover_motion+=(b-a)*0.4; + } + + if (recover_motion==Vector2()) { + collided=false; + break; + } + + body_transform.elements[2]+=recover_motion; + body_aabb.pos+=recover_motion; + + recover_attempts--; + + } while (recover_attempts); + } + + + + float safe = 1.0; + float unsafe = 1.0; + 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); + + int amount = _cull_aabb_for_body(p_body,motion_aabb); + + for(int j=0;j<p_body->get_shape_count();j++) { + + if (p_body->is_shape_set_as_trigger(j)) + continue; + + Matrix32 body_shape_xform = body_transform * p_body->get_shape_transform(j); + Shape2DSW *body_shape = p_body->get_shape(j); + + bool stuck=false; + + float best_safe=1; + float 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]; + + + Matrix32 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)) { + 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 (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()) { + continue; + } + } + + stuck=true; + break; + } + + + //just do kinematic solving + float low=0; + float hi=1; + Vector2 mnormal=p_motion.normalized(); + + for(int i=0;i<8;i++) { //steps should be customizable.. + + //Matrix32 xfa = p_xform; + float 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); + + if (collided) { + + hi=ofs; + } else { + + low=ofs; + } + } + + 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()) { + + Vector2 cd[2]; + Physics2DServerSW::CollCbkData cbk; + cbk.max=1; + cbk.amount=0; + cbk.ptr=cd; + cbk.valid_dir=body->get_one_way_collision_direction(); + cbk.valid_depth=body->get_one_way_collision_max_depth(); + + Vector2 sep=mnormal; //important optimization for this to work fast enough + bool collided = CollisionSolver2DSW::solve(body_shape,body_shape_xform,p_motion*(hi+contact_max_allowed_penetration),col_obj->get_shape(shape_idx),col_obj_xform,Vector2(),Physics2DServerSW::_shape_col_cbk,&cbk,&sep,0); + if (!collided || cbk.amount==0) { + continue; + } + + } + } + + + if (low<best_safe) { + best_safe=low; + best_unsafe=hi; + } + } + + if (stuck) { + + safe=0; + unsafe=0; + best_shape=j; //sadly it's the best + break; + } + if (best_safe==1.0) { + continue; + } + if (best_safe < safe) { + + safe=best_safe; + unsafe=best_unsafe; + best_shape=j; + } + } + } + + bool collided=false; + if (safe>=1) { + //not collided + collided=false; + if (r_result) { + + r_result->motion=p_motion+(body_transform.elements[2]-p_body->get_transform().elements[2]); + r_result->remainder=Vector2(); + } + + } else { + + //it collided, let's get the rest info in unsafe advance + Matrix32 ugt = body_transform; + ugt.elements[2]+=p_motion*unsafe; + + _RestCallbackData2D rcd; + rcd.best_len=0; + rcd.best_object=NULL; + rcd.best_shape=0; + + Matrix32 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); + + + for(int i=0;i<amount;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) { + + 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.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 (r_result) { + r_result->collider=rcd.best_object->get_self(); + r_result->collider_id=rcd.best_object->get_instance_id(); + r_result->collider_shape=rcd.best_shape; + r_result->collision_normal=rcd.best_normal; + r_result->collision_point=rcd.best_contact; + r_result->collider_metadata=rcd.best_object->get_shape_metadata(rcd.best_shape); + + const Body2DSW *body = static_cast<const Body2DSW*>(rcd.best_object); + Vector2 rel_vec = r_result->collision_point-body->get_transform().get_origin(); + r_result->collider_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+(body_transform.elements[2]-p_body->get_transform().elements[2]); + r_result->remainder=p_motion - safe * p_motion; + } + + collided=true; + } else { + if (r_result) { + + r_result->motion=p_motion+(body_transform.elements[2]-p_body->get_transform().elements[2]); + r_result->remainder=Vector2(); + } + + collided=false; + + } + } + + return collided; + + +#if 0 + //give me back regular physics engine logic + //this is madness + //and most people using this function will think + //what it does is simpler than using physics + //this took about a week to get right.. + //but is it right? who knows at this point.. + + + colliding=false; + ERR_FAIL_COND_V(!is_inside_tree(),Vector2()); + Physics2DDirectSpaceState *dss = Physics2DServer::get_singleton()->space_get_direct_state(get_world_2d()->get_space()); + ERR_FAIL_COND_V(!dss,Vector2()); + const int max_shapes=32; + Vector2 sr[max_shapes*2]; + int res_shapes; + + Set<RID> exclude; + exclude.insert(get_rid()); + + + //recover first + int recover_attempts=4; + + bool collided=false; + uint32_t mask=0; + if (collide_static) + mask|=Physics2DDirectSpaceState::TYPE_MASK_STATIC_BODY; + if (collide_kinematic) + mask|=Physics2DDirectSpaceState::TYPE_MASK_KINEMATIC_BODY; + if (collide_rigid) + mask|=Physics2DDirectSpaceState::TYPE_MASK_RIGID_BODY; + if (collide_character) + mask|=Physics2DDirectSpaceState::TYPE_MASK_CHARACTER_BODY; + +// print_line("motion: "+p_motion+" margin: "+rtos(margin)); + + //print_line("margin: "+rtos(margin)); + do { + + //motion recover + for(int i=0;i<get_shape_count();i++) { + + if (is_shape_set_as_trigger(i)) + continue; + if (dss->collide_shape(get_shape(i)->get_rid(), get_global_transform() * get_shape_transform(i),Vector2(),margin,sr,max_shapes,res_shapes,exclude,get_layer_mask(),mask)) + collided=true; + + } + + if (!collided) + break; + + Vector2 recover_motion; + + for(int i=0;i<res_shapes;i++) { + + Vector2 a = sr[i*2+0]; + Vector2 b = sr[i*2+1]; + + float d = a.distance_to(b); + + //if (d<margin) + /// continue; + recover_motion+=(b-a)*0.4; + } + + if (recover_motion==Vector2()) { + collided=false; + break; + } + + Matrix32 gt = get_global_transform(); + gt.elements[2]+=recover_motion; + set_global_transform(gt); + + recover_attempts--; + + } while (recover_attempts); + + + //move second + float safe = 1.0; + float unsafe = 1.0; + int best_shape=-1; + + for(int i=0;i<get_shape_count();i++) { + + if (is_shape_set_as_trigger(i)) + continue; + + float lsafe,lunsafe; + bool valid = dss->cast_motion(get_shape(i)->get_rid(), get_global_transform() * get_shape_transform(i), p_motion, 0,lsafe,lunsafe,exclude,get_layer_mask(),mask); + //print_line("shape: "+itos(i)+" travel:"+rtos(ltravel)); + if (!valid) { + + safe=0; + unsafe=0; + best_shape=i; //sadly it's the best + break; + } + if (lsafe==1.0) { + continue; + } + if (lsafe < safe) { + + safe=lsafe; + unsafe=lunsafe; + best_shape=i; + } + } + + + //print_line("best shape: "+itos(best_shape)+" motion "+p_motion); + + if (safe>=1) { + //not collided + colliding=false; + } else { + + //it collided, let's get the rest info in unsafe advance + Matrix32 ugt = get_global_transform(); + ugt.elements[2]+=p_motion*unsafe; + Physics2DDirectSpaceState::ShapeRestInfo rest_info; + bool c2 = dss->rest_info(get_shape(best_shape)->get_rid(), ugt*get_shape_transform(best_shape), Vector2(), margin,&rest_info,exclude,get_layer_mask(),mask); + if (!c2) { + //should not happen, but floating point precision is so weird.. + + colliding=false; + } else { + + + //print_line("Travel: "+rtos(travel)); + colliding=true; + collision=rest_info.point; + normal=rest_info.normal; + collider=rest_info.collider_id; + collider_vel=rest_info.linear_velocity; + collider_shape=rest_info.shape; + collider_metadata=rest_info.metadata; + } + + } + + Vector2 motion=p_motion*safe; + Matrix32 gt = get_global_transform(); + gt.elements[2]+=motion; + set_global_transform(gt); + + return p_motion-motion; + +#endif + return false; +} diff --git a/servers/physics_2d/space_2d_sw.h b/servers/physics_2d/space_2d_sw.h index 05b55fe807..abee8628fc 100644 --- a/servers/physics_2d/space_2d_sw.h +++ b/servers/physics_2d/space_2d_sw.h @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -101,6 +101,8 @@ class Space2DSW { int active_objects; int collision_pairs; + int _cull_aabb_for_body(Body2DSW *p_body,const Rect2& p_aabb); + friend class Physics2DDirectSpaceStateSW; public: @@ -165,6 +167,8 @@ public: int get_collision_pairs() const { return collision_pairs; } + bool test_body_motion(Body2DSW *p_body, const Vector2&p_motion, float p_margin, Physics2DServer::MotionResult *r_result); + Physics2DDirectSpaceStateSW *get_direct_state(); Space2DSW(); diff --git a/servers/physics_2d/step_2d_sw.cpp b/servers/physics_2d/step_2d_sw.cpp index e75f9300ce..bc87789937 100644 --- a/servers/physics_2d/step_2d_sw.cpp +++ b/servers/physics_2d/step_2d_sw.cpp @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/servers/physics_2d/step_2d_sw.h b/servers/physics_2d/step_2d_sw.h index 91ac9d8584..d5e919836c 100644 --- a/servers/physics_2d/step_2d_sw.h +++ b/servers/physics_2d/step_2d_sw.h @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ |