From d7d65fa2f2b51d03f7bdfcbceedca99188ce979c Mon Sep 17 00:00:00 2001 From: Juan Linietsky Date: Wed, 19 Feb 2014 11:57:14 -0300 Subject: -improved physics ccd -html5 exporter works again -disable repeat on image loader by default -can change shape offset en tileset, texture offset was broken --- servers/physics/body_pair_sw.cpp | 2 +- servers/physics/body_sw.cpp | 1206 ++++++------- servers/physics_2d/area_2d_sw.cpp | 2 +- servers/physics_2d/area_pair_2d_sw.cpp | 2 +- servers/physics_2d/body_2d_sw.cpp | 1167 ++++++------- servers/physics_2d/body_2d_sw.h | 624 +++---- servers/physics_2d/body_pair_2d_sw.cpp | 93 +- servers/physics_2d/body_pair_2d_sw.h | 3 +- servers/physics_2d/broad_phase_2d_hash_grid.cpp | 37 +- servers/physics_2d/broad_phase_2d_hash_grid.h | 3 +- servers/physics_2d/collision_object_2d_sw.cpp | 1 + servers/physics_2d/collision_object_2d_sw.h | 13 +- servers/physics_2d/collision_solver_2d_sat.cpp | 537 ++++-- servers/physics_2d/collision_solver_2d_sat.h | 2 +- servers/physics_2d/collision_solver_2d_sw.cpp | 39 +- servers/physics_2d/collision_solver_2d_sw.h | 8 +- servers/physics_2d/physics_2d_server_sw.cpp | 96 +- servers/physics_2d/physics_2d_server_sw.h | 21 +- servers/physics_2d/shape_2d_sw.cpp | 2113 +++++++++++------------ servers/physics_2d/shape_2d_sw.h | 927 +++++----- servers/physics_2d/space_2d_sw.cpp | 998 ++++++----- servers/physics_2d/space_2d_sw.h | 265 +-- servers/physics_2d/step_2d_sw.cpp | 13 +- servers/physics_2d_server.cpp | 45 +- servers/physics_2d_server.h | 45 +- servers/physics_server.cpp | 2 +- servers/physics_server.h | 2 +- 27 files changed, 4550 insertions(+), 3716 deletions(-) (limited to 'servers') diff --git a/servers/physics/body_pair_sw.cpp b/servers/physics/body_pair_sw.cpp index 06ae34098c..094f56a421 100644 --- a/servers/physics/body_pair_sw.cpp +++ b/servers/physics/body_pair_sw.cpp @@ -198,7 +198,7 @@ bool BodyPairSW::setup(float p_step) { //cannot collide - if (A->has_exception(B->get_self()) || B->has_exception(A->get_self()) || (A->get_mode()<=PhysicsServer::BODY_MODE_STATIC_ACTIVE && B->get_mode()<=PhysicsServer::BODY_MODE_STATIC_ACTIVE)) { + if (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)) { return false; } diff --git a/servers/physics/body_sw.cpp b/servers/physics/body_sw.cpp index b926a93773..f0f72b471c 100644 --- a/servers/physics/body_sw.cpp +++ b/servers/physics/body_sw.cpp @@ -26,606 +26,606 @@ /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ -#include "body_sw.h" -#include "space_sw.h" -#include "area_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_inertia_tensor() { - - Matrix3 tb = get_transform().basis; - tb.scale(_inv_inertia); - _inv_inertia_tensor = tb * get_transform().basis.transposed(); - -} - -void BodySW::update_inertias() { - - //update shapes and motions - - switch(mode) { - - case PhysicsServer::BODY_MODE_RIGID: { - - //update tensor for allshapes, not the best way but should be somehow OK. (inspired from bullet) - float total_area=0; - - for (int i=0;imass / total_area; - - _inertia += shape->get_moment_of_inertia(mass) + mass * get_shape_transform(i).get_origin(); - - } - - if (_inertia!=Vector3()) - _inv_inertia=_inertia.inverse(); - else - _inv_inertia=Vector3(); - - if (mass) - _inv_mass=1.0/mass; - else - _inv_mass=0; - - } break; - - case PhysicsServer::BODY_MODE_STATIC_ACTIVE: - case PhysicsServer::BODY_MODE_STATIC: { - - _inv_inertia=Vector3(); - _inv_mass=0; - } break; - case PhysicsServer::BODY_MODE_CHARACTER: { - - _inv_inertia=Vector3(); - _inv_mass=1.0/mass; - - } break; - } - _update_inertia_tensor(); - - //_update_shapes(); - -} - - - -void BodySW::set_active(bool p_active) { - - if (active==p_active) - return; - - active=p_active; - if (!p_active) { - if (get_space()) - get_space()->body_remove_from_active_list(&active_list); - } else { - if (mode==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; - - for(int i=0;i0) { - get_space()->get_broadphase()->set_active(s.bpid,active); - } - } -*/ -} - - - -void BodySW::set_param(PhysicsServer::BodyParameter p_param, float p_value) { - - switch(p_param) { - case PhysicsServer::BODY_PARAM_BOUNCE: { - - bounce=p_value; - } break; - case PhysicsServer::BODY_PARAM_FRICTION: { - - friction=p_value; - } break; - case PhysicsServer::BODY_PARAM_MASS: { - ERR_FAIL_COND(p_value<=0); - mass=p_value; - _update_inertia(); - - } break; - default:{} - } -} - -float BodySW::get_param(PhysicsServer::BodyParameter p_param) const { - - switch(p_param) { - case PhysicsServer::BODY_PARAM_BOUNCE: { - - return bounce; - } break; - case PhysicsServer::BODY_PARAM_FRICTION: { - - return friction; - } break; - case PhysicsServer::BODY_PARAM_MASS: { - return mass; - } break; - default:{} - } - - return 0; -} - -void BodySW::set_mode(PhysicsServer::BodyMode p_mode) { - - mode=p_mode; - - switch(p_mode) { - //CLEAR UP EVERYTHING IN CASE IT NOT WORKS! - case PhysicsServer::BODY_MODE_STATIC: - case PhysicsServer::BODY_MODE_STATIC_ACTIVE: { - - _set_inv_transform(get_transform().affine_inverse()); - _inv_mass=0; - _set_static(p_mode==PhysicsServer::BODY_MODE_STATIC); - set_active(p_mode==PhysicsServer::BODY_MODE_STATIC_ACTIVE); - linear_velocity=Vector3(); - angular_velocity=Vector3(); - } break; - case PhysicsServer::BODY_MODE_RIGID: { - - _inv_mass=mass>0?(1.0/mass):0; - _set_static(false); - simulated_motion=false; //jic - - } break; - case PhysicsServer::BODY_MODE_CHARACTER: { - - _inv_mass=mass>0?(1.0/mass):0; - _set_static(false); - simulated_motion=false; //jic - } break; - } - - _update_inertia(); - //if (get_space()) -// _update_queries(); - -} -PhysicsServer::BodyMode BodySW::get_mode() const { - - return mode; -} - -void BodySW::_shapes_changed() { - - _update_inertia(); -} - -void BodySW::set_state(PhysicsServer::BodyState p_state, const Variant& p_variant) { - - switch(p_state) { - case PhysicsServer::BODY_STATE_TRANSFORM: { - - - if (mode==PhysicsServer::BODY_MODE_STATIC || mode==PhysicsServer::BODY_MODE_STATIC_ACTIVE) { - _set_transform(p_variant); - _set_inv_transform(get_transform().affine_inverse()); - wakeup_neighbours(); - } else { - Transform t = p_variant; - t.orthonormalize(); - _set_transform(t); - _set_inv_transform(get_transform().inverse()); - - } - - } break; - case PhysicsServer::BODY_STATE_LINEAR_VELOCITY: { - - //if (mode==PhysicsServer::BODY_MODE_STATIC) - // break; - linear_velocity=p_variant; - } break; - case PhysicsServer::BODY_STATE_ANGULAR_VELOCITY: { - //if (mode!=PhysicsServer::BODY_MODE_RIGID) - // break; - angular_velocity=p_variant; - - } break; - case PhysicsServer::BODY_STATE_SLEEPING: { - //? - if (mode==PhysicsServer::BODY_MODE_STATIC || mode==PhysicsServer::BODY_MODE_STATIC_ACTIVE) - break; - bool do_sleep=p_variant; - if (do_sleep) { - linear_velocity=Vector3(); - //biased_linear_velocity=Vector3(); - angular_velocity=Vector3(); - //biased_angular_velocity=Vector3(); - set_active(false); - } else { - 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) - set_active(true); - - } break; - } - -} -Variant BodySW::get_state(PhysicsServer::BodyState p_state) const { - - switch(p_state) { - case PhysicsServer::BODY_STATE_TRANSFORM: { - return get_transform(); - } break; - case PhysicsServer::BODY_STATE_LINEAR_VELOCITY: { - return linear_velocity; - } break; - case PhysicsServer::BODY_STATE_ANGULAR_VELOCITY: { - return angular_velocity; - } break; - case PhysicsServer::BODY_STATE_SLEEPING: { - return !is_active(); - } break; - case PhysicsServer::BODY_STATE_CAN_SLEEP: { - return can_sleep; - } break; - } - - return Variant(); -} - - -void BodySW::set_space(SpaceSW *p_space){ - - if (get_space()) { - - if (inertia_update_list.in_list()) - get_space()->body_remove_from_inertia_update_list(&inertia_update_list); - if (active_list.in_list()) - get_space()->body_remove_from_active_list(&active_list); - if (direct_state_query_list.in_list()) - get_space()->body_remove_from_state_query_list(&direct_state_query_list); - - } - - _set_space(p_space); - - if (get_space()) { - - _update_inertia(); - if (active) - get_space()->body_add_to_active_list(&active_list); -// _update_queries(); - //if (is_active()) { - // active=false; - // set_active(true); - //} - - } - -} - -void BodySW::_compute_area_gravity(const AreaSW *p_area) { - - if (p_area->is_gravity_point()) { - - gravity = (p_area->get_gravity_vector() - get_transform().get_origin()).normalized() * p_area->get_gravity(); - - } else { - gravity = p_area->get_gravity_vector() * p_area->get_gravity(); - } -} - -void BodySW::integrate_forces(real_t p_step) { - - - if (mode==PhysicsServer::BODY_MODE_STATIC || mode==PhysicsServer::BODY_MODE_STATIC_ACTIVE) - return; - - AreaSW *current_area = get_space()->get_default_area(); - ERR_FAIL_COND(!current_area); - - int prio = current_area->get_priority(); - int ac = areas.size(); - if (ac) { - const AreaCMP *aa = &areas[0]; - for(int i=0;iget_priority() > prio) { - current_area=aa[i].area; - prio=current_area->get_priority(); - } - } - } - - _compute_area_gravity(current_area); - density=current_area->get_density(); - - if (!omit_force_integration) { - //overriden by direct state query - - Vector3 force=gravity*mass; - force+=applied_force; - Vector3 torque=applied_torque; - - real_t damp = 1.0 - p_step * density; - - if (damp<0) // reached zero in the given time - damp=0; - - real_t angular_damp = 1.0 - p_step * density * get_space()->get_body_angular_velocity_damp_ratio(); - - if (angular_damp<0) // reached zero in the given time - angular_damp=0; - - linear_velocity*=damp; - angular_velocity*=angular_damp; - - linear_velocity+=_inv_mass * force * p_step; - angular_velocity+=_inv_inertia_tensor.xform(torque)*p_step; - } - - applied_force=Vector3(); - applied_torque=Vector3(); - - //motion=linear_velocity*p_step; - - biased_angular_velocity=Vector3(); - biased_linear_velocity=Vector3(); - - if (continuous_cd) //shapes temporarily extend for raycast - _update_shapes_with_motion(linear_velocity*p_step); - - current_area=NULL; // clear the area, so it is set in the next frame - contact_count=0; - -} - -void BodySW::integrate_velocities(real_t p_step) { - - if (mode==PhysicsServer::BODY_MODE_STATIC) - return; - - if (mode==PhysicsServer::BODY_MODE_STATIC_ACTIVE) { - if (fi_callback) - get_space()->body_add_to_state_query_list(&direct_state_query_list); - return; - } - - Vector3 total_angular_velocity = angular_velocity+biased_angular_velocity; - - - - float ang_vel = total_angular_velocity.length(); - Transform transform = get_transform(); - - - if (ang_vel!=0.0) { - Vector3 ang_vel_axis = total_angular_velocity / ang_vel; - Matrix3 rot( ang_vel_axis, -ang_vel*p_step ); - transform.basis = rot * transform.basis; - transform.orthonormalize(); - } - - Vector3 total_linear_velocity=linear_velocity+biased_linear_velocity; - - - transform.origin+=total_linear_velocity * p_step; - - _set_transform(transform); - _set_inv_transform(get_transform().inverse()); - - _update_inertia_tensor(); - - if (fi_callback) { - - get_space()->body_add_to_state_query_list(&direct_state_query_list); - } - -} - - -void BodySW::simulate_motion(const Transform& p_xform,real_t p_step) { - - Transform inv_xform = p_xform.affine_inverse(); - if (!get_space()) { - _set_transform(p_xform); - _set_inv_transform(inv_xform); - - return; - } - - //compute a FAKE linear velocity - this is easy - - linear_velocity=(p_xform.origin - get_transform().origin)/p_step; - - //compute a FAKE angular velocity, not so easy - Matrix3 rot=get_transform().basis.orthonormalized().transposed() * p_xform.basis.orthonormalized(); - Vector3 axis; - float angle; - - rot.get_axis_and_angle(axis,angle); - axis.normalize(); - angular_velocity=axis.normalized() * (angle/p_step); - linear_velocity = (p_xform.origin - get_transform().origin)/p_step; - - if (!direct_state_query_list.in_list())// - callalways, so lv and av are cleared && (state_query || direct_state_query)) - get_space()->body_add_to_state_query_list(&direct_state_query_list); - simulated_motion=true; - _set_transform(p_xform); - - -} - -void BodySW::wakeup_neighbours() { - - for(Map::Element *E=constraint_map.front();E;E=E->next()) { - - const ConstraintSW *c=E->key(); - BodySW **n = c->get_body_ptr(); - int bc=c->get_body_count(); - - for(int i=0;iget()) - continue; - BodySW *b = n[i]; - if (b->mode!=PhysicsServer::BODY_MODE_RIGID) - continue; - - if (!b->is_active()) - b->set_active(true); - } - } -} - -void BodySW::call_queries() { - - - if (fi_callback) { - - PhysicsDirectBodyStateSW *dbs = PhysicsDirectBodyStateSW::singleton; - dbs->body=this; - - Variant v=dbs; - - Object *obj = ObjectDB::get_instance(fi_callback->id); - if (!obj) { - - set_force_integration_callback(0,StringName()); - } else { - 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); - } - - - } - - if (simulated_motion) { - - // linear_velocity=Vector3(); - // angular_velocity=0; - simulated_motion=false; - } -} - - -bool BodySW::sleep_test(real_t p_step) { - - if (mode==PhysicsServer::BODY_MODE_STATIC || mode==PhysicsServer::BODY_MODE_STATIC_ACTIVE) - return true; // - 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_body_angular_velocity_sleep_treshold() && Math::abs(linear_velocity.length_squared()) < get_space()->get_body_linear_velocity_sleep_treshold()*get_space()->get_body_linear_velocity_sleep_treshold()) { - - still_time+=p_step; - - return still_time > get_space()->get_body_time_to_sleep(); - } else { - - still_time=0; //maybe this should be set to 0 on set_active? - return false; - } -} - - -void 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; - } - - - if (p_id!=0) { - - 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) { - - - mode=PhysicsServer::BODY_MODE_RIGID; - active=true; - - mass=1; -// _inv_inertia=Transform(); - _inv_mass=1; - bounce=0; - friction=1; - omit_force_integration=false; -// applied_torque=0; - island_step=0; - island_next=NULL; - island_list_next=NULL; - _set_static(false); - density=0; - contact_count=0; - simulated_motion=false; - still_time=0; - continuous_cd=false; - can_sleep=false; - fi_callback=NULL; - -} - -BodySW::~BodySW() { - - if (fi_callback) - memdelete(fi_callback); -} - -PhysicsDirectBodyStateSW *PhysicsDirectBodyStateSW::singleton=NULL; - -PhysicsDirectSpaceState* PhysicsDirectBodyStateSW::get_space_state() { - - return body->get_space()->get_direct_state(); -} +#include "body_sw.h" +#include "space_sw.h" +#include "area_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_inertia_tensor() { + + Matrix3 tb = get_transform().basis; + tb.scale(_inv_inertia); + _inv_inertia_tensor = tb * get_transform().basis.transposed(); + +} + +void BodySW::update_inertias() { + + //update shapes and motions + + switch(mode) { + + case PhysicsServer::BODY_MODE_RIGID: { + + //update tensor for allshapes, not the best way but should be somehow OK. (inspired from bullet) + float total_area=0; + + for (int i=0;imass / total_area; + + _inertia += shape->get_moment_of_inertia(mass) + mass * get_shape_transform(i).get_origin(); + + } + + if (_inertia!=Vector3()) + _inv_inertia=_inertia.inverse(); + else + _inv_inertia=Vector3(); + + if (mass) + _inv_mass=1.0/mass; + else + _inv_mass=0; + + } break; + + case PhysicsServer::BODY_MODE_KINEMATIC: + case PhysicsServer::BODY_MODE_STATIC: { + + _inv_inertia=Vector3(); + _inv_mass=0; + } break; + case PhysicsServer::BODY_MODE_CHARACTER: { + + _inv_inertia=Vector3(); + _inv_mass=1.0/mass; + + } break; + } + _update_inertia_tensor(); + + //_update_shapes(); + +} + + + +void BodySW::set_active(bool p_active) { + + if (active==p_active) + return; + + active=p_active; + if (!p_active) { + if (get_space()) + get_space()->body_remove_from_active_list(&active_list); + } else { + if (mode==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; + + for(int i=0;i0) { + get_space()->get_broadphase()->set_active(s.bpid,active); + } + } +*/ +} + + + +void BodySW::set_param(PhysicsServer::BodyParameter p_param, float p_value) { + + switch(p_param) { + case PhysicsServer::BODY_PARAM_BOUNCE: { + + bounce=p_value; + } break; + case PhysicsServer::BODY_PARAM_FRICTION: { + + friction=p_value; + } break; + case PhysicsServer::BODY_PARAM_MASS: { + ERR_FAIL_COND(p_value<=0); + mass=p_value; + _update_inertia(); + + } break; + default:{} + } +} + +float BodySW::get_param(PhysicsServer::BodyParameter p_param) const { + + switch(p_param) { + case PhysicsServer::BODY_PARAM_BOUNCE: { + + return bounce; + } break; + case PhysicsServer::BODY_PARAM_FRICTION: { + + return friction; + } break; + case PhysicsServer::BODY_PARAM_MASS: { + return mass; + } break; + default:{} + } + + return 0; +} + +void BodySW::set_mode(PhysicsServer::BodyMode p_mode) { + + mode=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); + set_active(p_mode==PhysicsServer::BODY_MODE_KINEMATIC); + linear_velocity=Vector3(); + angular_velocity=Vector3(); + } break; + case PhysicsServer::BODY_MODE_RIGID: { + + _inv_mass=mass>0?(1.0/mass):0; + _set_static(false); + simulated_motion=false; //jic + + } break; + case PhysicsServer::BODY_MODE_CHARACTER: { + + _inv_mass=mass>0?(1.0/mass):0; + _set_static(false); + simulated_motion=false; //jic + } break; + } + + _update_inertia(); + //if (get_space()) +// _update_queries(); + +} +PhysicsServer::BodyMode BodySW::get_mode() const { + + return mode; +} + +void BodySW::_shapes_changed() { + + _update_inertia(); +} + +void BodySW::set_state(PhysicsServer::BodyState p_state, const Variant& p_variant) { + + switch(p_state) { + case PhysicsServer::BODY_STATE_TRANSFORM: { + + + if (mode==PhysicsServer::BODY_MODE_STATIC || mode==PhysicsServer::BODY_MODE_KINEMATIC) { + _set_transform(p_variant); + _set_inv_transform(get_transform().affine_inverse()); + wakeup_neighbours(); + } else { + Transform t = p_variant; + t.orthonormalize(); + _set_transform(t); + _set_inv_transform(get_transform().inverse()); + + } + + } break; + case PhysicsServer::BODY_STATE_LINEAR_VELOCITY: { + + //if (mode==PhysicsServer::BODY_MODE_STATIC) + // break; + linear_velocity=p_variant; + } break; + case PhysicsServer::BODY_STATE_ANGULAR_VELOCITY: { + //if (mode!=PhysicsServer::BODY_MODE_RIGID) + // break; + angular_velocity=p_variant; + + } break; + case PhysicsServer::BODY_STATE_SLEEPING: { + //? + if (mode==PhysicsServer::BODY_MODE_STATIC || mode==PhysicsServer::BODY_MODE_KINEMATIC) + break; + bool do_sleep=p_variant; + if (do_sleep) { + linear_velocity=Vector3(); + //biased_linear_velocity=Vector3(); + angular_velocity=Vector3(); + //biased_angular_velocity=Vector3(); + set_active(false); + } else { + 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) + set_active(true); + + } break; + } + +} +Variant BodySW::get_state(PhysicsServer::BodyState p_state) const { + + switch(p_state) { + case PhysicsServer::BODY_STATE_TRANSFORM: { + return get_transform(); + } break; + case PhysicsServer::BODY_STATE_LINEAR_VELOCITY: { + return linear_velocity; + } break; + case PhysicsServer::BODY_STATE_ANGULAR_VELOCITY: { + return angular_velocity; + } break; + case PhysicsServer::BODY_STATE_SLEEPING: { + return !is_active(); + } break; + case PhysicsServer::BODY_STATE_CAN_SLEEP: { + return can_sleep; + } break; + } + + return Variant(); +} + + +void BodySW::set_space(SpaceSW *p_space){ + + if (get_space()) { + + if (inertia_update_list.in_list()) + get_space()->body_remove_from_inertia_update_list(&inertia_update_list); + if (active_list.in_list()) + get_space()->body_remove_from_active_list(&active_list); + if (direct_state_query_list.in_list()) + get_space()->body_remove_from_state_query_list(&direct_state_query_list); + + } + + _set_space(p_space); + + if (get_space()) { + + _update_inertia(); + if (active) + get_space()->body_add_to_active_list(&active_list); +// _update_queries(); + //if (is_active()) { + // active=false; + // set_active(true); + //} + + } + +} + +void BodySW::_compute_area_gravity(const AreaSW *p_area) { + + if (p_area->is_gravity_point()) { + + gravity = (p_area->get_gravity_vector() - get_transform().get_origin()).normalized() * p_area->get_gravity(); + + } else { + gravity = p_area->get_gravity_vector() * p_area->get_gravity(); + } +} + +void BodySW::integrate_forces(real_t p_step) { + + + if (mode==PhysicsServer::BODY_MODE_STATIC || mode==PhysicsServer::BODY_MODE_KINEMATIC) + return; + + AreaSW *current_area = get_space()->get_default_area(); + ERR_FAIL_COND(!current_area); + + int prio = current_area->get_priority(); + int ac = areas.size(); + if (ac) { + const AreaCMP *aa = &areas[0]; + for(int i=0;iget_priority() > prio) { + current_area=aa[i].area; + prio=current_area->get_priority(); + } + } + } + + _compute_area_gravity(current_area); + density=current_area->get_density(); + + if (!omit_force_integration) { + //overriden by direct state query + + Vector3 force=gravity*mass; + force+=applied_force; + Vector3 torque=applied_torque; + + real_t damp = 1.0 - p_step * density; + + if (damp<0) // reached zero in the given time + damp=0; + + real_t angular_damp = 1.0 - p_step * density * get_space()->get_body_angular_velocity_damp_ratio(); + + if (angular_damp<0) // reached zero in the given time + angular_damp=0; + + linear_velocity*=damp; + angular_velocity*=angular_damp; + + linear_velocity+=_inv_mass * force * p_step; + angular_velocity+=_inv_inertia_tensor.xform(torque)*p_step; + } + + applied_force=Vector3(); + applied_torque=Vector3(); + + //motion=linear_velocity*p_step; + + biased_angular_velocity=Vector3(); + biased_linear_velocity=Vector3(); + + if (continuous_cd) //shapes temporarily extend for raycast + _update_shapes_with_motion(linear_velocity*p_step); + + current_area=NULL; // clear the area, so it is set in the next frame + contact_count=0; + +} + +void BodySW::integrate_velocities(real_t p_step) { + + if (mode==PhysicsServer::BODY_MODE_STATIC) + return; + + if (mode==PhysicsServer::BODY_MODE_KINEMATIC) { + if (fi_callback) + get_space()->body_add_to_state_query_list(&direct_state_query_list); + return; + } + + Vector3 total_angular_velocity = angular_velocity+biased_angular_velocity; + + + + float ang_vel = total_angular_velocity.length(); + Transform transform = get_transform(); + + + if (ang_vel!=0.0) { + Vector3 ang_vel_axis = total_angular_velocity / ang_vel; + Matrix3 rot( ang_vel_axis, -ang_vel*p_step ); + transform.basis = rot * transform.basis; + transform.orthonormalize(); + } + + Vector3 total_linear_velocity=linear_velocity+biased_linear_velocity; + + + transform.origin+=total_linear_velocity * p_step; + + _set_transform(transform); + _set_inv_transform(get_transform().inverse()); + + _update_inertia_tensor(); + + if (fi_callback) { + + get_space()->body_add_to_state_query_list(&direct_state_query_list); + } + +} + + +void BodySW::simulate_motion(const Transform& p_xform,real_t p_step) { + + Transform inv_xform = p_xform.affine_inverse(); + if (!get_space()) { + _set_transform(p_xform); + _set_inv_transform(inv_xform); + + return; + } + + //compute a FAKE linear velocity - this is easy + + linear_velocity=(p_xform.origin - get_transform().origin)/p_step; + + //compute a FAKE angular velocity, not so easy + Matrix3 rot=get_transform().basis.orthonormalized().transposed() * p_xform.basis.orthonormalized(); + Vector3 axis; + float angle; + + rot.get_axis_and_angle(axis,angle); + axis.normalize(); + angular_velocity=axis.normalized() * (angle/p_step); + linear_velocity = (p_xform.origin - get_transform().origin)/p_step; + + if (!direct_state_query_list.in_list())// - callalways, so lv and av are cleared && (state_query || direct_state_query)) + get_space()->body_add_to_state_query_list(&direct_state_query_list); + simulated_motion=true; + _set_transform(p_xform); + + +} + +void BodySW::wakeup_neighbours() { + + for(Map::Element *E=constraint_map.front();E;E=E->next()) { + + const ConstraintSW *c=E->key(); + BodySW **n = c->get_body_ptr(); + int bc=c->get_body_count(); + + for(int i=0;iget()) + continue; + BodySW *b = n[i]; + if (b->mode!=PhysicsServer::BODY_MODE_RIGID) + continue; + + if (!b->is_active()) + b->set_active(true); + } + } +} + +void BodySW::call_queries() { + + + if (fi_callback) { + + PhysicsDirectBodyStateSW *dbs = PhysicsDirectBodyStateSW::singleton; + dbs->body=this; + + Variant v=dbs; + + Object *obj = ObjectDB::get_instance(fi_callback->id); + if (!obj) { + + set_force_integration_callback(0,StringName()); + } else { + 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); + } + + + } + + if (simulated_motion) { + + // linear_velocity=Vector3(); + // angular_velocity=0; + simulated_motion=false; + } +} + + +bool BodySW::sleep_test(real_t p_step) { + + if (mode==PhysicsServer::BODY_MODE_STATIC || mode==PhysicsServer::BODY_MODE_KINEMATIC) + return true; // + 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_body_angular_velocity_sleep_treshold() && Math::abs(linear_velocity.length_squared()) < get_space()->get_body_linear_velocity_sleep_treshold()*get_space()->get_body_linear_velocity_sleep_treshold()) { + + still_time+=p_step; + + return still_time > get_space()->get_body_time_to_sleep(); + } else { + + still_time=0; //maybe this should be set to 0 on set_active? + return false; + } +} + + +void 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; + } + + + if (p_id!=0) { + + 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) { + + + mode=PhysicsServer::BODY_MODE_RIGID; + active=true; + + mass=1; +// _inv_inertia=Transform(); + _inv_mass=1; + bounce=0; + friction=1; + omit_force_integration=false; +// applied_torque=0; + island_step=0; + island_next=NULL; + island_list_next=NULL; + _set_static(false); + density=0; + contact_count=0; + simulated_motion=false; + still_time=0; + continuous_cd=false; + can_sleep=false; + fi_callback=NULL; + +} + +BodySW::~BodySW() { + + if (fi_callback) + memdelete(fi_callback); +} + +PhysicsDirectBodyStateSW *PhysicsDirectBodyStateSW::singleton=NULL; + +PhysicsDirectSpaceState* PhysicsDirectBodyStateSW::get_space_state() { + + return body->get_space()->get_direct_state(); +} diff --git a/servers/physics_2d/area_2d_sw.cpp b/servers/physics_2d/area_2d_sw.cpp index 6c391a7aa0..c840004190 100644 --- a/servers/physics_2d/area_2d_sw.cpp +++ b/servers/physics_2d/area_2d_sw.cpp @@ -84,7 +84,7 @@ void Area2DSW::set_monitor_callback(ObjectID p_id, const StringName& p_method) { 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) + if (do_override==(space_override_mode!=Physics2DServer::AREA_SPACE_OVERRIDE_DISABLED)) return; _unregister_shapes(); space_override_mode=p_mode; diff --git a/servers/physics_2d/area_pair_2d_sw.cpp b/servers/physics_2d/area_pair_2d_sw.cpp index da9a02922c..35286bdb67 100644 --- a/servers/physics_2d/area_pair_2d_sw.cpp +++ b/servers/physics_2d/area_pair_2d_sw.cpp @@ -32,7 +32,7 @@ bool AreaPair2DSW::setup(float p_step) { - bool result = CollisionSolver2DSW::solve_static(body->get_shape(body_shape),body->get_transform() * body->get_shape_transform(body_shape),body->get_shape_inv_transform(body_shape) * body->get_inv_transform(),area->get_shape(area_shape),area->get_transform() * area->get_shape_transform(area_shape),area->get_shape_inv_transform(area_shape) * area->get_inv_transform(),NULL,this); + 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); if (result!=colliding) { diff --git a/servers/physics_2d/body_2d_sw.cpp b/servers/physics_2d/body_2d_sw.cpp index f0e96ae5a6..f10cdadf4e 100644 --- a/servers/physics_2d/body_2d_sw.cpp +++ b/servers/physics_2d/body_2d_sw.cpp @@ -26,584 +26,589 @@ /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ -#include "body_2d_sw.h" -#include "space_2d_sw.h" -#include "area_2d_sw.h" - -void Body2DSW::_update_inertia() { - - if (get_space() && !inertia_update_list.in_list()) - get_space()->body_add_to_inertia_update_list(&inertia_update_list); - -} - - - -void Body2DSW::update_inertias() { - - //update shapes and motions - - switch(mode) { - - case Physics2DServer::BODY_MODE_RIGID: { - - //update tensor for allshapes, not the best way but should be somehow OK. (inspired from bullet) - float total_area=0; - - for (int i=0;imass / total_area; - - _inertia += shape->get_moment_of_inertia(mass) + mass * get_shape_transform(i).get_origin().length_squared(); - - } - - if (_inertia!=0) - _inv_inertia=1.0/_inertia; - else - _inv_inertia=0.0; //wathever - - if (mass) - _inv_mass=1.0/mass; - else - _inv_mass=0; - - } break; - case Physics2DServer::BODY_MODE_STATIC_ACTIVE: - case Physics2DServer::BODY_MODE_STATIC: { - - _inv_inertia=0; - _inv_mass=0; - } break; - case Physics2DServer::BODY_MODE_CHARACTER: { - - _inv_inertia=0; - _inv_mass=1.0/mass; - - } break; - } - //_update_inertia_tensor(); - - //_update_shapes(); - -} - - - -void Body2DSW::set_active(bool p_active) { - - if (active==p_active) - return; - - active=p_active; - if (!p_active) { - if (get_space()) - get_space()->body_remove_from_active_list(&active_list); - } else { - if (mode==Physics2DServer::BODY_MODE_STATIC) - return; //static bodies can't become active - if (get_space()) - get_space()->body_add_to_active_list(&active_list); - - //still_time=0; - } -/* - if (!space) - return; - - for(int i=0;i0) { - get_space()->get_broadphase()->set_active(s.bpid,active); - } - } -*/ -} - - - -void Body2DSW::set_param(Physics2DServer::BodyParameter p_param, float p_value) { - - switch(p_param) { - case Physics2DServer::BODY_PARAM_BOUNCE: { - - bounce=p_value; - } break; - case Physics2DServer::BODY_PARAM_FRICTION: { - - friction=p_value; - } break; - case Physics2DServer::BODY_PARAM_MASS: { - ERR_FAIL_COND(p_value<=0); - mass=p_value; - _update_inertia(); - - } break; - default:{} - } -} - -float Body2DSW::get_param(Physics2DServer::BodyParameter p_param) const { - - switch(p_param) { - case Physics2DServer::BODY_PARAM_BOUNCE: { - - return bounce; - } break; - case Physics2DServer::BODY_PARAM_FRICTION: { - - return friction; - } break; - case Physics2DServer::BODY_PARAM_MASS: { - return mass; - } break; - default:{} - } - - return 0; -} - -void Body2DSW::set_mode(Physics2DServer::BodyMode p_mode) { - - mode=p_mode; - - switch(p_mode) { - //CLEAR UP EVERYTHING IN CASE IT NOT WORKS! - case Physics2DServer::BODY_MODE_STATIC: - case Physics2DServer::BODY_MODE_STATIC_ACTIVE: { - - _set_inv_transform(get_transform().affine_inverse()); - _inv_mass=0; - _set_static(p_mode==Physics2DServer::BODY_MODE_STATIC); - set_active(p_mode==Physics2DServer::BODY_MODE_STATIC_ACTIVE); - linear_velocity=Vector2(); - angular_velocity=0; - } break; - case Physics2DServer::BODY_MODE_RIGID: { - - _inv_mass=mass>0?(1.0/mass):0; - _set_static(false); - simulated_motion=false; //jic - - } break; - case Physics2DServer::BODY_MODE_CHARACTER: { - - _inv_mass=mass>0?(1.0/mass):0; - _set_static(false); - simulated_motion=false; //jic - } break; - } - - _update_inertia(); - //if (get_space()) -// _update_queries(); - -} -Physics2DServer::BodyMode Body2DSW::get_mode() const { - - return mode; -} - -void Body2DSW::_shapes_changed() { - - _update_inertia(); - wakeup_neighbours(); -} - -void Body2DSW::set_state(Physics2DServer::BodyState p_state, const Variant& p_variant) { - - switch(p_state) { - case Physics2DServer::BODY_STATE_TRANSFORM: { - - - if (mode==Physics2DServer::BODY_MODE_STATIC || mode==Physics2DServer::BODY_MODE_STATIC_ACTIVE) { - _set_transform(p_variant); - _set_inv_transform(get_transform().affine_inverse()); - wakeup_neighbours(); - } else { - Matrix32 t = p_variant; - t.orthonormalize(); - _set_transform(t); - _set_inv_transform(get_transform().inverse()); - - } - - } break; - case Physics2DServer::BODY_STATE_LINEAR_VELOCITY: { - - //if (mode==Physics2DServer::BODY_MODE_STATIC) - // break; - linear_velocity=p_variant; - - } break; - case Physics2DServer::BODY_STATE_ANGULAR_VELOCITY: { - //if (mode!=Physics2DServer::BODY_MODE_RIGID) - // break; - angular_velocity=p_variant; - - } break; - case Physics2DServer::BODY_STATE_SLEEPING: { - //? - if (mode==Physics2DServer::BODY_MODE_STATIC || mode==Physics2DServer::BODY_MODE_STATIC_ACTIVE) - break; - bool do_sleep=p_variant; - if (do_sleep) { - linear_velocity=Vector2(); - //biased_linear_velocity=Vector3(); - angular_velocity=0; - //biased_angular_velocity=Vector3(); - set_active(false); - } else { - if (mode!=Physics2DServer::BODY_MODE_STATIC) - set_active(true); - } - } break; - case Physics2DServer::BODY_STATE_CAN_SLEEP: { - can_sleep=p_variant; - if (mode==Physics2DServer::BODY_MODE_RIGID && !active && !can_sleep) - set_active(true); - - } break; - } - -} -Variant Body2DSW::get_state(Physics2DServer::BodyState p_state) const { - - switch(p_state) { - case Physics2DServer::BODY_STATE_TRANSFORM: { - return get_transform(); - } break; - case Physics2DServer::BODY_STATE_LINEAR_VELOCITY: { - return linear_velocity; - } break; - case Physics2DServer::BODY_STATE_ANGULAR_VELOCITY: { - return angular_velocity; - } break; - case Physics2DServer::BODY_STATE_SLEEPING: { - return !is_active(); - } break; - case Physics2DServer::BODY_STATE_CAN_SLEEP: { - return can_sleep; - } break; - } - - return Variant(); -} - - -void Body2DSW::set_space(Space2DSW *p_space){ - - if (get_space()) { - - wakeup_neighbours(); - - if (inertia_update_list.in_list()) - get_space()->body_remove_from_inertia_update_list(&inertia_update_list); - if (active_list.in_list()) - get_space()->body_remove_from_active_list(&active_list); - if (direct_state_query_list.in_list()) - get_space()->body_remove_from_state_query_list(&direct_state_query_list); - - } - - _set_space(p_space); - - if (get_space()) { - - _update_inertia(); - if (active) - get_space()->body_add_to_active_list(&active_list); -// _update_queries(); - //if (is_active()) { - // active=false; - // set_active(true); - //} - - } - -} - -void Body2DSW::_compute_area_gravity(const Area2DSW *p_area) { - - if (p_area->is_gravity_point()) { - - gravity = (p_area->get_transform().get_origin()+p_area->get_gravity_vector() - get_transform().get_origin()).normalized() * p_area->get_gravity(); - - } else { - gravity = p_area->get_gravity_vector() * p_area->get_gravity(); - } -} - -void Body2DSW::integrate_forces(real_t p_step) { - - if (mode==Physics2DServer::BODY_MODE_STATIC || mode==Physics2DServer::BODY_MODE_STATIC_ACTIVE) - return; - - Area2DSW *current_area = get_space()->get_default_area(); - ERR_FAIL_COND(!current_area); - - int prio = current_area->get_priority(); - int ac = areas.size(); - if (ac) { - const AreaCMP *aa = &areas[0]; - for(int i=0;iget_priority() > prio) { - current_area=aa[i].area; - prio=current_area->get_priority(); - } - } - } - - _compute_area_gravity(current_area); - density=current_area->get_density(); - - if (!omit_force_integration) { - //overriden by direct state query - - Vector2 force=gravity*mass; - force+=applied_force; - real_t torque=applied_torque; - - real_t damp = 1.0 - p_step * density; - - if (damp<0) // reached zero in the given time - damp=0; - - real_t angular_damp = 1.0 - p_step * density * get_space()->get_body_angular_velocity_damp_ratio(); - - if (angular_damp<0) // reached zero in the given time - angular_damp=0; - - linear_velocity*=damp; - angular_velocity*=angular_damp; - - linear_velocity+=_inv_mass * force * p_step; - angular_velocity+=_inv_inertia * torque * p_step; - } - - - //motion=linear_velocity*p_step; - - biased_angular_velocity=0; - biased_linear_velocity=Vector2(); - - if (continuous_cd) //shapes temporarily extend for raycast - _update_shapes_with_motion(linear_velocity*p_step); - - current_area=NULL; // clear the area, so it is set in the next frame - contact_count=0; - -} - -void Body2DSW::integrate_velocities(real_t p_step) { - - if (mode==Physics2DServer::BODY_MODE_STATIC) - return; - if (mode==Physics2DServer::BODY_MODE_STATIC_ACTIVE) { - if (fi_callback) - get_space()->body_add_to_state_query_list(&direct_state_query_list); - return; - } - - real_t total_angular_velocity = angular_velocity+biased_angular_velocity; - Vector2 total_linear_velocity=linear_velocity+biased_linear_velocity; - - real_t angle = get_transform().get_rotation() - total_angular_velocity * p_step; - Vector2 pos = get_transform().get_origin() + total_linear_velocity * p_step; - - - _set_transform(Matrix32(angle,pos)); - _set_inv_transform(get_transform().inverse()); - - - if (fi_callback) { - - get_space()->body_add_to_state_query_list(&direct_state_query_list); - } - - //_update_inertia_tensor(); -} - - -void Body2DSW::simulate_motion(const Matrix32& p_xform,real_t p_step) { - - Matrix32 inv_xform = p_xform.affine_inverse(); - if (!get_space()) { - _set_transform(p_xform); - _set_inv_transform(inv_xform); - - return; - } - - - - linear_velocity = (p_xform.elements[2] - get_transform().elements[2])/p_step; - - real_t rot = inv_xform.basis_xform(get_transform().elements[1]).atan2(); - angular_velocity = rot / p_step; - - if (!direct_state_query_list.in_list())// - callalways, so lv and av are cleared && (state_query || direct_state_query)) - get_space()->body_add_to_state_query_list(&direct_state_query_list); - simulated_motion=true; - _set_transform(p_xform); - - -} - -void Body2DSW::wakeup_neighbours() { - - - - for(Map::Element *E=constraint_map.front();E;E=E->next()) { - - const Constraint2DSW *c=E->key(); - Body2DSW **n = c->get_body_ptr(); - int bc=c->get_body_count(); - - for(int i=0;iget()) - continue; - Body2DSW *b = n[i]; - if (b->mode!=Physics2DServer::BODY_MODE_RIGID) - continue; - - if (!b->is_active()) - b->set_active(true); - } - } -} - -void Body2DSW::call_queries() { - - - if (fi_callback) { - - Physics2DDirectBodyStateSW *dbs = Physics2DDirectBodyStateSW::singleton; - dbs->body=this; - - Variant v=dbs; - const Variant *vp[2]={&v,&fi_callback->callback_udata}; - - Object *obj = ObjectDB::get_instance(fi_callback->id); - if (!obj) { - - set_force_integration_callback(NULL,StringName()); - } else { - Variant::CallError ce; - if (fi_callback->callback_udata.get_type()) { - - obj->call(fi_callback->method,vp,2,ce); - - } else { - obj->call(fi_callback->method,vp,1,ce); - } - } - - - } - - if (simulated_motion) { - - // linear_velocity=Vector2(); - // angular_velocity=0; - simulated_motion=false; - } -} - - -bool Body2DSW::sleep_test(real_t p_step) { - - if (mode==Physics2DServer::BODY_MODE_STATIC || mode==Physics2DServer::BODY_MODE_STATIC_ACTIVE) - return true; // - else if (mode==Physics2DServer::BODY_MODE_CHARACTER) - return !active; // characters don't sleep unless asked to sleep - else if (!can_sleep) - return false; - - - - - if (Math::abs(angular_velocity)get_body_angular_velocity_sleep_treshold() && Math::abs(linear_velocity.length_squared()) < get_space()->get_body_linear_velocity_sleep_treshold()*get_space()->get_body_linear_velocity_sleep_treshold()) { - - still_time+=p_step; - - return still_time > get_space()->get_body_time_to_sleep(); - } else { - - still_time=0; //maybe this should be set to 0 on set_active? - return false; - } -} - - -void Body2DSW::set_force_integration_callback(ObjectID p_id,const StringName& p_method,const Variant& p_udata) { - - if (fi_callback) { - - memdelete(fi_callback); - fi_callback=NULL; - } - - - if (p_id!=0) { - - fi_callback=memnew(ForceIntegrationCallback); - fi_callback->id=p_id; - fi_callback->method=p_method; - fi_callback->callback_udata=p_udata; - } - -} - -Body2DSW::Body2DSW() : CollisionObject2DSW(TYPE_BODY), active_list(this), inertia_update_list(this), direct_state_query_list(this) { - - - mode=Physics2DServer::BODY_MODE_RIGID; - active=true; - angular_velocity=0; - biased_angular_velocity=0; - mass=1; - _inv_inertia=0; - _inv_mass=1; - bounce=0; - friction=1; - omit_force_integration=false; - applied_torque=0; - island_step=0; - island_next=NULL; - island_list_next=NULL; - _set_static(false); - density=0; - contact_count=0; - simulated_motion=false; - still_time=0; - continuous_cd=false; - can_sleep=false; - fi_callback=NULL; - -} - -Body2DSW::~Body2DSW() { - - if (fi_callback) - memdelete(fi_callback); -} - -Physics2DDirectBodyStateSW *Physics2DDirectBodyStateSW::singleton=NULL; - -Physics2DDirectSpaceState* Physics2DDirectBodyStateSW::get_space_state() { - - return body->get_space()->get_direct_state(); -} +#include "body_2d_sw.h" +#include "space_2d_sw.h" +#include "area_2d_sw.h" + +void Body2DSW::_update_inertia() { + + if (get_space() && !inertia_update_list.in_list()) + get_space()->body_add_to_inertia_update_list(&inertia_update_list); + +} + + + +void Body2DSW::update_inertias() { + + //update shapes and motions + + switch(mode) { + + case Physics2DServer::BODY_MODE_RIGID: { + + //update tensor for allshapes, not the best way but should be somehow OK. (inspired from bullet) + float total_area=0; + + for (int i=0;imass / total_area; + + _inertia += shape->get_moment_of_inertia(mass) + mass * get_shape_transform(i).get_origin().length_squared(); + + } + + if (_inertia!=0) + _inv_inertia=1.0/_inertia; + else + _inv_inertia=0.0; //wathever + + if (mass) + _inv_mass=1.0/mass; + else + _inv_mass=0; + + } break; + case Physics2DServer::BODY_MODE_KINEMATIC: + case Physics2DServer::BODY_MODE_STATIC: { + + _inv_inertia=0; + _inv_mass=0; + } break; + case Physics2DServer::BODY_MODE_CHARACTER: { + + _inv_inertia=0; + _inv_mass=1.0/mass; + + } break; + } + //_update_inertia_tensor(); + + //_update_shapes(); + +} + + + +void Body2DSW::set_active(bool p_active) { + + if (active==p_active) + return; + + active=p_active; + if (!p_active) { + if (get_space()) + get_space()->body_remove_from_active_list(&active_list); + } else { + if (mode==Physics2DServer::BODY_MODE_STATIC) + return; //static bodies can't become active + if (get_space()) + get_space()->body_add_to_active_list(&active_list); + + //still_time=0; + } +/* + if (!space) + return; + + for(int i=0;i0) { + get_space()->get_broadphase()->set_active(s.bpid,active); + } + } +*/ +} + + + +void Body2DSW::set_param(Physics2DServer::BodyParameter p_param, float p_value) { + + switch(p_param) { + case Physics2DServer::BODY_PARAM_BOUNCE: { + + bounce=p_value; + } break; + case Physics2DServer::BODY_PARAM_FRICTION: { + + friction=p_value; + } break; + case Physics2DServer::BODY_PARAM_MASS: { + ERR_FAIL_COND(p_value<=0); + mass=p_value; + _update_inertia(); + + } break; + default:{} + } +} + +float Body2DSW::get_param(Physics2DServer::BodyParameter p_param) const { + + switch(p_param) { + case Physics2DServer::BODY_PARAM_BOUNCE: { + + return bounce; + } break; + case Physics2DServer::BODY_PARAM_FRICTION: { + + return friction; + } break; + case Physics2DServer::BODY_PARAM_MASS: { + return mass; + } break; + default:{} + } + + return 0; +} + +void Body2DSW::set_mode(Physics2DServer::BodyMode p_mode) { + + mode=p_mode; + + switch(p_mode) { + //CLEAR UP EVERYTHING IN CASE IT NOT WORKS! + case Physics2DServer::BODY_MODE_STATIC: + case Physics2DServer::BODY_MODE_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); + linear_velocity=Vector2(); + angular_velocity=0; + } break; + case Physics2DServer::BODY_MODE_RIGID: { + + _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; + _set_static(false); + } break; + } + + _update_inertia(); + //if (get_space()) +// _update_queries(); + +} +Physics2DServer::BodyMode Body2DSW::get_mode() const { + + return mode; +} + +void Body2DSW::_shapes_changed() { + + _update_inertia(); + wakeup_neighbours(); +} + +void Body2DSW::set_state(Physics2DServer::BodyState p_state, const Variant& p_variant) { + + switch(p_state) { + case Physics2DServer::BODY_STATE_TRANSFORM: { + + + if (mode==Physics2DServer::BODY_MODE_KINEMATIC) { + new_transform=p_variant; + //wakeup_neighbours(); + set_active(true); + } else if (mode==Physics2DServer::BODY_MODE_STATIC) { + _set_transform(p_variant); + _set_inv_transform(get_transform().affine_inverse()); + wakeup_neighbours(); + } else { + Matrix32 t = p_variant; + t.orthonormalize(); + new_transform=get_transform(); //used as old to compute motion + _set_transform(t); + _set_inv_transform(get_transform().inverse()); + + } + + } break; + case Physics2DServer::BODY_STATE_LINEAR_VELOCITY: { + + //if (mode==Physics2DServer::BODY_MODE_STATIC) + // break; + linear_velocity=p_variant; + + } break; + case Physics2DServer::BODY_STATE_ANGULAR_VELOCITY: { + //if (mode!=Physics2DServer::BODY_MODE_RIGID) + // break; + angular_velocity=p_variant; + + } break; + case Physics2DServer::BODY_STATE_SLEEPING: { + //? + if (mode==Physics2DServer::BODY_MODE_STATIC || mode==Physics2DServer::BODY_MODE_KINEMATIC) + break; + bool do_sleep=p_variant; + if (do_sleep) { + linear_velocity=Vector2(); + //biased_linear_velocity=Vector3(); + angular_velocity=0; + //biased_angular_velocity=Vector3(); + set_active(false); + } else { + if (mode!=Physics2DServer::BODY_MODE_STATIC) + set_active(true); + } + } break; + case Physics2DServer::BODY_STATE_CAN_SLEEP: { + can_sleep=p_variant; + if (mode==Physics2DServer::BODY_MODE_RIGID && !active && !can_sleep) + set_active(true); + + } break; + } + +} +Variant Body2DSW::get_state(Physics2DServer::BodyState p_state) const { + + switch(p_state) { + case Physics2DServer::BODY_STATE_TRANSFORM: { + return get_transform(); + } break; + case Physics2DServer::BODY_STATE_LINEAR_VELOCITY: { + return linear_velocity; + } break; + case Physics2DServer::BODY_STATE_ANGULAR_VELOCITY: { + return angular_velocity; + } break; + case Physics2DServer::BODY_STATE_SLEEPING: { + return !is_active(); + } break; + case Physics2DServer::BODY_STATE_CAN_SLEEP: { + return can_sleep; + } break; + } + + return Variant(); +} + + +void Body2DSW::set_space(Space2DSW *p_space){ + + if (get_space()) { + + wakeup_neighbours(); + + if (inertia_update_list.in_list()) + get_space()->body_remove_from_inertia_update_list(&inertia_update_list); + if (active_list.in_list()) + get_space()->body_remove_from_active_list(&active_list); + if (direct_state_query_list.in_list()) + get_space()->body_remove_from_state_query_list(&direct_state_query_list); + + } + + _set_space(p_space); + + if (get_space()) { + + _update_inertia(); + if (active) + get_space()->body_add_to_active_list(&active_list); +// _update_queries(); + //if (is_active()) { + // active=false; + // set_active(true); + //} + + } + +} + +void Body2DSW::_compute_area_gravity(const Area2DSW *p_area) { + + if (p_area->is_gravity_point()) { + + gravity = (p_area->get_transform().get_origin()+p_area->get_gravity_vector() - get_transform().get_origin()).normalized() * p_area->get_gravity(); + + } else { + gravity = p_area->get_gravity_vector() * p_area->get_gravity(); + } +} + +void Body2DSW::integrate_forces(real_t p_step) { + + if (mode==Physics2DServer::BODY_MODE_STATIC) + return; + + Area2DSW *current_area = get_space()->get_default_area(); + ERR_FAIL_COND(!current_area); + + int prio = current_area->get_priority(); + int ac = areas.size(); + if (ac) { + const AreaCMP *aa = &areas[0]; + for(int i=0;iget_priority() > prio) { + current_area=aa[i].area; + prio=current_area->get_priority(); + } + } + } + + _compute_area_gravity(current_area); + density=current_area->get_density(); + + Vector2 motion; + bool do_motion=false; + + if (mode==Physics2DServer::BODY_MODE_KINEMATIC) { + + //compute motion, angular and etc. velocities from prev transform + linear_velocity = (new_transform.elements[2] - get_transform().elements[2])/p_step; + + real_t rot = new_transform.affine_inverse().basis_xform(get_transform().elements[1]).atan2(); + angular_velocity = rot / p_step; + + motion = new_transform.elements[2] - get_transform().elements[2]; + do_motion=true; + + for(int i=0;iget_body_angular_velocity_damp_ratio(); + + if (angular_damp<0) // reached zero in the given time + angular_damp=0; + + linear_velocity*=damp; + angular_velocity*=angular_damp; + + linear_velocity+=_inv_mass * force * p_step; + angular_velocity+=_inv_inertia * torque * p_step; + } + + if (continuous_cd_mode!=Physics2DServer::CCD_MODE_DISABLED) { + + motion = new_transform.get_origin() - get_transform().get_origin(); + //linear_velocity*p_step; + do_motion=true; + } + } + + + //motion=linear_velocity*p_step; + + biased_angular_velocity=0; + biased_linear_velocity=Vector2(); + + if (do_motion) {//shapes temporarily extend for raycast + _update_shapes_with_motion(motion); + } + + current_area=NULL; // clear the area, so it is set in the next frame + contact_count=0; + +} + +void Body2DSW::integrate_velocities(real_t p_step) { + + if (mode==Physics2DServer::BODY_MODE_STATIC) + return; + + if (fi_callback) + get_space()->body_add_to_state_query_list(&direct_state_query_list); + + if (mode==Physics2DServer::BODY_MODE_KINEMATIC) { + + _set_transform(new_transform,false); + _set_inv_transform(new_transform.affine_inverse()); ; + if (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 angle = get_transform().get_rotation() - total_angular_velocity * p_step; + Vector2 pos = get_transform().get_origin() + total_linear_velocity * p_step; + + _set_transform(Matrix32(angle,pos),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(); + + //_update_inertia_tensor(); +} + + + +void Body2DSW::wakeup_neighbours() { + + + + for(Map::Element *E=constraint_map.front();E;E=E->next()) { + + const Constraint2DSW *c=E->key(); + Body2DSW **n = c->get_body_ptr(); + int bc=c->get_body_count(); + + for(int i=0;iget()) + continue; + Body2DSW *b = n[i]; + if (b->mode!=Physics2DServer::BODY_MODE_RIGID) + continue; + + if (!b->is_active()) + b->set_active(true); + } + } +} + +void Body2DSW::call_queries() { + + + if (fi_callback) { + + Physics2DDirectBodyStateSW *dbs = Physics2DDirectBodyStateSW::singleton; + dbs->body=this; + + Variant v=dbs; + const Variant *vp[2]={&v,&fi_callback->callback_udata}; + + Object *obj = ObjectDB::get_instance(fi_callback->id); + if (!obj) { + + set_force_integration_callback(0,StringName()); + } else { + Variant::CallError ce; + if (fi_callback->callback_udata.get_type()) { + + obj->call(fi_callback->method,vp,2,ce); + + } else { + obj->call(fi_callback->method,vp,1,ce); + } + } + + + } + +} + + +bool Body2DSW::sleep_test(real_t p_step) { + + if (mode==Physics2DServer::BODY_MODE_STATIC || mode==Physics2DServer::BODY_MODE_KINEMATIC) + return true; // + 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_body_angular_velocity_sleep_treshold() && Math::abs(linear_velocity.length_squared()) < get_space()->get_body_linear_velocity_sleep_treshold()*get_space()->get_body_linear_velocity_sleep_treshold()) { + + still_time+=p_step; + + return still_time > get_space()->get_body_time_to_sleep(); + } else { + + still_time=0; //maybe this should be set to 0 on set_active? + return false; + } +} + + +void Body2DSW::set_force_integration_callback(ObjectID p_id,const StringName& p_method,const Variant& p_udata) { + + if (fi_callback) { + + memdelete(fi_callback); + fi_callback=NULL; + } + + + if (p_id!=0) { + + fi_callback=memnew(ForceIntegrationCallback); + fi_callback->id=p_id; + fi_callback->method=p_method; + fi_callback->callback_udata=p_udata; + } + +} + +Body2DSW::Body2DSW() : CollisionObject2DSW(TYPE_BODY), active_list(this), inertia_update_list(this), direct_state_query_list(this) { + + + mode=Physics2DServer::BODY_MODE_RIGID; + active=true; + angular_velocity=0; + biased_angular_velocity=0; + mass=1; + _inv_inertia=0; + _inv_mass=1; + bounce=0; + friction=1; + omit_force_integration=false; + applied_torque=0; + island_step=0; + island_next=NULL; + island_list_next=NULL; + _set_static(false); + density=0; + contact_count=0; + + still_time=0; + continuous_cd_mode=Physics2DServer::CCD_MODE_DISABLED; + can_sleep=false; + fi_callback=NULL; + +} + +Body2DSW::~Body2DSW() { + + if (fi_callback) + memdelete(fi_callback); +} + +Physics2DDirectBodyStateSW *Physics2DDirectBodyStateSW::singleton=NULL; + +Physics2DDirectSpaceState* Physics2DDirectBodyStateSW::get_space_state() { + + return body->get_space()->get_direct_state(); +} diff --git a/servers/physics_2d/body_2d_sw.h b/servers/physics_2d/body_2d_sw.h index 55b84ce7a7..14cae3dbb0 100644 --- a/servers/physics_2d/body_2d_sw.h +++ b/servers/physics_2d/body_2d_sw.h @@ -26,309 +26,321 @@ /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ -#ifndef BODY_2D_SW_H -#define BODY_2D_SW_H - -#include "collision_object_2d_sw.h" -#include "vset.h" -#include "area_2d_sw.h" - -class Constraint2DSW; - - -class Body2DSW : public CollisionObject2DSW { - - - Physics2DServer::BodyMode mode; - - Vector2 biased_linear_velocity; - real_t biased_angular_velocity; - - Vector2 linear_velocity; - real_t angular_velocity; - - real_t mass; - real_t bounce; - real_t friction; - - real_t _inv_mass; - real_t _inv_inertia; - - Vector2 gravity; - real_t density; - - real_t still_time; - - Vector2 applied_force; - real_t applied_torque; - - SelfList active_list; - SelfList inertia_update_list; - SelfList direct_state_query_list; - - VSet exceptions; - bool omit_force_integration; - bool active; - bool simulated_motion; - bool continuous_cd; - bool can_sleep; - void _update_inertia(); - virtual void _shapes_changed(); - - - Map constraint_map; - - struct AreaCMP { - - Area2DSW *area; - _FORCE_INLINE_ bool operator<(const AreaCMP& p_cmp) const { return area->get_self() < p_cmp.area->get_self() ; } - _FORCE_INLINE_ AreaCMP() {} - _FORCE_INLINE_ AreaCMP(Area2DSW *p_area) { area=p_area;} - }; - - - VSet areas; - - struct Contact { - - - Vector2 local_pos; - Vector2 local_normal; - float depth; - int local_shape; - Vector2 collider_pos; - int collider_shape; - ObjectID collider_instance_id; - RID collider; - Vector2 collider_velocity_at_pos; - }; - - Vector contacts; //no contacts by default - int contact_count; - - struct ForceIntegrationCallback { - - ObjectID id; - StringName method; - Variant callback_udata; - }; - - ForceIntegrationCallback *fi_callback; - - - uint64_t island_step; - Body2DSW *island_next; - Body2DSW *island_list_next; - - _FORCE_INLINE_ void _compute_area_gravity(const Area2DSW *p_area); - -friend class Physics2DDirectBodyStateSW; // i give up, too many functions to expose - -public: - - - void set_force_integration_callback(ObjectID p_id, const StringName& p_method, const Variant &p_udata=Variant()); - - - _FORCE_INLINE_ void add_area(Area2DSW *p_area) { areas.insert(AreaCMP(p_area)); } - _FORCE_INLINE_ void remove_area(Area2DSW *p_area) { areas.erase(AreaCMP(p_area)); } - - _FORCE_INLINE_ void set_max_contacts_reported(int p_size) { contacts.resize(p_size); contact_count=0; } - _FORCE_INLINE_ int get_max_contacts_reported() const { return contacts.size(); } - - _FORCE_INLINE_ bool can_report_contacts() const { return !contacts.empty(); } - _FORCE_INLINE_ void add_contact(const Vector2& p_local_pos,const Vector2& p_local_normal, float p_depth, int p_local_shape, const Vector2& p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID& p_collider,const Vector2& p_collider_velocity_at_pos); - - - _FORCE_INLINE_ void add_exception(const RID& p_exception) { exceptions.insert(p_exception);} - _FORCE_INLINE_ void remove_exception(const RID& p_exception) { exceptions.erase(p_exception);} - _FORCE_INLINE_ bool has_exception(const RID& p_exception) const { return exceptions.has(p_exception);} - _FORCE_INLINE_ const VSet& get_exceptions() const { return exceptions;} - - _FORCE_INLINE_ uint64_t get_island_step() const { return island_step; } - _FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step=p_step; } - - _FORCE_INLINE_ Body2DSW* get_island_next() const { return island_next; } - _FORCE_INLINE_ void set_island_next(Body2DSW* p_next) { island_next=p_next; } - - _FORCE_INLINE_ Body2DSW* get_island_list_next() const { return island_list_next; } - _FORCE_INLINE_ void set_island_list_next(Body2DSW* p_next) { island_list_next=p_next; } - - _FORCE_INLINE_ void add_constraint(Constraint2DSW* p_constraint, int p_pos) { constraint_map[p_constraint]=p_pos; } - _FORCE_INLINE_ void remove_constraint(Constraint2DSW* p_constraint) { constraint_map.erase(p_constraint); } - const Map& get_constraint_map() const { return constraint_map; } - - _FORCE_INLINE_ void set_omit_force_integration(bool p_omit_force_integration) { omit_force_integration=p_omit_force_integration; } - _FORCE_INLINE_ bool get_omit_force_integration() const { return omit_force_integration; } - - _FORCE_INLINE_ void set_linear_velocity(const Vector2& p_velocity) {linear_velocity=p_velocity; } - _FORCE_INLINE_ Vector2 get_linear_velocity() const { return linear_velocity; } - - _FORCE_INLINE_ void set_angular_velocity(real_t p_velocity) { angular_velocity=p_velocity; } - _FORCE_INLINE_ real_t get_angular_velocity() const { return angular_velocity; } - - _FORCE_INLINE_ void set_biased_linear_velocity(const Vector2& p_velocity) {biased_linear_velocity=p_velocity; } - _FORCE_INLINE_ Vector2 get_biased_linear_velocity() const { return biased_linear_velocity; } - - _FORCE_INLINE_ void set_biased_angular_velocity(real_t p_velocity) { biased_angular_velocity=p_velocity; } - _FORCE_INLINE_ real_t get_biased_angular_velocity() const { return biased_angular_velocity; } - - _FORCE_INLINE_ void apply_impulse(const Vector2& p_pos, const Vector2& p_j) { - - linear_velocity += p_j * _inv_mass; - angular_velocity += _inv_inertia * p_pos.cross(p_j); - } - - _FORCE_INLINE_ void apply_bias_impulse(const Vector2& p_pos, const Vector2& p_j) { - - biased_linear_velocity += p_j * _inv_mass; - biased_angular_velocity += _inv_inertia * p_pos.cross(p_j); - } - - void set_active(bool p_active); - _FORCE_INLINE_ bool is_active() const { return active; } - - void set_param(Physics2DServer::BodyParameter p_param, float); - float get_param(Physics2DServer::BodyParameter p_param) const; - - void set_mode(Physics2DServer::BodyMode p_mode); - Physics2DServer::BodyMode get_mode() const; - - void set_state(Physics2DServer::BodyState p_state, const Variant& p_variant); - Variant get_state(Physics2DServer::BodyState p_state) const; - - void set_applied_force(const Vector2& p_force) { applied_force=p_force; } - Vector2 get_applied_force() const { return applied_force; } - - void set_applied_torque(real_t p_torque) { applied_torque=p_torque; } - real_t get_applied_torque() const { return applied_torque; } - - _FORCE_INLINE_ void set_continuous_collision_detection(bool p_enable) { continuous_cd=p_enable; } - _FORCE_INLINE_ bool is_continuous_collision_detection_enabled() const { return continuous_cd; } - - void set_space(Space2DSW *p_space); - - void update_inertias(); - - _FORCE_INLINE_ real_t get_inv_mass() const { return _inv_mass; } - _FORCE_INLINE_ real_t get_inv_inertia() const { return _inv_inertia; } - _FORCE_INLINE_ real_t get_friction() const { return friction; } - _FORCE_INLINE_ Vector2 get_gravity() const { return gravity; } - _FORCE_INLINE_ real_t get_density() const { return density; } - - void integrate_forces(real_t p_step); - void integrate_velocities(real_t p_step); - - void simulate_motion(const Matrix32& p_xform,real_t p_step); - void call_queries(); - void wakeup_neighbours(); - - bool sleep_test(real_t p_step); - - Body2DSW(); - ~Body2DSW(); - -}; - - -//add contact inline - -void Body2DSW::add_contact(const Vector2& p_local_pos,const Vector2& p_local_normal, float p_depth, int p_local_shape, const Vector2& p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID& p_collider,const Vector2& p_collider_velocity_at_pos) { - - int c_max=contacts.size(); - - if (c_max==0) - return; - - Contact *c = &contacts[0]; - - - int idx=-1; - - if (contact_count=0 && least_depthget_gravity(); } // get gravity vector working on this body space/area - virtual float get_total_density() const { return body->get_density(); } // get density of this body space/area - - virtual float get_inverse_mass() const { return body->get_inv_mass(); } // get the mass - virtual real_t get_inverse_inertia() const { return body->get_inv_inertia(); } // get density of this body space - - virtual void set_linear_velocity(const Vector2& p_velocity) { body->set_linear_velocity(p_velocity); } - virtual Vector2 get_linear_velocity() const { return body->get_linear_velocity(); } - - virtual void set_angular_velocity(real_t p_velocity) { body->set_angular_velocity(p_velocity); } - virtual real_t get_angular_velocity() const { return body->get_angular_velocity(); } - - virtual void set_transform(const Matrix32& p_transform) { body->set_state(Physics2DServer::BODY_STATE_TRANSFORM,p_transform); } - virtual Matrix32 get_transform() const { return body->get_transform(); } - - virtual void set_sleep_state(bool p_enable) { body->set_active(!p_enable); } - virtual bool is_sleeping() const { return !body->is_active(); } - - virtual int get_contact_count() const { return body->contact_count; } - - virtual Vector2 get_contact_local_pos(int p_contact_idx) const { - ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector2()); - return body->contacts[p_contact_idx].local_pos; - } - virtual Vector2 get_contact_local_normal(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector2()); return body->contacts[p_contact_idx].local_normal; } - virtual int get_contact_local_shape(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,-1); return body->contacts[p_contact_idx].local_shape; } - - virtual RID get_contact_collider(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,RID()); return body->contacts[p_contact_idx].collider; } - virtual Vector2 get_contact_collider_pos(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector2()); return body->contacts[p_contact_idx].collider_pos; } - virtual ObjectID get_contact_collider_id(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,0); return body->contacts[p_contact_idx].collider_instance_id; } - virtual int get_contact_collider_shape(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,0); return body->contacts[p_contact_idx].collider_shape; } - virtual Vector2 get_contact_collider_velocity_at_pos(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector2()); return body->contacts[p_contact_idx].collider_velocity_at_pos; } - - virtual Physics2DDirectSpaceState* get_space_state(); - - - virtual real_t get_step() const { return step; } - Physics2DDirectBodyStateSW() { singleton=this; body=NULL; } -}; - - -#endif // BODY_2D_SW_H +#ifndef BODY_2D_SW_H +#define BODY_2D_SW_H + +#include "collision_object_2d_sw.h" +#include "vset.h" +#include "area_2d_sw.h" + +class Constraint2DSW; + + +class Body2DSW : public CollisionObject2DSW { + + + Physics2DServer::BodyMode mode; + + Vector2 biased_linear_velocity; + real_t biased_angular_velocity; + + Vector2 linear_velocity; + real_t angular_velocity; + + real_t mass; + real_t bounce; + real_t friction; + + real_t _inv_mass; + real_t _inv_inertia; + + Vector2 gravity; + real_t density; + + real_t still_time; + + Vector2 applied_force; + real_t applied_torque; + + SelfList active_list; + SelfList inertia_update_list; + SelfList direct_state_query_list; + + VSet exceptions; + Physics2DServer::CCDMode continuous_cd_mode; + bool omit_force_integration; + bool active; + bool can_sleep; + void _update_inertia(); + virtual void _shapes_changed(); + Matrix32 new_transform; + + + Map constraint_map; + + struct AreaCMP { + + Area2DSW *area; + _FORCE_INLINE_ bool operator<(const AreaCMP& p_cmp) const { return area->get_self() < p_cmp.area->get_self() ; } + _FORCE_INLINE_ AreaCMP() {} + _FORCE_INLINE_ AreaCMP(Area2DSW *p_area) { area=p_area;} + }; + + + VSet areas; + + struct Contact { + + + Vector2 local_pos; + Vector2 local_normal; + float depth; + int local_shape; + Vector2 collider_pos; + int collider_shape; + ObjectID collider_instance_id; + RID collider; + Vector2 collider_velocity_at_pos; + }; + + Vector contacts; //no contacts by default + int contact_count; + + struct ForceIntegrationCallback { + + ObjectID id; + StringName method; + Variant callback_udata; + }; + + ForceIntegrationCallback *fi_callback; + + + uint64_t island_step; + Body2DSW *island_next; + Body2DSW *island_list_next; + + _FORCE_INLINE_ void _compute_area_gravity(const Area2DSW *p_area); + +friend class Physics2DDirectBodyStateSW; // i give up, too many functions to expose + +public: + + + void set_force_integration_callback(ObjectID p_id, const StringName& p_method, const Variant &p_udata=Variant()); + + + _FORCE_INLINE_ void add_area(Area2DSW *p_area) { areas.insert(AreaCMP(p_area)); } + _FORCE_INLINE_ void remove_area(Area2DSW *p_area) { areas.erase(AreaCMP(p_area)); } + + _FORCE_INLINE_ void set_max_contacts_reported(int p_size) { contacts.resize(p_size); contact_count=0; } + _FORCE_INLINE_ int get_max_contacts_reported() const { return contacts.size(); } + + _FORCE_INLINE_ bool can_report_contacts() const { return !contacts.empty(); } + _FORCE_INLINE_ void add_contact(const Vector2& p_local_pos,const Vector2& p_local_normal, float p_depth, int p_local_shape, const Vector2& p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID& p_collider,const Vector2& p_collider_velocity_at_pos); + + + _FORCE_INLINE_ void add_exception(const RID& p_exception) { exceptions.insert(p_exception);} + _FORCE_INLINE_ void remove_exception(const RID& p_exception) { exceptions.erase(p_exception);} + _FORCE_INLINE_ bool has_exception(const RID& p_exception) const { return exceptions.has(p_exception);} + _FORCE_INLINE_ const VSet& get_exceptions() const { return exceptions;} + + _FORCE_INLINE_ uint64_t get_island_step() const { return island_step; } + _FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step=p_step; } + + _FORCE_INLINE_ Body2DSW* get_island_next() const { return island_next; } + _FORCE_INLINE_ void set_island_next(Body2DSW* p_next) { island_next=p_next; } + + _FORCE_INLINE_ Body2DSW* get_island_list_next() const { return island_list_next; } + _FORCE_INLINE_ void set_island_list_next(Body2DSW* p_next) { island_list_next=p_next; } + + _FORCE_INLINE_ void add_constraint(Constraint2DSW* p_constraint, int p_pos) { constraint_map[p_constraint]=p_pos; } + _FORCE_INLINE_ void remove_constraint(Constraint2DSW* p_constraint) { constraint_map.erase(p_constraint); } + const Map& get_constraint_map() const { return constraint_map; } + + _FORCE_INLINE_ void set_omit_force_integration(bool p_omit_force_integration) { omit_force_integration=p_omit_force_integration; } + _FORCE_INLINE_ bool get_omit_force_integration() const { return omit_force_integration; } + + _FORCE_INLINE_ void set_linear_velocity(const Vector2& p_velocity) {linear_velocity=p_velocity; } + _FORCE_INLINE_ Vector2 get_linear_velocity() const { return linear_velocity; } + + _FORCE_INLINE_ void set_angular_velocity(real_t p_velocity) { angular_velocity=p_velocity; } + _FORCE_INLINE_ real_t get_angular_velocity() const { return angular_velocity; } + + _FORCE_INLINE_ void set_biased_linear_velocity(const Vector2& p_velocity) {biased_linear_velocity=p_velocity; } + _FORCE_INLINE_ Vector2 get_biased_linear_velocity() const { return biased_linear_velocity; } + + _FORCE_INLINE_ void set_biased_angular_velocity(real_t p_velocity) { biased_angular_velocity=p_velocity; } + _FORCE_INLINE_ real_t get_biased_angular_velocity() const { return biased_angular_velocity; } + + + _FORCE_INLINE_ void apply_impulse(const Vector2& p_pos, const Vector2& p_j) { + + linear_velocity += p_j * _inv_mass; + angular_velocity += _inv_inertia * p_pos.cross(p_j); + } + + _FORCE_INLINE_ void apply_bias_impulse(const Vector2& p_pos, const Vector2& p_j) { + + biased_linear_velocity += p_j * _inv_mass; + biased_angular_velocity += _inv_inertia * p_pos.cross(p_j); + } + + void set_active(bool p_active); + _FORCE_INLINE_ bool is_active() const { return active; } + + void set_param(Physics2DServer::BodyParameter p_param, float); + float get_param(Physics2DServer::BodyParameter p_param) const; + + void set_mode(Physics2DServer::BodyMode p_mode); + Physics2DServer::BodyMode get_mode() const; + + void set_state(Physics2DServer::BodyState p_state, const Variant& p_variant); + Variant get_state(Physics2DServer::BodyState p_state) const; + + void set_applied_force(const Vector2& p_force) { applied_force=p_force; } + Vector2 get_applied_force() const { return applied_force; } + + void set_applied_torque(real_t p_torque) { applied_torque=p_torque; } + real_t get_applied_torque() const { return applied_torque; } + + + _FORCE_INLINE_ void set_continuous_collision_detection_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_space(Space2DSW *p_space); + + void update_inertias(); + + _FORCE_INLINE_ real_t get_inv_mass() const { return _inv_mass; } + _FORCE_INLINE_ real_t get_inv_inertia() const { return _inv_inertia; } + _FORCE_INLINE_ real_t get_friction() const { return friction; } + _FORCE_INLINE_ Vector2 get_gravity() const { return gravity; } + _FORCE_INLINE_ real_t get_density() const { return density; } + _FORCE_INLINE_ real_t get_bounce() const { return bounce; } + + 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) { + 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 + } + return Vector2(); + } + + void call_queries(); + void wakeup_neighbours(); + + bool sleep_test(real_t p_step); + + Body2DSW(); + ~Body2DSW(); + +}; + + +//add contact inline + +void Body2DSW::add_contact(const Vector2& p_local_pos,const Vector2& p_local_normal, float p_depth, int p_local_shape, const Vector2& p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID& p_collider,const Vector2& p_collider_velocity_at_pos) { + + int c_max=contacts.size(); + + if (c_max==0) + return; + + Contact *c = &contacts[0]; + + + int idx=-1; + + if (contact_count=0 && least_depthget_gravity(); } // get gravity vector working on this body space/area + virtual float get_total_density() const { return body->get_density(); } // get density of this body space/area + + virtual float get_inverse_mass() const { return body->get_inv_mass(); } // get the mass + virtual real_t get_inverse_inertia() const { return body->get_inv_inertia(); } // get density of this body space + + virtual void set_linear_velocity(const Vector2& p_velocity) { body->set_linear_velocity(p_velocity); } + virtual Vector2 get_linear_velocity() const { return body->get_linear_velocity(); } + + virtual void set_angular_velocity(real_t p_velocity) { body->set_angular_velocity(p_velocity); } + virtual real_t get_angular_velocity() const { return body->get_angular_velocity(); } + + virtual void set_transform(const Matrix32& p_transform) { body->set_state(Physics2DServer::BODY_STATE_TRANSFORM,p_transform); } + virtual Matrix32 get_transform() const { return body->get_transform(); } + + virtual void set_sleep_state(bool p_enable) { body->set_active(!p_enable); } + virtual bool is_sleeping() const { return !body->is_active(); } + + virtual int get_contact_count() const { return body->contact_count; } + + virtual Vector2 get_contact_local_pos(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector2()); + return body->contacts[p_contact_idx].local_pos; + } + virtual Vector2 get_contact_local_normal(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector2()); return body->contacts[p_contact_idx].local_normal; } + virtual int get_contact_local_shape(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,-1); return body->contacts[p_contact_idx].local_shape; } + + virtual RID get_contact_collider(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,RID()); return body->contacts[p_contact_idx].collider; } + virtual Vector2 get_contact_collider_pos(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector2()); return body->contacts[p_contact_idx].collider_pos; } + virtual ObjectID get_contact_collider_id(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,0); return body->contacts[p_contact_idx].collider_instance_id; } + virtual int get_contact_collider_shape(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,0); return body->contacts[p_contact_idx].collider_shape; } + virtual Vector2 get_contact_collider_velocity_at_pos(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector2()); return body->contacts[p_contact_idx].collider_velocity_at_pos; } + + virtual Physics2DDirectSpaceState* get_space_state(); + + + virtual real_t get_step() const { return step; } + Physics2DDirectBodyStateSW() { singleton=this; body=NULL; } +}; + + +#endif // BODY_2D_SW_H diff --git a/servers/physics_2d/body_pair_2d_sw.cpp b/servers/physics_2d/body_pair_2d_sw.cpp index 6d8215840a..931125a1c0 100644 --- a/servers/physics_2d/body_pair_2d_sw.cpp +++ b/servers/physics_2d/body_pair_2d_sw.cpp @@ -46,7 +46,6 @@ void BodyPair2DSW::_contact_added_callback(const Vector2& p_point_A,const Vector // 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); @@ -61,6 +60,7 @@ void BodyPair2DSW::_contact_added_callback(const Vector2& p_point_A,const Vector 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 @@ -77,7 +77,7 @@ void BodyPair2DSW::_contact_added_callback(const Vector2& p_point_A,const Vector 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; + new_index=i; break; } } @@ -139,12 +139,26 @@ void BodyPair2DSW::_validate_contacts() { Contact& c = contacts[i]; - Vector2 global_A = A->get_transform().basis_xform(c.local_A); - Vector2 global_B = B->get_transform().basis_xform(c.local_B)+offset_B; - Vector2 axis = global_A - global_B; - float depth = axis.dot( c.normal ); + bool erase=false; + if (c.reused==false) { + //was left behind in previous frame + erase=true; + } else { + 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 axis = global_A - global_B; + float depth = axis.dot( c.normal ); + + + + if (depth < -max_separation || (global_B + c.normal * depth - global_A).length_squared() > max_separation2) { + erase=true; + } + } - if (depth < -max_separation || (global_B + c.normal * depth - global_A).length_squared() > max_separation2) { + if (erase) { // contact no longer needed, remove @@ -161,7 +175,9 @@ void BodyPair2DSW::_validate_contacts() { } -bool BodyPair2DSW::_test_ccd(float p_step,Body2DSW *p_A, int p_shape_A,const Matrix32& p_xform_A,const Matrix32& p_xform_inv_A,Body2DSW *p_B, int p_shape_B,const Matrix32& p_xform_B,const Matrix32& p_xform_inv_B,bool p_swap_result) { +bool BodyPair2DSW::_test_ccd(float p_step,Body2DSW *p_A, int p_shape_A,const Matrix32& p_xform_A,Body2DSW *p_B, int p_shape_B,const Matrix32& p_xform_B,bool p_swap_result) { + + Vector2 motion = p_A->get_linear_velocity()*p_step; real_t mlen = motion.length(); @@ -172,18 +188,24 @@ bool BodyPair2DSW::_test_ccd(float p_step,Body2DSW *p_A, int p_shape_A,const Mat real_t min,max; p_A->get_shape(p_shape_A)->project_rangev(mnormal,p_xform_A,min,max); - if (mlen < (max-min)*0.3) //did it move enough in this direction to even attempt raycast? let's say it should move more than 1/3 the size of the object in that axis + bool fast_object = mlen > (max-min)*0.3; //going too fast in that direction + + if (fast_object) { //did it move enough in this direction to even attempt raycast? let's say it should move more than 1/3 the size of the object in that axis return false; + } //cast a segment from support in motion normal, in the same direction of motion by motion length + //support is the worst case collision point, so real collision happened before int a; Vector2 s[2]; p_A->get_shape(p_shape_A)->get_supports(p_xform_A.basis_xform(mnormal).normalized(),s,a); Vector2 from = p_xform_A.xform(s[0]); Vector2 to = from + motion; - Vector2 local_from = p_xform_inv_B.xform(from); - Vector2 local_to = p_xform_inv_B.xform(to); + Matrix32 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_to = from_inv.xform(to); Vector2 rpos,rnorm; if (!p_B->get_shape(p_shape_B)->intersect_segment(local_from,local_to,rpos,rnorm)) @@ -191,8 +213,11 @@ bool BodyPair2DSW::_test_ccd(float p_step,Body2DSW *p_A, int p_shape_A,const Mat //ray hit something + + Vector2 hitpos = p_xform_B.xform(rpos); + Vector2 contact_A = to; - Vector2 contact_B = p_xform_B.xform(rpos); + Vector2 contact_B = hitpos; //create a contact @@ -208,41 +233,50 @@ bool BodyPair2DSW::_test_ccd(float p_step,Body2DSW *p_A, int p_shape_A,const Mat bool BodyPair2DSW::setup(float p_step) { + //cannot collide + if (A->is_shape_set_as_trigger(shape_A) || B->is_shape_set_as_trigger(shape_B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self()) || (A->get_mode()<=Physics2DServer::BODY_MODE_KINEMATIC && B->get_mode()<=Physics2DServer::BODY_MODE_KINEMATIC)) { + collided=false; + return false; + } + //use local A coordinates to avoid numerical issues on collision detection offset_B = B->get_transform().get_origin() - A->get_transform().get_origin(); _validate_contacts(); - //cannot collide - if (A->is_shape_set_as_trigger(shape_A) || B->is_shape_set_as_trigger(shape_B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self()) || (A->get_mode()<=Physics2DServer::BODY_MODE_STATIC_ACTIVE && B->get_mode()<=Physics2DServer::BODY_MODE_STATIC_ACTIVE)) { - collided=false; - - return false; - } Vector2 offset_A = A->get_transform().get_origin(); Matrix32 xform_Au = A->get_transform().untranslated(); Matrix32 xform_A = xform_Au * A->get_shape_transform(shape_A); - Matrix32 xform_inv_A = xform_A.affine_inverse(); Matrix32 xform_Bu = B->get_transform(); xform_Bu.elements[2]-=A->get_transform().get_origin(); Matrix32 xform_B = xform_Bu * B->get_shape_transform(shape_B); - Matrix32 xform_inv_B = xform_B.affine_inverse(); Shape2DSW *shape_A_ptr=A->get_shape(shape_A); Shape2DSW *shape_B_ptr=B->get_shape(shape_B); - collided = CollisionSolver2DSW::solve_static(shape_A_ptr,xform_A,xform_inv_A,shape_B_ptr,xform_B,xform_inv_B,_add_contact,this,&sep_axis); + Vector2 motion_A,motion_B; + + 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(); + } + //faster to set than to check.. + + 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->is_continuous_collision_detection_enabled() && A->get_type()>Physics2DServer::BODY_MODE_STATIC_ACTIVE) { - if (_test_ccd(p_step,A,shape_A,xform_A,xform_inv_A,B,shape_B,xform_B,xform_inv_B)) + + 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->is_continuous_collision_detection_enabled() && B->get_type()>Physics2DServer::BODY_MODE_STATIC_ACTIVE) { - if (_test_ccd(p_step,B,shape_B,xform_B,xform_inv_B,A,shape_A,xform_A,xform_inv_A,true)) + 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; } @@ -251,8 +285,6 @@ bool BodyPair2DSW::setup(float p_step) { } - - real_t max_penetration = space->get_contact_max_allowed_penetration(); float bias = 0.3f; @@ -280,7 +312,7 @@ bool BodyPair2DSW::setup(float p_step) { real_t depth = c.normal.dot(global_A - global_B); - if (depth<=0) { + if (depth<=0 || !c.reused) { c.active=false; continue; } @@ -311,7 +343,6 @@ bool BodyPair2DSW::setup(float p_step) { } } - // Precompute normal mass, tangent mass, and bias. real_t rnA = c.rA.dot(c.normal); real_t rnB = c.rB.dot(c.normal); @@ -373,6 +404,7 @@ void BodyPair2DSW::solve(float p_step) { 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(); @@ -388,7 +420,7 @@ void BodyPair2DSW::solve(float p_step) { A->apply_bias_impulse(c.rA,-jb); B->apply_bias_impulse(c.rB, jb); - real_t bounce=0; + real_t bounce=MAX(A->get_bounce(),B->get_bounce()); real_t jn = -(bounce + vn)*c.mass_normal; real_t jnOld = c.acc_normal_impulse; c.acc_normal_impulse = MAX(jnOld + jn, 0.0f); @@ -403,7 +435,6 @@ void BodyPair2DSW::solve(float p_step) { Vector2 j =c.normal * (c.acc_normal_impulse - jnOld) + tangent * ( c.acc_tangent_impulse - jtOld ); - A->apply_impulse(c.rA,-j); B->apply_impulse(c.rB, j); diff --git a/servers/physics_2d/body_pair_2d_sw.h b/servers/physics_2d/body_pair_2d_sw.h index 26278f87cd..ebe26776ed 100644 --- a/servers/physics_2d/body_pair_2d_sw.h +++ b/servers/physics_2d/body_pair_2d_sw.h @@ -65,6 +65,7 @@ class BodyPair2DSW : public Constraint2DSW { real_t depth; bool active; Vector2 rA,rB; + bool reused; }; Vector2 offset_B; //use local A coordinates to avoid numerical issues on collision detection @@ -76,7 +77,7 @@ class BodyPair2DSW : public Constraint2DSW { int cc; - bool _test_ccd(float p_step,Body2DSW *p_A, int p_shape_A,const Matrix32& p_xform_A,const Matrix32& p_xform_inv_A,Body2DSW *p_B, int p_shape_B,const Matrix32& p_xform_B,const Matrix32& p_xform_inv_B,bool p_swap_result=false); + bool _test_ccd(float p_step,Body2DSW *p_A, int p_shape_A,const Matrix32& p_xform_A,Body2DSW *p_B, int p_shape_B,const Matrix32& 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); diff --git a/servers/physics_2d/broad_phase_2d_hash_grid.cpp b/servers/physics_2d/broad_phase_2d_hash_grid.cpp index 10da376dfd..129c9ecb9c 100644 --- a/servers/physics_2d/broad_phase_2d_hash_grid.cpp +++ b/servers/physics_2d/broad_phase_2d_hash_grid.cpp @@ -378,7 +378,8 @@ int BroadPhase2DHashGrid::get_subindex(ID p_id) const { return E->get().subindex; } -void BroadPhase2DHashGrid::_cull(const Point2i p_cell,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices,int &index) { +template +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; @@ -411,10 +412,18 @@ void BroadPhase2DHashGrid::_cull(const Point2i p_cell,CollisionObject2DSW** p_re continue; 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)) + continue; + p_results[index]=E->key()->owner; p_result_indices[index]=E->key()->subindex; index++; + } for(Map::Element *E=pb->static_object_set.front();E;E=E->next()) { @@ -425,6 +434,12 @@ void BroadPhase2DHashGrid::_cull(const Point2i p_cell,CollisionObject2DSW** p_re 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)) + continue; + E->key()->pass=pass; p_results[index]=E->key()->owner; p_result_indices[index]=E->key()->subindex; @@ -468,7 +483,7 @@ int BroadPhase2DHashGrid::cull_segment(const Vector2& p_from, const Vector2& p_t max.y= (Math::floor(pos.y + 1)*cell_size - p_from.y) / dir.y; int cullcount=0; - _cull(pos,p_results,p_max_results,p_result_indices,cullcount); + _cull(pos,Rect2(),p_from,p_to,p_results,p_max_results,p_result_indices,cullcount); bool reached_x=false; bool reached_y=false; @@ -502,7 +517,7 @@ int BroadPhase2DHashGrid::cull_segment(const Vector2& p_from, const Vector2& p_t reached_y=true; } - _cull(pos,p_results,p_max_results,p_result_indices,cullcount); + _cull(pos,Rect2(),p_from,p_to,p_results,p_max_results,p_result_indices,cullcount); if (reached_x && reached_y) break; @@ -515,8 +530,22 @@ int BroadPhase2DHashGrid::cull_segment(const Vector2& p_from, const Vector2& p_t 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; + + for(int i=from.x;i<=to.x;i++) { + + for(int j=from.y;j<=to.y;j++) { - return 0; + _cull(Point2i(i,j),p_aabb,Point2(),Point2(),p_results,p_max_results,p_result_indices,cullcount); + } + + } + + return cullcount; } void BroadPhase2DHashGrid::set_pair_callback(PairCallback p_pair_callback,void *p_userdata) { diff --git a/servers/physics_2d/broad_phase_2d_hash_grid.h b/servers/physics_2d/broad_phase_2d_hash_grid.h index 713d960487..d530b35d5d 100644 --- a/servers/physics_2d/broad_phase_2d_hash_grid.h +++ b/servers/physics_2d/broad_phase_2d_hash_grid.h @@ -94,7 +94,8 @@ class BroadPhase2DHashGrid : public BroadPhase2DSW { void _enter_grid(Element* p_elem, const Rect2& p_rect,bool p_static); void _exit_grid(Element* p_elem, const Rect2& p_rect,bool p_static); - _FORCE_INLINE_ void _cull(const Point2i p_cell,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices,int &index); + template + _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 { diff --git a/servers/physics_2d/collision_object_2d_sw.cpp b/servers/physics_2d/collision_object_2d_sw.cpp index 6e5a703aa2..3a5c8c3ade 100644 --- a/servers/physics_2d/collision_object_2d_sw.cpp +++ b/servers/physics_2d/collision_object_2d_sw.cpp @@ -130,6 +130,7 @@ void CollisionObject2DSW::_update_shapes() { if (!space) return; + for(int i=0;i shapes; @@ -73,7 +75,7 @@ protected: void _update_shapes_with_motion(const Vector2& p_motion); void _unregister_shapes(); - _FORCE_INLINE_ void _set_transform(const Matrix32& p_transform) { transform=p_transform; _update_shapes(); } + _FORCE_INLINE_ void _set_transform(const Matrix32& p_transform, bool p_update_shapes=true) { transform=p_transform; if (p_update_shapes) {_update_shapes();} } _FORCE_INLINE_ void _set_inv_transform(const Matrix32& p_transform) { inv_transform=p_transform; } void _set_static(bool p_static); @@ -101,6 +103,12 @@ public: _FORCE_INLINE_ const Matrix32& get_shape_inv_transform(int p_index) const { return shapes[p_index].xform_inv; } _FORCE_INLINE_ const Rect2& get_shape_aabb(int p_index) const { return shapes[p_index].aabb_cache; } + _FORCE_INLINE_ void set_shape_kinematic_advance(int p_index,const Vector2& p_advance) { shapes[p_index].kinematic_advance=p_advance; } + _FORCE_INLINE_ Vector2 get_shape_kinematic_advance(int p_index) const { return shapes[p_index].kinematic_advance; } + + _FORCE_INLINE_ void set_shape_kinematic_retreat(int p_index,float p_retreat) { shapes[p_index].kinematic_retreat=p_retreat; } + _FORCE_INLINE_ float get_shape_kinematic_retreat(int p_index) const { return shapes[p_index].kinematic_retreat; } + _FORCE_INLINE_ Matrix32 get_transform() const { return transform; } _FORCE_INLINE_ Matrix32 get_inv_transform() const { return inv_transform; } _FORCE_INLINE_ Space2DSW* get_space() const { return space; } @@ -109,6 +117,7 @@ public: _FORCE_INLINE_ bool is_shape_set_as_trigger(int p_idx) const { return shapes[p_idx].trigger; } + void remove_shape(Shape2DSW *p_shape); void remove_shape(int p_index); diff --git a/servers/physics_2d/collision_solver_2d_sat.cpp b/servers/physics_2d/collision_solver_2d_sat.cpp index 4b87ff02a2..fdbbebefcf 100644 --- a/servers/physics_2d/collision_solver_2d_sat.cpp +++ b/servers/physics_2d/collision_solver_2d_sat.cpp @@ -321,19 +321,19 @@ static void _generate_contacts_from_supports(const Vector2 * p_points_A,int p_po -template +template class SeparatorAxisTest2D { const ShapeA *shape_A; const ShapeB *shape_B; const Matrix32 *transform_A; const Matrix32 *transform_B; - const Matrix32 *transform_inv_A; - const Matrix32 *transform_inv_B; real_t best_depth; Vector2 best_axis; int best_axis_count; int best_axis_index; + Vector2 motion_A; + Vector2 motion_B; _CollectorCallback2D *callback; public: @@ -351,6 +351,29 @@ public: return true; } + _FORCE_INLINE_ bool test_cast() { + + if (castA) { + + Vector2 na = motion_A.normalized(); + if (!test_axis(na)) + return false; + if (!test_axis(na.tangent())) + return false; + } + + if (castB) { + + Vector2 nb = motion_B.normalized(); + if (!test_axis(nb)) + return false; + if (!test_axis(nb.tangent())) + return false; + } + + return true; + } + _FORCE_INLINE_ bool test_axis(const Vector2& p_axis) { Vector2 axis=p_axis; @@ -364,8 +387,15 @@ public: real_t min_A,max_A,min_B,max_B; - shape_A->project_range(axis,*transform_A,min_A,max_A); - shape_B->project_range(axis,*transform_B,min_B,max_B); + if (castA) + 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); + + if (castB) + 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); min_B -= ( max_A - min_A ) * 0.5; max_B += ( max_A - min_A ) * 0.5; @@ -427,21 +457,28 @@ public: return; //only collide, no callback static const int max_supports=2; - Vector2 supports_A[max_supports]; int support_count_A; - shape_A->get_supports(transform_A->basis_xform_inv(-best_axis).normalized(),supports_A,support_count_A); - for(int i=0;ixform(supports_A[i]); + if (castA) { + 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;ixform(supports_A[i]); + } } Vector2 supports_B[max_supports]; int support_count_B; - shape_B->get_supports(transform_B->basis_xform_inv(best_axis).normalized(),supports_B,support_count_B); - for(int i=0;ixform(supports_B[i]); + if (castB) { + 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;ixform(supports_B[i]); + } } /* @@ -480,14 +517,15 @@ public: } - _FORCE_INLINE_ SeparatorAxisTest2D(const ShapeA *p_shape_A,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a, const ShapeB *p_shape_B,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) { + _FORCE_INLINE_ SeparatorAxisTest2D(const ShapeA *p_shape_A,const Matrix32& p_transform_a, const ShapeB *p_shape_B,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_A=Vector2(), const Vector2& p_motion_B=Vector2()) { best_depth=1e15; shape_A=p_shape_A; shape_B=p_shape_B; transform_A=&p_transform_a; transform_B=&p_transform_b; - transform_inv_A=&p_transform_inv_a; - transform_inv_B=&p_transform_inv_b; + motion_A=p_motion_A; + motion_B=p_motion_B; + callback=p_collector; #ifdef DEBUG_ENABLED best_axis_count=0; @@ -503,68 +541,92 @@ public: /****** SAT TESTS *******/ -typedef void (*CollisionFunc)(const Shape2DSW*,const Matrix32&,const Matrix32&,const Shape2DSW*,const Matrix32&,const Matrix32&,_CollectorCallback2D *p_collector); +#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 Matrix32&,const Shape2DSW*,const Matrix32&,_CollectorCallback2D *p_collector,const Vector2&,const Vector2&); -static void _collision_segment_segment(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) { + +template +static void _collision_segment_segment(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) { const SegmentShape2DSW *segment_A = static_cast(p_a); const SegmentShape2DSW *segment_B = static_cast(p_b); - SeparatorAxisTest2D separator(segment_A,p_transform_a,p_transform_inv_a,segment_B,p_transform_b,p_transform_inv_b,p_collector); + SeparatorAxisTest2D separator(segment_A,p_transform_a,segment_B,p_transform_b,p_collector,p_motion_a,p_motion_b); if (!separator.test_previous_axis()) return; + //this collision is kind of pointless + - if (!separator.test_axis(p_transform_inv_a.basis_xform_inv(segment_A->get_normal()).normalized())) + if (!separator.test_previous_axis()) return; - if (!separator.test_axis(p_transform_inv_a.basis_xform_inv(segment_B->get_normal()).normalized())) + + 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))) return; separator.generate_contacts(); } -static void _collision_segment_circle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) { +template +static void _collision_segment_circle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) { const SegmentShape2DSW *segment_A = static_cast(p_a); const CircleShape2DSW *circle_B = static_cast(p_b); - SeparatorAxisTest2D separator(segment_A,p_transform_a,p_transform_inv_a,circle_B,p_transform_b,p_transform_inv_b,p_collector); + SeparatorAxisTest2D separator(segment_A,p_transform_a,circle_B,p_transform_b,p_collector,p_motion_a,p_motion_b); if (!separator.test_previous_axis()) return; + if (!separator.test_cast()) + return; + + //segment normal if (!separator.test_axis( (p_transform_a.xform(segment_A->get_b())-p_transform_a.xform(segment_A->get_a())).normalized().tangent() )) return; -// if (!separator.test_axis(p_transform_inv_a.basis_xform_inv(segment_A->get_normal()).normalized())) -// return; - if (!separator.test_axis((p_transform_a.xform(segment_A->get_a())-p_transform_b.get_origin()).normalized())) + //endpoint a vs circle + if (TEST_POINT(p_transform_a.xform(segment_A->get_a()),p_transform_b.get_origin())) return; - if (!separator.test_axis((p_transform_a.xform(segment_A->get_b())-p_transform_b.get_origin()).normalized())) + //endpoint b vs circle + if (TEST_POINT(p_transform_a.xform(segment_A->get_b()),p_transform_b.get_origin())) return; separator.generate_contacts(); - - } -static void _collision_segment_rectangle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) { +template +static void _collision_segment_rectangle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) { const SegmentShape2DSW *segment_A = static_cast(p_a); const RectangleShape2DSW *rectangle_B = static_cast(p_b); - SeparatorAxisTest2D separator(segment_A,p_transform_a,p_transform_inv_a,rectangle_B,p_transform_b,p_transform_inv_b,p_collector); + SeparatorAxisTest2D separator(segment_A,p_transform_a,rectangle_B,p_transform_b,p_collector,p_motion_a,p_motion_b); if (!separator.test_previous_axis()) return; - if (!separator.test_axis(p_transform_inv_a.basis_xform_inv(segment_A->get_normal()).normalized())) + if (!separator.test_cast()) + return; + + if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a))) return; if (!separator.test_axis(p_transform_b.elements[0].normalized())) @@ -577,50 +639,58 @@ static void _collision_segment_rectangle(const Shape2DSW* p_a,const Matrix32& p_ } -static void _collision_segment_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) { +template +static void _collision_segment_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) { const SegmentShape2DSW *segment_A = static_cast(p_a); const CapsuleShape2DSW *capsule_B = static_cast(p_b); - SeparatorAxisTest2D separator(segment_A,p_transform_a,p_transform_inv_a,capsule_B,p_transform_b,p_transform_inv_b,p_collector); + SeparatorAxisTest2D separator(segment_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_motion_a,p_motion_b); if (!separator.test_previous_axis()) return; - if (!separator.test_axis(p_transform_inv_a.basis_xform_inv(segment_A->get_normal()).normalized())) + if (!separator.test_cast()) + return; + + if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a))) return; if (!separator.test_axis(p_transform_b.elements[0].normalized())) return; - if (!separator.test_axis((p_transform_a.xform(segment_A->get_a())-(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*0.5)).normalized())) + 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 (!separator.test_axis((p_transform_a.xform(segment_A->get_a())-(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*-0.5)).normalized())) + 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 (!separator.test_axis((p_transform_a.xform(segment_A->get_b())-(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*0.5)).normalized())) + 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 (!separator.test_axis((p_transform_a.xform(segment_A->get_b())-(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*-0.5)).normalized())) + 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(); } -static void _collision_segment_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) { +template +static void _collision_segment_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) { const SegmentShape2DSW *segment_A = static_cast(p_a); const ConvexPolygonShape2DSW *convex_B = static_cast(p_b); - SeparatorAxisTest2D separator(segment_A,p_transform_a,p_transform_inv_a,convex_B,p_transform_b,p_transform_inv_b,p_collector); + SeparatorAxisTest2D separator(segment_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b); if (!separator.test_previous_axis()) return; - if (!separator.test_axis(p_transform_inv_a.basis_xform_inv(segment_A->get_normal()).normalized())) + if (!separator.test_cast()) + return; + + if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a))) return; for(int i=0;iget_point_count();i++) { - if (!separator.test_axis( p_transform_inv_b.basis_xform_inv(convex_B->get_segment_normal(i)).normalized() )) + if (!separator.test_axis( convex_B->get_xformed_segment_normal(p_transform_b,i))) return; } @@ -631,35 +701,44 @@ static void _collision_segment_convex_polygon(const Shape2DSW* p_a,const Matrix3 ///////// -static void _collision_circle_circle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) { +template +static void _collision_circle_circle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) { const CircleShape2DSW *circle_A = static_cast(p_a); const CircleShape2DSW *circle_B = static_cast(p_b); - SeparatorAxisTest2D separator(circle_A,p_transform_a,p_transform_inv_a,circle_B,p_transform_b,p_transform_inv_b,p_collector); + SeparatorAxisTest2D separator(circle_A,p_transform_a,circle_B,p_transform_b,p_collector,p_motion_a,p_motion_b); if (!separator.test_previous_axis()) return; - if (!separator.test_axis((p_transform_a.get_origin()-p_transform_b.get_origin()).normalized())) + if (!separator.test_cast()) return; + if (TEST_POINT(p_transform_a.get_origin(),p_transform_b.get_origin())) + return; + + separator.generate_contacts(); } -static void _collision_circle_rectangle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) { +template +static void _collision_circle_rectangle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) { const CircleShape2DSW *circle_A = static_cast(p_a); const RectangleShape2DSW *rectangle_B = static_cast(p_b); - SeparatorAxisTest2D separator(circle_A,p_transform_a,p_transform_inv_a,rectangle_B,p_transform_b,p_transform_inv_b,p_collector); + SeparatorAxisTest2D separator(circle_A,p_transform_a,rectangle_B,p_transform_b,p_collector,p_motion_a,p_motion_b); if (!separator.test_previous_axis()) return; + if (!separator.test_cast()) + return; + const Vector2 &sphere=p_transform_a.elements[2]; const Vector2 *axis=&p_transform_b.elements[0]; const Vector2& half_extents = rectangle_B->get_half_extents(); @@ -670,65 +749,120 @@ static void _collision_circle_rectangle(const Shape2DSW* p_a,const Matrix32& p_t if (!separator.test_axis(axis[1].normalized())) return; - Vector2 local_v = p_transform_inv_b.xform(p_transform_a.get_origin()); + { + Vector2 local_v = p_transform_b.affine_inverse().xform(p_transform_a.get_origin()); - Vector2 he( - (local_v.x<0) ? -half_extents.x : half_extents.x, - (local_v.y<0) ? -half_extents.y : half_extents.y - ); + Vector2 he( + (local_v.x<0) ? -half_extents.x : half_extents.x, + (local_v.y<0) ? -half_extents.y : half_extents.y + ); - if (!separator.test_axis((p_transform_b.xform(he)-sphere).normalized())) - return; + if (!separator.test_axis((p_transform_b.xform(he)-sphere).normalized())) + return; + } + + if (castA) { + + Vector2 sphereofs = sphere + p_motion_a; + Vector2 local_v = p_transform_b.affine_inverse().xform(sphereofs); + + Vector2 he( + (local_v.x<0) ? -half_extents.x : half_extents.x, + (local_v.y<0) ? -half_extents.y : half_extents.y + ); + + + if (!separator.test_axis((p_transform_b.xform(he)-sphereofs).normalized())) + return; + } + + if (castB) { + + Vector2 sphereofs = sphere - p_motion_b; + Vector2 local_v = p_transform_b.affine_inverse().xform(sphereofs); + + Vector2 he( + (local_v.x<0) ? -half_extents.x : half_extents.x, + (local_v.y<0) ? -half_extents.y : half_extents.y + ); + + + if (!separator.test_axis((p_transform_b.xform(he)-sphereofs).normalized())) + return; + } + + if (castA && castB) { + + Vector2 sphereofs = sphere - p_motion_b + p_motion_a; + Vector2 local_v = p_transform_b.affine_inverse().xform(sphereofs); + + Vector2 he( + (local_v.x<0) ? -half_extents.x : half_extents.x, + (local_v.y<0) ? -half_extents.y : half_extents.y + ); + + + if (!separator.test_axis((p_transform_b.xform(he)-sphereofs).normalized())) + return; + } separator.generate_contacts(); } -static void _collision_circle_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) { +template +static void _collision_circle_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) { const CircleShape2DSW *circle_A = static_cast(p_a); const CapsuleShape2DSW *capsule_B = static_cast(p_b); - SeparatorAxisTest2D separator(circle_A,p_transform_a,p_transform_inv_a,capsule_B,p_transform_b,p_transform_inv_b,p_collector); + SeparatorAxisTest2D separator(circle_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_motion_a,p_motion_b); if (!separator.test_previous_axis()) return; + if (!separator.test_cast()) + return; + //capsule axis if (!separator.test_axis(p_transform_b.elements[0].normalized())) return; //capsule endpoints - if (!separator.test_axis((p_transform_a.get_origin()-(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*0.5)).normalized())) + 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 (!separator.test_axis((p_transform_a.get_origin()-(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*-0.5)).normalized())) + 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(); } -static void _collision_circle_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) { +template +static void _collision_circle_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) { const CircleShape2DSW *circle_A = static_cast(p_a); const ConvexPolygonShape2DSW *convex_B = static_cast(p_b); - SeparatorAxisTest2D separator(circle_A,p_transform_a,p_transform_inv_a,convex_B,p_transform_b,p_transform_inv_b,p_collector); + SeparatorAxisTest2D separator(circle_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b); if (!separator.test_previous_axis()) return; + if (!separator.test_cast()) + return; + + //poly faces and poly points vs circle for(int i=0;iget_point_count();i++) { - if (!separator.test_axis( (p_transform_b.xform(convex_B->get_point(i))-p_transform_a.get_origin()).normalized() )) + if (TEST_POINT( p_transform_a.get_origin(),p_transform_b.xform(convex_B->get_point(i)) )) return; - if (!separator.test_axis( p_transform_inv_b.basis_xform_inv(convex_B->get_segment_normal(i)).normalized() )) + if (!separator.test_axis( convex_B->get_xformed_segment_normal(p_transform_b,i))) return; } @@ -738,17 +872,21 @@ static void _collision_circle_convex_polygon(const Shape2DSW* p_a,const Matrix32 ///////// -static void _collision_rectangle_rectangle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) { +template +static void _collision_rectangle_rectangle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) { const RectangleShape2DSW *rectangle_A = static_cast(p_a); const RectangleShape2DSW *rectangle_B = static_cast(p_b); - SeparatorAxisTest2D separator(rectangle_A,p_transform_a,p_transform_inv_a,rectangle_B,p_transform_b,p_transform_inv_b,p_collector); + SeparatorAxisTest2D separator(rectangle_A,p_transform_a,rectangle_B,p_transform_b,p_collector,p_motion_a,p_motion_b); if (!separator.test_previous_axis()) return; + if (!separator.test_cast()) + return; + //box faces A if (!separator.test_axis(p_transform_a.elements[0].normalized())) return; @@ -766,17 +904,21 @@ static void _collision_rectangle_rectangle(const Shape2DSW* p_a,const Matrix32& separator.generate_contacts(); } -static void _collision_rectangle_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) { +template +static void _collision_rectangle_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) { const RectangleShape2DSW *rectangle_A = static_cast(p_a); const CapsuleShape2DSW *capsule_B = static_cast(p_b); - SeparatorAxisTest2D separator(rectangle_A,p_transform_a,p_transform_inv_a,capsule_B,p_transform_b,p_transform_inv_b,p_collector); + SeparatorAxisTest2D separator(rectangle_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_motion_a,p_motion_b); if (!separator.test_previous_axis()) return; + if (!separator.test_cast()) + return; + //box faces if (!separator.test_axis(p_transform_a.elements[0].normalized())) return; @@ -791,21 +933,77 @@ static void _collision_rectangle_capsule(const Shape2DSW* p_a,const Matrix32& p_ //box endpoints to capsule circles + Matrix32 boxinv = p_transform_a.affine_inverse(); + 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); - const Vector2& half_extents = rectangle_A->get_half_extents(); - Vector2 local_v = p_transform_inv_a.xform(capsule_endpoint); + const Vector2& half_extents = rectangle_A->get_half_extents(); + Vector2 local_v = boxinv.xform(capsule_endpoint); - Vector2 he( - (local_v.x<0) ? -half_extents.x : half_extents.x, - (local_v.y<0) ? -half_extents.y : half_extents.y - ); + Vector2 he( + (local_v.x<0) ? -half_extents.x : half_extents.x, + (local_v.y<0) ? -half_extents.y : half_extents.y + ); + if (!separator.test_axis(p_transform_a.xform(he-capsule_endpoint).normalized())) + 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; + + + const Vector2& half_extents = rectangle_A->get_half_extents(); + Vector2 local_v = boxinv.xform(capsule_endpoint); + + Vector2 he( + (local_v.x<0) ? -half_extents.x : half_extents.x, + (local_v.y<0) ? -half_extents.y : half_extents.y + ); + + if (!separator.test_axis(p_transform_a.xform(he-capsule_endpoint).normalized())) + 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; - if (!separator.test_axis(p_transform_a.xform(he).normalized())) - return; + + const Vector2& half_extents = rectangle_A->get_half_extents(); + Vector2 local_v = boxinv.xform(capsule_endpoint); + + Vector2 he( + (local_v.x<0) ? -half_extents.x : half_extents.x, + (local_v.y<0) ? -half_extents.y : half_extents.y + ); + + if (!separator.test_axis(p_transform_a.xform(he-capsule_endpoint).normalized())) + 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; + + + const Vector2& half_extents = rectangle_A->get_half_extents(); + Vector2 local_v = boxinv.xform(capsule_endpoint); + + Vector2 he( + (local_v.x<0) ? -half_extents.x : half_extents.x, + (local_v.y<0) ? -half_extents.y : half_extents.y + ); + + if (!separator.test_axis(p_transform_a.xform(he-capsule_endpoint).normalized())) + return; + } } @@ -813,16 +1011,20 @@ static void _collision_rectangle_capsule(const Shape2DSW* p_a,const Matrix32& p_ separator.generate_contacts(); } -static void _collision_rectangle_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) { +template +static void _collision_rectangle_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) { const RectangleShape2DSW *rectangle_A = static_cast(p_a); const ConvexPolygonShape2DSW *convex_B = static_cast(p_b); - SeparatorAxisTest2D separator(rectangle_A,p_transform_a,p_transform_inv_a,convex_B,p_transform_b,p_transform_inv_b,p_collector); + SeparatorAxisTest2D separator(rectangle_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b); if (!separator.test_previous_axis()) return; + if (!separator.test_cast()) + return; + //box faces if (!separator.test_axis(p_transform_a.elements[0].normalized())) return; @@ -833,7 +1035,7 @@ static void _collision_rectangle_convex_polygon(const Shape2DSW* p_a,const Matri //convex faces for(int i=0;iget_point_count();i++) { - if (!separator.test_axis( p_transform_inv_b.basis_xform_inv(convex_B->get_segment_normal(i)).normalized() )) + if (!separator.test_axis( convex_B->get_xformed_segment_normal(p_transform_b,i))) return; } @@ -844,17 +1046,21 @@ static void _collision_rectangle_convex_polygon(const Shape2DSW* p_a,const Matri ///////// -static void _collision_capsule_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) { +template +static void _collision_capsule_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) { const CapsuleShape2DSW *capsule_A = static_cast(p_a); const CapsuleShape2DSW *capsule_B = static_cast(p_b); - SeparatorAxisTest2D separator(capsule_A,p_transform_a,p_transform_inv_a,capsule_B,p_transform_b,p_transform_inv_b,p_collector); + SeparatorAxisTest2D separator(capsule_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_motion_a,p_motion_b); if (!separator.test_previous_axis()) return; + if (!separator.test_cast()) + return; + //capsule axis if (!separator.test_axis(p_transform_b.elements[0].normalized())) @@ -873,7 +1079,7 @@ static void _collision_capsule_capsule(const Shape2DSW* p_a,const Matrix32& p_tr Vector2 capsule_endpoint_B = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(j==0?0.5:-0.5); - if (!separator.test_axis( (capsule_endpoint_A-capsule_endpoint_B).normalized() )) + if (TEST_POINT(capsule_endpoint_A,capsule_endpoint_B) ) return; } @@ -883,17 +1089,21 @@ static void _collision_capsule_capsule(const Shape2DSW* p_a,const Matrix32& p_tr } -static void _collision_capsule_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) { +template +static void _collision_capsule_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) { const CapsuleShape2DSW *capsule_A = static_cast(p_a); const ConvexPolygonShape2DSW *convex_B = static_cast(p_b); - SeparatorAxisTest2D separator(capsule_A,p_transform_a,p_transform_inv_a,convex_B,p_transform_b,p_transform_inv_b,p_collector); + SeparatorAxisTest2D separator(capsule_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b); if (!separator.test_previous_axis()) return; + if (!separator.test_cast()) + return; + //capsule axis if (!separator.test_axis(p_transform_a.elements[0].normalized())) @@ -909,12 +1119,12 @@ static void _collision_capsule_convex_polygon(const Shape2DSW* p_a,const Matrix3 Vector2 capsule_endpoint_A = p_transform_a.get_origin()+p_transform_a.elements[1]*capsule_A->get_height()*(j==0?0.5:-0.5); - if (!separator.test_axis( (cpoint - capsule_endpoint_A).normalized() )) + if (TEST_POINT(capsule_endpoint_A,cpoint )) return; } - if (!separator.test_axis( p_transform_inv_b.basis_xform_inv(convex_B->get_segment_normal(i)).normalized() )) + if (!separator.test_axis( convex_B->get_xformed_segment_normal(p_transform_b,i))) return; } @@ -925,26 +1135,31 @@ static void _collision_capsule_convex_polygon(const Shape2DSW* p_a,const Matrix3 ///////// -static void _collision_convex_polygon_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) { +template +static void _collision_convex_polygon_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) { const ConvexPolygonShape2DSW *convex_A = static_cast(p_a); const ConvexPolygonShape2DSW *convex_B = static_cast(p_b); - SeparatorAxisTest2D separator(convex_A,p_transform_a,p_transform_inv_a,convex_B,p_transform_b,p_transform_inv_b,p_collector); + SeparatorAxisTest2D separator(convex_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b); if (!separator.test_previous_axis()) return; + if (!separator.test_cast()) + return; + + for(int i=0;iget_point_count();i++) { - if (!separator.test_axis( p_transform_inv_a.basis_xform_inv(convex_A->get_segment_normal(i)).normalized() )) + if (!separator.test_axis( convex_A->get_xformed_segment_normal(p_transform_a,i))) return; } for(int i=0;iget_point_count();i++) { - if (!separator.test_axis( p_transform_inv_b.basis_xform_inv(convex_B->get_segment_normal(i)).normalized() )) + if (!separator.test_axis( convex_B->get_xformed_segment_normal(p_transform_b,i))) return; } @@ -955,7 +1170,7 @@ static void _collision_convex_polygon_convex_polygon(const Shape2DSW* p_a,const //////// -bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Matrix32& p_transform_A, const Matrix32& p_transform_inv_A, const Shape2DSW *p_shape_B, const Matrix32& p_transform_B, const Matrix32& p_transform_inv_B, CollisionSolver2DSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap,Vector2 *sep_axis) { +bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Matrix32& p_transform_A, const Vector2& p_motion_A, const Shape2DSW *p_shape_B, const Matrix32& p_transform_B,const Vector2& p_motion_B, CollisionSolver2DSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap,Vector2 *sep_axis) { Physics2DServer::ShapeType type_A=p_shape_A->get_type(); @@ -971,31 +1186,118 @@ bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Matrix32& p_ static const CollisionFunc collision_table[5][5]={ - {_collision_segment_segment, - _collision_segment_circle, - _collision_segment_rectangle, - _collision_segment_capsule, - _collision_segment_convex_polygon}, + {_collision_segment_segment, + _collision_segment_circle, + _collision_segment_rectangle, + _collision_segment_capsule, + _collision_segment_convex_polygon}, {0, - _collision_circle_circle, - _collision_circle_rectangle, - _collision_circle_capsule, - _collision_circle_convex_polygon}, + _collision_circle_circle, + _collision_circle_rectangle, + _collision_circle_capsule, + _collision_circle_convex_polygon}, {0, 0, - _collision_rectangle_rectangle, - _collision_rectangle_capsule, - _collision_rectangle_convex_polygon}, + _collision_rectangle_rectangle, + _collision_rectangle_capsule, + _collision_rectangle_convex_polygon}, {0, 0, 0, - _collision_capsule_capsule, - _collision_capsule_convex_polygon}, + _collision_capsule_capsule, + _collision_capsule_convex_polygon}, {0, 0, 0, 0, - _collision_convex_polygon_convex_polygon} + _collision_convex_polygon_convex_polygon} + + }; + + static const CollisionFunc collision_table_castA[5][5]={ + {_collision_segment_segment, + _collision_segment_circle, + _collision_segment_rectangle, + _collision_segment_capsule, + _collision_segment_convex_polygon}, + {0, + _collision_circle_circle, + _collision_circle_rectangle, + _collision_circle_capsule, + _collision_circle_convex_polygon}, + {0, + 0, + _collision_rectangle_rectangle, + _collision_rectangle_capsule, + _collision_rectangle_convex_polygon}, + {0, + 0, + 0, + _collision_capsule_capsule, + _collision_capsule_convex_polygon}, + {0, + 0, + 0, + 0, + _collision_convex_polygon_convex_polygon} + + }; + + static const CollisionFunc collision_table_castB[5][5]={ + {_collision_segment_segment, + _collision_segment_circle, + _collision_segment_rectangle, + _collision_segment_capsule, + _collision_segment_convex_polygon}, + {0, + _collision_circle_circle, + _collision_circle_rectangle, + _collision_circle_capsule, + _collision_circle_convex_polygon}, + {0, + 0, + _collision_rectangle_rectangle, + _collision_rectangle_capsule, + _collision_rectangle_convex_polygon}, + {0, + 0, + 0, + _collision_capsule_capsule, + _collision_capsule_convex_polygon}, + {0, + 0, + 0, + 0, + _collision_convex_polygon_convex_polygon} + + }; + + static const CollisionFunc collision_table_castA_castB[5][5]={ + {_collision_segment_segment, + _collision_segment_circle, + _collision_segment_rectangle, + _collision_segment_capsule, + _collision_segment_convex_polygon}, + {0, + _collision_circle_circle, + _collision_circle_rectangle, + _collision_circle_capsule, + _collision_circle_convex_polygon}, + {0, + 0, + _collision_rectangle_rectangle, + _collision_rectangle_capsule, + _collision_rectangle_convex_polygon}, + {0, + 0, + 0, + _collision_capsule_capsule, + _collision_capsule_convex_polygon}, + {0, + 0, + 0, + 0, + _collision_convex_polygon_convex_polygon} }; @@ -1010,23 +1312,34 @@ bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Matrix32& p_ const Shape2DSW *B=p_shape_B; const Matrix32 *transform_A=&p_transform_A; const Matrix32 *transform_B=&p_transform_B; - const Matrix32 *transform_inv_A=&p_transform_inv_A; - const Matrix32 *transform_inv_B=&p_transform_inv_B; + const Vector2 *motion_A=&p_motion_A; + const Vector2 *motion_B=&p_motion_B; if (type_A > type_B) { SWAP(A,B); SWAP(transform_A,transform_B); - SWAP(transform_inv_A,transform_inv_B); SWAP(type_A,type_B); + SWAP(motion_A,motion_B); callback.swap = !callback.swap; } - CollisionFunc collision_func = collision_table[type_A-2][type_B-2]; - ERR_FAIL_COND_V(!collision_func,false); + CollisionFunc collision_func; + 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(A,*transform_A,*transform_inv_A,B,*transform_B,*transform_inv_B,&callback); + ERR_FAIL_COND_V(!collision_func,false); + + collision_func(A,*transform_A,B,*transform_B,&callback,*motion_A,*motion_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 dc6767d651..95468a18b8 100644 --- a/servers/physics_2d/collision_solver_2d_sat.h +++ b/servers/physics_2d/collision_solver_2d_sat.h @@ -32,6 +32,6 @@ #include "collision_solver_2d_sw.h" -bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Matrix32& p_transform_A, const Matrix32& p_transform_inv_A, const Shape2DSW *p_shape_B, const Matrix32& p_transform_B, const Matrix32& p_transform_inv_B, CollisionSolver2DSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap=false,Vector2 *sep_axis=NULL); +bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Matrix32& p_transform_A, const Vector2& p_motion_A,const Shape2DSW *p_shape_B, const Matrix32& p_transform_B,const Vector2& p_motion_B, CollisionSolver2DSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap=false,Vector2 *sep_axis=NULL); #endif // COLLISION_SOLVER_2D_SAT_H diff --git a/servers/physics_2d/collision_solver_2d_sw.cpp b/servers/physics_2d/collision_solver_2d_sw.cpp index cee5582c6f..5d43510aea 100644 --- a/servers/physics_2d/collision_solver_2d_sw.cpp +++ b/servers/physics_2d/collision_solver_2d_sw.cpp @@ -34,7 +34,7 @@ //#define collision_solver gjk_epa_calculate_penetration -bool CollisionSolver2DSW::solve_static_line(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Matrix32& p_transform_inv_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Matrix32& p_transform_inv_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result) { +bool CollisionSolver2DSW::solve_static_line(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result) { const LineShape2DSW *line = static_cast(p_shape_A); @@ -49,7 +49,7 @@ bool CollisionSolver2DSW::solve_static_line(const Shape2DSW *p_shape_A,const Mat Vector2 supports[2]; int support_count; - p_shape_B->get_supports(p_transform_inv_B.basis_xform(-n).normalized(),supports,support_count); + p_shape_B->get_supports(p_transform_A.affine_inverse().basis_xform(-n).normalized(),supports,support_count); bool found=false; @@ -77,7 +77,7 @@ bool CollisionSolver2DSW::solve_static_line(const Shape2DSW *p_shape_A,const Mat return found; } -bool CollisionSolver2DSW::solve_raycast(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Matrix32& p_transform_inv_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Matrix32& p_transform_inv_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis) { +bool CollisionSolver2DSW::solve_raycast(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis) { @@ -89,8 +89,9 @@ bool CollisionSolver2DSW::solve_raycast(const Shape2DSW *p_shape_A,const Matrix3 Vector2 to = from+p_transform_A[1]*ray->get_length(); Vector2 support_A=to; - from = p_transform_inv_B.xform(from); - to = p_transform_inv_B.xform(to); + Matrix32 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)) { @@ -145,10 +146,10 @@ bool CollisionSolver2DSW::solve_ray(const Shape2DSW *p_shape_A,const Matrix32& p struct _ConcaveCollisionInfo2D { const Matrix32 *transform_A; - const Matrix32 *transform_inv_A; const Shape2DSW *shape_A; const Matrix32 *transform_B; - const Matrix32 *transform_inv_B; + Vector2 motion_A; + Vector2 motion_B; CollisionSolver2DSW::CallbackResult result_callback; void *userdata; bool swap_result; @@ -168,7 +169,7 @@ void CollisionSolver2DSW::concave_callback(void *p_userdata, Shape2DSW *p_convex if (!cinfo.result_callback && cinfo.collided) return; //already collided and no contacts requested, don't test anymore - bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, *cinfo.transform_inv_A, p_convex,*cinfo.transform_B,*cinfo.transform_inv_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result,cinfo.sep_axis ); + 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 ); if (!collided) return; @@ -178,17 +179,16 @@ void CollisionSolver2DSW::concave_callback(void *p_userdata, Shape2DSW *p_convex } -bool CollisionSolver2DSW::solve_concave(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Matrix32& p_transform_inv_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Matrix32& p_transform_inv_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis) { +bool CollisionSolver2DSW::solve_concave(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Vector2& p_motion_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Vector2& p_motion_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis) { const ConcaveShape2DSW *concave_B=static_cast(p_shape_B); _ConcaveCollisionInfo2D cinfo; cinfo.transform_A=&p_transform_A; - cinfo.transform_inv_A=&p_transform_inv_A; cinfo.shape_A=p_shape_A; cinfo.transform_B=&p_transform_B; - cinfo.transform_inv_B=&p_transform_inv_B; + cinfo.motion_A=p_motion_A; cinfo.result_callback=p_result_callback; cinfo.userdata=p_userdata; cinfo.swap_result=p_swap_result; @@ -227,7 +227,8 @@ bool CollisionSolver2DSW::solve_concave(const Shape2DSW *p_shape_A,const Matrix3 } -bool CollisionSolver2DSW::solve_static(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Matrix32& p_transform_inv_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Matrix32& p_transform_inv_B,CallbackResult p_result_callback,void *p_userdata,Vector2 *sep_axis) { +bool CollisionSolver2DSW::solve(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Vector2& p_motion_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Vector2& p_motion_B,CallbackResult p_result_callback,void *p_userdata,Vector2 *sep_axis) { + @@ -253,9 +254,9 @@ bool CollisionSolver2DSW::solve_static(const Shape2DSW *p_shape_A,const Matrix32 } if (swap) { - return solve_static_line(p_shape_B,p_transform_B,p_transform_inv_B,p_shape_A,p_transform_A,p_transform_inv_A,p_result_callback,p_userdata,true); + 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_transform_inv_A,p_shape_B,p_transform_B,p_transform_inv_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) { @@ -278,9 +279,9 @@ bool CollisionSolver2DSW::solve_static(const Shape2DSW *p_shape_A,const Matrix32 if (swap) { - return solve_raycast(p_shape_B,p_transform_B,p_transform_inv_B,p_shape_A,p_transform_A,p_transform_inv_A,p_result_callback,p_userdata,true,sep_axis); + 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_transform_inv_A,p_shape_B,p_transform_B,p_transform_inv_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); } @@ -291,16 +292,16 @@ bool CollisionSolver2DSW::solve_static(const Shape2DSW *p_shape_A,const Matrix32 return false; if (!swap) - return solve_concave(p_shape_A,p_transform_A,p_transform_inv_A,p_shape_B,p_transform_B,p_transform_inv_B,p_result_callback,p_userdata,false,sep_axis); + 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); else - return solve_concave(p_shape_B,p_transform_B,p_transform_inv_B,p_shape_A,p_transform_A,p_transform_inv_A,p_result_callback,p_userdata,true,sep_axis); + 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); } else { - return collision_solver(p_shape_A, p_transform_A, p_transform_inv_A, p_shape_B, p_transform_B, p_transform_inv_B, p_result_callback,p_userdata,false,sep_axis); + return 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); } diff --git a/servers/physics_2d/collision_solver_2d_sw.h b/servers/physics_2d/collision_solver_2d_sw.h index fc5500bfb9..11b5701f46 100644 --- a/servers/physics_2d/collision_solver_2d_sw.h +++ b/servers/physics_2d/collision_solver_2d_sw.h @@ -35,16 +35,16 @@ class CollisionSolver2DSW { public: typedef void (*CallbackResult)(const Vector2& p_point_A,const Vector2& p_point_B,void *p_userdata); private: - static bool solve_static_line(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Matrix32& p_transform_inv_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Matrix32& p_transform_inv_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result); + static bool solve_static_line(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Shape2DSW *p_shape_B,const Matrix32& 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 Matrix32& p_transform_A,const Matrix32& p_transform_inv_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Matrix32& p_transform_inv_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis=NULL); - static bool solve_raycast(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Matrix32& p_transform_inv_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Matrix32& p_transform_inv_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis=NULL); + static bool solve_concave(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Vector2& p_motion_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Vector2& p_motion_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis=NULL); + static bool solve_raycast(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis=NULL); public: - static bool solve_static(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Matrix32& p_inverse_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Matrix32& p_inverse_B,CallbackResult p_result_callback,void *p_userdata,Vector2 *sep_axis=NULL); + static bool solve(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Vector2& p_motion_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Vector2& p_motion_B,CallbackResult p_result_callback,void *p_userdata,Vector2 *sep_axis=NULL); }; diff --git a/servers/physics_2d/physics_2d_server_sw.cpp b/servers/physics_2d/physics_2d_server_sw.cpp index 70e3d843b4..0fbd461f46 100644 --- a/servers/physics_2d/physics_2d_server_sw.cpp +++ b/servers/physics_2d/physics_2d_server_sw.cpp @@ -29,6 +29,7 @@ #include "physics_2d_server_sw.h" #include "broad_phase_2d_basic.h" #include "broad_phase_2d_hash_grid.h" +#include "collision_solver_2d_sw.h" RID Physics2DServerSW::shape_create(ShapeType p_shape) { @@ -81,6 +82,9 @@ RID Physics2DServerSW::shape_create(ShapeType p_shape) { return id; }; + + + void Physics2DServerSW::shape_set_data(RID p_shape, const Variant& p_data) { Shape2DSW *shape = shape_owner.get(p_shape); @@ -126,6 +130,63 @@ real_t Physics2DServerSW::shape_get_custom_solver_bias(RID p_shape) const { } +void Physics2DServerSW::_shape_col_cbk(const Vector2& p_point_A,const Vector2& p_point_B,void *p_userdata) { + + CollCbkData *cbk=(CollCbkData *)p_userdata; + + if (cbk->amount == cbk->max) { + //find least deep + float min_depth=1e20; + int min_depth_idx=0; + for(int i=0;iamount;i++) { + + float d = cbk->ptr[i*2+0].distance_squared_to(cbk->ptr[i*2+1]); + if (dptr[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; + } +} + +bool Physics2DServerSW::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) { + + + Shape2DSW *shape_A = shape_owner.get(p_shape_A); + ERR_FAIL_COND_V(!shape_A,false); + Shape2DSW *shape_B = shape_owner.get(p_shape_B); + ERR_FAIL_COND_V(!shape_B,false); + + 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); + } + + CollCbkData cbk; + 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; + return res; + +} + + RID Physics2DServerSW::space_create() { Space2DSW *space = memnew( Space2DSW ); @@ -442,7 +503,7 @@ void Physics2DServerSW::body_set_mode(RID p_body, BodyMode p_mode) { body->set_mode(p_mode); }; -Physics2DServer::BodyMode Physics2DServerSW::body_get_mode(RID p_body, BodyMode p_mode) const { +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); @@ -550,23 +611,25 @@ bool Physics2DServerSW::body_is_shape_set_as_trigger(RID p_body, int p_shape_idx } -void Physics2DServerSW::body_set_enable_continuous_collision_detection(RID p_body,bool p_enable) { +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(p_enable); + body->set_continuous_collision_detection_mode(p_mode); } -bool Physics2DServerSW::body_is_continuous_collision_detection_enabled(RID p_body) const { +Physics2DServerSW::CCDMode Physics2DServerSW::body_get_continuous_collision_detection_mode(RID p_body) const{ - Body2DSW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body,false); + const Body2DSW *body = body_owner.get(p_body); + ERR_FAIL_COND_V(!body,CCD_MODE_DISABLED); + + return body->get_continuous_collision_detection_mode(); - return body->is_continuous_collision_detection_enabled(); } + + void Physics2DServerSW::body_attach_object_instance_ID(RID p_body,uint32_t p_ID) { Body2DSW *body = body_owner.get(p_body); @@ -617,13 +680,6 @@ float Physics2DServerSW::body_get_param(RID p_body, BodyParameter p_param) const }; -void Physics2DServerSW::body_static_simulate_motion(RID p_body,const Matrix32& p_new_transform) { - - Body2DSW *body = body_owner.get(p_body); - ERR_FAIL_COND(!body); - body->simulate_motion(p_new_transform,last_step); - -}; void Physics2DServerSW::body_set_state(RID p_body, BodyState p_state, const Variant& p_variant) { @@ -775,6 +831,16 @@ void Physics2DServerSW::body_set_force_integration_callback(RID p_body,Object *p } +bool Physics2DServerSW::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) { + + 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); + +} + /* JOINT API */ diff --git a/servers/physics_2d/physics_2d_server_sw.h b/servers/physics_2d/physics_2d_server_sw.h index f6665da73f..ef00eae7e4 100644 --- a/servers/physics_2d/physics_2d_server_sw.h +++ b/servers/physics_2d/physics_2d_server_sw.h @@ -58,6 +58,16 @@ friend class Physics2DDirectSpaceStateSW; mutable RID_Owner body_owner; mutable RID_Owner joint_owner; + struct CollCbkData { + + int max; + int amount; + Vector2 *ptr; + }; + + static void _shape_col_cbk(const Vector2& p_point_A,const Vector2& p_point_B,void *p_userdata); + + // void _clear_query(Query2DSW *p_query); public: @@ -69,6 +79,8 @@ public: 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 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); + /* SPACE API */ virtual RID space_create(); @@ -124,7 +136,7 @@ public: virtual RID body_get_space(RID p_body) const; virtual void body_set_mode(RID p_body, BodyMode p_mode); - virtual BodyMode body_get_mode(RID p_body, BodyMode p_mode) const; + virtual BodyMode body_get_mode(RID p_body) const; virtual void body_add_shape(RID p_body, RID p_shape, const Matrix32& p_transform=Matrix32()); virtual void body_set_shape(RID p_body, int p_shape_idx,RID p_shape); @@ -143,8 +155,8 @@ public: virtual void body_attach_object_instance_ID(RID p_body,uint32_t p_ID); virtual uint32_t body_get_object_instance_ID(RID p_body) const; - virtual void body_set_enable_continuous_collision_detection(RID p_body,bool p_enable); - virtual bool body_is_continuous_collision_detection_enabled(RID p_body) const; + virtual void body_set_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_user_flags(RID p_body, uint32_t p_flags); virtual uint32_t body_get_user_flags(RID p_body, uint32_t p_flags) const; @@ -152,8 +164,6 @@ public: virtual void body_set_param(RID p_body, BodyParameter p_param, float p_value); virtual float body_get_param(RID p_body, BodyParameter p_param) const; - //advanced simulation - virtual void body_static_simulate_motion(RID p_body,const Matrix32& p_new_transform); virtual void body_set_state(RID p_body, BodyState p_state, const Variant& p_variant); virtual Variant body_get_state(RID p_body, BodyState p_state) const; @@ -181,6 +191,7 @@ public: 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 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); /* JOINT API */ diff --git a/servers/physics_2d/shape_2d_sw.cpp b/servers/physics_2d/shape_2d_sw.cpp index 5ad05a18ac..012c4ed404 100644 --- a/servers/physics_2d/shape_2d_sw.cpp +++ b/servers/physics_2d/shape_2d_sw.cpp @@ -26,1060 +26,1059 @@ /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ -#include "shape_2d_sw.h" -#include "geometry.h" -#include "sort.h" - -#define _SEGMENT_IS_VALID_SUPPORT_TRESHOLD 0.99998 - - -void Shape2DSW::configure(const Rect2& p_aabb) { - aabb=p_aabb; - configured=true; - for (Map::Element *E=owners.front();E;E=E->next()) { - ShapeOwner2DSW* co=(ShapeOwner2DSW*)E->key(); - co->_shape_changed(); - } -} - - -Vector2 Shape2DSW::get_support(const Vector2& p_normal) const { - - Vector2 res[2]; - int amnt; - get_supports(p_normal,res,amnt); - return res[0]; -} - -void Shape2DSW::add_owner(ShapeOwner2DSW *p_owner) { - - Map::Element *E=owners.find(p_owner); - if (E) { - E->get()++; - } else { - owners[p_owner]=1; - } -} - -void Shape2DSW::remove_owner(ShapeOwner2DSW *p_owner){ - - Map::Element *E=owners.find(p_owner); - ERR_FAIL_COND(!E); - E->get()--; - if (E->get()==0) { - owners.erase(E); - } - -} - -bool Shape2DSW::is_owner(ShapeOwner2DSW *p_owner) const{ - - return owners.has(p_owner); - -} - -const Map& Shape2DSW::get_owners() const{ - return owners; -} - - -Shape2DSW::Shape2DSW() { - - custom_bias=0; - configured=false; -} - - -Shape2DSW::~Shape2DSW() { - - ERR_FAIL_COND(owners.size()); -} - - -/*********************************************************/ -/*********************************************************/ -/*********************************************************/ - - - -void LineShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const { - - r_amount=0; -} - -bool LineShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const { - - Vector2 segment= p_begin - p_end; - real_t den=normal.dot( segment ); - - //printf("den is %i\n",den); - if (Math::abs(den)<=CMP_EPSILON) { - - return false; - } - - real_t dist=(normal.dot( p_begin ) - d) / den; - //printf("dist is %i\n",dist); - - if (dist<-CMP_EPSILON || dist > (1.0 +CMP_EPSILON)) { - - return false; - } - - r_point = p_begin + segment * -dist; - r_normal=normal; - - return true; -} - -real_t LineShape2DSW::get_moment_of_inertia(float p_mass) const { - - return 0; -} - -void LineShape2DSW::set_data(const Variant& p_data) { - - ERR_FAIL_COND(p_data.get_type()!=Variant::ARRAY); - Array arr = p_data; - ERR_FAIL_COND(arr.size()!=2); - normal=arr[0]; - d=arr[1]; - configure(Rect2(Vector2(-1e4,-1e4),Vector2(1e4*2,1e4*2))); - -} - -Variant LineShape2DSW::get_data() const { - - Array arr; - arr.resize(2); - arr[0]=normal; - arr[1]=d; - return arr; -} - -/*********************************************************/ -/*********************************************************/ -/*********************************************************/ - - - -void RayShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const { - - - r_amount=1; - - if (p_normal.y>0) - *r_supports=Vector2(0,length); - else - *r_supports=Vector2(); - -} - -bool RayShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const { - - return false; //rays can't be intersected - -} - -real_t RayShape2DSW::get_moment_of_inertia(float p_mass) const { - - return 0; //rays are mass-less -} - -void RayShape2DSW::set_data(const Variant& p_data) { - - length=p_data; - configure(Rect2(0,0,0.001,length)); -} - -Variant RayShape2DSW::get_data() const { - - return length; -} - - -/*********************************************************/ -/*********************************************************/ -/*********************************************************/ - - - -void SegmentShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const { - - if (Math::abs(p_normal.dot(n))>_SEGMENT_IS_VALID_SUPPORT_TRESHOLD) { - r_supports[0]=a; - r_supports[1]=b; - r_amount=2; - return; - - } - - float dp=p_normal.dot(b-a); - if (dp>0) - *r_supports=b; - else - *r_supports=a; - r_amount=1; - -} - -bool SegmentShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const { - - if (!Geometry::segment_intersects_segment_2d(p_begin,p_end,a,b,&r_point)) - return false; - - Vector2 d = p_end-p_begin; - if (n.dot(p_begin) > n.dot(a)) { - r_normal=n; - } else { - r_normal=-n; - } - - return true; -} - -real_t SegmentShape2DSW::get_moment_of_inertia(float p_mass) const { - - real_t l = b.distance_to(a); - Vector2 ofs = (a+b)*0.5; - - return p_mass*(l*l/12.0f + ofs.length_squared()); -} - -void SegmentShape2DSW::set_data(const Variant& p_data) { - - ERR_FAIL_COND(p_data.get_type()!=Variant::RECT2); - - Rect2 r = p_data; - a=r.pos; - b=r.size; - n=(b-a).tangent(); - - Rect2 aabb; - aabb.pos=a; - aabb.expand_to(b); - if (aabb.size.x==0) - aabb.size.x=0.001; - if (aabb.size.y==0) - aabb.size.y=0.001; - configure(aabb); -} - -Variant SegmentShape2DSW::get_data() const { - - Rect2 r; - r.pos=a; - r.size=b; - return r; -} - -/*********************************************************/ -/*********************************************************/ -/*********************************************************/ - - - -void CircleShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const { - - r_amount=1; - *r_supports=p_normal*radius; - -} - -bool CircleShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const { - - - Vector2 line_vec = p_end - p_begin; - - real_t a, b, c; - - a = line_vec.dot(line_vec); - b = 2 * p_begin.dot(line_vec); - c = p_begin.dot(p_begin) - radius * radius; - - real_t sqrtterm = b*b - 4*a*c; - - if(sqrtterm < 0) - return false; - sqrtterm = Math::sqrt(sqrtterm); - real_t res = ( -b - sqrtterm ) / (2 * a); - - if (res <0 || res >1+CMP_EPSILON) { - return false; - } - - r_point=p_begin+line_vec*res; - r_normal=r_point.normalized(); - return true; -} - -real_t CircleShape2DSW::get_moment_of_inertia(float p_mass) const { - - return radius*radius; -} - -void CircleShape2DSW::set_data(const Variant& p_data) { - - ERR_FAIL_COND(!p_data.is_num()); - radius=p_data; - configure(Rect2(-radius,-radius,radius*2,radius*2)); -} - -Variant CircleShape2DSW::get_data() const { - - return radius; -} - - -/*********************************************************/ -/*********************************************************/ -/*********************************************************/ - - - -void RectangleShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const { - - for(int i=0;i<2;i++) { - - Vector2 ag; - ag[i]=1.0; - float dp = ag.dot(p_normal); - if (Math::abs(dp)<_SEGMENT_IS_VALID_SUPPORT_TRESHOLD) - continue; - - float sgn = dp>0 ? 1.0 : -1.0; - - r_amount=2; - - r_supports[0][i]=half_extents[i]*sgn; - r_supports[0][i^1]=half_extents[i^1]; - - r_supports[1][i]=half_extents[i]*sgn; - r_supports[1][i^1]=-half_extents[i^1]; - - return; - - - } - - /* USE POINT */ - - r_amount=1; - r_supports[0]=Vector2( - (p_normal.x<0) ? -half_extents.x : half_extents.x, - (p_normal.y<0) ? -half_extents.y : half_extents.y - ); - -} - -bool RectangleShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const { - - - return get_aabb().intersects_segment(p_begin,p_end,&r_point,&r_normal); -} - -real_t RectangleShape2DSW::get_moment_of_inertia(float p_mass) const { - - Vector2 he2=half_extents*2; - return p_mass*he2.dot(he2)/12.0f; -} - -void RectangleShape2DSW::set_data(const Variant& p_data) { - - ERR_FAIL_COND(p_data.get_type()!=Variant::VECTOR2); - - half_extents=p_data; - configure(Rect2(-half_extents,half_extents*2.0)); -} - -Variant RectangleShape2DSW::get_data() const { - - return half_extents; -} - - - -/*********************************************************/ -/*********************************************************/ -/*********************************************************/ - - - -void CapsuleShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const { - - Vector2 n=p_normal; - - float d = n.y; - - if (Math::abs( d )<(1.0-_SEGMENT_IS_VALID_SUPPORT_TRESHOLD) ) { - - // make it flat - n.y=0.0; - n.normalize(); - n*=radius; - - r_amount=2; - r_supports[0]=n; - r_supports[0].y+=height*0.5; - r_supports[1]=n; - r_supports[1].y-=height*0.5; - - } else { - - float h = (d > 0) ? height : -height; - - n*=radius; - n.y += h*0.5; - r_amount=1; - *r_supports=n; - - } -} - -bool CapsuleShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const { - - - float d = 1e10; - Vector2 n = (p_end-p_begin).normalized(); - bool collided=false; - - //try spheres - for(int i=0;i<2;i++) { - - Vector2 begin = p_begin; - Vector2 end = p_end; - float ofs = (i==0)?-height*0.5:height*0.5; - begin.y+=ofs; - end.y+=ofs; - - Vector2 line_vec = end - begin; - - real_t a, b, c; - - a = line_vec.dot(line_vec); - b = 2 * begin.dot(line_vec); - c = begin.dot(begin) - radius * radius; - - real_t sqrtterm = b*b - 4*a*c; - - if(sqrtterm < 0) - continue; - - sqrtterm = Math::sqrt(sqrtterm); - real_t res = ( -b - sqrtterm ) / (2 * a); - - if (res <0 || res >1+CMP_EPSILON) { - continue; - } - - Vector2 point = begin+line_vec*res; - Vector2 pointf(point.x,point.y-ofs); - real_t pd = n.dot(pointf); - if (pdd) { - support_idx=i; - d=ld; - } - - //test segment - if (points[i].normal.dot(p_normal)>_SEGMENT_IS_VALID_SUPPORT_TRESHOLD) { - - r_amount=2; - r_supports[0]=points[i].pos; - r_supports[1]=points[(i+1)%point_count].pos; - return; - } - } - - ERR_FAIL_COND(support_idx==-1); - - r_amount=1; - r_supports[0]=points[support_idx].pos; - -} - -bool ConvexPolygonShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const { - - Vector2 n = (p_end-p_begin).normalized(); - real_t d=1e10; - bool inters=false; - - for(int i=0;i=0) - // continue; - - - Vector2 res; - - if (!Geometry::segment_intersects_segment_2d(p_begin,p_end,points[i].pos,points[(i+1)%point_count].pos,&res)) - continue; - - float nd = n.dot(res); - if (nd0) - r_normal=-r_normal; - } - - //return get_aabb().intersects_segment(p_begin,p_end,&r_point,&r_normal); - return inters; //todo -} - -real_t ConvexPolygonShape2DSW::get_moment_of_inertia(float p_mass) const { - - Rect2 aabb; - aabb.pos=points[0].pos; - for(int i=0;i arr=p_data; - ERR_FAIL_COND(arr.size()==0); - point_count=arr.size(); - points = memnew_arr(Point,point_count); - DVector::Read r = arr.read(); - - for(int i=0;i dvr = p_data; - point_count=dvr.size()/4; - ERR_FAIL_COND(point_count==0); - - points = memnew_arr(Point,point_count); - DVector::Read r = dvr.read(); - - for(int i=0;i dvr; - - dvr.resize(point_count); - - for(int i=0;id) { - d=ld; - idx=i; - } - } - - - r_amount=1; - ERR_FAIL_COND(idx==-1); - *r_supports=points[idx]; - -} - -bool ConcavePolygonShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const{ - - uint32_t* stack = (uint32_t*)alloca(sizeof(int)*bvh_depth); - - enum { - TEST_AABB_BIT=0, - VISIT_LEFT_BIT=1, - VISIT_RIGHT_BIT=2, - VISIT_DONE_BIT=3, - VISITED_BIT_SHIFT=29, - NODE_IDX_MASK=(1<>VISITED_BIT_SHIFT) { - case TEST_AABB_BIT: { - - - bool valid = b.aabb.intersects_segment(p_begin,p_end); - if (!valid) { - - stack[level]=(VISIT_DONE_BIT<0) - r_normal=-r_normal; - } - - return inters; - -} - - - -int ConcavePolygonShape2DSW::_generate_bvh(BVH *p_bvh,int p_len,int p_depth) { - - if (p_len==1) { - - bvh_depth=MAX(p_depth,bvh_depth); - bvh.push_back(*p_bvh); - return bvh.size()-1; - } - - //else sort best - - Rect2 global_aabb=p_bvh[0].aabb; - for(int i=1;i global_aabb.size.y) { - - SortArray sort; - sort.sort(p_bvh,p_len); - - } else { - - SortArray sort; - sort.sort(p_bvh,p_len); - - } - - int median = p_len/2; - - - BVH node; - node.aabb=global_aabb; - int node_idx = bvh.size(); - bvh.push_back(node); - - int l = _generate_bvh(p_bvh,median,p_depth+1); - int r = _generate_bvh(&p_bvh[median],p_len-median,p_depth+1); - bvh[node_idx].left=l; - bvh[node_idx].right=r; - - return node_idx; - -} - -void ConcavePolygonShape2DSW::set_data(const Variant& p_data) { - - ERR_FAIL_COND(p_data.get_type()!=Variant::VECTOR2_ARRAY && p_data.get_type()!=Variant::REAL_ARRAY); - - segments.clear();; - points.clear();; - bvh.clear();; - bvh_depth=1; - - Rect2 aabb; - - if (p_data.get_type()==Variant::VECTOR2_ARRAY) { - - DVector p2arr = p_data; - int len = p2arr.size(); - DVector::Read arr = p2arr.read(); - - - Map pointmap; - for(int i=0;ikey(); - for(Map::Element *E=pointmap.front();E;E=E->next()) { - - aabb.expand_to(E->key()); - points[E->get()]=E->key(); - } - - Vector main_vbh; - main_vbh.resize(segments.size()); - for(int i=0;i rsegments; - int len = segments.size(); - rsegments.resize(len*2); - DVector::Write w = rsegments.write(); - for(int i=0;i::Write(); - - return rsegments; -} - -void ConcavePolygonShape2DSW::cull(const Rect2& p_local_aabb,Callback p_callback,void* p_userdata) const { - - uint32_t* stack = (uint32_t*)alloca(sizeof(int)*bvh_depth); - - enum { - TEST_AABB_BIT=0, - VISIT_LEFT_BIT=1, - VISIT_RIGHT_BIT=2, - VISIT_DONE_BIT=3, - VISITED_BIT_SHIFT=29, - NODE_IDX_MASK=(1<>VISITED_BIT_SHIFT) { - case TEST_AABB_BIT: { - - - bool valid = p_local_aabb.intersects(b.aabb); - if (!valid) { - - stack[level]=(VISIT_DONE_BIT<::Element *E=owners.front();E;E=E->next()) { + ShapeOwner2DSW* co=(ShapeOwner2DSW*)E->key(); + co->_shape_changed(); + } +} + + +Vector2 Shape2DSW::get_support(const Vector2& p_normal) const { + + Vector2 res[2]; + int amnt; + get_supports(p_normal,res,amnt); + return res[0]; +} + +void Shape2DSW::add_owner(ShapeOwner2DSW *p_owner) { + + Map::Element *E=owners.find(p_owner); + if (E) { + E->get()++; + } else { + owners[p_owner]=1; + } +} + +void Shape2DSW::remove_owner(ShapeOwner2DSW *p_owner){ + + Map::Element *E=owners.find(p_owner); + ERR_FAIL_COND(!E); + E->get()--; + if (E->get()==0) { + owners.erase(E); + } + +} + +bool Shape2DSW::is_owner(ShapeOwner2DSW *p_owner) const{ + + return owners.has(p_owner); + +} + +const Map& Shape2DSW::get_owners() const{ + return owners; +} + + +Shape2DSW::Shape2DSW() { + + custom_bias=0; + configured=false; +} + + +Shape2DSW::~Shape2DSW() { + + ERR_FAIL_COND(owners.size()); +} + + +/*********************************************************/ +/*********************************************************/ +/*********************************************************/ + + + +void LineShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const { + + r_amount=0; +} + +bool LineShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const { + + Vector2 segment= p_begin - p_end; + real_t den=normal.dot( segment ); + + //printf("den is %i\n",den); + if (Math::abs(den)<=CMP_EPSILON) { + + return false; + } + + real_t dist=(normal.dot( p_begin ) - d) / den; + //printf("dist is %i\n",dist); + + if (dist<-CMP_EPSILON || dist > (1.0 +CMP_EPSILON)) { + + return false; + } + + r_point = p_begin + segment * -dist; + r_normal=normal; + + return true; +} + +real_t LineShape2DSW::get_moment_of_inertia(float p_mass) const { + + return 0; +} + +void LineShape2DSW::set_data(const Variant& p_data) { + + ERR_FAIL_COND(p_data.get_type()!=Variant::ARRAY); + Array arr = p_data; + ERR_FAIL_COND(arr.size()!=2); + normal=arr[0]; + d=arr[1]; + configure(Rect2(Vector2(-1e4,-1e4),Vector2(1e4*2,1e4*2))); + +} + +Variant LineShape2DSW::get_data() const { + + Array arr; + arr.resize(2); + arr[0]=normal; + arr[1]=d; + return arr; +} + +/*********************************************************/ +/*********************************************************/ +/*********************************************************/ + + + +void RayShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const { + + + r_amount=1; + + if (p_normal.y>0) + *r_supports=Vector2(0,length); + else + *r_supports=Vector2(); + +} + +bool RayShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const { + + return false; //rays can't be intersected + +} + +real_t RayShape2DSW::get_moment_of_inertia(float p_mass) const { + + return 0; //rays are mass-less +} + +void RayShape2DSW::set_data(const Variant& p_data) { + + length=p_data; + configure(Rect2(0,0,0.001,length)); +} + +Variant RayShape2DSW::get_data() const { + + return length; +} + + +/*********************************************************/ +/*********************************************************/ +/*********************************************************/ + + + +void SegmentShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const { + + if (Math::abs(p_normal.dot(n))>_SEGMENT_IS_VALID_SUPPORT_TRESHOLD) { + r_supports[0]=a; + r_supports[1]=b; + r_amount=2; + return; + + } + + float dp=p_normal.dot(b-a); + if (dp>0) + *r_supports=b; + else + *r_supports=a; + r_amount=1; + +} + +bool SegmentShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const { + + if (!Geometry::segment_intersects_segment_2d(p_begin,p_end,a,b,&r_point)) + return false; + + Vector2 d = p_end-p_begin; + if (n.dot(p_begin) > n.dot(a)) { + r_normal=n; + } else { + r_normal=-n; + } + + return true; +} + +real_t SegmentShape2DSW::get_moment_of_inertia(float p_mass) const { + + real_t l = b.distance_to(a); + Vector2 ofs = (a+b)*0.5; + + return p_mass*(l*l/12.0f + ofs.length_squared()); +} + +void SegmentShape2DSW::set_data(const Variant& p_data) { + + ERR_FAIL_COND(p_data.get_type()!=Variant::RECT2); + + Rect2 r = p_data; + a=r.pos; + b=r.size; + n=(b-a).tangent(); + + Rect2 aabb; + aabb.pos=a; + aabb.expand_to(b); + if (aabb.size.x==0) + aabb.size.x=0.001; + if (aabb.size.y==0) + aabb.size.y=0.001; + configure(aabb); +} + +Variant SegmentShape2DSW::get_data() const { + + Rect2 r; + r.pos=a; + r.size=b; + return r; +} + +/*********************************************************/ +/*********************************************************/ +/*********************************************************/ + + + +void CircleShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const { + + r_amount=1; + *r_supports=p_normal*radius; + +} + +bool CircleShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const { + + + Vector2 line_vec = p_end - p_begin; + + real_t a, b, c; + + a = line_vec.dot(line_vec); + b = 2 * p_begin.dot(line_vec); + c = p_begin.dot(p_begin) - radius * radius; + + real_t sqrtterm = b*b - 4*a*c; + + if(sqrtterm < 0) + return false; + sqrtterm = Math::sqrt(sqrtterm); + real_t res = ( -b - sqrtterm ) / (2 * a); + + if (res <0 || res >1+CMP_EPSILON) { + return false; + } + + r_point=p_begin+line_vec*res; + r_normal=r_point.normalized(); + return true; +} + +real_t CircleShape2DSW::get_moment_of_inertia(float p_mass) const { + + return radius*radius; +} + +void CircleShape2DSW::set_data(const Variant& p_data) { + + ERR_FAIL_COND(!p_data.is_num()); + radius=p_data; + configure(Rect2(-radius,-radius,radius*2,radius*2)); +} + +Variant CircleShape2DSW::get_data() const { + + return radius; +} + + +/*********************************************************/ +/*********************************************************/ +/*********************************************************/ + + + +void RectangleShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const { + + for(int i=0;i<2;i++) { + + Vector2 ag; + ag[i]=1.0; + float dp = ag.dot(p_normal); + if (Math::abs(dp)<_SEGMENT_IS_VALID_SUPPORT_TRESHOLD) + continue; + + float sgn = dp>0 ? 1.0 : -1.0; + + r_amount=2; + + r_supports[0][i]=half_extents[i]*sgn; + r_supports[0][i^1]=half_extents[i^1]; + + r_supports[1][i]=half_extents[i]*sgn; + r_supports[1][i^1]=-half_extents[i^1]; + + return; + + + } + + /* USE POINT */ + + r_amount=1; + r_supports[0]=Vector2( + (p_normal.x<0) ? -half_extents.x : half_extents.x, + (p_normal.y<0) ? -half_extents.y : half_extents.y + ); + +} + +bool RectangleShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const { + + + return get_aabb().intersects_segment(p_begin,p_end,&r_point,&r_normal); +} + +real_t RectangleShape2DSW::get_moment_of_inertia(float p_mass) const { + + Vector2 he2=half_extents*2; + return p_mass*he2.dot(he2)/12.0f; +} + +void RectangleShape2DSW::set_data(const Variant& p_data) { + + ERR_FAIL_COND(p_data.get_type()!=Variant::VECTOR2); + + half_extents=p_data; + configure(Rect2(-half_extents,half_extents*2.0)); +} + +Variant RectangleShape2DSW::get_data() const { + + return half_extents; +} + + + +/*********************************************************/ +/*********************************************************/ +/*********************************************************/ + + + +void CapsuleShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const { + + Vector2 n=p_normal; + + float d = n.y; + + if (Math::abs( d )<(1.0-_SEGMENT_IS_VALID_SUPPORT_TRESHOLD) ) { + + // make it flat + n.y=0.0; + n.normalize(); + n*=radius; + + r_amount=2; + r_supports[0]=n; + r_supports[0].y+=height*0.5; + r_supports[1]=n; + r_supports[1].y-=height*0.5; + + } else { + + float h = (d > 0) ? height : -height; + + n*=radius; + n.y += h*0.5; + r_amount=1; + *r_supports=n; + + } +} + +bool CapsuleShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const { + + + float d = 1e10; + Vector2 n = (p_end-p_begin).normalized(); + bool collided=false; + + //try spheres + for(int i=0;i<2;i++) { + + Vector2 begin = p_begin; + Vector2 end = p_end; + float ofs = (i==0)?-height*0.5:height*0.5; + begin.y+=ofs; + end.y+=ofs; + + Vector2 line_vec = end - begin; + + real_t a, b, c; + + a = line_vec.dot(line_vec); + b = 2 * begin.dot(line_vec); + c = begin.dot(begin) - radius * radius; + + real_t sqrtterm = b*b - 4*a*c; + + if(sqrtterm < 0) + continue; + + sqrtterm = Math::sqrt(sqrtterm); + real_t res = ( -b - sqrtterm ) / (2 * a); + + if (res <0 || res >1+CMP_EPSILON) { + continue; + } + + Vector2 point = begin+line_vec*res; + Vector2 pointf(point.x,point.y-ofs); + real_t pd = n.dot(pointf); + if (pdd) { + support_idx=i; + d=ld; + } + + //test segment + if (points[i].normal.dot(p_normal)>_SEGMENT_IS_VALID_SUPPORT_TRESHOLD) { + + r_amount=2; + r_supports[0]=points[i].pos; + r_supports[1]=points[(i+1)%point_count].pos; + return; + } + } + + ERR_FAIL_COND(support_idx==-1); + + r_amount=1; + r_supports[0]=points[support_idx].pos; + +} + +bool ConvexPolygonShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const { + + Vector2 n = (p_end-p_begin).normalized(); + real_t d=1e10; + bool inters=false; + + for(int i=0;i=0) + // continue; + + + Vector2 res; + + if (!Geometry::segment_intersects_segment_2d(p_begin,p_end,points[i].pos,points[(i+1)%point_count].pos,&res)) + continue; + + float nd = n.dot(res); + if (nd0) + r_normal=-r_normal; + } + + //return get_aabb().intersects_segment(p_begin,p_end,&r_point,&r_normal); + return inters; //todo +} + +real_t ConvexPolygonShape2DSW::get_moment_of_inertia(float p_mass) const { + + Rect2 aabb; + aabb.pos=points[0].pos; + for(int i=0;i arr=p_data; + ERR_FAIL_COND(arr.size()==0); + point_count=arr.size(); + points = memnew_arr(Point,point_count); + DVector::Read r = arr.read(); + + for(int i=0;i dvr = p_data; + point_count=dvr.size()/4; + ERR_FAIL_COND(point_count==0); + + points = memnew_arr(Point,point_count); + DVector::Read r = dvr.read(); + + for(int i=0;i dvr; + + dvr.resize(point_count); + + for(int i=0;id) { + d=ld; + idx=i; + } + } + + + r_amount=1; + ERR_FAIL_COND(idx==-1); + *r_supports=points[idx]; + +} + +bool ConcavePolygonShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const{ + + uint32_t* stack = (uint32_t*)alloca(sizeof(int)*bvh_depth); + + enum { + TEST_AABB_BIT=0, + VISIT_LEFT_BIT=1, + VISIT_RIGHT_BIT=2, + VISIT_DONE_BIT=3, + VISITED_BIT_SHIFT=29, + NODE_IDX_MASK=(1<>VISITED_BIT_SHIFT) { + case TEST_AABB_BIT: { + + + bool valid = b.aabb.intersects_segment(p_begin,p_end); + if (!valid) { + + stack[level]=(VISIT_DONE_BIT<0) + r_normal=-r_normal; + } + + return inters; + +} + + + +int ConcavePolygonShape2DSW::_generate_bvh(BVH *p_bvh,int p_len,int p_depth) { + + if (p_len==1) { + + bvh_depth=MAX(p_depth,bvh_depth); + bvh.push_back(*p_bvh); + return bvh.size()-1; + } + + //else sort best + + Rect2 global_aabb=p_bvh[0].aabb; + for(int i=1;i global_aabb.size.y) { + + SortArray sort; + sort.sort(p_bvh,p_len); + + } else { + + SortArray sort; + sort.sort(p_bvh,p_len); + + } + + int median = p_len/2; + + + BVH node; + node.aabb=global_aabb; + int node_idx = bvh.size(); + bvh.push_back(node); + + int l = _generate_bvh(p_bvh,median,p_depth+1); + int r = _generate_bvh(&p_bvh[median],p_len-median,p_depth+1); + bvh[node_idx].left=l; + bvh[node_idx].right=r; + + return node_idx; + +} + +void ConcavePolygonShape2DSW::set_data(const Variant& p_data) { + + ERR_FAIL_COND(p_data.get_type()!=Variant::VECTOR2_ARRAY && p_data.get_type()!=Variant::REAL_ARRAY); + + segments.clear();; + points.clear();; + bvh.clear();; + bvh_depth=1; + + Rect2 aabb; + + if (p_data.get_type()==Variant::VECTOR2_ARRAY) { + + DVector p2arr = p_data; + int len = p2arr.size(); + DVector::Read arr = p2arr.read(); + + + Map pointmap; + for(int i=0;ikey(); + for(Map::Element *E=pointmap.front();E;E=E->next()) { + + aabb.expand_to(E->key()); + points[E->get()]=E->key(); + } + + Vector main_vbh; + main_vbh.resize(segments.size()); + for(int i=0;i rsegments; + int len = segments.size(); + rsegments.resize(len*2); + DVector::Write w = rsegments.write(); + for(int i=0;i::Write(); + + return rsegments; +} + +void ConcavePolygonShape2DSW::cull(const Rect2& p_local_aabb,Callback p_callback,void* p_userdata) const { + + uint32_t* stack = (uint32_t*)alloca(sizeof(int)*bvh_depth); + + enum { + TEST_AABB_BIT=0, + VISIT_LEFT_BIT=1, + VISIT_RIGHT_BIT=2, + VISIT_DONE_BIT=3, + VISITED_BIT_SHIFT=29, + NODE_IDX_MASK=(1<>VISITED_BIT_SHIFT) { + case TEST_AABB_BIT: { + + + bool valid = p_local_aabb.intersects(b.aabb); + if (!valid) { + + stack[level]=(VISIT_DONE_BIT< owners; -protected: - - void configure(const Rect2& p_aabb); -public: - - _FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; } - _FORCE_INLINE_ RID get_self() const {return self; } - - virtual Physics2DServer::ShapeType get_type() const=0; - - _FORCE_INLINE_ Rect2 get_aabb() const { return aabb; } - _FORCE_INLINE_ bool is_configured() const { return configured; } - - virtual bool is_concave() const { return false; } - - virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const=0; - virtual Vector2 get_support(const Vector2& p_normal) const; - virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const=0; - - virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const=0; - virtual real_t get_moment_of_inertia(float p_mass) const=0; - - virtual void set_data(const Variant& p_data)=0; - virtual Variant get_data() const=0; - - _FORCE_INLINE_ void set_custom_bias(real_t p_bias) { custom_bias=p_bias; } - _FORCE_INLINE_ real_t get_custom_bias() const { return custom_bias; } - - void add_owner(ShapeOwner2DSW *p_owner); - void remove_owner(ShapeOwner2DSW *p_owner); - bool is_owner(ShapeOwner2DSW *p_owner) const; - const Map& get_owners() const; - - Shape2DSW(); - virtual ~Shape2DSW(); -}; - - -class LineShape2DSW : public Shape2DSW { - - - Vector2 normal; - real_t d; - -public: - - _FORCE_INLINE_ Vector2 get_normal() const { return normal; } - _FORCE_INLINE_ real_t get_d() const { return d; } - - virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_LINE; } - - virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); } - virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const; - - virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const; - virtual real_t get_moment_of_inertia(float p_mass) const; - - virtual void set_data(const Variant& p_data); - virtual Variant get_data() const; - - _FORCE_INLINE_ void project_range(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { - //real large - r_min=-1e10; - r_max=1e10; - } - -}; - - -class RayShape2DSW : public Shape2DSW { - - - real_t length; - -public: - - - _FORCE_INLINE_ real_t get_length() const { return length; } - - virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_RAY; } - - virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); } - virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const; - - virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const; - virtual real_t get_moment_of_inertia(float p_mass) const; - - virtual void set_data(const Variant& p_data); - virtual Variant get_data() const; - - _FORCE_INLINE_ void project_range(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { - //real large - r_max = p_normal.dot(p_transform.get_origin()); - r_min = p_normal.dot(p_transform.xform(Vector2(0,length))); - if (r_max 0) ? height : -height; - - n *= radius; - n.y += h * 0.5; - - r_max = p_normal.dot(p_transform.xform(n)); - r_min = p_normal.dot(p_transform.xform(-n)); - - if (r_maxr_max) - r_max=d; - if (d segments; - Vector points; - - struct BVH { - - Rect2 aabb; - int left,right; - }; - - - Vector bvh; - int bvh_depth; - - - struct BVH_CompareX { - - _FORCE_INLINE_ bool operator ()(const BVH& a, const BVH& b) const { - - return (a.aabb.pos.x+a.aabb.size.x*0.5) < (b.aabb.pos.x+b.aabb.size.x*0.5); - } - }; - - struct BVH_CompareY { - - _FORCE_INLINE_ bool operator ()(const BVH& a, const BVH& b) const { - - return (a.aabb.pos.y+a.aabb.size.y*0.5) < (b.aabb.pos.y+b.aabb.size.y*0.5); - } - }; - - int _generate_bvh(BVH *p_bvh,int p_len,int p_depth); - -public: - - virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_CONCAVE_POLYGON; } - - virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { /*project_range(p_normal,p_transform,r_min,r_max);*/ } - virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const; - - virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const; - - virtual real_t get_moment_of_inertia(float p_mass) const { return 0; } - - virtual void set_data(const Variant& p_data); - virtual Variant get_data() const; - - virtual void cull(const Rect2& p_local_aabb,Callback p_callback,void* p_userdata) const; - -}; - - -#endif // SHAPE_2D_2DSW_H +#ifndef SHAPE_2D_2DSW_H +#define SHAPE_2D_2DSW_H + +#include "servers/physics_2d_server.h" +#define _SEGMENT_IS_VALID_SUPPORT_TRESHOLD 0.99998 + +/* + +SHAPE_LINE, ///< plane:"plane" +SHAPE_SEGMENT, ///< float:"length" +SHAPE_CIRCLE, ///< float:"radius" +SHAPE_RECTANGLE, ///< vec3:"extents" +SHAPE_CONVEX_POLYGON, ///< array of planes:"planes" +SHAPE_CONCAVE_POLYGON, ///< Vector2 array:"triangles" , or Dictionary with "indices" (int array) and "triangles" (Vector2 array) +SHAPE_CUSTOM, ///< Server-Implementation based custom shape, calling shape_create() with this value will result in an error + +*/ + +class Shape2DSW; + +class ShapeOwner2DSW { +public: + + virtual void _shape_changed()=0; + virtual void remove_shape(Shape2DSW *p_shape)=0; + + virtual ~ShapeOwner2DSW() {} +}; + +class Shape2DSW { + + RID self; + Rect2 aabb; + bool configured; + real_t custom_bias; + + Map owners; +protected: + + void configure(const Rect2& p_aabb); +public: + + _FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; } + _FORCE_INLINE_ RID get_self() const {return self; } + + virtual Physics2DServer::ShapeType get_type() const=0; + + _FORCE_INLINE_ Rect2 get_aabb() const { return aabb; } + _FORCE_INLINE_ bool is_configured() const { return configured; } + + virtual bool is_concave() const { return false; } + + virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const=0; + virtual void project_range_castv(const Vector2& p_cast, const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const=0; + virtual Vector2 get_support(const Vector2& p_normal) const; + virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const=0; + + virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const=0; + virtual real_t get_moment_of_inertia(float p_mass) const=0; + + virtual void set_data(const Variant& p_data)=0; + virtual Variant get_data() const=0; + + _FORCE_INLINE_ void set_custom_bias(real_t p_bias) { custom_bias=p_bias; } + _FORCE_INLINE_ real_t get_custom_bias() const { return custom_bias; } + + void add_owner(ShapeOwner2DSW *p_owner); + void remove_owner(ShapeOwner2DSW *p_owner); + bool is_owner(ShapeOwner2DSW *p_owner) const; + const Map& get_owners() const; + + + _FORCE_INLINE_ void get_supports_transformed_cast(const Vector2& p_cast,const Vector2& p_normal,const Matrix32& 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;i0) { + //normal points towards cast, add cast + r_supports[0]+=p_cast; + } + + } else { + + 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) { + //larger towards 1 + r_supports[1]+=p_cast; + } else { + //larger towards 0 + 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[1]+=p_cast; + } + + } + } + + Shape2DSW(); + virtual ~Shape2DSW(); +}; + +//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 Matrix32& 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 Matrix32& p_transform, real_t &r_min, real_t &r_max) const {\ +\ + real_t mina,maxa;\ + real_t minb,maxb;\ + Matrix32 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 Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); } + virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const; + + virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const; + virtual real_t get_moment_of_inertia(float p_mass) const; + + virtual void set_data(const Variant& p_data); + virtual Variant get_data() const; + + _FORCE_INLINE_ void project_range(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { + //real large + r_min=-1e10; + r_max=1e10; + } + + virtual void project_range_castv(const Vector2& p_cast, const Vector2& p_normal, const Matrix32& 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 Matrix32& p_transform, real_t &r_min, real_t &r_max) const { + //real large + r_min=-1e10; + r_max=1e10; + } + + + +}; + + +class RayShape2DSW : public Shape2DSW { + + + real_t length; + +public: + + + _FORCE_INLINE_ real_t get_length() const { return length; } + + virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_RAY; } + + virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); } + virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const; + + virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const; + virtual real_t get_moment_of_inertia(float p_mass) const; + + virtual void set_data(const Variant& p_data); + virtual Variant get_data() const; + + _FORCE_INLINE_ void project_range(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { + //real large + r_max = p_normal.dot(p_transform.get_origin()); + r_min = p_normal.dot(p_transform.xform(Vector2(0,length))); + if (r_max 0) ? height : -height; + + n *= radius; + n.y += h * 0.5; + + r_max = p_normal.dot(p_transform.xform(n)); + r_min = p_normal.dot(p_transform.xform(-n)); + + if (r_maxr_max) + r_max=d; + if (d segments; + Vector points; + + struct BVH { + + Rect2 aabb; + int left,right; + }; + + + Vector bvh; + int bvh_depth; + + + struct BVH_CompareX { + + _FORCE_INLINE_ bool operator ()(const BVH& a, const BVH& b) const { + + return (a.aabb.pos.x+a.aabb.size.x*0.5) < (b.aabb.pos.x+b.aabb.size.x*0.5); + } + }; + + struct BVH_CompareY { + + _FORCE_INLINE_ bool operator ()(const BVH& a, const BVH& b) const { + + return (a.aabb.pos.y+a.aabb.size.y*0.5) < (b.aabb.pos.y+b.aabb.size.y*0.5); + } + }; + + int _generate_bvh(BVH *p_bvh,int p_len,int p_depth); + +public: + + virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_CONCAVE_POLYGON; } + + virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { /*project_range(p_normal,p_transform,r_min,r_max);*/ } + virtual void project_range(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { /*project_range(p_normal,p_transform,r_min,r_max);*/ } + virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const; + + virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const; + + virtual real_t get_moment_of_inertia(float p_mass) const { return 0; } + + virtual void set_data(const Variant& p_data); + virtual Variant get_data() const; + + virtual void cull(const Rect2& p_local_aabb,Callback p_callback,void* p_userdata) const; + + + DEFAULT_PROJECT_RANGE_CAST + +}; + + +#endif // SHAPE_2D_2DSW_H diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp index 4fe53aeb4d..2c714f5065 100644 --- a/servers/physics_2d/space_2d_sw.cpp +++ b/servers/physics_2d/space_2d_sw.cpp @@ -26,407 +26,597 @@ /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ -#include "space_2d_sw.h" -#include "collision_solver_2d_sw.h" -#include "physics_2d_server_sw.h" - - -bool Physics2DDirectSpaceStateSW::intersect_ray(const Vector2& p_from, const Vector2& p_to,RayResult &r_result,const Set& p_exclude,uint32_t p_user_mask) { - - - ERR_FAIL_COND_V(space->locked,false); - - Vector2 begin,end; - Vector2 normal; - begin=p_from; - end=p_to; - normal=(end-begin).normalized(); - - - int amount = space->broadphase->cull_segment(begin,end,space->intersection_query_results,Space2DSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results); - - //todo, create another array tha references results, compute AABBs and check closest point to ray origin, sort, and stop evaluating results when beyond first collision - - bool collided=false; - Vector2 res_point,res_normal; - int res_shape; - const CollisionObject2DSW *res_obj; - real_t min_d=1e10; - - - for(int i=0;iintersection_query_results[i]->get_type()==CollisionObject2DSW::TYPE_AREA) - continue; //ignore area - - if (p_exclude.has( space->intersection_query_results[i]->get_self())) - continue; - - const CollisionObject2DSW *col_obj=space->intersection_query_results[i]; - - int shape_idx=space->intersection_query_subindex_results[i]; - Matrix32 inv_xform = col_obj->get_shape_inv_transform(shape_idx) * col_obj->get_inv_transform(); - - Vector2 local_from = inv_xform.xform(begin); - Vector2 local_to = inv_xform.xform(end); - - /*local_from = col_obj->get_inv_transform().xform(begin); - local_from = col_obj->get_shape_inv_transform(shape_idx).xform(local_from); - - local_to = col_obj->get_inv_transform().xform(end); - local_to = col_obj->get_shape_inv_transform(shape_idx).xform(local_to);*/ - - const Shape2DSW *shape = col_obj->get_shape(shape_idx); - - Vector2 shape_point,shape_normal; - - - if (shape->intersect_segment(local_from,local_to,shape_point,shape_normal)) { - - - //print_line("inters sgment!"); - Matrix32 xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); - shape_point=xform.xform(shape_point); - - real_t ld = normal.dot(shape_point); - - - if (ldget_instance_id(); - if (r_result.collider_id!=0) - r_result.collider=ObjectDB::get_instance(r_result.collider_id); - r_result.normal=res_normal; - r_result.position=res_point; - r_result.rid=res_obj->get_self(); - r_result.shape=res_shape; - - return true; - -} - - -int Physics2DDirectSpaceStateSW::intersect_shape(const RID& p_shape, const Matrix32& p_xform,ShapeResult *r_results,int p_result_max,const Set& p_exclude,uint32_t p_user_mask) { - - if (p_result_max<=0) - return 0; - - Shape2DSW *shape = static_cast(Physics2DServer::get_singleton())->shape_owner.get(p_shape); - ERR_FAIL_COND_V(!shape,0); - - Rect2 aabb = p_xform.xform(shape->get_aabb()); - - int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,Space2DSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results); - - bool collided=false; - int cc=0; - - for(int i=0;i=p_result_max) - break; - - if (space->intersection_query_results[i]->get_type()==CollisionObject2DSW::TYPE_AREA) - continue; //ignore area - - if (p_exclude.has( space->intersection_query_results[i]->get_self())) - continue; - - - const CollisionObject2DSW *col_obj=space->intersection_query_results[i]; - int shape_idx=space->intersection_query_subindex_results[i]; - - if (!CollisionSolver2DSW::solve_static(shape,p_xform,p_xform.affine_inverse(),col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), col_obj->get_inv_transform() * col_obj->get_shape_inv_transform(shape_idx),NULL,NULL,NULL)) - continue; - - r_results[cc].collider_id=col_obj->get_instance_id(); - if (r_results[cc].collider_id!=0) - r_results[cc].collider=ObjectDB::get_instance(r_results[cc].collider_id); - r_results[cc].rid=col_obj->get_self(); - r_results[cc].shape=shape_idx; - - cc++; - - } - - return cc; - -} - -Physics2DDirectSpaceStateSW::Physics2DDirectSpaceStateSW() { - - - space=NULL; -} - - -//////////////////////////////////////////////////////////////////////////////////////////////////////////// - - - - - - - - - - -void* Space2DSW::_broadphase_pair(CollisionObject2DSW *A,int p_subindex_A,CollisionObject2DSW *B,int p_subindex_B,void *p_self) { - - CollisionObject2DSW::Type type_A=A->get_type(); - CollisionObject2DSW::Type type_B=B->get_type(); - if (type_A>type_B) { - - SWAP(A,B); - SWAP(p_subindex_A,p_subindex_B); - SWAP(type_A,type_B); - } - - Space2DSW *self = (Space2DSW*)p_self; - - if (type_A==CollisionObject2DSW::TYPE_AREA) { - - - ERR_FAIL_COND_V(type_B!=CollisionObject2DSW::TYPE_BODY,NULL); - Area2DSW *area=static_cast(A); - Body2DSW *body=static_cast(B); - - AreaPair2DSW *area_pair = memnew(AreaPair2DSW(body,p_subindex_B,area,p_subindex_A) ); - - return area_pair; - } else { - - - BodyPair2DSW *b = memnew( BodyPair2DSW((Body2DSW*)A,p_subindex_A,(Body2DSW*)B,p_subindex_B) ); - return b; - - } - - return NULL; -} - -void Space2DSW::_broadphase_unpair(CollisionObject2DSW *A,int p_subindex_A,CollisionObject2DSW *B,int p_subindex_B,void *p_data,void *p_self) { - - - - Space2DSW *self = (Space2DSW*)p_self; - Constraint2DSW *c = (Constraint2DSW*)p_data; - memdelete(c); -} - - -const SelfList::List& Space2DSW::get_active_body_list() const { - - return active_list; -} -void Space2DSW::body_add_to_active_list(SelfList* p_body) { - - active_list.add(p_body); -} -void Space2DSW::body_remove_from_active_list(SelfList* p_body) { - - active_list.remove(p_body); - -} - -void Space2DSW::body_add_to_inertia_update_list(SelfList* p_body) { - - - inertia_update_list.add(p_body); -} - -void Space2DSW::body_remove_from_inertia_update_list(SelfList* p_body) { - - inertia_update_list.remove(p_body); -} - -BroadPhase2DSW *Space2DSW::get_broadphase() { - - return broadphase; -} - -void Space2DSW::add_object(CollisionObject2DSW *p_object) { - - ERR_FAIL_COND( objects.has(p_object) ); - objects.insert(p_object); -} - -void Space2DSW::remove_object(CollisionObject2DSW *p_object) { - - ERR_FAIL_COND( !objects.has(p_object) ); - objects.erase(p_object); -} - -const Set &Space2DSW::get_objects() const { - - return objects; -} - -void Space2DSW::body_add_to_state_query_list(SelfList* p_body) { - - state_query_list.add(p_body); -} -void Space2DSW::body_remove_from_state_query_list(SelfList* p_body) { - - state_query_list.remove(p_body); -} - -void Space2DSW::area_add_to_monitor_query_list(SelfList* p_area) { - - monitor_query_list.add(p_area); -} -void Space2DSW::area_remove_from_monitor_query_list(SelfList* p_area) { - - monitor_query_list.remove(p_area); -} - -void Space2DSW::area_add_to_moved_list(SelfList* p_area) { - - area_moved_list.add(p_area); -} - -void Space2DSW::area_remove_from_moved_list(SelfList* p_area) { - - area_moved_list.remove(p_area); -} - -const SelfList::List& Space2DSW::get_moved_area_list() const { - - return area_moved_list; -} - - -void Space2DSW::call_queries() { - - while(state_query_list.first()) { - - Body2DSW * b = state_query_list.first()->self(); - b->call_queries(); - state_query_list.remove(state_query_list.first()); - } - - while(monitor_query_list.first()) { - - Area2DSW * a = monitor_query_list.first()->self(); - a->call_queries(); - monitor_query_list.remove(monitor_query_list.first()); - } - -} - -void Space2DSW::setup() { - - - while(inertia_update_list.first()) { - inertia_update_list.first()->self()->update_inertias(); - inertia_update_list.remove(inertia_update_list.first()); - } - - -} - -void Space2DSW::update() { - - broadphase->update(); - -} - - -void Space2DSW::set_param(Physics2DServer::SpaceParameter p_param, real_t p_value) { - - switch(p_param) { - - case Physics2DServer::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: contact_recycle_radius=p_value; break; - case Physics2DServer::SPACE_PARAM_CONTACT_MAX_SEPARATION: contact_max_separation=p_value; break; - case Physics2DServer::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: contact_max_allowed_penetration=p_value; break; - case Physics2DServer::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_TRESHOLD: body_linear_velocity_sleep_treshold=p_value; break; - case Physics2DServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_TRESHOLD: body_angular_velocity_sleep_treshold=p_value; break; - case Physics2DServer::SPACE_PARAM_BODY_TIME_TO_SLEEP: body_time_to_sleep=p_value; break; - case Physics2DServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO: body_angular_velocity_damp_ratio=p_value; break; - case Physics2DServer::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: constraint_bias=p_value; break; - } -} - -real_t Space2DSW::get_param(Physics2DServer::SpaceParameter p_param) const { - - switch(p_param) { - - case Physics2DServer::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: return contact_recycle_radius; - case Physics2DServer::SPACE_PARAM_CONTACT_MAX_SEPARATION: return contact_max_separation; - case Physics2DServer::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: return contact_max_allowed_penetration; - case Physics2DServer::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_TRESHOLD: return body_linear_velocity_sleep_treshold; - case Physics2DServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_TRESHOLD: return body_angular_velocity_sleep_treshold; - case Physics2DServer::SPACE_PARAM_BODY_TIME_TO_SLEEP: return body_time_to_sleep; - case Physics2DServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO: return body_angular_velocity_damp_ratio; - case Physics2DServer::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: return constraint_bias; - } - return 0; -} - -void Space2DSW::lock() { - - locked=true; -} - -void Space2DSW::unlock() { - - locked=false; -} - -bool Space2DSW::is_locked() const { - - return locked; -} - -Physics2DDirectSpaceStateSW *Space2DSW::get_direct_state() { - - return direct_access; -} - -Space2DSW::Space2DSW() { - - - locked=false; - contact_recycle_radius=0.01; - contact_max_separation=0.05; - contact_max_allowed_penetration= 0.01; - - constraint_bias = 0.01; - body_linear_velocity_sleep_treshold=0.01; - body_angular_velocity_sleep_treshold=(8.0 / 180.0 * Math_PI); - body_time_to_sleep=0.5; - body_angular_velocity_damp_ratio=15; - - - broadphase = BroadPhase2DSW::create_func(); - broadphase->set_pair_callback(_broadphase_pair,this); - broadphase->set_unpair_callback(_broadphase_unpair,this); - area=NULL; - - direct_access = memnew( Physics2DDirectSpaceStateSW ); - direct_access->space=this; -} - -Space2DSW::~Space2DSW() { - - memdelete(broadphase); - memdelete( direct_access ); -} - - - +#include "space_2d_sw.h" +#include "collision_solver_2d_sw.h" +#include "physics_2d_server_sw.h" + + +bool Physics2DDirectSpaceStateSW::intersect_ray(const Vector2& p_from, const Vector2& p_to,RayResult &r_result,const Set& p_exclude,uint32_t p_user_mask) { + + + ERR_FAIL_COND_V(space->locked,false); + + Vector2 begin,end; + Vector2 normal; + begin=p_from; + end=p_to; + normal=(end-begin).normalized(); + + int amount = space->broadphase->cull_segment(begin,end,space->intersection_query_results,Space2DSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results); + + //todo, create another array tha references results, compute AABBs and check closest point to ray origin, sort, and stop evaluating results when beyond first collision + + bool collided=false; + Vector2 res_point,res_normal; + int res_shape; + const CollisionObject2DSW *res_obj; + real_t min_d=1e10; + + + for(int i=0;iintersection_query_results[i]->get_type()==CollisionObject2DSW::TYPE_AREA) + continue; //ignore area + + if (p_exclude.has( space->intersection_query_results[i]->get_self())) + continue; + + const CollisionObject2DSW *col_obj=space->intersection_query_results[i]; + + int shape_idx=space->intersection_query_subindex_results[i]; + Matrix32 inv_xform = col_obj->get_shape_inv_transform(shape_idx) * col_obj->get_inv_transform(); + + Vector2 local_from = inv_xform.xform(begin); + Vector2 local_to = inv_xform.xform(end); + + /*local_from = col_obj->get_inv_transform().xform(begin); + local_from = col_obj->get_shape_inv_transform(shape_idx).xform(local_from); + + local_to = col_obj->get_inv_transform().xform(end); + local_to = col_obj->get_shape_inv_transform(shape_idx).xform(local_to);*/ + + const Shape2DSW *shape = col_obj->get_shape(shape_idx); + + Vector2 shape_point,shape_normal; + + + if (shape->intersect_segment(local_from,local_to,shape_point,shape_normal)) { + + + //print_line("inters sgment!"); + Matrix32 xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); + shape_point=xform.xform(shape_point); + + real_t ld = normal.dot(shape_point); + + + if (ldget_instance_id(); + if (r_result.collider_id!=0) + r_result.collider=ObjectDB::get_instance(r_result.collider_id); + r_result.normal=res_normal; + r_result.position=res_point; + r_result.rid=res_obj->get_self(); + r_result.shape=res_shape; + + return true; + +} + + +int Physics2DDirectSpaceStateSW::intersect_shape(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,ShapeResult *r_results,int p_result_max,const Set& p_exclude,uint32_t p_user_mask) { + + if (p_result_max<=0) + return 0; + + Shape2DSW *shape = static_cast(Physics2DServer::get_singleton())->shape_owner.get(p_shape); + ERR_FAIL_COND_V(!shape,0); + + Rect2 aabb = p_xform.xform(shape->get_aabb()); + + int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,Space2DSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results); + + bool collided=false; + int cc=0; + + for(int i=0;i=p_result_max) + break; + + if (space->intersection_query_results[i]->get_type()==CollisionObject2DSW::TYPE_AREA) + continue; //ignore area + + if (p_exclude.has( space->intersection_query_results[i]->get_self())) + continue; + + + const CollisionObject2DSW *col_obj=space->intersection_query_results[i]; + int shape_idx=space->intersection_query_subindex_results[i]; + + if (!CollisionSolver2DSW::solve(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)) + continue; + + r_results[cc].collider_id=col_obj->get_instance_id(); + if (r_results[cc].collider_id!=0) + r_results[cc].collider=ObjectDB::get_instance(r_results[cc].collider_id); + r_results[cc].rid=col_obj->get_self(); + r_results[cc].shape=shape_idx; + + cc++; + + } + + return cc; + +} + + +struct MotionCallbackRayCastData { + + Vector2 best_contact; + Vector2 best_normal; + float best_len; + Matrix32 b_xform_inv; + Matrix32 b_xform; + Vector2 motion; + Shape2DSW * shape_B; + +}; + +static void _motion_cbk_result(const Vector2& p_point_A,const Vector2& p_point_B,void *p_userdata) { + + + MotionCallbackRayCastData *rd=(MotionCallbackRayCastData*)p_userdata; + + Vector2 contact_normal = (p_point_B-p_point_A).normalized(); + + Vector2 from=p_point_A-(rd->motion*1.01); + Vector2 p,n; + + if (contact_normal.dot(rd->motion.normalized())motion; //avoid precission issues + + + bool res = rd->shape_B->intersect_segment(rd->b_xform_inv.xform(from),rd->b_xform_inv.xform(to),p,n); + + + if (!res) { + print_line("lolwut failed"); + return; + } + + p = rd->b_xform.xform(p); + + n = rd->b_xform_inv.basis_xform_inv(n).normalized(); + } + + float len = p.distance_to(from); + + if (lenbest_len) { + rd->best_contact=p; + rd->best_normal=n; + rd->best_len=len; + } +} + +bool Physics2DDirectSpaceStateSW::cast_motion(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion, MotionCastCollision &r_result, const Set& p_exclude,uint32_t p_user_mask) { + + Shape2DSW *shape = static_cast(Physics2DServer::get_singleton())->shape_owner.get(p_shape); + ERR_FAIL_COND_V(!shape,0); + + Rect2 aabb = p_xform.xform(shape->get_aabb()); + aabb=aabb.merge(Rect2(aabb.pos+p_motion,aabb.size)); //motion + + 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.travel=1; + + MotionCallbackRayCastData best_normal; + best_normal.best_len=1e20; + for(int i=0;iintersection_query_results[i]->get_type()==CollisionObject2DSW::TYPE_AREA) + continue; //ignore area + + 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]; + + + 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(shape,p_xform,p_motion,col_obj->get_shape(shape_idx),col_obj_xform,Vector2() ,NULL,NULL,NULL)) { + + continue; + } + + + //test initial overlap + if (CollisionSolver2DSW::solve(shape,p_xform,Vector2(),col_obj->get_shape(shape_idx),col_obj_xform,Vector2() ,NULL,NULL,NULL)) { + + r_result.collider_id=col_obj->get_instance_id(); + r_result.collider=r_result.collider_id!=0 ? ObjectDB::get_instance(col_obj->get_instance_id()) : NULL; + r_result.shape=shape_idx; + r_result.rid=col_obj->get_self(); + r_result.travel=0; + r_result.point=Vector2(); + r_result.normal=Vector2(); + return true; + } + +#if 0 + Vector2 mnormal=p_motion.normalized(); + Matrix32 col_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); + ShapeSW *col_shape = col_obj->get_shape(shape_idx); + + real_t min,max; + col_shape->project_rangev(mnormal,col_shape_xform,min,max); + real_t width = max-min; + + int a; + Vector2 s[2]; + col_shape->get_supports(col_shape_xform.basis_xform(mnormal).normalized(),s,a); + Vector2 from = col_shape_xform.xform(s[0]); + Vector2 to = from + p_motion; + + Matrix32 from_inv = col_shape_xform.affine_inverse(); + + Vector2 local_from = from_inv.xform(from-mnormal*width*0.1); //start from a little inside the bounding box + Vector2 local_to = from_inv.xform(to); + + Vector2 rpos,rnorm; + if (!col_shape->intersect_segment(local_from,local_to,rpos,rnorm)) + return false; + + //ray hit something + + + Vector2 hitpos = p_xform_B.xform(rpos); +#endif + + //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(shape,p_xform,p_motion*ofs,col_obj->get_shape(shape_idx),col_obj_xform,Vector2(),NULL,NULL,&sep); + + if (collided) { + + hi=ofs; + } else { + + low=ofs; + } + } + + + best_normal.shape_B=col_obj->get_shape(shape_idx); + best_normal.motion=p_motion*hi; + best_normal.b_xform=col_obj_xform; + best_normal.b_xform_inv=col_obj_xform.affine_inverse(); + + bool sc = CollisionSolver2DSW::solve(shape,p_xform,p_motion*hi,col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx),Vector2() ,_motion_cbk_result,&best_normal); + print_line("CLD: "+itos(sc)); + + + if (collided && low>=r_result.travel) + continue; + + collided=true; + r_result.travel=low; + + r_result.collider_id=col_obj->get_instance_id(); + r_result.collider=r_result.collider_id!=0 ? ObjectDB::get_instance(col_obj->get_instance_id()) : NULL; + r_result.shape=shape_idx; + r_result.rid=col_obj->get_self(); + + } + + if (collided) { + ERR_FAIL_COND_V(best_normal.best_normal==Vector2(),false); + r_result.normal=best_normal.best_normal; + r_result.point=best_normal.best_contact; + } + + return collided; + + +} + + +Physics2DDirectSpaceStateSW::Physics2DDirectSpaceStateSW() { + + + space=NULL; +} + + +//////////////////////////////////////////////////////////////////////////////////////////////////////////// + + + + + + + + + + +void* Space2DSW::_broadphase_pair(CollisionObject2DSW *A,int p_subindex_A,CollisionObject2DSW *B,int p_subindex_B,void *p_self) { + + CollisionObject2DSW::Type type_A=A->get_type(); + CollisionObject2DSW::Type type_B=B->get_type(); + if (type_A>type_B) { + + SWAP(A,B); + SWAP(p_subindex_A,p_subindex_B); + SWAP(type_A,type_B); + } + + Space2DSW *self = (Space2DSW*)p_self; + + if (type_A==CollisionObject2DSW::TYPE_AREA) { + + + ERR_FAIL_COND_V(type_B!=CollisionObject2DSW::TYPE_BODY,NULL); + Area2DSW *area=static_cast(A); + Body2DSW *body=static_cast(B); + + AreaPair2DSW *area_pair = memnew(AreaPair2DSW(body,p_subindex_B,area,p_subindex_A) ); + + return area_pair; + } else { + + + BodyPair2DSW *b = memnew( BodyPair2DSW((Body2DSW*)A,p_subindex_A,(Body2DSW*)B,p_subindex_B) ); + return b; + + } + + return NULL; +} + +void Space2DSW::_broadphase_unpair(CollisionObject2DSW *A,int p_subindex_A,CollisionObject2DSW *B,int p_subindex_B,void *p_data,void *p_self) { + + + + Space2DSW *self = (Space2DSW*)p_self; + Constraint2DSW *c = (Constraint2DSW*)p_data; + memdelete(c); +} + + +const SelfList::List& Space2DSW::get_active_body_list() const { + + return active_list; +} +void Space2DSW::body_add_to_active_list(SelfList* p_body) { + + active_list.add(p_body); +} +void Space2DSW::body_remove_from_active_list(SelfList* p_body) { + + active_list.remove(p_body); + +} + +void Space2DSW::body_add_to_inertia_update_list(SelfList* p_body) { + + + inertia_update_list.add(p_body); +} + +void Space2DSW::body_remove_from_inertia_update_list(SelfList* p_body) { + + inertia_update_list.remove(p_body); +} + +BroadPhase2DSW *Space2DSW::get_broadphase() { + + return broadphase; +} + +void Space2DSW::add_object(CollisionObject2DSW *p_object) { + + ERR_FAIL_COND( objects.has(p_object) ); + objects.insert(p_object); +} + +void Space2DSW::remove_object(CollisionObject2DSW *p_object) { + + ERR_FAIL_COND( !objects.has(p_object) ); + objects.erase(p_object); +} + +const Set &Space2DSW::get_objects() const { + + return objects; +} + +void Space2DSW::body_add_to_state_query_list(SelfList* p_body) { + + state_query_list.add(p_body); +} +void Space2DSW::body_remove_from_state_query_list(SelfList* p_body) { + + state_query_list.remove(p_body); +} + +void Space2DSW::area_add_to_monitor_query_list(SelfList* p_area) { + + monitor_query_list.add(p_area); +} +void Space2DSW::area_remove_from_monitor_query_list(SelfList* p_area) { + + monitor_query_list.remove(p_area); +} + +void Space2DSW::area_add_to_moved_list(SelfList* p_area) { + + area_moved_list.add(p_area); +} + +void Space2DSW::area_remove_from_moved_list(SelfList* p_area) { + + area_moved_list.remove(p_area); +} + +const SelfList::List& Space2DSW::get_moved_area_list() const { + + return area_moved_list; +} + + +void Space2DSW::call_queries() { + + while(state_query_list.first()) { + + Body2DSW * b = state_query_list.first()->self(); + b->call_queries(); + state_query_list.remove(state_query_list.first()); + } + + while(monitor_query_list.first()) { + + Area2DSW * a = monitor_query_list.first()->self(); + a->call_queries(); + monitor_query_list.remove(monitor_query_list.first()); + } + +} + +void Space2DSW::setup() { + + + while(inertia_update_list.first()) { + inertia_update_list.first()->self()->update_inertias(); + inertia_update_list.remove(inertia_update_list.first()); + } + + +} + +void Space2DSW::update() { + + broadphase->update(); + +} + + +void Space2DSW::set_param(Physics2DServer::SpaceParameter p_param, real_t p_value) { + + switch(p_param) { + + case Physics2DServer::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: contact_recycle_radius=p_value; break; + case Physics2DServer::SPACE_PARAM_CONTACT_MAX_SEPARATION: contact_max_separation=p_value; break; + case Physics2DServer::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: contact_max_allowed_penetration=p_value; break; + case Physics2DServer::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_TRESHOLD: body_linear_velocity_sleep_treshold=p_value; break; + case Physics2DServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_TRESHOLD: body_angular_velocity_sleep_treshold=p_value; break; + case Physics2DServer::SPACE_PARAM_BODY_TIME_TO_SLEEP: body_time_to_sleep=p_value; break; + case Physics2DServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO: body_angular_velocity_damp_ratio=p_value; break; + case Physics2DServer::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: constraint_bias=p_value; break; + } +} + +real_t Space2DSW::get_param(Physics2DServer::SpaceParameter p_param) const { + + switch(p_param) { + + case Physics2DServer::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: return contact_recycle_radius; + case Physics2DServer::SPACE_PARAM_CONTACT_MAX_SEPARATION: return contact_max_separation; + case Physics2DServer::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: return contact_max_allowed_penetration; + case Physics2DServer::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_TRESHOLD: return body_linear_velocity_sleep_treshold; + case Physics2DServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_TRESHOLD: return body_angular_velocity_sleep_treshold; + case Physics2DServer::SPACE_PARAM_BODY_TIME_TO_SLEEP: return body_time_to_sleep; + case Physics2DServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO: return body_angular_velocity_damp_ratio; + case Physics2DServer::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: return constraint_bias; + } + return 0; +} + +void Space2DSW::lock() { + + locked=true; +} + +void Space2DSW::unlock() { + + locked=false; +} + +bool Space2DSW::is_locked() const { + + return locked; +} + +Physics2DDirectSpaceStateSW *Space2DSW::get_direct_state() { + + return direct_access; +} + +Space2DSW::Space2DSW() { + + + locked=false; + contact_recycle_radius=0.01; + contact_max_separation=0.05; + contact_max_allowed_penetration= 0.01; + + constraint_bias = 0.01; + body_linear_velocity_sleep_treshold=0.01; + body_angular_velocity_sleep_treshold=(8.0 / 180.0 * Math_PI); + body_time_to_sleep=0.5; + body_angular_velocity_damp_ratio=15; + + + broadphase = BroadPhase2DSW::create_func(); + broadphase->set_pair_callback(_broadphase_pair,this); + broadphase->set_unpair_callback(_broadphase_unpair,this); + area=NULL; + + direct_access = memnew( Physics2DDirectSpaceStateSW ); + direct_access->space=this; +} + +Space2DSW::~Space2DSW() { + + memdelete(broadphase); + memdelete( direct_access ); +} + + + diff --git a/servers/physics_2d/space_2d_sw.h b/servers/physics_2d/space_2d_sw.h index f65ec14427..978e88479d 100644 --- a/servers/physics_2d/space_2d_sw.h +++ b/servers/physics_2d/space_2d_sw.h @@ -26,135 +26,136 @@ /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ -#ifndef SPACE_2D_SW_H -#define SPACE_2D_SW_H - -#include "typedefs.h" -#include "hash_map.h" -#include "body_2d_sw.h" -#include "area_2d_sw.h" -#include "body_pair_2d_sw.h" -#include "area_pair_2d_sw.h" -#include "broad_phase_2d_sw.h" -#include "collision_object_2d_sw.h" - - -class Physics2DDirectSpaceStateSW : public Physics2DDirectSpaceState { - - OBJ_TYPE( Physics2DDirectSpaceStateSW, Physics2DDirectSpaceState ); -public: - - Space2DSW *space; - - bool intersect_ray(const Vector2& p_from, const Vector2& p_to,RayResult &r_result,const Set& p_exclude=Set(),uint32_t p_user_mask=0); - int intersect_shape(const RID& p_shape, const Matrix32& p_xform,ShapeResult *r_results,int p_result_max,const Set& p_exclude=Set(),uint32_t p_user_mask=0); - - Physics2DDirectSpaceStateSW(); -}; - - - -class Space2DSW { - - - Physics2DDirectSpaceStateSW *direct_access; - RID self; - - BroadPhase2DSW *broadphase; - SelfList::List active_list; - SelfList::List inertia_update_list; - SelfList::List state_query_list; - SelfList::List monitor_query_list; - SelfList::List area_moved_list; - - static void* _broadphase_pair(CollisionObject2DSW *A,int p_subindex_A,CollisionObject2DSW *B,int p_subindex_B,void *p_self); - static void _broadphase_unpair(CollisionObject2DSW *A,int p_subindex_A,CollisionObject2DSW *B,int p_subindex_B,void *p_data,void *p_self); - - Set objects; - - Area2DSW *area; - - real_t contact_recycle_radius; - real_t contact_max_separation; - real_t contact_max_allowed_penetration; - real_t constraint_bias; - - enum { - - INTERSECTION_QUERY_MAX=2048 - }; - - CollisionObject2DSW *intersection_query_results[INTERSECTION_QUERY_MAX]; - int intersection_query_subindex_results[INTERSECTION_QUERY_MAX]; - - float body_linear_velocity_sleep_treshold; - float body_angular_velocity_sleep_treshold; - float body_time_to_sleep; - float body_angular_velocity_damp_ratio; - - bool locked; - -friend class Physics2DDirectSpaceStateSW; - -public: - - _FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; } - _FORCE_INLINE_ RID get_self() const { return self; } - - void set_default_area(Area2DSW *p_area) { area=p_area; } - Area2DSW *get_default_area() const { return area; } - - const SelfList::List& get_active_body_list() const; - void body_add_to_active_list(SelfList* p_body); - void body_remove_from_active_list(SelfList* p_body); - void body_add_to_inertia_update_list(SelfList* p_body); - void body_remove_from_inertia_update_list(SelfList* p_body); - void area_add_to_moved_list(SelfList* p_area); - void area_remove_from_moved_list(SelfList* p_area); - const SelfList::List& get_moved_area_list() const; - - - - - void body_add_to_state_query_list(SelfList* p_body); - void body_remove_from_state_query_list(SelfList* p_body); - - void area_add_to_monitor_query_list(SelfList* p_area); - void area_remove_from_monitor_query_list(SelfList* p_area); - - BroadPhase2DSW *get_broadphase(); - - void add_object(CollisionObject2DSW *p_object); - void remove_object(CollisionObject2DSW *p_object); - const Set &get_objects() const; - - _FORCE_INLINE_ real_t get_contact_recycle_radius() const { return contact_recycle_radius; } - _FORCE_INLINE_ real_t get_contact_max_separation() const { return contact_max_separation; } - _FORCE_INLINE_ real_t get_contact_max_allowed_penetration() const { return contact_max_allowed_penetration; } - _FORCE_INLINE_ real_t get_constraint_bias() const { return constraint_bias; } - _FORCE_INLINE_ real_t get_body_linear_velocity_sleep_treshold() const { return body_linear_velocity_sleep_treshold; } - _FORCE_INLINE_ real_t get_body_angular_velocity_sleep_treshold() const { return body_angular_velocity_sleep_treshold; } - _FORCE_INLINE_ real_t get_body_time_to_sleep() const { return body_time_to_sleep; } - _FORCE_INLINE_ real_t get_body_angular_velocity_damp_ratio() const { return body_angular_velocity_damp_ratio; } - - - void update(); - void setup(); - void call_queries(); - - - bool is_locked() const; - void lock(); - void unlock(); - - void set_param(Physics2DServer::SpaceParameter p_param, real_t p_value); - real_t get_param(Physics2DServer::SpaceParameter p_param) const; - - Physics2DDirectSpaceStateSW *get_direct_state(); - - Space2DSW(); - ~Space2DSW(); -}; - - -#endif // SPACE_2D_SW_H +#ifndef SPACE_2D_SW_H +#define SPACE_2D_SW_H + +#include "typedefs.h" +#include "hash_map.h" +#include "body_2d_sw.h" +#include "area_2d_sw.h" +#include "body_pair_2d_sw.h" +#include "area_pair_2d_sw.h" +#include "broad_phase_2d_sw.h" +#include "collision_object_2d_sw.h" + + +class Physics2DDirectSpaceStateSW : public Physics2DDirectSpaceState { + + OBJ_TYPE( Physics2DDirectSpaceStateSW, Physics2DDirectSpaceState ); +public: + + Space2DSW *space; + + bool intersect_ray(const Vector2& p_from, const Vector2& p_to,RayResult &r_result,const Set& p_exclude=Set(),uint32_t p_user_mask=0); + int intersect_shape(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,ShapeResult *r_results,int p_result_max,const Set& p_exclude=Set(),uint32_t p_user_mask=0); + bool cast_motion(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion, MotionCastCollision &r_result, const Set& p_exclude=Set(),uint32_t p_user_mask=0); + + Physics2DDirectSpaceStateSW(); +}; + + + +class Space2DSW { + + + Physics2DDirectSpaceStateSW *direct_access; + RID self; + + BroadPhase2DSW *broadphase; + SelfList::List active_list; + SelfList::List inertia_update_list; + SelfList::List state_query_list; + SelfList::List monitor_query_list; + SelfList::List area_moved_list; + + static void* _broadphase_pair(CollisionObject2DSW *A,int p_subindex_A,CollisionObject2DSW *B,int p_subindex_B,void *p_self); + static void _broadphase_unpair(CollisionObject2DSW *A,int p_subindex_A,CollisionObject2DSW *B,int p_subindex_B,void *p_data,void *p_self); + + Set objects; + + Area2DSW *area; + + real_t contact_recycle_radius; + real_t contact_max_separation; + real_t contact_max_allowed_penetration; + real_t constraint_bias; + + enum { + + INTERSECTION_QUERY_MAX=2048 + }; + + CollisionObject2DSW *intersection_query_results[INTERSECTION_QUERY_MAX]; + int intersection_query_subindex_results[INTERSECTION_QUERY_MAX]; + + float body_linear_velocity_sleep_treshold; + float body_angular_velocity_sleep_treshold; + float body_time_to_sleep; + float body_angular_velocity_damp_ratio; + + bool locked; + +friend class Physics2DDirectSpaceStateSW; + +public: + + _FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; } + _FORCE_INLINE_ RID get_self() const { return self; } + + void set_default_area(Area2DSW *p_area) { area=p_area; } + Area2DSW *get_default_area() const { return area; } + + const SelfList::List& get_active_body_list() const; + void body_add_to_active_list(SelfList* p_body); + void body_remove_from_active_list(SelfList* p_body); + void body_add_to_inertia_update_list(SelfList* p_body); + void body_remove_from_inertia_update_list(SelfList* p_body); + void area_add_to_moved_list(SelfList* p_area); + void area_remove_from_moved_list(SelfList* p_area); + const SelfList::List& get_moved_area_list() const; + + + + + void body_add_to_state_query_list(SelfList* p_body); + void body_remove_from_state_query_list(SelfList* p_body); + + void area_add_to_monitor_query_list(SelfList* p_area); + void area_remove_from_monitor_query_list(SelfList* p_area); + + BroadPhase2DSW *get_broadphase(); + + void add_object(CollisionObject2DSW *p_object); + void remove_object(CollisionObject2DSW *p_object); + const Set &get_objects() const; + + _FORCE_INLINE_ real_t get_contact_recycle_radius() const { return contact_recycle_radius; } + _FORCE_INLINE_ real_t get_contact_max_separation() const { return contact_max_separation; } + _FORCE_INLINE_ real_t get_contact_max_allowed_penetration() const { return contact_max_allowed_penetration; } + _FORCE_INLINE_ real_t get_constraint_bias() const { return constraint_bias; } + _FORCE_INLINE_ real_t get_body_linear_velocity_sleep_treshold() const { return body_linear_velocity_sleep_treshold; } + _FORCE_INLINE_ real_t get_body_angular_velocity_sleep_treshold() const { return body_angular_velocity_sleep_treshold; } + _FORCE_INLINE_ real_t get_body_time_to_sleep() const { return body_time_to_sleep; } + _FORCE_INLINE_ real_t get_body_angular_velocity_damp_ratio() const { return body_angular_velocity_damp_ratio; } + + + void update(); + void setup(); + void call_queries(); + + + bool is_locked() const; + void lock(); + void unlock(); + + void set_param(Physics2DServer::SpaceParameter p_param, real_t p_value); + real_t get_param(Physics2DServer::SpaceParameter p_param) const; + + Physics2DDirectSpaceStateSW *get_direct_state(); + + Space2DSW(); + ~Space2DSW(); +}; + + +#endif // SPACE_2D_SW_H diff --git a/servers/physics_2d/step_2d_sw.cpp b/servers/physics_2d/step_2d_sw.cpp index 9f41fc94eb..29f4a58287 100644 --- a/servers/physics_2d/step_2d_sw.cpp +++ b/servers/physics_2d/step_2d_sw.cpp @@ -49,7 +49,7 @@ void Step2DSW::_populate_island(Body2DSW* p_body,Body2DSW** p_island,Constraint2 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) + 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); } @@ -87,8 +87,10 @@ void Step2DSW::_check_suspend(Body2DSW *p_island,float p_delta) { Body2DSW *b = p_island; while(b) { - if (b->get_mode()==Physics2DServer::BODY_MODE_STATIC) + 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; @@ -101,8 +103,10 @@ void Step2DSW::_check_suspend(Body2DSW *p_island,float p_delta) { b = p_island; while(b) { - if (b->get_mode()==Physics2DServer::BODY_MODE_STATIC) + 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(); @@ -210,8 +214,9 @@ void Step2DSW::step(Space2DSW* p_space,float p_delta,int p_iterations) { b = body_list->first(); while(b) { + const SelfList*n=b->next(); b->self()->integrate_velocities(p_delta); - b=b->next(); + b=n; // in case it shuts itself down } /* SLEEP / WAKE UP ISLANDS */ diff --git a/servers/physics_2d_server.cpp b/servers/physics_2d_server.cpp index cae9565c46..bf07b8ea8c 100644 --- a/servers/physics_2d_server.cpp +++ b/servers/physics_2d_server.cpp @@ -122,7 +122,7 @@ Variant Physics2DDirectSpaceState::_intersect_ray(const Vector2& p_from, const V if (!res) return Variant(); - Dictionary d; + Dictionary d(true); d["position"]=inters.position; d["normal"]=inters.normal; d["collider_id"]=inters.collider_id; @@ -145,7 +145,7 @@ Variant Physics2DDirectSpaceState::_intersect_shape(const RID& p_shape, const Ma ShapeResult *res=(ShapeResult*)alloca(p_result_max*sizeof(ShapeResult)); - int rc = intersect_shape(p_shape,p_xform,res,p_result_max,exclude,p_user_mask); + int rc = intersect_shape(p_shape,p_xform,Vector2(),res,p_result_max,exclude,p_user_mask); if (rc==0) return Variant(); @@ -160,6 +160,34 @@ Variant Physics2DDirectSpaceState::_intersect_shape(const RID& p_shape, const Ma } +Variant Physics2DDirectSpaceState::_cast_motion(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,const Vector& p_exclude,uint32_t p_user_mask) { + + + Set exclude; + for(int i=0;i& p_exclude=Vector(),uint32_t p_user_mask=0); Variant _intersect_shape(const RID& p_shape, const Matrix32& p_xform,int p_result_max=64,const Vector& p_exclude=Vector(),uint32_t p_user_mask=0); + Variant _cast_motion(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,const Vector& p_exclude=Vector(),uint32_t p_user_mask=0); protected: @@ -118,7 +119,26 @@ public: }; - virtual int intersect_shape(const RID& p_shape, const Matrix32& p_xform,ShapeResult *r_results,int p_result_max,const Set& p_exclude=Set(),uint32_t p_user_mask=0)=0; + virtual int intersect_shape(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,ShapeResult *r_results,int p_result_max,const Set& p_exclude=Set(),uint32_t p_user_mask=0)=0; + + + + struct MotionCastCollision { + + float travel; //0 to 1, if 0 then it's blocked + Vector2 point; + Vector2 normal; + RID rid; + ObjectID collider_id; + Object *collider; + int shape; + + }; + + virtual bool cast_motion(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion, MotionCastCollision &r_result, const Set& p_exclude=Set(),uint32_t p_user_mask=0)=0; + + + Physics2DDirectSpaceState(); }; @@ -179,6 +199,8 @@ public: 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 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)=0; /* SPACE API */ @@ -265,10 +287,10 @@ public: enum BodyMode { BODY_MODE_STATIC, - BODY_MODE_STATIC_ACTIVE, + BODY_MODE_KINEMATIC, BODY_MODE_RIGID, - //BODY_MODE_SOFT BODY_MODE_CHARACTER + //BODY_MODE_SOFT ?? }; virtual RID body_create(BodyMode p_mode=BODY_MODE_RIGID,bool p_init_sleeping=false)=0; @@ -277,7 +299,7 @@ public: 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, BodyMode p_mode) const=0; + virtual BodyMode body_get_mode(RID p_body) const=0; virtual void body_add_shape(RID p_body, RID p_shape, const Matrix32& p_transform=Matrix32())=0; virtual void body_set_shape(RID p_body, int p_shape_idx,RID p_shape)=0; @@ -296,8 +318,14 @@ public: 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; + enum CCDMode { + CCD_MODE_DISABLED, + CCD_MODE_CAST_RAY, + 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_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; @@ -313,8 +341,6 @@ public: 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; - //advanced simulation - virtual void body_static_simulate_motion(RID p_body,const Matrix32& p_new_transform)=0; //state enum BodyState { @@ -355,6 +381,8 @@ public: 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 Matrix32& p_shape_xform,const Vector2& p_motion,Vector2 *r_results,int p_result_max,int &r_result_count)=0; + /* JOINT API */ enum JointType { @@ -417,6 +445,7 @@ 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 ); diff --git a/servers/physics_server.cpp b/servers/physics_server.cpp index 69a2adae77..f1b4627b6c 100644 --- a/servers/physics_server.cpp +++ b/servers/physics_server.cpp @@ -374,7 +374,7 @@ void PhysicsServer::_bind_methods() { BIND_CONSTANT( AREA_SPACE_OVERRIDE_REPLACE ); BIND_CONSTANT( BODY_MODE_STATIC ); - BIND_CONSTANT( BODY_MODE_STATIC_ACTIVE ); + BIND_CONSTANT( BODY_MODE_KINEMATIC ); BIND_CONSTANT( BODY_MODE_RIGID ); BIND_CONSTANT( BODY_MODE_CHARACTER ); diff --git a/servers/physics_server.h b/servers/physics_server.h index 1fe477adc3..4276a4dab8 100644 --- a/servers/physics_server.h +++ b/servers/physics_server.h @@ -268,7 +268,7 @@ public: enum BodyMode { BODY_MODE_STATIC, - BODY_MODE_STATIC_ACTIVE, + BODY_MODE_KINEMATIC, BODY_MODE_RIGID, //BODY_MODE_SOFT BODY_MODE_CHARACTER -- cgit v1.2.3