diff options
Diffstat (limited to 'servers')
28 files changed, 4551 insertions, 3717 deletions
diff --git a/servers/SCsub b/servers/SCsub index 3871c30cfa..1a858533b7 100644 --- a/servers/SCsub +++ b/servers/SCsub @@ -12,7 +12,7 @@ SConscript('audio/SCsub'); SConscript('spatial_sound/SCsub'); SConscript('spatial_sound_2d/SCsub'); -lib = env.Library("servers",env.servers_sources) +lib = env.Library("servers",env.servers_sources, LIBSUFFIX=env['platform_libsuffix']) env.Prepend(LIBS=[lib]) 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;i<get_shape_count();i++) {
-
- total_area+=get_shape_aabb(i).get_area();
- }
-
- Vector3 _inertia;
-
-
- for (int i=0;i<get_shape_count();i++) {
-
- const ShapeSW* shape=get_shape(i);
-
- float area=get_shape_aabb(i).get_area();
-
- float mass = area * this->mass / total_area;
-
- _inertia += shape->get_moment_of_inertia(mass) + mass * get_shape_transform(i).get_origin();
-
- }
-
- 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;i<get_shape_count();i++) {
- Shape &s=shapes[i];
- if (s.bpid>0) {
- 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;i<ac;i++) {
- if (aa[i].area->get_priority() > prio) {
- current_area=aa[i].area;
- prio=current_area->get_priority();
- }
- }
- }
-
- _compute_area_gravity(current_area);
- density=current_area->get_density();
-
- if (!omit_force_integration) {
- //overriden by direct state query
-
- 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<ConstraintSW*,int>::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;i<bc;i++) {
-
- if (i==E->get())
- 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_space()->get_body_angular_velocity_sleep_treshold() && Math::abs(linear_velocity.length_squared()) < get_space()->get_body_linear_velocity_sleep_treshold()*get_space()->get_body_linear_velocity_sleep_treshold()) {
-
- still_time+=p_step;
-
- return still_time > get_space()->get_body_time_to_sleep();
- } else {
-
- still_time=0; //maybe this should be set to 0 on set_active?
- return false;
- }
-}
-
-
-void 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;i<get_shape_count();i++) { + + total_area+=get_shape_aabb(i).get_area(); + } + + Vector3 _inertia; + + + for (int i=0;i<get_shape_count();i++) { + + const ShapeSW* shape=get_shape(i); + + float area=get_shape_aabb(i).get_area(); + + float mass = area * this->mass / total_area; + + _inertia += shape->get_moment_of_inertia(mass) + mass * get_shape_transform(i).get_origin(); + + } + + 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;i<get_shape_count();i++) { + Shape &s=shapes[i]; + if (s.bpid>0) { + 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;i<ac;i++) { + if (aa[i].area->get_priority() > prio) { + current_area=aa[i].area; + prio=current_area->get_priority(); + } + } + } + + _compute_area_gravity(current_area); + density=current_area->get_density(); + + if (!omit_force_integration) { + //overriden by direct state query + + 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<ConstraintSW*,int>::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;i<bc;i++) { + + if (i==E->get()) + 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_space()->get_body_angular_velocity_sleep_treshold() && Math::abs(linear_velocity.length_squared()) < get_space()->get_body_linear_velocity_sleep_treshold()*get_space()->get_body_linear_velocity_sleep_treshold()) { + + still_time+=p_step; + + return still_time > get_space()->get_body_time_to_sleep(); + } else { + + still_time=0; //maybe this should be set to 0 on set_active? + return false; + } +} + + +void 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;i<get_shape_count();i++) {
-
- total_area+=get_shape_aabb(i).get_area();
- }
-
- real_t _inertia=0;
-
- for (int i=0;i<get_shape_count();i++) {
-
- const Shape2DSW* shape=get_shape(i);
-
- float area=get_shape_aabb(i).get_area();
-
- float mass = area * this->mass / total_area;
-
- _inertia += shape->get_moment_of_inertia(mass) + mass * get_shape_transform(i).get_origin().length_squared();
-
- }
-
- if (_inertia!=0)
- _inv_inertia=1.0/_inertia;
- else
- _inv_inertia=0.0; //wathever
-
- if (mass)
- _inv_mass=1.0/mass;
- else
- _inv_mass=0;
-
- } break;
- case Physics2DServer::BODY_MODE_STATIC_ACTIVE:
- case Physics2DServer::BODY_MODE_STATIC: {
-
- _inv_inertia=0;
- _inv_mass=0;
- } break;
- case Physics2DServer::BODY_MODE_CHARACTER: {
-
- _inv_inertia=0;
- _inv_mass=1.0/mass;
-
- } break;
- }
- //_update_inertia_tensor();
-
- //_update_shapes();
-
-}
-
-
-
-void Body2DSW::set_active(bool p_active) {
-
- if (active==p_active)
- return;
-
- active=p_active;
- if (!p_active) {
- if (get_space())
- get_space()->body_remove_from_active_list(&active_list);
- } else {
- if (mode==Physics2DServer::BODY_MODE_STATIC)
- return; //static bodies can't become active
- if (get_space())
- get_space()->body_add_to_active_list(&active_list);
-
- //still_time=0;
- }
-/*
- if (!space)
- return;
-
- for(int i=0;i<get_shape_count();i++) {
- Shape &s=shapes[i];
- if (s.bpid>0) {
- get_space()->get_broadphase()->set_active(s.bpid,active);
- }
- }
-*/
-}
-
-
-
-void Body2DSW::set_param(Physics2DServer::BodyParameter p_param, float p_value) {
-
- switch(p_param) {
- case Physics2DServer::BODY_PARAM_BOUNCE: {
-
- bounce=p_value;
- } break;
- case Physics2DServer::BODY_PARAM_FRICTION: {
-
- friction=p_value;
- } break;
- case Physics2DServer::BODY_PARAM_MASS: {
- ERR_FAIL_COND(p_value<=0);
- mass=p_value;
- _update_inertia();
-
- } break;
- default:{}
- }
-}
-
-float Body2DSW::get_param(Physics2DServer::BodyParameter p_param) const {
-
- switch(p_param) {
- case Physics2DServer::BODY_PARAM_BOUNCE: {
-
- return bounce;
- } break;
- case Physics2DServer::BODY_PARAM_FRICTION: {
-
- return friction;
- } break;
- case Physics2DServer::BODY_PARAM_MASS: {
- return mass;
- } break;
- default:{}
- }
-
- return 0;
-}
-
-void Body2DSW::set_mode(Physics2DServer::BodyMode p_mode) {
-
- mode=p_mode;
-
- switch(p_mode) {
- //CLEAR UP EVERYTHING IN CASE IT NOT WORKS!
- case Physics2DServer::BODY_MODE_STATIC:
- case Physics2DServer::BODY_MODE_STATIC_ACTIVE: {
-
- _set_inv_transform(get_transform().affine_inverse());
- _inv_mass=0;
- _set_static(p_mode==Physics2DServer::BODY_MODE_STATIC);
- set_active(p_mode==Physics2DServer::BODY_MODE_STATIC_ACTIVE);
- linear_velocity=Vector2();
- angular_velocity=0;
- } break;
- case Physics2DServer::BODY_MODE_RIGID: {
-
- _inv_mass=mass>0?(1.0/mass):0;
- _set_static(false);
- simulated_motion=false; //jic
-
- } break;
- case Physics2DServer::BODY_MODE_CHARACTER: {
-
- _inv_mass=mass>0?(1.0/mass):0;
- _set_static(false);
- simulated_motion=false; //jic
- } break;
- }
-
- _update_inertia();
- //if (get_space())
-// _update_queries();
-
-}
-Physics2DServer::BodyMode Body2DSW::get_mode() const {
-
- return mode;
-}
-
-void Body2DSW::_shapes_changed() {
-
- _update_inertia();
- wakeup_neighbours();
-}
-
-void Body2DSW::set_state(Physics2DServer::BodyState p_state, const Variant& p_variant) {
-
- switch(p_state) {
- case Physics2DServer::BODY_STATE_TRANSFORM: {
-
-
- if (mode==Physics2DServer::BODY_MODE_STATIC || mode==Physics2DServer::BODY_MODE_STATIC_ACTIVE) {
- _set_transform(p_variant);
- _set_inv_transform(get_transform().affine_inverse());
- wakeup_neighbours();
- } else {
- Matrix32 t = p_variant;
- t.orthonormalize();
- _set_transform(t);
- _set_inv_transform(get_transform().inverse());
-
- }
-
- } break;
- case Physics2DServer::BODY_STATE_LINEAR_VELOCITY: {
-
- //if (mode==Physics2DServer::BODY_MODE_STATIC)
- // break;
- linear_velocity=p_variant;
-
- } break;
- case Physics2DServer::BODY_STATE_ANGULAR_VELOCITY: {
- //if (mode!=Physics2DServer::BODY_MODE_RIGID)
- // break;
- angular_velocity=p_variant;
-
- } break;
- case Physics2DServer::BODY_STATE_SLEEPING: {
- //?
- if (mode==Physics2DServer::BODY_MODE_STATIC || mode==Physics2DServer::BODY_MODE_STATIC_ACTIVE)
- break;
- bool do_sleep=p_variant;
- if (do_sleep) {
- linear_velocity=Vector2();
- //biased_linear_velocity=Vector3();
- angular_velocity=0;
- //biased_angular_velocity=Vector3();
- set_active(false);
- } else {
- if (mode!=Physics2DServer::BODY_MODE_STATIC)
- set_active(true);
- }
- } break;
- case Physics2DServer::BODY_STATE_CAN_SLEEP: {
- can_sleep=p_variant;
- if (mode==Physics2DServer::BODY_MODE_RIGID && !active && !can_sleep)
- set_active(true);
-
- } break;
- }
-
-}
-Variant Body2DSW::get_state(Physics2DServer::BodyState p_state) const {
-
- switch(p_state) {
- case Physics2DServer::BODY_STATE_TRANSFORM: {
- return get_transform();
- } break;
- case Physics2DServer::BODY_STATE_LINEAR_VELOCITY: {
- return linear_velocity;
- } break;
- case Physics2DServer::BODY_STATE_ANGULAR_VELOCITY: {
- return angular_velocity;
- } break;
- case Physics2DServer::BODY_STATE_SLEEPING: {
- return !is_active();
- } break;
- case Physics2DServer::BODY_STATE_CAN_SLEEP: {
- return can_sleep;
- } break;
- }
-
- return Variant();
-}
-
-
-void Body2DSW::set_space(Space2DSW *p_space){
-
- if (get_space()) {
-
- wakeup_neighbours();
-
- if (inertia_update_list.in_list())
- get_space()->body_remove_from_inertia_update_list(&inertia_update_list);
- if (active_list.in_list())
- get_space()->body_remove_from_active_list(&active_list);
- if (direct_state_query_list.in_list())
- get_space()->body_remove_from_state_query_list(&direct_state_query_list);
-
- }
-
- _set_space(p_space);
-
- if (get_space()) {
-
- _update_inertia();
- if (active)
- get_space()->body_add_to_active_list(&active_list);
-// _update_queries();
- //if (is_active()) {
- // active=false;
- // set_active(true);
- //}
-
- }
-
-}
-
-void Body2DSW::_compute_area_gravity(const Area2DSW *p_area) {
-
- if (p_area->is_gravity_point()) {
-
- gravity = (p_area->get_transform().get_origin()+p_area->get_gravity_vector() - get_transform().get_origin()).normalized() * p_area->get_gravity();
-
- } else {
- gravity = p_area->get_gravity_vector() * p_area->get_gravity();
- }
-}
-
-void Body2DSW::integrate_forces(real_t p_step) {
-
- if (mode==Physics2DServer::BODY_MODE_STATIC || mode==Physics2DServer::BODY_MODE_STATIC_ACTIVE)
- return;
-
- Area2DSW *current_area = get_space()->get_default_area();
- ERR_FAIL_COND(!current_area);
-
- int prio = current_area->get_priority();
- int ac = areas.size();
- if (ac) {
- const AreaCMP *aa = &areas[0];
- for(int i=0;i<ac;i++) {
- if (aa[i].area->get_priority() > prio) {
- current_area=aa[i].area;
- prio=current_area->get_priority();
- }
- }
- }
-
- _compute_area_gravity(current_area);
- density=current_area->get_density();
-
- if (!omit_force_integration) {
- //overriden by direct state query
-
- Vector2 force=gravity*mass;
- force+=applied_force;
- real_t torque=applied_torque;
-
- real_t damp = 1.0 - p_step * density;
-
- if (damp<0) // reached zero in the given time
- damp=0;
-
- real_t angular_damp = 1.0 - p_step * density * get_space()->get_body_angular_velocity_damp_ratio();
-
- if (angular_damp<0) // reached zero in the given time
- angular_damp=0;
-
- linear_velocity*=damp;
- angular_velocity*=angular_damp;
-
- linear_velocity+=_inv_mass * force * p_step;
- angular_velocity+=_inv_inertia * torque * p_step;
- }
-
-
- //motion=linear_velocity*p_step;
-
- biased_angular_velocity=0;
- biased_linear_velocity=Vector2();
-
- if (continuous_cd) //shapes temporarily extend for raycast
- _update_shapes_with_motion(linear_velocity*p_step);
-
- current_area=NULL; // clear the area, so it is set in the next frame
- contact_count=0;
-
-}
-
-void Body2DSW::integrate_velocities(real_t p_step) {
-
- if (mode==Physics2DServer::BODY_MODE_STATIC)
- return;
- if (mode==Physics2DServer::BODY_MODE_STATIC_ACTIVE) {
- if (fi_callback)
- get_space()->body_add_to_state_query_list(&direct_state_query_list);
- return;
- }
-
- real_t total_angular_velocity = angular_velocity+biased_angular_velocity;
- Vector2 total_linear_velocity=linear_velocity+biased_linear_velocity;
-
- real_t angle = get_transform().get_rotation() - total_angular_velocity * p_step;
- Vector2 pos = get_transform().get_origin() + total_linear_velocity * p_step;
-
-
- _set_transform(Matrix32(angle,pos));
- _set_inv_transform(get_transform().inverse());
-
-
- if (fi_callback) {
-
- get_space()->body_add_to_state_query_list(&direct_state_query_list);
- }
-
- //_update_inertia_tensor();
-}
-
-
-void Body2DSW::simulate_motion(const Matrix32& p_xform,real_t p_step) {
-
- Matrix32 inv_xform = p_xform.affine_inverse();
- if (!get_space()) {
- _set_transform(p_xform);
- _set_inv_transform(inv_xform);
-
- return;
- }
-
-
-
- linear_velocity = (p_xform.elements[2] - get_transform().elements[2])/p_step;
-
- real_t rot = inv_xform.basis_xform(get_transform().elements[1]).atan2();
- angular_velocity = rot / p_step;
-
- if (!direct_state_query_list.in_list())// - callalways, so lv and av are cleared && (state_query || direct_state_query))
- get_space()->body_add_to_state_query_list(&direct_state_query_list);
- simulated_motion=true;
- _set_transform(p_xform);
-
-
-}
-
-void Body2DSW::wakeup_neighbours() {
-
-
-
- for(Map<Constraint2DSW*,int>::Element *E=constraint_map.front();E;E=E->next()) {
-
- const Constraint2DSW *c=E->key();
- Body2DSW **n = c->get_body_ptr();
- int bc=c->get_body_count();
-
- for(int i=0;i<bc;i++) {
-
- if (i==E->get())
- continue;
- Body2DSW *b = n[i];
- if (b->mode!=Physics2DServer::BODY_MODE_RIGID)
- continue;
-
- if (!b->is_active())
- b->set_active(true);
- }
- }
-}
-
-void Body2DSW::call_queries() {
-
-
- if (fi_callback) {
-
- Physics2DDirectBodyStateSW *dbs = Physics2DDirectBodyStateSW::singleton;
- dbs->body=this;
-
- Variant v=dbs;
- const Variant *vp[2]={&v,&fi_callback->callback_udata};
-
- Object *obj = ObjectDB::get_instance(fi_callback->id);
- if (!obj) {
-
- set_force_integration_callback(NULL,StringName());
- } else {
- Variant::CallError ce;
- if (fi_callback->callback_udata.get_type()) {
-
- obj->call(fi_callback->method,vp,2,ce);
-
- } else {
- obj->call(fi_callback->method,vp,1,ce);
- }
- }
-
-
- }
-
- if (simulated_motion) {
-
- // linear_velocity=Vector2();
- // angular_velocity=0;
- simulated_motion=false;
- }
-}
-
-
-bool Body2DSW::sleep_test(real_t p_step) {
-
- if (mode==Physics2DServer::BODY_MODE_STATIC || mode==Physics2DServer::BODY_MODE_STATIC_ACTIVE)
- return true; //
- else if (mode==Physics2DServer::BODY_MODE_CHARACTER)
- return !active; // characters don't sleep unless asked to sleep
- else if (!can_sleep)
- return false;
-
-
-
-
- if (Math::abs(angular_velocity)<get_space()->get_body_angular_velocity_sleep_treshold() && Math::abs(linear_velocity.length_squared()) < get_space()->get_body_linear_velocity_sleep_treshold()*get_space()->get_body_linear_velocity_sleep_treshold()) {
-
- still_time+=p_step;
-
- return still_time > get_space()->get_body_time_to_sleep();
- } else {
-
- still_time=0; //maybe this should be set to 0 on set_active?
- return false;
- }
-}
-
-
-void Body2DSW::set_force_integration_callback(ObjectID p_id,const StringName& p_method,const Variant& p_udata) {
-
- if (fi_callback) {
-
- memdelete(fi_callback);
- fi_callback=NULL;
- }
-
-
- if (p_id!=0) {
-
- fi_callback=memnew(ForceIntegrationCallback);
- fi_callback->id=p_id;
- fi_callback->method=p_method;
- fi_callback->callback_udata=p_udata;
- }
-
-}
-
-Body2DSW::Body2DSW() : CollisionObject2DSW(TYPE_BODY), active_list(this), inertia_update_list(this), direct_state_query_list(this) {
-
-
- mode=Physics2DServer::BODY_MODE_RIGID;
- active=true;
- angular_velocity=0;
- biased_angular_velocity=0;
- mass=1;
- _inv_inertia=0;
- _inv_mass=1;
- bounce=0;
- friction=1;
- omit_force_integration=false;
- applied_torque=0;
- island_step=0;
- island_next=NULL;
- island_list_next=NULL;
- _set_static(false);
- density=0;
- contact_count=0;
- simulated_motion=false;
- still_time=0;
- continuous_cd=false;
- can_sleep=false;
- fi_callback=NULL;
-
-}
-
-Body2DSW::~Body2DSW() {
-
- if (fi_callback)
- memdelete(fi_callback);
-}
-
-Physics2DDirectBodyStateSW *Physics2DDirectBodyStateSW::singleton=NULL;
-
-Physics2DDirectSpaceState* Physics2DDirectBodyStateSW::get_space_state() {
-
- return body->get_space()->get_direct_state();
-}
+#include "body_2d_sw.h" +#include "space_2d_sw.h" +#include "area_2d_sw.h" + +void Body2DSW::_update_inertia() { + + if (get_space() && !inertia_update_list.in_list()) + get_space()->body_add_to_inertia_update_list(&inertia_update_list); + +} + + + +void Body2DSW::update_inertias() { + + //update shapes and motions + + switch(mode) { + + case Physics2DServer::BODY_MODE_RIGID: { + + //update tensor for allshapes, not the best way but should be somehow OK. (inspired from bullet) + float total_area=0; + + for (int i=0;i<get_shape_count();i++) { + + total_area+=get_shape_aabb(i).get_area(); + } + + real_t _inertia=0; + + for (int i=0;i<get_shape_count();i++) { + + const Shape2DSW* shape=get_shape(i); + + float area=get_shape_aabb(i).get_area(); + + float mass = area * this->mass / total_area; + + _inertia += shape->get_moment_of_inertia(mass) + mass * get_shape_transform(i).get_origin().length_squared(); + + } + + if (_inertia!=0) + _inv_inertia=1.0/_inertia; + else + _inv_inertia=0.0; //wathever + + if (mass) + _inv_mass=1.0/mass; + else + _inv_mass=0; + + } break; + case Physics2DServer::BODY_MODE_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;i<get_shape_count();i++) { + Shape &s=shapes[i]; + if (s.bpid>0) { + get_space()->get_broadphase()->set_active(s.bpid,active); + } + } +*/ +} + + + +void Body2DSW::set_param(Physics2DServer::BodyParameter p_param, float p_value) { + + switch(p_param) { + case Physics2DServer::BODY_PARAM_BOUNCE: { + + bounce=p_value; + } break; + case Physics2DServer::BODY_PARAM_FRICTION: { + + friction=p_value; + } break; + case Physics2DServer::BODY_PARAM_MASS: { + ERR_FAIL_COND(p_value<=0); + mass=p_value; + _update_inertia(); + + } break; + default:{} + } +} + +float Body2DSW::get_param(Physics2DServer::BodyParameter p_param) const { + + switch(p_param) { + case Physics2DServer::BODY_PARAM_BOUNCE: { + + return bounce; + } break; + case Physics2DServer::BODY_PARAM_FRICTION: { + + return friction; + } break; + case Physics2DServer::BODY_PARAM_MASS: { + return mass; + } break; + default:{} + } + + return 0; +} + +void Body2DSW::set_mode(Physics2DServer::BodyMode p_mode) { + + mode=p_mode; + + switch(p_mode) { + //CLEAR UP EVERYTHING IN CASE IT NOT WORKS! + case Physics2DServer::BODY_MODE_STATIC: + case Physics2DServer::BODY_MODE_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;i<ac;i++) { + if (aa[i].area->get_priority() > prio) { + current_area=aa[i].area; + prio=current_area->get_priority(); + } + } + } + + _compute_area_gravity(current_area); + density=current_area->get_density(); + + 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;i<get_shape_count();i++) { + set_shape_kinematic_advance(i,Vector2()); + set_shape_kinematic_retreat(i,0); + } + + } else { + 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; + } + + 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<Constraint2DSW*,int>::Element *E=constraint_map.front();E;E=E->next()) { + + const Constraint2DSW *c=E->key(); + Body2DSW **n = c->get_body_ptr(); + int bc=c->get_body_count(); + + for(int i=0;i<bc;i++) { + + if (i==E->get()) + continue; + Body2DSW *b = n[i]; + if (b->mode!=Physics2DServer::BODY_MODE_RIGID) + continue; + + if (!b->is_active()) + b->set_active(true); + } + } +} + +void Body2DSW::call_queries() { + + + if (fi_callback) { + + Physics2DDirectBodyStateSW *dbs = Physics2DDirectBodyStateSW::singleton; + dbs->body=this; + + Variant v=dbs; + const Variant *vp[2]={&v,&fi_callback->callback_udata}; + + Object *obj = ObjectDB::get_instance(fi_callback->id); + if (!obj) { + + set_force_integration_callback(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_space()->get_body_angular_velocity_sleep_treshold() && Math::abs(linear_velocity.length_squared()) < get_space()->get_body_linear_velocity_sleep_treshold()*get_space()->get_body_linear_velocity_sleep_treshold()) { + + still_time+=p_step; + + return still_time > get_space()->get_body_time_to_sleep(); + } else { + + still_time=0; //maybe this should be set to 0 on set_active? + return false; + } +} + + +void Body2DSW::set_force_integration_callback(ObjectID p_id,const StringName& p_method,const Variant& p_udata) { + + if (fi_callback) { + + memdelete(fi_callback); + fi_callback=NULL; + } + + + if (p_id!=0) { + + fi_callback=memnew(ForceIntegrationCallback); + fi_callback->id=p_id; + fi_callback->method=p_method; + fi_callback->callback_udata=p_udata; + } + +} + +Body2DSW::Body2DSW() : CollisionObject2DSW(TYPE_BODY), active_list(this), inertia_update_list(this), direct_state_query_list(this) { + + + mode=Physics2DServer::BODY_MODE_RIGID; + active=true; + angular_velocity=0; + biased_angular_velocity=0; + mass=1; + _inv_inertia=0; + _inv_mass=1; + bounce=0; + friction=1; + omit_force_integration=false; + applied_torque=0; + island_step=0; + island_next=NULL; + island_list_next=NULL; + _set_static(false); + density=0; + contact_count=0; + + 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<Body2DSW> active_list;
- SelfList<Body2DSW> inertia_update_list;
- SelfList<Body2DSW> direct_state_query_list;
-
- VSet<RID> exceptions;
- bool omit_force_integration;
- bool active;
- bool simulated_motion;
- bool continuous_cd;
- bool can_sleep;
- void _update_inertia();
- virtual void _shapes_changed();
-
-
- Map<Constraint2DSW*,int> constraint_map;
-
- struct AreaCMP {
-
- Area2DSW *area;
- _FORCE_INLINE_ bool operator<(const AreaCMP& p_cmp) const { return area->get_self() < p_cmp.area->get_self() ; }
- _FORCE_INLINE_ AreaCMP() {}
- _FORCE_INLINE_ AreaCMP(Area2DSW *p_area) { area=p_area;}
- };
-
-
- VSet<AreaCMP> areas;
-
- struct Contact {
-
-
- Vector2 local_pos;
- Vector2 local_normal;
- float depth;
- int local_shape;
- Vector2 collider_pos;
- int collider_shape;
- ObjectID collider_instance_id;
- RID collider;
- Vector2 collider_velocity_at_pos;
- };
-
- Vector<Contact> contacts; //no contacts by default
- int contact_count;
-
- struct ForceIntegrationCallback {
-
- ObjectID id;
- StringName method;
- Variant callback_udata;
- };
-
- ForceIntegrationCallback *fi_callback;
-
-
- uint64_t island_step;
- Body2DSW *island_next;
- Body2DSW *island_list_next;
-
- _FORCE_INLINE_ void _compute_area_gravity(const Area2DSW *p_area);
-
-friend class Physics2DDirectBodyStateSW; // i give up, too many functions to expose
-
-public:
-
-
- void set_force_integration_callback(ObjectID p_id, const StringName& p_method, const Variant &p_udata=Variant());
-
-
- _FORCE_INLINE_ void add_area(Area2DSW *p_area) { areas.insert(AreaCMP(p_area)); }
- _FORCE_INLINE_ void remove_area(Area2DSW *p_area) { areas.erase(AreaCMP(p_area)); }
-
- _FORCE_INLINE_ void set_max_contacts_reported(int p_size) { contacts.resize(p_size); contact_count=0; }
- _FORCE_INLINE_ int get_max_contacts_reported() const { return contacts.size(); }
-
- _FORCE_INLINE_ bool can_report_contacts() const { return !contacts.empty(); }
- _FORCE_INLINE_ void add_contact(const Vector2& p_local_pos,const Vector2& p_local_normal, float p_depth, int p_local_shape, const Vector2& p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID& p_collider,const Vector2& p_collider_velocity_at_pos);
-
-
- _FORCE_INLINE_ void add_exception(const RID& p_exception) { exceptions.insert(p_exception);}
- _FORCE_INLINE_ void remove_exception(const RID& p_exception) { exceptions.erase(p_exception);}
- _FORCE_INLINE_ bool has_exception(const RID& p_exception) const { return exceptions.has(p_exception);}
- _FORCE_INLINE_ const VSet<RID>& get_exceptions() const { return exceptions;}
-
- _FORCE_INLINE_ uint64_t get_island_step() const { return island_step; }
- _FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step=p_step; }
-
- _FORCE_INLINE_ Body2DSW* get_island_next() const { return island_next; }
- _FORCE_INLINE_ void set_island_next(Body2DSW* p_next) { island_next=p_next; }
-
- _FORCE_INLINE_ Body2DSW* get_island_list_next() const { return island_list_next; }
- _FORCE_INLINE_ void set_island_list_next(Body2DSW* p_next) { island_list_next=p_next; }
-
- _FORCE_INLINE_ void add_constraint(Constraint2DSW* p_constraint, int p_pos) { constraint_map[p_constraint]=p_pos; }
- _FORCE_INLINE_ void remove_constraint(Constraint2DSW* p_constraint) { constraint_map.erase(p_constraint); }
- const Map<Constraint2DSW*,int>& get_constraint_map() const { return constraint_map; }
-
- _FORCE_INLINE_ void set_omit_force_integration(bool p_omit_force_integration) { omit_force_integration=p_omit_force_integration; }
- _FORCE_INLINE_ bool get_omit_force_integration() const { return omit_force_integration; }
-
- _FORCE_INLINE_ void set_linear_velocity(const Vector2& p_velocity) {linear_velocity=p_velocity; }
- _FORCE_INLINE_ Vector2 get_linear_velocity() const { return linear_velocity; }
-
- _FORCE_INLINE_ void set_angular_velocity(real_t p_velocity) { angular_velocity=p_velocity; }
- _FORCE_INLINE_ real_t get_angular_velocity() const { return angular_velocity; }
-
- _FORCE_INLINE_ void set_biased_linear_velocity(const Vector2& p_velocity) {biased_linear_velocity=p_velocity; }
- _FORCE_INLINE_ Vector2 get_biased_linear_velocity() const { return biased_linear_velocity; }
-
- _FORCE_INLINE_ void set_biased_angular_velocity(real_t p_velocity) { biased_angular_velocity=p_velocity; }
- _FORCE_INLINE_ real_t get_biased_angular_velocity() const { return biased_angular_velocity; }
-
- _FORCE_INLINE_ void apply_impulse(const Vector2& p_pos, const Vector2& p_j) {
-
- linear_velocity += p_j * _inv_mass;
- angular_velocity += _inv_inertia * p_pos.cross(p_j);
- }
-
- _FORCE_INLINE_ void apply_bias_impulse(const Vector2& p_pos, const Vector2& p_j) {
-
- biased_linear_velocity += p_j * _inv_mass;
- biased_angular_velocity += _inv_inertia * p_pos.cross(p_j);
- }
-
- void set_active(bool p_active);
- _FORCE_INLINE_ bool is_active() const { return active; }
-
- void set_param(Physics2DServer::BodyParameter p_param, float);
- float get_param(Physics2DServer::BodyParameter p_param) const;
-
- void set_mode(Physics2DServer::BodyMode p_mode);
- Physics2DServer::BodyMode get_mode() const;
-
- void set_state(Physics2DServer::BodyState p_state, const Variant& p_variant);
- Variant get_state(Physics2DServer::BodyState p_state) const;
-
- void set_applied_force(const Vector2& p_force) { applied_force=p_force; }
- Vector2 get_applied_force() const { return applied_force; }
-
- void set_applied_torque(real_t p_torque) { applied_torque=p_torque; }
- real_t get_applied_torque() const { return applied_torque; }
-
- _FORCE_INLINE_ void set_continuous_collision_detection(bool p_enable) { continuous_cd=p_enable; }
- _FORCE_INLINE_ bool is_continuous_collision_detection_enabled() const { return continuous_cd; }
-
- void set_space(Space2DSW *p_space);
-
- void update_inertias();
-
- _FORCE_INLINE_ real_t get_inv_mass() const { return _inv_mass; }
- _FORCE_INLINE_ real_t get_inv_inertia() const { return _inv_inertia; }
- _FORCE_INLINE_ real_t get_friction() const { return friction; }
- _FORCE_INLINE_ Vector2 get_gravity() const { return gravity; }
- _FORCE_INLINE_ real_t get_density() const { return density; }
-
- void integrate_forces(real_t p_step);
- void integrate_velocities(real_t p_step);
-
- void simulate_motion(const Matrix32& p_xform,real_t p_step);
- void call_queries();
- void wakeup_neighbours();
-
- bool sleep_test(real_t p_step);
-
- Body2DSW();
- ~Body2DSW();
-
-};
-
-
-//add contact inline
-
-void Body2DSW::add_contact(const Vector2& p_local_pos,const Vector2& p_local_normal, float p_depth, int p_local_shape, const Vector2& p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID& p_collider,const Vector2& p_collider_velocity_at_pos) {
-
- int c_max=contacts.size();
-
- if (c_max==0)
- return;
-
- Contact *c = &contacts[0];
-
-
- int idx=-1;
-
- if (contact_count<c_max) {
- idx=contact_count++;
- } else {
-
- float least_depth=1e20;
- int least_deep=-1;
- for(int i=0;i<c_max;i++) {
-
- if (i==0 || c[i].depth<least_depth) {
- least_deep=i;
- least_depth=c[i].depth;
- }
- }
-
- if (least_deep>=0 && least_depth<p_depth) {
-
- idx=least_deep;
- }
- if (idx==-1)
- return; //none least deepe than this
- }
-
- c[idx].local_pos=p_local_pos;
- c[idx].local_normal=p_local_normal;
- c[idx].depth=p_depth;
- c[idx].local_shape=p_local_shape;
- c[idx].collider_pos=p_collider_pos;
- c[idx].collider_shape=p_collider_shape;
- c[idx].collider_instance_id=p_collider_instance_id;
- c[idx].collider=p_collider;
- c[idx].collider_velocity_at_pos=p_collider_velocity_at_pos;
-
-}
-
-
-class Physics2DDirectBodyStateSW : public Physics2DDirectBodyState {
-
- OBJ_TYPE( Physics2DDirectBodyStateSW, Physics2DDirectBodyState );
-
-public:
-
- static Physics2DDirectBodyStateSW *singleton;
- Body2DSW *body;
- real_t step;
-
- virtual Vector2 get_total_gravity() const { return body->get_gravity(); } // get gravity vector working on this body space/area
- virtual float get_total_density() const { return body->get_density(); } // get density of this body space/area
-
- virtual float get_inverse_mass() const { return body->get_inv_mass(); } // get the mass
- virtual real_t get_inverse_inertia() const { return body->get_inv_inertia(); } // get density of this body space
-
- virtual void set_linear_velocity(const Vector2& p_velocity) { body->set_linear_velocity(p_velocity); }
- virtual Vector2 get_linear_velocity() const { return body->get_linear_velocity(); }
-
- virtual void set_angular_velocity(real_t p_velocity) { body->set_angular_velocity(p_velocity); }
- virtual real_t get_angular_velocity() const { return body->get_angular_velocity(); }
-
- virtual void set_transform(const Matrix32& p_transform) { body->set_state(Physics2DServer::BODY_STATE_TRANSFORM,p_transform); }
- virtual Matrix32 get_transform() const { return body->get_transform(); }
-
- virtual void set_sleep_state(bool p_enable) { body->set_active(!p_enable); }
- virtual bool is_sleeping() const { return !body->is_active(); }
-
- virtual int get_contact_count() const { return body->contact_count; }
-
- virtual Vector2 get_contact_local_pos(int p_contact_idx) const {
- ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector2());
- return body->contacts[p_contact_idx].local_pos;
- }
- virtual Vector2 get_contact_local_normal(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector2()); return body->contacts[p_contact_idx].local_normal; }
- virtual int get_contact_local_shape(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,-1); return body->contacts[p_contact_idx].local_shape; }
-
- virtual RID get_contact_collider(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,RID()); return body->contacts[p_contact_idx].collider; }
- virtual Vector2 get_contact_collider_pos(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector2()); return body->contacts[p_contact_idx].collider_pos; }
- virtual ObjectID get_contact_collider_id(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,0); return body->contacts[p_contact_idx].collider_instance_id; }
- virtual int get_contact_collider_shape(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,0); return body->contacts[p_contact_idx].collider_shape; }
- virtual Vector2 get_contact_collider_velocity_at_pos(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector2()); return body->contacts[p_contact_idx].collider_velocity_at_pos; }
-
- virtual Physics2DDirectSpaceState* get_space_state();
-
-
- virtual real_t get_step() const { return step; }
- Physics2DDirectBodyStateSW() { singleton=this; body=NULL; }
-};
-
-
-#endif // BODY_2D_SW_H
+#ifndef BODY_2D_SW_H +#define BODY_2D_SW_H + +#include "collision_object_2d_sw.h" +#include "vset.h" +#include "area_2d_sw.h" + +class Constraint2DSW; + + +class Body2DSW : public CollisionObject2DSW { + + + Physics2DServer::BodyMode mode; + + Vector2 biased_linear_velocity; + real_t biased_angular_velocity; + + Vector2 linear_velocity; + real_t angular_velocity; + + real_t mass; + real_t bounce; + real_t friction; + + real_t _inv_mass; + real_t _inv_inertia; + + Vector2 gravity; + real_t density; + + real_t still_time; + + Vector2 applied_force; + real_t applied_torque; + + SelfList<Body2DSW> active_list; + SelfList<Body2DSW> inertia_update_list; + SelfList<Body2DSW> direct_state_query_list; + + VSet<RID> exceptions; + 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<Constraint2DSW*,int> constraint_map; + + struct AreaCMP { + + Area2DSW *area; + _FORCE_INLINE_ bool operator<(const AreaCMP& p_cmp) const { return area->get_self() < p_cmp.area->get_self() ; } + _FORCE_INLINE_ AreaCMP() {} + _FORCE_INLINE_ AreaCMP(Area2DSW *p_area) { area=p_area;} + }; + + + VSet<AreaCMP> areas; + + struct Contact { + + + Vector2 local_pos; + Vector2 local_normal; + float depth; + int local_shape; + Vector2 collider_pos; + int collider_shape; + ObjectID collider_instance_id; + RID collider; + Vector2 collider_velocity_at_pos; + }; + + Vector<Contact> contacts; //no contacts by default + int contact_count; + + struct ForceIntegrationCallback { + + ObjectID id; + StringName method; + Variant callback_udata; + }; + + ForceIntegrationCallback *fi_callback; + + + uint64_t island_step; + Body2DSW *island_next; + Body2DSW *island_list_next; + + _FORCE_INLINE_ void _compute_area_gravity(const Area2DSW *p_area); + +friend class Physics2DDirectBodyStateSW; // i give up, too many functions to expose + +public: + + + void set_force_integration_callback(ObjectID p_id, const StringName& p_method, const Variant &p_udata=Variant()); + + + _FORCE_INLINE_ void add_area(Area2DSW *p_area) { areas.insert(AreaCMP(p_area)); } + _FORCE_INLINE_ void remove_area(Area2DSW *p_area) { areas.erase(AreaCMP(p_area)); } + + _FORCE_INLINE_ void set_max_contacts_reported(int p_size) { contacts.resize(p_size); contact_count=0; } + _FORCE_INLINE_ int get_max_contacts_reported() const { return contacts.size(); } + + _FORCE_INLINE_ bool can_report_contacts() const { return !contacts.empty(); } + _FORCE_INLINE_ void add_contact(const Vector2& p_local_pos,const Vector2& p_local_normal, float p_depth, int p_local_shape, const Vector2& p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID& p_collider,const Vector2& p_collider_velocity_at_pos); + + + _FORCE_INLINE_ void add_exception(const RID& p_exception) { exceptions.insert(p_exception);} + _FORCE_INLINE_ void remove_exception(const RID& p_exception) { exceptions.erase(p_exception);} + _FORCE_INLINE_ bool has_exception(const RID& p_exception) const { return exceptions.has(p_exception);} + _FORCE_INLINE_ const VSet<RID>& get_exceptions() const { return exceptions;} + + _FORCE_INLINE_ uint64_t get_island_step() const { return island_step; } + _FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step=p_step; } + + _FORCE_INLINE_ Body2DSW* get_island_next() const { return island_next; } + _FORCE_INLINE_ void set_island_next(Body2DSW* p_next) { island_next=p_next; } + + _FORCE_INLINE_ Body2DSW* get_island_list_next() const { return island_list_next; } + _FORCE_INLINE_ void set_island_list_next(Body2DSW* p_next) { island_list_next=p_next; } + + _FORCE_INLINE_ void add_constraint(Constraint2DSW* p_constraint, int p_pos) { constraint_map[p_constraint]=p_pos; } + _FORCE_INLINE_ void remove_constraint(Constraint2DSW* p_constraint) { constraint_map.erase(p_constraint); } + const Map<Constraint2DSW*,int>& get_constraint_map() const { return constraint_map; } + + _FORCE_INLINE_ void set_omit_force_integration(bool p_omit_force_integration) { omit_force_integration=p_omit_force_integration; } + _FORCE_INLINE_ bool get_omit_force_integration() const { return omit_force_integration; } + + _FORCE_INLINE_ void set_linear_velocity(const Vector2& p_velocity) {linear_velocity=p_velocity; } + _FORCE_INLINE_ Vector2 get_linear_velocity() const { return linear_velocity; } + + _FORCE_INLINE_ void set_angular_velocity(real_t p_velocity) { angular_velocity=p_velocity; } + _FORCE_INLINE_ real_t get_angular_velocity() const { return angular_velocity; } + + _FORCE_INLINE_ void set_biased_linear_velocity(const Vector2& p_velocity) {biased_linear_velocity=p_velocity; } + _FORCE_INLINE_ Vector2 get_biased_linear_velocity() const { return biased_linear_velocity; } + + _FORCE_INLINE_ void set_biased_angular_velocity(real_t p_velocity) { biased_angular_velocity=p_velocity; } + _FORCE_INLINE_ real_t get_biased_angular_velocity() const { return biased_angular_velocity; } + + + _FORCE_INLINE_ void apply_impulse(const Vector2& p_pos, const Vector2& p_j) { + + linear_velocity += p_j * _inv_mass; + angular_velocity += _inv_inertia * p_pos.cross(p_j); + } + + _FORCE_INLINE_ void apply_bias_impulse(const Vector2& p_pos, const Vector2& p_j) { + + biased_linear_velocity += p_j * _inv_mass; + biased_angular_velocity += _inv_inertia * p_pos.cross(p_j); + } + + void set_active(bool p_active); + _FORCE_INLINE_ bool is_active() const { return active; } + + void set_param(Physics2DServer::BodyParameter p_param, float); + float get_param(Physics2DServer::BodyParameter p_param) const; + + void set_mode(Physics2DServer::BodyMode p_mode); + Physics2DServer::BodyMode get_mode() const; + + void set_state(Physics2DServer::BodyState p_state, const Variant& p_variant); + Variant get_state(Physics2DServer::BodyState p_state) const; + + void set_applied_force(const Vector2& p_force) { applied_force=p_force; } + Vector2 get_applied_force() const { return applied_force; } + + void set_applied_torque(real_t p_torque) { applied_torque=p_torque; } + real_t get_applied_torque() const { return applied_torque; } + + + _FORCE_INLINE_ void set_continuous_collision_detection_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<c_max) { + idx=contact_count++; + } else { + + float least_depth=1e20; + int least_deep=-1; + for(int i=0;i<c_max;i++) { + + if (i==0 || c[i].depth<least_depth) { + least_deep=i; + least_depth=c[i].depth; + } + } + + if (least_deep>=0 && least_depth<p_depth) { + + idx=least_deep; + } + if (idx==-1) + return; //none least deepe than this + } + + c[idx].local_pos=p_local_pos; + c[idx].local_normal=p_local_normal; + c[idx].depth=p_depth; + c[idx].local_shape=p_local_shape; + c[idx].collider_pos=p_collider_pos; + c[idx].collider_shape=p_collider_shape; + c[idx].collider_instance_id=p_collider_instance_id; + c[idx].collider=p_collider; + c[idx].collider_velocity_at_pos=p_collider_velocity_at_pos; + +} + + +class Physics2DDirectBodyStateSW : public Physics2DDirectBodyState { + + OBJ_TYPE( Physics2DDirectBodyStateSW, Physics2DDirectBodyState ); + +public: + + static Physics2DDirectBodyStateSW *singleton; + Body2DSW *body; + real_t step; + + virtual Vector2 get_total_gravity() const { return body->get_gravity(); } // get gravity vector working on this body space/area + virtual float get_total_density() const { return body->get_density(); } // get density of this body space/area + + virtual float get_inverse_mass() const { return body->get_inv_mass(); } // get the mass + virtual real_t get_inverse_inertia() const { return body->get_inv_inertia(); } // get density of this body space + + virtual void set_linear_velocity(const Vector2& p_velocity) { body->set_linear_velocity(p_velocity); } + virtual Vector2 get_linear_velocity() const { return body->get_linear_velocity(); } + + virtual void set_angular_velocity(real_t p_velocity) { body->set_angular_velocity(p_velocity); } + virtual real_t get_angular_velocity() const { return body->get_angular_velocity(); } + + virtual void set_transform(const Matrix32& p_transform) { body->set_state(Physics2DServer::BODY_STATE_TRANSFORM,p_transform); } + virtual Matrix32 get_transform() const { return body->get_transform(); } + + virtual void set_sleep_state(bool p_enable) { body->set_active(!p_enable); } + virtual bool is_sleeping() const { return !body->is_active(); } + + virtual int get_contact_count() const { return body->contact_count; } + + virtual Vector2 get_contact_local_pos(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector2()); + return body->contacts[p_contact_idx].local_pos; + } + virtual Vector2 get_contact_local_normal(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector2()); return body->contacts[p_contact_idx].local_normal; } + virtual int get_contact_local_shape(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,-1); return body->contacts[p_contact_idx].local_shape; } + + virtual RID get_contact_collider(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,RID()); return body->contacts[p_contact_idx].collider; } + virtual Vector2 get_contact_collider_pos(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector2()); return body->contacts[p_contact_idx].collider_pos; } + virtual ObjectID get_contact_collider_id(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,0); return body->contacts[p_contact_idx].collider_instance_id; } + virtual int get_contact_collider_shape(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,0); return body->contacts[p_contact_idx].collider_shape; } + virtual Vector2 get_contact_collider_velocity_at_pos(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector2()); return body->contacts[p_contact_idx].collider_velocity_at_pos; } + + virtual Physics2DDirectSpaceState* get_space_state(); + + + virtual real_t get_step() const { return step; } + Physics2DDirectBodyStateSW() { singleton=this; body=NULL; } +}; + + +#endif // BODY_2D_SW_H diff --git a/servers/physics_2d/body_pair_2d_sw.cpp b/servers/physics_2d/body_pair_2d_sw.cpp 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<bool use_aabb,bool use_segment> +void BroadPhase2DHashGrid::_cull(const Point2i p_cell,const Rect2& p_aabb,const Point2& p_from, const Point2& p_to,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices,int &index) { PosKey pk; @@ -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*,RC>::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<false,true>(pos,Rect2(),p_from,p_to,p_results,p_max_results,p_result_indices,cullcount); bool reached_x=false; bool reached_y=false; @@ -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<false,true>(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<true,false>(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<bool use_aabb,bool use_segment> + _FORCE_INLINE_ void _cull(const Point2i p_cell,const Rect2& p_aabb,const Point2& p_from, const Point2& p_to,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices,int &index); struct PosKey { 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.size();i++) { Shape &s=shapes[i]; diff --git a/servers/physics_2d/collision_object_2d_sw.h b/servers/physics_2d/collision_object_2d_sw.h index b2d2c25451..74457cfa0a 100644 --- a/servers/physics_2d/collision_object_2d_sw.h +++ b/servers/physics_2d/collision_object_2d_sw.h @@ -55,8 +55,10 @@ private: BroadPhase2DSW::ID bpid; Rect2 aabb_cache; //for rayqueries Shape2DSW *shape; + Vector2 kinematic_advance; + float kinematic_retreat; bool trigger; - Shape() { trigger=false; } + Shape() { trigger=false; kinematic_retreat=0; } }; Vector<Shape> 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<class ShapeA, class ShapeB> +template<class ShapeA, class ShapeB,bool castA=false,bool castB=false> 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;i<support_count_A;i++) { - supports_A[i] = transform_A->xform(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;i<support_count_A;i++) { + supports_A[i] = transform_A->xform(supports_A[i]); + } } Vector2 supports_B[max_supports]; int support_count_B; - shape_B->get_supports(transform_B->basis_xform_inv(best_axis).normalized(),supports_B,support_count_B); - for(int i=0;i<support_count_B;i++) { - supports_B[i] = transform_B->xform(supports_B[i]); + 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;i<support_count_B;i++) { + supports_B[i] = transform_B->xform(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<bool castA, bool castB> +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<const SegmentShape2DSW*>(p_a); const SegmentShape2DSW *segment_B = static_cast<const SegmentShape2DSW*>(p_b); - SeparatorAxisTest2D<SegmentShape2DSW,SegmentShape2DSW> separator(segment_A,p_transform_a,p_transform_inv_a,segment_B,p_transform_b,p_transform_inv_b,p_collector); + SeparatorAxisTest2D<SegmentShape2DSW,SegmentShape2DSW,castA,castB> 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<bool castA, bool castB> +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<const SegmentShape2DSW*>(p_a); const CircleShape2DSW *circle_B = static_cast<const CircleShape2DSW*>(p_b); - SeparatorAxisTest2D<SegmentShape2DSW,CircleShape2DSW> separator(segment_A,p_transform_a,p_transform_inv_a,circle_B,p_transform_b,p_transform_inv_b,p_collector); + SeparatorAxisTest2D<SegmentShape2DSW,CircleShape2DSW,castA,castB> 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<bool castA, bool castB> +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<const SegmentShape2DSW*>(p_a); const RectangleShape2DSW *rectangle_B = static_cast<const RectangleShape2DSW*>(p_b); - SeparatorAxisTest2D<SegmentShape2DSW,RectangleShape2DSW> separator(segment_A,p_transform_a,p_transform_inv_a,rectangle_B,p_transform_b,p_transform_inv_b,p_collector); + SeparatorAxisTest2D<SegmentShape2DSW,RectangleShape2DSW,castA,castB> 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<bool castA, bool castB> +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<const SegmentShape2DSW*>(p_a); const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b); - SeparatorAxisTest2D<SegmentShape2DSW,CapsuleShape2DSW> separator(segment_A,p_transform_a,p_transform_inv_a,capsule_B,p_transform_b,p_transform_inv_b,p_collector); + SeparatorAxisTest2D<SegmentShape2DSW,CapsuleShape2DSW,castA,castB> 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<bool castA, bool castB> +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<const SegmentShape2DSW*>(p_a); const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b); - SeparatorAxisTest2D<SegmentShape2DSW,ConvexPolygonShape2DSW> separator(segment_A,p_transform_a,p_transform_inv_a,convex_B,p_transform_b,p_transform_inv_b,p_collector); + SeparatorAxisTest2D<SegmentShape2DSW,ConvexPolygonShape2DSW,castA,castB> 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;i<convex_B->get_point_count();i++) { - if (!separator.test_axis( p_transform_inv_b.basis_xform_inv(convex_B->get_segment_normal(i)).normalized() )) + 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<bool castA, bool castB> +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<const CircleShape2DSW*>(p_a); const CircleShape2DSW *circle_B = static_cast<const CircleShape2DSW*>(p_b); - SeparatorAxisTest2D<CircleShape2DSW,CircleShape2DSW> separator(circle_A,p_transform_a,p_transform_inv_a,circle_B,p_transform_b,p_transform_inv_b,p_collector); + SeparatorAxisTest2D<CircleShape2DSW,CircleShape2DSW,castA,castB> 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<bool castA, bool castB> +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<const CircleShape2DSW*>(p_a); const RectangleShape2DSW *rectangle_B = static_cast<const RectangleShape2DSW*>(p_b); - SeparatorAxisTest2D<CircleShape2DSW,RectangleShape2DSW> separator(circle_A,p_transform_a,p_transform_inv_a,rectangle_B,p_transform_b,p_transform_inv_b,p_collector); + SeparatorAxisTest2D<CircleShape2DSW,RectangleShape2DSW,castA,castB> 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<bool castA, bool castB> +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<const CircleShape2DSW*>(p_a); const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b); - SeparatorAxisTest2D<CircleShape2DSW,CapsuleShape2DSW> separator(circle_A,p_transform_a,p_transform_inv_a,capsule_B,p_transform_b,p_transform_inv_b,p_collector); + SeparatorAxisTest2D<CircleShape2DSW,CapsuleShape2DSW,castA,castB> 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<bool castA, bool castB> +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<const CircleShape2DSW*>(p_a); const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b); - SeparatorAxisTest2D<CircleShape2DSW,ConvexPolygonShape2DSW> separator(circle_A,p_transform_a,p_transform_inv_a,convex_B,p_transform_b,p_transform_inv_b,p_collector); + SeparatorAxisTest2D<CircleShape2DSW,ConvexPolygonShape2DSW,castA,castB> 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;i<convex_B->get_point_count();i++) { - if (!separator.test_axis( (p_transform_b.xform(convex_B->get_point(i))-p_transform_a.get_origin()).normalized() )) + 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<bool castA, bool castB> +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<const RectangleShape2DSW*>(p_a); const RectangleShape2DSW *rectangle_B = static_cast<const RectangleShape2DSW*>(p_b); - SeparatorAxisTest2D<RectangleShape2DSW,RectangleShape2DSW> separator(rectangle_A,p_transform_a,p_transform_inv_a,rectangle_B,p_transform_b,p_transform_inv_b,p_collector); + SeparatorAxisTest2D<RectangleShape2DSW,RectangleShape2DSW,castA,castB> 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<bool castA, bool castB> +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<const RectangleShape2DSW*>(p_a); const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b); - SeparatorAxisTest2D<RectangleShape2DSW,CapsuleShape2DSW> separator(rectangle_A,p_transform_a,p_transform_inv_a,capsule_B,p_transform_b,p_transform_inv_b,p_collector); + SeparatorAxisTest2D<RectangleShape2DSW,CapsuleShape2DSW,castA,castB> 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<bool castA, bool castB> +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<const RectangleShape2DSW*>(p_a); const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b); - SeparatorAxisTest2D<RectangleShape2DSW,ConvexPolygonShape2DSW> separator(rectangle_A,p_transform_a,p_transform_inv_a,convex_B,p_transform_b,p_transform_inv_b,p_collector); + SeparatorAxisTest2D<RectangleShape2DSW,ConvexPolygonShape2DSW,castA,castB> 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;i<convex_B->get_point_count();i++) { - if (!separator.test_axis( p_transform_inv_b.basis_xform_inv(convex_B->get_segment_normal(i)).normalized() )) + 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<bool castA, bool castB> +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<const CapsuleShape2DSW*>(p_a); const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b); - SeparatorAxisTest2D<CapsuleShape2DSW,CapsuleShape2DSW> separator(capsule_A,p_transform_a,p_transform_inv_a,capsule_B,p_transform_b,p_transform_inv_b,p_collector); + SeparatorAxisTest2D<CapsuleShape2DSW,CapsuleShape2DSW,castA,castB> 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<bool castA, bool castB> +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<const CapsuleShape2DSW*>(p_a); const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b); - SeparatorAxisTest2D<CapsuleShape2DSW,ConvexPolygonShape2DSW> separator(capsule_A,p_transform_a,p_transform_inv_a,convex_B,p_transform_b,p_transform_inv_b,p_collector); + SeparatorAxisTest2D<CapsuleShape2DSW,ConvexPolygonShape2DSW,castA,castB> 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<bool castA, bool castB> +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<const ConvexPolygonShape2DSW*>(p_a); const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b); - SeparatorAxisTest2D<ConvexPolygonShape2DSW,ConvexPolygonShape2DSW> separator(convex_A,p_transform_a,p_transform_inv_a,convex_B,p_transform_b,p_transform_inv_b,p_collector); + SeparatorAxisTest2D<ConvexPolygonShape2DSW,ConvexPolygonShape2DSW,castA,castB> 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;i<convex_A->get_point_count();i++) { - if (!separator.test_axis( p_transform_inv_a.basis_xform_inv(convex_A->get_segment_normal(i)).normalized() )) + if (!separator.test_axis( convex_A->get_xformed_segment_normal(p_transform_a,i))) return; } for(int i=0;i<convex_B->get_point_count();i++) { - 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<false,false>, + _collision_segment_circle<false,false>, + _collision_segment_rectangle<false,false>, + _collision_segment_capsule<false,false>, + _collision_segment_convex_polygon<false,false>}, {0, - _collision_circle_circle, - _collision_circle_rectangle, - _collision_circle_capsule, - _collision_circle_convex_polygon}, + _collision_circle_circle<false,false>, + _collision_circle_rectangle<false,false>, + _collision_circle_capsule<false,false>, + _collision_circle_convex_polygon<false,false>}, {0, 0, - _collision_rectangle_rectangle, - _collision_rectangle_capsule, - _collision_rectangle_convex_polygon}, + _collision_rectangle_rectangle<false,false>, + _collision_rectangle_capsule<false,false>, + _collision_rectangle_convex_polygon<false,false>}, {0, 0, 0, - _collision_capsule_capsule, - _collision_capsule_convex_polygon}, + _collision_capsule_capsule<false,false>, + _collision_capsule_convex_polygon<false,false>}, {0, 0, 0, 0, - _collision_convex_polygon_convex_polygon} + _collision_convex_polygon_convex_polygon<false,false>} + + }; + + static const CollisionFunc collision_table_castA[5][5]={ + {_collision_segment_segment<true,false>, + _collision_segment_circle<true,false>, + _collision_segment_rectangle<true,false>, + _collision_segment_capsule<true,false>, + _collision_segment_convex_polygon<true,false>}, + {0, + _collision_circle_circle<true,false>, + _collision_circle_rectangle<true,false>, + _collision_circle_capsule<true,false>, + _collision_circle_convex_polygon<true,false>}, + {0, + 0, + _collision_rectangle_rectangle<true,false>, + _collision_rectangle_capsule<true,false>, + _collision_rectangle_convex_polygon<true,false>}, + {0, + 0, + 0, + _collision_capsule_capsule<true,false>, + _collision_capsule_convex_polygon<true,false>}, + {0, + 0, + 0, + 0, + _collision_convex_polygon_convex_polygon<true,false>} + + }; + + static const CollisionFunc collision_table_castB[5][5]={ + {_collision_segment_segment<false,true>, + _collision_segment_circle<false,true>, + _collision_segment_rectangle<false,true>, + _collision_segment_capsule<false,true>, + _collision_segment_convex_polygon<false,true>}, + {0, + _collision_circle_circle<false,true>, + _collision_circle_rectangle<false,true>, + _collision_circle_capsule<false,true>, + _collision_circle_convex_polygon<false,true>}, + {0, + 0, + _collision_rectangle_rectangle<false,true>, + _collision_rectangle_capsule<false,true>, + _collision_rectangle_convex_polygon<false,true>}, + {0, + 0, + 0, + _collision_capsule_capsule<false,true>, + _collision_capsule_convex_polygon<false,true>}, + {0, + 0, + 0, + 0, + _collision_convex_polygon_convex_polygon<false,true>} + + }; + + static const CollisionFunc collision_table_castA_castB[5][5]={ + {_collision_segment_segment<true,true>, + _collision_segment_circle<true,true>, + _collision_segment_rectangle<true,true>, + _collision_segment_capsule<true,true>, + _collision_segment_convex_polygon<true,true>}, + {0, + _collision_circle_circle<true,true>, + _collision_circle_rectangle<true,true>, + _collision_circle_capsule<true,true>, + _collision_circle_convex_polygon<true,true>}, + {0, + 0, + _collision_rectangle_rectangle<true,true>, + _collision_rectangle_capsule<true,true>, + _collision_rectangle_convex_polygon<true,true>}, + {0, + 0, + 0, + _collision_capsule_capsule<true,true>, + _collision_capsule_convex_polygon<true,true>}, + {0, + 0, + 0, + 0, + _collision_convex_polygon_convex_polygon<true,true>} }; @@ -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<const LineShape2DSW*>(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<const ConcaveShape2DSW*>(p_shape_B); _ConcaveCollisionInfo2D cinfo; cinfo.transform_A=&p_transform_A; - cinfo.transform_inv_A=&p_transform_inv_A; cinfo.shape_A=p_shape_A; cinfo.transform_B=&p_transform_B; - cinfo.transform_inv_B=&p_transform_inv_B; + cinfo.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;i<cbk->amount;i++) { + + float d = cbk->ptr[i*2+0].distance_squared_to(cbk->ptr[i*2+1]); + if (d<min_depth) { + min_depth=d; + min_depth_idx=i; + } + + } + + float d = p_point_A.distance_squared_to(p_point_B); + if (d<min_depth) + return; + cbk->ptr[min_depth_idx*2+0]=p_point_A; + cbk->ptr[min_depth_idx*2+1]=p_point_B; + + + } 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<Body2DSW> body_owner; mutable RID_Owner<Joint2DSW> 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<ShapeOwner2DSW*,int>::Element *E=owners.front();E;E=E->next()) {
- ShapeOwner2DSW* co=(ShapeOwner2DSW*)E->key();
- co->_shape_changed();
- }
-}
-
-
-Vector2 Shape2DSW::get_support(const Vector2& p_normal) const {
-
- Vector2 res[2];
- int amnt;
- get_supports(p_normal,res,amnt);
- return res[0];
-}
-
-void Shape2DSW::add_owner(ShapeOwner2DSW *p_owner) {
-
- Map<ShapeOwner2DSW*,int>::Element *E=owners.find(p_owner);
- if (E) {
- E->get()++;
- } else {
- owners[p_owner]=1;
- }
-}
-
-void Shape2DSW::remove_owner(ShapeOwner2DSW *p_owner){
-
- Map<ShapeOwner2DSW*,int>::Element *E=owners.find(p_owner);
- ERR_FAIL_COND(!E);
- E->get()--;
- if (E->get()==0) {
- owners.erase(E);
- }
-
-}
-
-bool Shape2DSW::is_owner(ShapeOwner2DSW *p_owner) const{
-
- return owners.has(p_owner);
-
-}
-
-const Map<ShapeOwner2DSW*,int>& Shape2DSW::get_owners() const{
- return owners;
-}
-
-
-Shape2DSW::Shape2DSW() {
-
- custom_bias=0;
- configured=false;
-}
-
-
-Shape2DSW::~Shape2DSW() {
-
- ERR_FAIL_COND(owners.size());
-}
-
-
-/*********************************************************/
-/*********************************************************/
-/*********************************************************/
-
-
-
-void LineShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const {
-
- r_amount=0;
-}
-
-bool LineShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const {
-
- Vector2 segment= p_begin - p_end;
- real_t den=normal.dot( segment );
-
- //printf("den is %i\n",den);
- if (Math::abs(den)<=CMP_EPSILON) {
-
- return false;
- }
-
- real_t dist=(normal.dot( p_begin ) - d) / den;
- //printf("dist is %i\n",dist);
-
- if (dist<-CMP_EPSILON || dist > (1.0 +CMP_EPSILON)) {
-
- return false;
- }
-
- r_point = p_begin + segment * -dist;
- r_normal=normal;
-
- return true;
-}
-
-real_t LineShape2DSW::get_moment_of_inertia(float p_mass) const {
-
- return 0;
-}
-
-void LineShape2DSW::set_data(const Variant& p_data) {
-
- ERR_FAIL_COND(p_data.get_type()!=Variant::ARRAY);
- Array arr = p_data;
- ERR_FAIL_COND(arr.size()!=2);
- normal=arr[0];
- d=arr[1];
- configure(Rect2(Vector2(-1e4,-1e4),Vector2(1e4*2,1e4*2)));
-
-}
-
-Variant LineShape2DSW::get_data() const {
-
- Array arr;
- arr.resize(2);
- arr[0]=normal;
- arr[1]=d;
- return arr;
-}
-
-/*********************************************************/
-/*********************************************************/
-/*********************************************************/
-
-
-
-void RayShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const {
-
-
- r_amount=1;
-
- if (p_normal.y>0)
- *r_supports=Vector2(0,length);
- else
- *r_supports=Vector2();
-
-}
-
-bool RayShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const {
-
- return false; //rays can't be intersected
-
-}
-
-real_t RayShape2DSW::get_moment_of_inertia(float p_mass) const {
-
- return 0; //rays are mass-less
-}
-
-void RayShape2DSW::set_data(const Variant& p_data) {
-
- length=p_data;
- configure(Rect2(0,0,0.001,length));
-}
-
-Variant RayShape2DSW::get_data() const {
-
- return length;
-}
-
-
-/*********************************************************/
-/*********************************************************/
-/*********************************************************/
-
-
-
-void SegmentShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const {
-
- if (Math::abs(p_normal.dot(n))>_SEGMENT_IS_VALID_SUPPORT_TRESHOLD) {
- r_supports[0]=a;
- r_supports[1]=b;
- r_amount=2;
- return;
-
- }
-
- float dp=p_normal.dot(b-a);
- if (dp>0)
- *r_supports=b;
- else
- *r_supports=a;
- r_amount=1;
-
-}
-
-bool SegmentShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const {
-
- if (!Geometry::segment_intersects_segment_2d(p_begin,p_end,a,b,&r_point))
- return false;
-
- Vector2 d = p_end-p_begin;
- if (n.dot(p_begin) > n.dot(a)) {
- r_normal=n;
- } else {
- r_normal=-n;
- }
-
- return true;
-}
-
-real_t SegmentShape2DSW::get_moment_of_inertia(float p_mass) const {
-
- real_t l = b.distance_to(a);
- Vector2 ofs = (a+b)*0.5;
-
- return p_mass*(l*l/12.0f + ofs.length_squared());
-}
-
-void SegmentShape2DSW::set_data(const Variant& p_data) {
-
- ERR_FAIL_COND(p_data.get_type()!=Variant::RECT2);
-
- Rect2 r = p_data;
- a=r.pos;
- b=r.size;
- n=(b-a).tangent();
-
- Rect2 aabb;
- aabb.pos=a;
- aabb.expand_to(b);
- if (aabb.size.x==0)
- aabb.size.x=0.001;
- if (aabb.size.y==0)
- aabb.size.y=0.001;
- configure(aabb);
-}
-
-Variant SegmentShape2DSW::get_data() const {
-
- Rect2 r;
- r.pos=a;
- r.size=b;
- return r;
-}
-
-/*********************************************************/
-/*********************************************************/
-/*********************************************************/
-
-
-
-void CircleShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const {
-
- r_amount=1;
- *r_supports=p_normal*radius;
-
-}
-
-bool CircleShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const {
-
-
- Vector2 line_vec = p_end - p_begin;
-
- real_t a, b, c;
-
- a = line_vec.dot(line_vec);
- b = 2 * p_begin.dot(line_vec);
- c = p_begin.dot(p_begin) - radius * radius;
-
- real_t sqrtterm = b*b - 4*a*c;
-
- if(sqrtterm < 0)
- return false;
- sqrtterm = Math::sqrt(sqrtterm);
- real_t res = ( -b - sqrtterm ) / (2 * a);
-
- if (res <0 || res >1+CMP_EPSILON) {
- return false;
- }
-
- r_point=p_begin+line_vec*res;
- r_normal=r_point.normalized();
- return true;
-}
-
-real_t CircleShape2DSW::get_moment_of_inertia(float p_mass) const {
-
- return radius*radius;
-}
-
-void CircleShape2DSW::set_data(const Variant& p_data) {
-
- ERR_FAIL_COND(!p_data.is_num());
- radius=p_data;
- configure(Rect2(-radius,-radius,radius*2,radius*2));
-}
-
-Variant CircleShape2DSW::get_data() const {
-
- return radius;
-}
-
-
-/*********************************************************/
-/*********************************************************/
-/*********************************************************/
-
-
-
-void RectangleShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const {
-
- for(int i=0;i<2;i++) {
-
- Vector2 ag;
- ag[i]=1.0;
- float dp = ag.dot(p_normal);
- if (Math::abs(dp)<_SEGMENT_IS_VALID_SUPPORT_TRESHOLD)
- continue;
-
- float sgn = dp>0 ? 1.0 : -1.0;
-
- r_amount=2;
-
- r_supports[0][i]=half_extents[i]*sgn;
- r_supports[0][i^1]=half_extents[i^1];
-
- r_supports[1][i]=half_extents[i]*sgn;
- r_supports[1][i^1]=-half_extents[i^1];
-
- return;
-
-
- }
-
- /* USE POINT */
-
- r_amount=1;
- r_supports[0]=Vector2(
- (p_normal.x<0) ? -half_extents.x : half_extents.x,
- (p_normal.y<0) ? -half_extents.y : half_extents.y
- );
-
-}
-
-bool RectangleShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const {
-
-
- return get_aabb().intersects_segment(p_begin,p_end,&r_point,&r_normal);
-}
-
-real_t RectangleShape2DSW::get_moment_of_inertia(float p_mass) const {
-
- Vector2 he2=half_extents*2;
- return p_mass*he2.dot(he2)/12.0f;
-}
-
-void RectangleShape2DSW::set_data(const Variant& p_data) {
-
- ERR_FAIL_COND(p_data.get_type()!=Variant::VECTOR2);
-
- half_extents=p_data;
- configure(Rect2(-half_extents,half_extents*2.0));
-}
-
-Variant RectangleShape2DSW::get_data() const {
-
- return half_extents;
-}
-
-
-
-/*********************************************************/
-/*********************************************************/
-/*********************************************************/
-
-
-
-void CapsuleShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const {
-
- Vector2 n=p_normal;
-
- float d = n.y;
-
- if (Math::abs( d )<(1.0-_SEGMENT_IS_VALID_SUPPORT_TRESHOLD) ) {
-
- // make it flat
- n.y=0.0;
- n.normalize();
- n*=radius;
-
- r_amount=2;
- r_supports[0]=n;
- r_supports[0].y+=height*0.5;
- r_supports[1]=n;
- r_supports[1].y-=height*0.5;
-
- } else {
-
- float h = (d > 0) ? height : -height;
-
- n*=radius;
- n.y += h*0.5;
- r_amount=1;
- *r_supports=n;
-
- }
-}
-
-bool CapsuleShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const {
-
-
- float d = 1e10;
- Vector2 n = (p_end-p_begin).normalized();
- bool collided=false;
-
- //try spheres
- for(int i=0;i<2;i++) {
-
- Vector2 begin = p_begin;
- Vector2 end = p_end;
- float ofs = (i==0)?-height*0.5:height*0.5;
- begin.y+=ofs;
- end.y+=ofs;
-
- Vector2 line_vec = end - begin;
-
- real_t a, b, c;
-
- a = line_vec.dot(line_vec);
- b = 2 * begin.dot(line_vec);
- c = begin.dot(begin) - radius * radius;
-
- real_t sqrtterm = b*b - 4*a*c;
-
- if(sqrtterm < 0)
- continue;
-
- sqrtterm = Math::sqrt(sqrtterm);
- real_t res = ( -b - sqrtterm ) / (2 * a);
-
- if (res <0 || res >1+CMP_EPSILON) {
- continue;
- }
-
- Vector2 point = begin+line_vec*res;
- Vector2 pointf(point.x,point.y-ofs);
- real_t pd = n.dot(pointf);
- if (pd<d) {
- r_point=pointf;
- r_normal=point.normalized();
- d=pd;
- collided=true;
- }
- }
-
-
- Vector2 rpos,rnorm;
- if (Rect2( Point2(-radius,-height*0.5), Size2(radius*2.0,height) ).intersects_segment(p_begin,p_end,&rpos,&rnorm)) {
-
- real_t pd = n.dot(rpos);
- if (pd<d) {
- r_point=rpos;
- r_normal=rnorm;
- d=pd;
- collided=true;
- }
- }
-
- //return get_aabb().intersects_segment(p_begin,p_end,&r_point,&r_normal);
- return collided; //todo
-}
-
-real_t CapsuleShape2DSW::get_moment_of_inertia(float p_mass) const {
-
- Vector2 he2(radius*2,height+radius*2);
- return p_mass*he2.dot(he2)/12.0f;
-}
-
-void CapsuleShape2DSW::set_data(const Variant& p_data) {
-
- ERR_FAIL_COND(p_data.get_type()!=Variant::ARRAY && p_data.get_type()!=Variant::VECTOR2);
-
- if (p_data.get_type()==Variant::ARRAY) {
- Array arr=p_data;
- ERR_FAIL_COND(arr.size()!=2);
- height=arr[0];
- radius=arr[1];
- } else {
-
- Point2 p = p_data;
- radius=p.x;
- height=p.y;
- }
-
- Point2 he(radius,height*0.5+radius);
- configure(Rect2(-he,he*2));
-
-}
-
-Variant CapsuleShape2DSW::get_data() const {
-
- return Point2(height,radius);
-}
-
-
-
-/*********************************************************/
-/*********************************************************/
-/*********************************************************/
-
-
-
-void ConvexPolygonShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const {
-
- int support_idx=-1;
- real_t d=-1e10;
-
- for(int i=0;i<point_count;i++) {
-
- //test point
- real_t ld = p_normal.dot(points[i].pos);
- if (ld>d) {
- support_idx=i;
- d=ld;
- }
-
- //test segment
- if (points[i].normal.dot(p_normal)>_SEGMENT_IS_VALID_SUPPORT_TRESHOLD) {
-
- r_amount=2;
- r_supports[0]=points[i].pos;
- r_supports[1]=points[(i+1)%point_count].pos;
- return;
- }
- }
-
- ERR_FAIL_COND(support_idx==-1);
-
- r_amount=1;
- r_supports[0]=points[support_idx].pos;
-
-}
-
-bool ConvexPolygonShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const {
-
- Vector2 n = (p_end-p_begin).normalized();
- real_t d=1e10;
- bool inters=false;
-
- for(int i=0;i<point_count;i++) {
-
- //hmm crap.. no can do..
- //if (d.dot(points[i].normal)>=0)
- // continue;
-
-
- Vector2 res;
-
- if (!Geometry::segment_intersects_segment_2d(p_begin,p_end,points[i].pos,points[(i+1)%point_count].pos,&res))
- continue;
-
- float nd = n.dot(res);
- if (nd<d) {
-
- d=nd;
- r_point=res;
- r_normal=points[i].normal;
- inters=true;
- }
-
- }
-
-
- if (inters) {
-
- if (n.dot(r_normal)>0)
- r_normal=-r_normal;
- }
-
- //return get_aabb().intersects_segment(p_begin,p_end,&r_point,&r_normal);
- return inters; //todo
-}
-
-real_t ConvexPolygonShape2DSW::get_moment_of_inertia(float p_mass) const {
-
- Rect2 aabb;
- aabb.pos=points[0].pos;
- for(int i=0;i<point_count;i++) {
-
- aabb.expand_to(points[i].pos);
- }
-
- return p_mass*aabb.size.dot(aabb.size)/12.0f;
-}
-
-void ConvexPolygonShape2DSW::set_data(const Variant& p_data) {
-
- ERR_FAIL_COND(p_data.get_type()!=Variant::VECTOR2_ARRAY && p_data.get_type()!=Variant::REAL_ARRAY);
-
-
- if (points)
- memdelete_arr(points);
- points=NULL;
- point_count=0;
-
- if (p_data.get_type()==Variant::VECTOR2_ARRAY) {
- DVector<Vector2> arr=p_data;
- ERR_FAIL_COND(arr.size()==0);
- point_count=arr.size();
- points = memnew_arr(Point,point_count);
- DVector<Vector2>::Read r = arr.read();
-
- for(int i=0;i<point_count;i++) {
- points[i].pos=r[i];
- }
-
- for(int i=0;i<point_count;i++) {
-
- Vector2 p = points[i].pos;
- Vector2 pn = points[(i+1)%point_count].pos;
- points[i].normal=(pn-p).tangent().normalized();
- }
- } else {
-
- DVector<real_t> dvr = p_data;
- point_count=dvr.size()/4;
- ERR_FAIL_COND(point_count==0);
-
- points = memnew_arr(Point,point_count);
- DVector<real_t>::Read r = dvr.read();
-
- for(int i=0;i<point_count;i++) {
-
- int idx=i<<2;
- points[i].pos.x=r[idx+0];
- points[i].pos.y=r[idx+1];
- points[i].normal.x=r[idx+2];
- points[i].normal.y=r[idx+3];
-
- }
- }
-
-
- ERR_FAIL_COND(point_count==0);
- Rect2 aabb;
- aabb.pos=points[0].pos;
- for(int i=1;i<point_count;i++)
- aabb.expand_to(points[i].pos);
-
- configure(aabb);
-}
-
-Variant ConvexPolygonShape2DSW::get_data() const {
-
- DVector<Vector2> dvr;
-
- dvr.resize(point_count);
-
- for(int i=0;i<point_count;i++) {
- dvr.set(i,points[i].pos);
- }
-
- return dvr;
-}
-
-
-ConvexPolygonShape2DSW::ConvexPolygonShape2DSW() {
-
- points=NULL;
- point_count=0;
-
-}
-
-ConvexPolygonShape2DSW::~ConvexPolygonShape2DSW(){
-
- if (points)
- memdelete_arr(points);
-
-}
-
-//////////////////////////////////////////////////
-
-
-void ConcavePolygonShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const {
-
- real_t d=-1e10;
- int idx=-1;
- for(int i=0;i<points.size();i++) {
-
- real_t ld = p_normal.dot(points[i]);
- if (ld>d) {
- d=ld;
- idx=i;
- }
- }
-
-
- r_amount=1;
- ERR_FAIL_COND(idx==-1);
- *r_supports=points[idx];
-
-}
-
-bool ConcavePolygonShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const{
-
- uint32_t* stack = (uint32_t*)alloca(sizeof(int)*bvh_depth);
-
- enum {
- TEST_AABB_BIT=0,
- VISIT_LEFT_BIT=1,
- VISIT_RIGHT_BIT=2,
- VISIT_DONE_BIT=3,
- VISITED_BIT_SHIFT=29,
- NODE_IDX_MASK=(1<<VISITED_BIT_SHIFT)-1,
- VISITED_BIT_MASK=~NODE_IDX_MASK,
-
-
- };
-
- Vector2 n = (p_end-p_begin).normalized();
- real_t d=1e10;
- bool inters=false;
-
- //for(int i=0;i<bvh_depth;i++)
- // stack[i]=0;
-
- int level=0;
-
- const Segment *segmentptr=&segments[0];
- const Vector2 *pointptr=&points[0];
- const BVH *bvhptr = &bvh[0];
- int pos=bvh.size()-1;
-
-
- stack[0]=0;
- while(true) {
-
- uint32_t node = stack[level]&NODE_IDX_MASK;
- const BVH &b = bvhptr[ node ];
- bool done=false;
-
- switch(stack[level]>>VISITED_BIT_SHIFT) {
- case TEST_AABB_BIT: {
-
-
- bool valid = b.aabb.intersects_segment(p_begin,p_end);
- if (!valid) {
-
- stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node;
-
- } else {
-
- if (b.left<0) {
-
- const Segment &s=segmentptr[ b.right ];
- Vector2 a = pointptr[ s.points[0] ];
- Vector2 b = pointptr[ s.points[1] ];
-
-
- Vector2 res;
-
- if (Geometry::segment_intersects_segment_2d(p_begin,p_end,a,b,&res)) {
-
- float nd = n.dot(res);
- if (nd<d) {
-
- d=nd;
- r_point=res;
- r_normal=(b-a).tangent().normalized();
- inters=true;
- }
-
- }
-
- stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node;
-
- } else {
-
- stack[level]=(VISIT_LEFT_BIT<<VISITED_BIT_SHIFT)|node;
- }
- }
-
- } continue;
- case VISIT_LEFT_BIT: {
-
- stack[level]=(VISIT_RIGHT_BIT<<VISITED_BIT_SHIFT)|node;
- stack[level+1]=b.left|TEST_AABB_BIT;
- level++;
-
- } continue;
- case VISIT_RIGHT_BIT: {
-
- stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node;
- stack[level+1]=b.right|TEST_AABB_BIT;
- level++;
- } continue;
- case VISIT_DONE_BIT: {
-
- if (level==0) {
- done=true;
- break;
- } else
- level--;
-
- } continue;
- }
-
-
- if (done)
- break;
- }
-
-
- if (inters) {
-
- if (n.dot(r_normal)>0)
- r_normal=-r_normal;
- }
-
- return inters;
-
-}
-
-
-
-int ConcavePolygonShape2DSW::_generate_bvh(BVH *p_bvh,int p_len,int p_depth) {
-
- if (p_len==1) {
-
- bvh_depth=MAX(p_depth,bvh_depth);
- bvh.push_back(*p_bvh);
- return bvh.size()-1;
- }
-
- //else sort best
-
- Rect2 global_aabb=p_bvh[0].aabb;
- for(int i=1;i<p_len;i++) {
- global_aabb=global_aabb.merge(p_bvh[i].aabb);
- }
-
- if (global_aabb.size.x > global_aabb.size.y) {
-
- SortArray<BVH,BVH_CompareX> sort;
- sort.sort(p_bvh,p_len);
-
- } else {
-
- SortArray<BVH,BVH_CompareY> sort;
- sort.sort(p_bvh,p_len);
-
- }
-
- int median = p_len/2;
-
-
- BVH node;
- node.aabb=global_aabb;
- int node_idx = bvh.size();
- bvh.push_back(node);
-
- int l = _generate_bvh(p_bvh,median,p_depth+1);
- int r = _generate_bvh(&p_bvh[median],p_len-median,p_depth+1);
- bvh[node_idx].left=l;
- bvh[node_idx].right=r;
-
- return node_idx;
-
-}
-
-void ConcavePolygonShape2DSW::set_data(const Variant& p_data) {
-
- ERR_FAIL_COND(p_data.get_type()!=Variant::VECTOR2_ARRAY && p_data.get_type()!=Variant::REAL_ARRAY);
-
- segments.clear();;
- points.clear();;
- bvh.clear();;
- bvh_depth=1;
-
- Rect2 aabb;
-
- if (p_data.get_type()==Variant::VECTOR2_ARRAY) {
-
- DVector<Vector2> p2arr = p_data;
- int len = p2arr.size();
- DVector<Vector2>::Read arr = p2arr.read();
-
-
- Map<Point2,int> pointmap;
- for(int i=0;i<len;i+=2) {
-
- Point2 p1 =arr[i];
- Point2 p2 =arr[i+1];
- int idx_p1,idx_p2;
- if (p1==p2)
- continue; //don't want it
-
- if (pointmap.has(p1)) {
- idx_p1=pointmap[p1];
- } else {
- idx_p1=pointmap.size();
- pointmap[p1]=idx_p1;
- }
-
- if (pointmap.has(p2)) {
- idx_p2=pointmap[p2];
- } else {
- idx_p2=pointmap.size();
- pointmap[p2]=idx_p2;
- }
-
- Segment s;
- s.points[0]=idx_p1;
- s.points[1]=idx_p2;
- segments.push_back(s);
- }
-
- points.resize(pointmap.size());
- aabb.pos=pointmap.front()->key();
- for(Map<Point2,int>::Element *E=pointmap.front();E;E=E->next()) {
-
- aabb.expand_to(E->key());
- points[E->get()]=E->key();
- }
-
- Vector<BVH> main_vbh;
- main_vbh.resize(segments.size());
- for(int i=0;i<main_vbh.size();i++) {
-
-
- main_vbh[i].aabb.pos=points[segments[i].points[0]];
- main_vbh[i].aabb.expand_to(points[segments[i].points[1]]);
- main_vbh[i].left=-1;
- main_vbh[i].right=i;
- }
-
- _generate_bvh(&main_vbh[0],main_vbh.size(),1);
-
-
- } else {
- //dictionary with arrays
-
- }
-
-
- configure(aabb);
-}
-Variant ConcavePolygonShape2DSW::get_data() const {
-
-
- DVector<Vector2> rsegments;
- int len = segments.size();
- rsegments.resize(len*2);
- DVector<Vector2>::Write w = rsegments.write();
- for(int i=0;i<len;i++) {
-
- w[(i<<1)+0]=points[segments[i].points[0]];
- w[(i<<1)+1]=points[segments[i].points[1]];
- }
-
- w=DVector<Vector2>::Write();
-
- return rsegments;
-}
-
-void ConcavePolygonShape2DSW::cull(const Rect2& p_local_aabb,Callback p_callback,void* p_userdata) const {
-
- uint32_t* stack = (uint32_t*)alloca(sizeof(int)*bvh_depth);
-
- enum {
- TEST_AABB_BIT=0,
- VISIT_LEFT_BIT=1,
- VISIT_RIGHT_BIT=2,
- VISIT_DONE_BIT=3,
- VISITED_BIT_SHIFT=29,
- NODE_IDX_MASK=(1<<VISITED_BIT_SHIFT)-1,
- VISITED_BIT_MASK=~NODE_IDX_MASK,
-
-
- };
-
- //for(int i=0;i<bvh_depth;i++)
- // stack[i]=0;
-
-
- int level=0;
-
- const Segment *segmentptr=&segments[0];
- const Vector2 *pointptr=&points[0];
- const BVH *bvhptr = &bvh[0];
- int pos=bvh.size()-1;
-
-
- stack[0]=0;
- while(true) {
-
- uint32_t node = stack[level]&NODE_IDX_MASK;
- const BVH &b = bvhptr[ node ];
-
- switch(stack[level]>>VISITED_BIT_SHIFT) {
- case TEST_AABB_BIT: {
-
-
- bool valid = p_local_aabb.intersects(b.aabb);
- if (!valid) {
-
- stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node;
-
- } else {
-
- if (b.left<0) {
-
- const Segment &s=segmentptr[ b.right ];
- Vector2 a = pointptr[ s.points[0] ];
- Vector2 b = pointptr[ s.points[1] ];
-
- SegmentShape2DSW ss(a,b,(b-a).tangent().normalized());
-
- p_callback(p_userdata,&ss);
- stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node;
-
- } else {
-
- stack[level]=(VISIT_LEFT_BIT<<VISITED_BIT_SHIFT)|node;
- }
- }
-
- } continue;
- case VISIT_LEFT_BIT: {
-
- stack[level]=(VISIT_RIGHT_BIT<<VISITED_BIT_SHIFT)|node;
- stack[level+1]=b.left|TEST_AABB_BIT;
- level++;
-
- } continue;
- case VISIT_RIGHT_BIT: {
-
- stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node;
- stack[level+1]=b.right|TEST_AABB_BIT;
- level++;
- } continue;
- case VISIT_DONE_BIT: {
-
- if (level==0)
- return;
- else
- level--;
-
- } continue;
- }
- }
-
-}
-
-
+#include "shape_2d_sw.h" +#include "geometry.h" +#include "sort.h" + + + +void Shape2DSW::configure(const Rect2& p_aabb) { + aabb=p_aabb; + configured=true; + for (Map<ShapeOwner2DSW*,int>::Element *E=owners.front();E;E=E->next()) { + ShapeOwner2DSW* co=(ShapeOwner2DSW*)E->key(); + co->_shape_changed(); + } +} + + +Vector2 Shape2DSW::get_support(const Vector2& p_normal) const { + + Vector2 res[2]; + int amnt; + get_supports(p_normal,res,amnt); + return res[0]; +} + +void Shape2DSW::add_owner(ShapeOwner2DSW *p_owner) { + + Map<ShapeOwner2DSW*,int>::Element *E=owners.find(p_owner); + if (E) { + E->get()++; + } else { + owners[p_owner]=1; + } +} + +void Shape2DSW::remove_owner(ShapeOwner2DSW *p_owner){ + + Map<ShapeOwner2DSW*,int>::Element *E=owners.find(p_owner); + ERR_FAIL_COND(!E); + E->get()--; + if (E->get()==0) { + owners.erase(E); + } + +} + +bool Shape2DSW::is_owner(ShapeOwner2DSW *p_owner) const{ + + return owners.has(p_owner); + +} + +const Map<ShapeOwner2DSW*,int>& Shape2DSW::get_owners() const{ + return owners; +} + + +Shape2DSW::Shape2DSW() { + + custom_bias=0; + configured=false; +} + + +Shape2DSW::~Shape2DSW() { + + ERR_FAIL_COND(owners.size()); +} + + +/*********************************************************/ +/*********************************************************/ +/*********************************************************/ + + + +void LineShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const { + + r_amount=0; +} + +bool LineShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const { + + Vector2 segment= p_begin - p_end; + real_t den=normal.dot( segment ); + + //printf("den is %i\n",den); + if (Math::abs(den)<=CMP_EPSILON) { + + return false; + } + + real_t dist=(normal.dot( p_begin ) - d) / den; + //printf("dist is %i\n",dist); + + if (dist<-CMP_EPSILON || dist > (1.0 +CMP_EPSILON)) { + + return false; + } + + r_point = p_begin + segment * -dist; + r_normal=normal; + + return true; +} + +real_t LineShape2DSW::get_moment_of_inertia(float p_mass) const { + + return 0; +} + +void LineShape2DSW::set_data(const Variant& p_data) { + + ERR_FAIL_COND(p_data.get_type()!=Variant::ARRAY); + Array arr = p_data; + ERR_FAIL_COND(arr.size()!=2); + normal=arr[0]; + d=arr[1]; + configure(Rect2(Vector2(-1e4,-1e4),Vector2(1e4*2,1e4*2))); + +} + +Variant LineShape2DSW::get_data() const { + + Array arr; + arr.resize(2); + arr[0]=normal; + arr[1]=d; + return arr; +} + +/*********************************************************/ +/*********************************************************/ +/*********************************************************/ + + + +void RayShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const { + + + r_amount=1; + + if (p_normal.y>0) + *r_supports=Vector2(0,length); + else + *r_supports=Vector2(); + +} + +bool RayShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const { + + return false; //rays can't be intersected + +} + +real_t RayShape2DSW::get_moment_of_inertia(float p_mass) const { + + return 0; //rays are mass-less +} + +void RayShape2DSW::set_data(const Variant& p_data) { + + length=p_data; + configure(Rect2(0,0,0.001,length)); +} + +Variant RayShape2DSW::get_data() const { + + return length; +} + + +/*********************************************************/ +/*********************************************************/ +/*********************************************************/ + + + +void SegmentShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const { + + if (Math::abs(p_normal.dot(n))>_SEGMENT_IS_VALID_SUPPORT_TRESHOLD) { + r_supports[0]=a; + r_supports[1]=b; + r_amount=2; + return; + + } + + float dp=p_normal.dot(b-a); + if (dp>0) + *r_supports=b; + else + *r_supports=a; + r_amount=1; + +} + +bool SegmentShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const { + + if (!Geometry::segment_intersects_segment_2d(p_begin,p_end,a,b,&r_point)) + return false; + + Vector2 d = p_end-p_begin; + if (n.dot(p_begin) > n.dot(a)) { + r_normal=n; + } else { + r_normal=-n; + } + + return true; +} + +real_t SegmentShape2DSW::get_moment_of_inertia(float p_mass) const { + + real_t l = b.distance_to(a); + Vector2 ofs = (a+b)*0.5; + + return p_mass*(l*l/12.0f + ofs.length_squared()); +} + +void SegmentShape2DSW::set_data(const Variant& p_data) { + + ERR_FAIL_COND(p_data.get_type()!=Variant::RECT2); + + Rect2 r = p_data; + a=r.pos; + b=r.size; + n=(b-a).tangent(); + + Rect2 aabb; + aabb.pos=a; + aabb.expand_to(b); + if (aabb.size.x==0) + aabb.size.x=0.001; + if (aabb.size.y==0) + aabb.size.y=0.001; + configure(aabb); +} + +Variant SegmentShape2DSW::get_data() const { + + Rect2 r; + r.pos=a; + r.size=b; + return r; +} + +/*********************************************************/ +/*********************************************************/ +/*********************************************************/ + + + +void CircleShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const { + + r_amount=1; + *r_supports=p_normal*radius; + +} + +bool CircleShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const { + + + Vector2 line_vec = p_end - p_begin; + + real_t a, b, c; + + a = line_vec.dot(line_vec); + b = 2 * p_begin.dot(line_vec); + c = p_begin.dot(p_begin) - radius * radius; + + real_t sqrtterm = b*b - 4*a*c; + + if(sqrtterm < 0) + return false; + sqrtterm = Math::sqrt(sqrtterm); + real_t res = ( -b - sqrtterm ) / (2 * a); + + if (res <0 || res >1+CMP_EPSILON) { + return false; + } + + r_point=p_begin+line_vec*res; + r_normal=r_point.normalized(); + return true; +} + +real_t CircleShape2DSW::get_moment_of_inertia(float p_mass) const { + + return radius*radius; +} + +void CircleShape2DSW::set_data(const Variant& p_data) { + + ERR_FAIL_COND(!p_data.is_num()); + radius=p_data; + configure(Rect2(-radius,-radius,radius*2,radius*2)); +} + +Variant CircleShape2DSW::get_data() const { + + return radius; +} + + +/*********************************************************/ +/*********************************************************/ +/*********************************************************/ + + + +void RectangleShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const { + + for(int i=0;i<2;i++) { + + Vector2 ag; + ag[i]=1.0; + float dp = ag.dot(p_normal); + if (Math::abs(dp)<_SEGMENT_IS_VALID_SUPPORT_TRESHOLD) + continue; + + float sgn = dp>0 ? 1.0 : -1.0; + + r_amount=2; + + r_supports[0][i]=half_extents[i]*sgn; + r_supports[0][i^1]=half_extents[i^1]; + + r_supports[1][i]=half_extents[i]*sgn; + r_supports[1][i^1]=-half_extents[i^1]; + + return; + + + } + + /* USE POINT */ + + r_amount=1; + r_supports[0]=Vector2( + (p_normal.x<0) ? -half_extents.x : half_extents.x, + (p_normal.y<0) ? -half_extents.y : half_extents.y + ); + +} + +bool RectangleShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const { + + + return get_aabb().intersects_segment(p_begin,p_end,&r_point,&r_normal); +} + +real_t RectangleShape2DSW::get_moment_of_inertia(float p_mass) const { + + Vector2 he2=half_extents*2; + return p_mass*he2.dot(he2)/12.0f; +} + +void RectangleShape2DSW::set_data(const Variant& p_data) { + + ERR_FAIL_COND(p_data.get_type()!=Variant::VECTOR2); + + half_extents=p_data; + configure(Rect2(-half_extents,half_extents*2.0)); +} + +Variant RectangleShape2DSW::get_data() const { + + return half_extents; +} + + + +/*********************************************************/ +/*********************************************************/ +/*********************************************************/ + + + +void CapsuleShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const { + + Vector2 n=p_normal; + + float d = n.y; + + if (Math::abs( d )<(1.0-_SEGMENT_IS_VALID_SUPPORT_TRESHOLD) ) { + + // make it flat + n.y=0.0; + n.normalize(); + n*=radius; + + r_amount=2; + r_supports[0]=n; + r_supports[0].y+=height*0.5; + r_supports[1]=n; + r_supports[1].y-=height*0.5; + + } else { + + float h = (d > 0) ? height : -height; + + n*=radius; + n.y += h*0.5; + r_amount=1; + *r_supports=n; + + } +} + +bool CapsuleShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const { + + + float d = 1e10; + Vector2 n = (p_end-p_begin).normalized(); + bool collided=false; + + //try spheres + for(int i=0;i<2;i++) { + + Vector2 begin = p_begin; + Vector2 end = p_end; + float ofs = (i==0)?-height*0.5:height*0.5; + begin.y+=ofs; + end.y+=ofs; + + Vector2 line_vec = end - begin; + + real_t a, b, c; + + a = line_vec.dot(line_vec); + b = 2 * begin.dot(line_vec); + c = begin.dot(begin) - radius * radius; + + real_t sqrtterm = b*b - 4*a*c; + + if(sqrtterm < 0) + continue; + + sqrtterm = Math::sqrt(sqrtterm); + real_t res = ( -b - sqrtterm ) / (2 * a); + + if (res <0 || res >1+CMP_EPSILON) { + continue; + } + + Vector2 point = begin+line_vec*res; + Vector2 pointf(point.x,point.y-ofs); + real_t pd = n.dot(pointf); + if (pd<d) { + r_point=pointf; + r_normal=point.normalized(); + d=pd; + collided=true; + } + } + + + Vector2 rpos,rnorm; + if (Rect2( Point2(-radius,-height*0.5), Size2(radius*2.0,height) ).intersects_segment(p_begin,p_end,&rpos,&rnorm)) { + + real_t pd = n.dot(rpos); + if (pd<d) { + r_point=rpos; + r_normal=rnorm; + d=pd; + collided=true; + } + } + + //return get_aabb().intersects_segment(p_begin,p_end,&r_point,&r_normal); + return collided; //todo +} + +real_t CapsuleShape2DSW::get_moment_of_inertia(float p_mass) const { + + Vector2 he2(radius*2,height+radius*2); + return p_mass*he2.dot(he2)/12.0f; +} + +void CapsuleShape2DSW::set_data(const Variant& p_data) { + + ERR_FAIL_COND(p_data.get_type()!=Variant::ARRAY && p_data.get_type()!=Variant::VECTOR2); + + if (p_data.get_type()==Variant::ARRAY) { + Array arr=p_data; + ERR_FAIL_COND(arr.size()!=2); + height=arr[0]; + radius=arr[1]; + } else { + + Point2 p = p_data; + radius=p.x; + height=p.y; + } + + Point2 he(radius,height*0.5+radius); + configure(Rect2(-he,he*2)); + +} + +Variant CapsuleShape2DSW::get_data() const { + + return Point2(height,radius); +} + + + +/*********************************************************/ +/*********************************************************/ +/*********************************************************/ + + + +void ConvexPolygonShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const { + + int support_idx=-1; + real_t d=-1e10; + + for(int i=0;i<point_count;i++) { + + //test point + real_t ld = p_normal.dot(points[i].pos); + if (ld>d) { + support_idx=i; + d=ld; + } + + //test segment + if (points[i].normal.dot(p_normal)>_SEGMENT_IS_VALID_SUPPORT_TRESHOLD) { + + r_amount=2; + r_supports[0]=points[i].pos; + r_supports[1]=points[(i+1)%point_count].pos; + return; + } + } + + ERR_FAIL_COND(support_idx==-1); + + r_amount=1; + r_supports[0]=points[support_idx].pos; + +} + +bool ConvexPolygonShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const { + + Vector2 n = (p_end-p_begin).normalized(); + real_t d=1e10; + bool inters=false; + + for(int i=0;i<point_count;i++) { + + //hmm crap.. no can do.. + //if (d.dot(points[i].normal)>=0) + // continue; + + + Vector2 res; + + if (!Geometry::segment_intersects_segment_2d(p_begin,p_end,points[i].pos,points[(i+1)%point_count].pos,&res)) + continue; + + float nd = n.dot(res); + if (nd<d) { + + d=nd; + r_point=res; + r_normal=points[i].normal; + inters=true; + } + + } + + + if (inters) { + + if (n.dot(r_normal)>0) + r_normal=-r_normal; + } + + //return get_aabb().intersects_segment(p_begin,p_end,&r_point,&r_normal); + return inters; //todo +} + +real_t ConvexPolygonShape2DSW::get_moment_of_inertia(float p_mass) const { + + Rect2 aabb; + aabb.pos=points[0].pos; + for(int i=0;i<point_count;i++) { + + aabb.expand_to(points[i].pos); + } + + return p_mass*aabb.size.dot(aabb.size)/12.0f; +} + +void ConvexPolygonShape2DSW::set_data(const Variant& p_data) { + + ERR_FAIL_COND(p_data.get_type()!=Variant::VECTOR2_ARRAY && p_data.get_type()!=Variant::REAL_ARRAY); + + + if (points) + memdelete_arr(points); + points=NULL; + point_count=0; + + if (p_data.get_type()==Variant::VECTOR2_ARRAY) { + DVector<Vector2> arr=p_data; + ERR_FAIL_COND(arr.size()==0); + point_count=arr.size(); + points = memnew_arr(Point,point_count); + DVector<Vector2>::Read r = arr.read(); + + for(int i=0;i<point_count;i++) { + points[i].pos=r[i]; + } + + for(int i=0;i<point_count;i++) { + + Vector2 p = points[i].pos; + Vector2 pn = points[(i+1)%point_count].pos; + points[i].normal=(pn-p).tangent().normalized(); + } + } else { + + DVector<real_t> dvr = p_data; + point_count=dvr.size()/4; + ERR_FAIL_COND(point_count==0); + + points = memnew_arr(Point,point_count); + DVector<real_t>::Read r = dvr.read(); + + for(int i=0;i<point_count;i++) { + + int idx=i<<2; + points[i].pos.x=r[idx+0]; + points[i].pos.y=r[idx+1]; + points[i].normal.x=r[idx+2]; + points[i].normal.y=r[idx+3]; + + } + } + + + ERR_FAIL_COND(point_count==0); + Rect2 aabb; + aabb.pos=points[0].pos; + for(int i=1;i<point_count;i++) + aabb.expand_to(points[i].pos); + + configure(aabb); +} + +Variant ConvexPolygonShape2DSW::get_data() const { + + DVector<Vector2> dvr; + + dvr.resize(point_count); + + for(int i=0;i<point_count;i++) { + dvr.set(i,points[i].pos); + } + + return dvr; +} + + +ConvexPolygonShape2DSW::ConvexPolygonShape2DSW() { + + points=NULL; + point_count=0; + +} + +ConvexPolygonShape2DSW::~ConvexPolygonShape2DSW(){ + + if (points) + memdelete_arr(points); + +} + +////////////////////////////////////////////////// + + +void ConcavePolygonShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const { + + real_t d=-1e10; + int idx=-1; + for(int i=0;i<points.size();i++) { + + real_t ld = p_normal.dot(points[i]); + if (ld>d) { + d=ld; + idx=i; + } + } + + + r_amount=1; + ERR_FAIL_COND(idx==-1); + *r_supports=points[idx]; + +} + +bool ConcavePolygonShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const{ + + uint32_t* stack = (uint32_t*)alloca(sizeof(int)*bvh_depth); + + enum { + TEST_AABB_BIT=0, + VISIT_LEFT_BIT=1, + VISIT_RIGHT_BIT=2, + VISIT_DONE_BIT=3, + VISITED_BIT_SHIFT=29, + NODE_IDX_MASK=(1<<VISITED_BIT_SHIFT)-1, + VISITED_BIT_MASK=~NODE_IDX_MASK, + + + }; + + Vector2 n = (p_end-p_begin).normalized(); + real_t d=1e10; + bool inters=false; + + //for(int i=0;i<bvh_depth;i++) + // stack[i]=0; + + int level=0; + + const Segment *segmentptr=&segments[0]; + const Vector2 *pointptr=&points[0]; + const BVH *bvhptr = &bvh[0]; + int pos=bvh.size()-1; + + + stack[0]=0; + while(true) { + + uint32_t node = stack[level]&NODE_IDX_MASK; + const BVH &b = bvhptr[ node ]; + bool done=false; + + switch(stack[level]>>VISITED_BIT_SHIFT) { + case TEST_AABB_BIT: { + + + bool valid = b.aabb.intersects_segment(p_begin,p_end); + if (!valid) { + + stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node; + + } else { + + if (b.left<0) { + + const Segment &s=segmentptr[ b.right ]; + Vector2 a = pointptr[ s.points[0] ]; + Vector2 b = pointptr[ s.points[1] ]; + + + Vector2 res; + + if (Geometry::segment_intersects_segment_2d(p_begin,p_end,a,b,&res)) { + + float nd = n.dot(res); + if (nd<d) { + + d=nd; + r_point=res; + r_normal=(b-a).tangent().normalized(); + inters=true; + } + + } + + stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node; + + } else { + + stack[level]=(VISIT_LEFT_BIT<<VISITED_BIT_SHIFT)|node; + } + } + + } continue; + case VISIT_LEFT_BIT: { + + stack[level]=(VISIT_RIGHT_BIT<<VISITED_BIT_SHIFT)|node; + stack[level+1]=b.left|TEST_AABB_BIT; + level++; + + } continue; + case VISIT_RIGHT_BIT: { + + stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node; + stack[level+1]=b.right|TEST_AABB_BIT; + level++; + } continue; + case VISIT_DONE_BIT: { + + if (level==0) { + done=true; + break; + } else + level--; + + } continue; + } + + + if (done) + break; + } + + + if (inters) { + + if (n.dot(r_normal)>0) + r_normal=-r_normal; + } + + return inters; + +} + + + +int ConcavePolygonShape2DSW::_generate_bvh(BVH *p_bvh,int p_len,int p_depth) { + + if (p_len==1) { + + bvh_depth=MAX(p_depth,bvh_depth); + bvh.push_back(*p_bvh); + return bvh.size()-1; + } + + //else sort best + + Rect2 global_aabb=p_bvh[0].aabb; + for(int i=1;i<p_len;i++) { + global_aabb=global_aabb.merge(p_bvh[i].aabb); + } + + if (global_aabb.size.x > global_aabb.size.y) { + + SortArray<BVH,BVH_CompareX> sort; + sort.sort(p_bvh,p_len); + + } else { + + SortArray<BVH,BVH_CompareY> sort; + sort.sort(p_bvh,p_len); + + } + + int median = p_len/2; + + + BVH node; + node.aabb=global_aabb; + int node_idx = bvh.size(); + bvh.push_back(node); + + int l = _generate_bvh(p_bvh,median,p_depth+1); + int r = _generate_bvh(&p_bvh[median],p_len-median,p_depth+1); + bvh[node_idx].left=l; + bvh[node_idx].right=r; + + return node_idx; + +} + +void ConcavePolygonShape2DSW::set_data(const Variant& p_data) { + + ERR_FAIL_COND(p_data.get_type()!=Variant::VECTOR2_ARRAY && p_data.get_type()!=Variant::REAL_ARRAY); + + segments.clear();; + points.clear();; + bvh.clear();; + bvh_depth=1; + + Rect2 aabb; + + if (p_data.get_type()==Variant::VECTOR2_ARRAY) { + + DVector<Vector2> p2arr = p_data; + int len = p2arr.size(); + DVector<Vector2>::Read arr = p2arr.read(); + + + Map<Point2,int> pointmap; + for(int i=0;i<len;i+=2) { + + Point2 p1 =arr[i]; + Point2 p2 =arr[i+1]; + int idx_p1,idx_p2; + if (p1==p2) + continue; //don't want it + + if (pointmap.has(p1)) { + idx_p1=pointmap[p1]; + } else { + idx_p1=pointmap.size(); + pointmap[p1]=idx_p1; + } + + if (pointmap.has(p2)) { + idx_p2=pointmap[p2]; + } else { + idx_p2=pointmap.size(); + pointmap[p2]=idx_p2; + } + + Segment s; + s.points[0]=idx_p1; + s.points[1]=idx_p2; + segments.push_back(s); + } + + points.resize(pointmap.size()); + aabb.pos=pointmap.front()->key(); + for(Map<Point2,int>::Element *E=pointmap.front();E;E=E->next()) { + + aabb.expand_to(E->key()); + points[E->get()]=E->key(); + } + + Vector<BVH> main_vbh; + main_vbh.resize(segments.size()); + for(int i=0;i<main_vbh.size();i++) { + + + main_vbh[i].aabb.pos=points[segments[i].points[0]]; + main_vbh[i].aabb.expand_to(points[segments[i].points[1]]); + main_vbh[i].left=-1; + main_vbh[i].right=i; + } + + _generate_bvh(&main_vbh[0],main_vbh.size(),1); + + + } else { + //dictionary with arrays + + } + + + configure(aabb); +} +Variant ConcavePolygonShape2DSW::get_data() const { + + + DVector<Vector2> rsegments; + int len = segments.size(); + rsegments.resize(len*2); + DVector<Vector2>::Write w = rsegments.write(); + for(int i=0;i<len;i++) { + + w[(i<<1)+0]=points[segments[i].points[0]]; + w[(i<<1)+1]=points[segments[i].points[1]]; + } + + w=DVector<Vector2>::Write(); + + return rsegments; +} + +void ConcavePolygonShape2DSW::cull(const Rect2& p_local_aabb,Callback p_callback,void* p_userdata) const { + + uint32_t* stack = (uint32_t*)alloca(sizeof(int)*bvh_depth); + + enum { + TEST_AABB_BIT=0, + VISIT_LEFT_BIT=1, + VISIT_RIGHT_BIT=2, + VISIT_DONE_BIT=3, + VISITED_BIT_SHIFT=29, + NODE_IDX_MASK=(1<<VISITED_BIT_SHIFT)-1, + VISITED_BIT_MASK=~NODE_IDX_MASK, + + + }; + + //for(int i=0;i<bvh_depth;i++) + // stack[i]=0; + + + int level=0; + + const Segment *segmentptr=&segments[0]; + const Vector2 *pointptr=&points[0]; + const BVH *bvhptr = &bvh[0]; + int pos=bvh.size()-1; + + + stack[0]=0; + while(true) { + + uint32_t node = stack[level]&NODE_IDX_MASK; + const BVH &b = bvhptr[ node ]; + + switch(stack[level]>>VISITED_BIT_SHIFT) { + case TEST_AABB_BIT: { + + + bool valid = p_local_aabb.intersects(b.aabb); + if (!valid) { + + stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node; + + } else { + + if (b.left<0) { + + const Segment &s=segmentptr[ b.right ]; + Vector2 a = pointptr[ s.points[0] ]; + Vector2 b = pointptr[ s.points[1] ]; + + SegmentShape2DSW ss(a,b,(b-a).tangent().normalized()); + + p_callback(p_userdata,&ss); + stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node; + + } else { + + stack[level]=(VISIT_LEFT_BIT<<VISITED_BIT_SHIFT)|node; + } + } + + } continue; + case VISIT_LEFT_BIT: { + + stack[level]=(VISIT_RIGHT_BIT<<VISITED_BIT_SHIFT)|node; + stack[level+1]=b.left|TEST_AABB_BIT; + level++; + + } continue; + case VISIT_RIGHT_BIT: { + + stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node; + stack[level+1]=b.right|TEST_AABB_BIT; + level++; + } continue; + case VISIT_DONE_BIT: { + + if (level==0) + return; + else + level--; + + } continue; + } + } + +} + + diff --git a/servers/physics_2d/shape_2d_sw.h b/servers/physics_2d/shape_2d_sw.h index 20e76fad5a..ba5f60cb32 100644 --- a/servers/physics_2d/shape_2d_sw.h +++ b/servers/physics_2d/shape_2d_sw.h @@ -26,417 +26,516 @@ /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ -#ifndef SHAPE_2D_2DSW_H
-#define SHAPE_2D_2DSW_H
-
-#include "servers/physics_2d_server.h"
-/*
-
-SHAPE_LINE, ///< plane:"plane"
-SHAPE_SEGMENT, ///< float:"length"
-SHAPE_CIRCLE, ///< float:"radius"
-SHAPE_RECTANGLE, ///< vec3:"extents"
-SHAPE_CONVEX_POLYGON, ///< array of planes:"planes"
-SHAPE_CONCAVE_POLYGON, ///< Vector2 array:"triangles" , or Dictionary with "indices" (int array) and "triangles" (Vector2 array)
-SHAPE_CUSTOM, ///< Server-Implementation based custom shape, calling shape_create() with this value will result in an error
-
-*/
-
-class Shape2DSW;
-
-class ShapeOwner2DSW {
-public:
-
- virtual void _shape_changed()=0;
- virtual void remove_shape(Shape2DSW *p_shape)=0;
-
- virtual ~ShapeOwner2DSW() {}
-};
-
-class Shape2DSW {
-
- RID self;
- Rect2 aabb;
- bool configured;
- real_t custom_bias;
-
- Map<ShapeOwner2DSW*,int> owners;
-protected:
-
- void configure(const Rect2& p_aabb);
-public:
-
- _FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; }
- _FORCE_INLINE_ RID get_self() const {return self; }
-
- virtual Physics2DServer::ShapeType get_type() const=0;
-
- _FORCE_INLINE_ Rect2 get_aabb() const { return aabb; }
- _FORCE_INLINE_ bool is_configured() const { return configured; }
-
- virtual bool is_concave() const { return false; }
-
- virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const=0;
- virtual Vector2 get_support(const Vector2& p_normal) const;
- virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const=0;
-
- virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const=0;
- virtual real_t get_moment_of_inertia(float p_mass) const=0;
-
- virtual void set_data(const Variant& p_data)=0;
- virtual Variant get_data() const=0;
-
- _FORCE_INLINE_ void set_custom_bias(real_t p_bias) { custom_bias=p_bias; }
- _FORCE_INLINE_ real_t get_custom_bias() const { return custom_bias; }
-
- void add_owner(ShapeOwner2DSW *p_owner);
- void remove_owner(ShapeOwner2DSW *p_owner);
- bool is_owner(ShapeOwner2DSW *p_owner) const;
- const Map<ShapeOwner2DSW*,int>& get_owners() const;
-
- Shape2DSW();
- virtual ~Shape2DSW();
-};
-
-
-class LineShape2DSW : public Shape2DSW {
-
-
- Vector2 normal;
- real_t d;
-
-public:
-
- _FORCE_INLINE_ Vector2 get_normal() const { return normal; }
- _FORCE_INLINE_ real_t get_d() const { return d; }
-
- virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_LINE; }
-
- virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }
- virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;
-
- virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;
- virtual real_t get_moment_of_inertia(float p_mass) const;
-
- virtual void set_data(const Variant& p_data);
- virtual Variant get_data() const;
-
- _FORCE_INLINE_ void project_range(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const {
- //real large
- r_min=-1e10;
- r_max=1e10;
- }
-
-};
-
-
-class RayShape2DSW : public Shape2DSW {
-
-
- real_t length;
-
-public:
-
-
- _FORCE_INLINE_ real_t get_length() const { return length; }
-
- virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_RAY; }
-
- virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }
- virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;
-
- virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;
- virtual real_t get_moment_of_inertia(float p_mass) const;
-
- virtual void set_data(const Variant& p_data);
- virtual Variant get_data() const;
-
- _FORCE_INLINE_ void project_range(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const {
- //real large
- r_max = p_normal.dot(p_transform.get_origin());
- r_min = p_normal.dot(p_transform.xform(Vector2(0,length)));
- if (r_max<r_min) {
-
- SWAP(r_max,r_min);
- }
- }
-
- _FORCE_INLINE_ RayShape2DSW() {}
- _FORCE_INLINE_ RayShape2DSW(real_t p_length) { length=p_length; }
-};
-
-
-class SegmentShape2DSW : public Shape2DSW {
-
-
- Vector2 a;
- Vector2 b;
- Vector2 n;
-
-public:
-
-
- _FORCE_INLINE_ const Vector2& get_a() const { return a; }
- _FORCE_INLINE_ const Vector2& get_b() const { return b; }
- _FORCE_INLINE_ const Vector2& get_normal() const { return n; }
-
- virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_SEGMENT; }
-
- virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }
- virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;
-
- virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;
- virtual real_t get_moment_of_inertia(float p_mass) const;
-
- virtual void set_data(const Variant& p_data);
- virtual Variant get_data() const;
-
- _FORCE_INLINE_ void project_range(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const {
- //real large
- r_max = p_normal.dot(p_transform.xform(a));
- r_min = p_normal.dot(p_transform.xform(b));
- if (r_max<r_min) {
-
- SWAP(r_max,r_min);
- }
- }
-
- _FORCE_INLINE_ SegmentShape2DSW() {}
- _FORCE_INLINE_ SegmentShape2DSW(const Vector2& p_a,const Vector2& p_b,const Vector2& p_n) { a=p_a; b=p_b; n=p_n; }
-};
-
-
-class CircleShape2DSW : public Shape2DSW {
-
-
- real_t radius;
-
-public:
-
- _FORCE_INLINE_ const real_t& get_radius() const { return radius; }
-
- virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_CIRCLE; }
-
- virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }
- virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;
-
- virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;
- virtual real_t get_moment_of_inertia(float p_mass) const;
-
- virtual void set_data(const Variant& p_data);
- virtual Variant get_data() const;
-
- _FORCE_INLINE_ void project_range(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const {
- //real large
- real_t d = p_normal.dot( p_transform.get_origin() );
-
- // figure out scale at point
- Vector2 local_normal = p_transform.basis_xform_inv(p_normal);
- real_t scale = local_normal.length();
-
- r_min = d - (radius) * scale;
- r_max = d + (radius) * scale;
- }
-
-};
-
-
-
-class RectangleShape2DSW : public Shape2DSW {
-
-
- Vector2 half_extents;
-
-public:
-
- _FORCE_INLINE_ const Vector2& get_half_extents() const { return half_extents; }
-
- virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_RECTANGLE; }
-
- virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }
- virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;
-
- virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;
- virtual real_t get_moment_of_inertia(float p_mass) const;
-
- virtual void set_data(const Variant& p_data);
- virtual Variant get_data() const;
-
- _FORCE_INLINE_ void project_range(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const {
- // no matter the angle, the box is mirrored anyway
- Vector2 local_normal=p_transform.basis_xform_inv(p_normal);
-
- float length = local_normal.abs().dot(half_extents);
- float distance = p_normal.dot( p_transform.get_origin() );
-
- r_min = distance - length;
- r_max = distance + length;
- }
-
-};
-
-class CapsuleShape2DSW : public Shape2DSW {
-
-
- real_t radius;
- real_t height;
-
-public:
-
- _FORCE_INLINE_ const real_t& get_radius() const { return radius; }
- _FORCE_INLINE_ const real_t& get_height() const { return height; }
-
- virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_CAPSULE; }
-
- virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }
- virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;
-
- virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;
- virtual real_t get_moment_of_inertia(float p_mass) const;
-
- virtual void set_data(const Variant& p_data);
- virtual Variant get_data() const;
-
- _FORCE_INLINE_ void project_range(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const {
- // no matter the angle, the box is mirrored anyway
- Vector2 n=p_transform.basis_xform_inv(p_normal).normalized();
- float h = (n.y > 0) ? height : -height;
-
- n *= radius;
- n.y += h * 0.5;
-
- r_max = p_normal.dot(p_transform.xform(n));
- r_min = p_normal.dot(p_transform.xform(-n));
-
- if (r_max<r_min) {
-
- SWAP(r_max,r_min);
- }
-
- //ERR_FAIL_COND( r_max < r_min );
- }
-
-};
-
-
-class ConvexPolygonShape2DSW : public Shape2DSW {
-
-
- struct Point {
-
- Vector2 pos;
- Vector2 normal; //normal to next segment
- };
-
- Point *points;
- int point_count;
-
-public:
-
- _FORCE_INLINE_ int get_point_count() const { return point_count; }
- _FORCE_INLINE_ const Vector2& get_point(int p_idx) const { return points[p_idx].pos; }
- _FORCE_INLINE_ const Vector2& get_segment_normal(int p_idx) const { return points[p_idx].normal; }
-
- virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_CONVEX_POLYGON; }
-
- virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }
- virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;
-
- virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;
- virtual real_t get_moment_of_inertia(float p_mass) const;
-
- virtual void set_data(const Variant& p_data);
- virtual Variant get_data() const;
-
- _FORCE_INLINE_ void project_range(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const {
- // no matter the angle, the box is mirrored anyway
-
- r_min = r_max = p_normal.dot(p_transform.xform(points[0].pos));
- for(int i=1;i<point_count;i++) {
-
- float d = p_normal.dot(p_transform.xform(points[i].pos));
- if (d>r_max)
- r_max=d;
- if (d<r_min)
- r_min=d;
-
- }
-
- }
-
-
- ConvexPolygonShape2DSW();
- ~ConvexPolygonShape2DSW();
-};
-
-
-class ConcaveShape2DSW : public Shape2DSW {
-
-public:
-
- virtual bool is_concave() const { return true; }
- typedef void (*Callback)(void* p_userdata,Shape2DSW *p_convex);
-
- virtual void cull(const Rect2& p_local_aabb,Callback p_callback,void* p_userdata) const=0;
-
-};
-
-class ConcavePolygonShape2DSW : public ConcaveShape2DSW {
-
- struct Segment {
-
- int points[2];
- };
-
- Vector<Segment> segments;
- Vector<Point2> points;
-
- struct BVH {
-
- Rect2 aabb;
- int left,right;
- };
-
-
- Vector<BVH> bvh;
- int bvh_depth;
-
-
- struct BVH_CompareX {
-
- _FORCE_INLINE_ bool operator ()(const BVH& a, const BVH& b) const {
-
- return (a.aabb.pos.x+a.aabb.size.x*0.5) < (b.aabb.pos.x+b.aabb.size.x*0.5);
- }
- };
-
- struct BVH_CompareY {
-
- _FORCE_INLINE_ bool operator ()(const BVH& a, const BVH& b) const {
-
- return (a.aabb.pos.y+a.aabb.size.y*0.5) < (b.aabb.pos.y+b.aabb.size.y*0.5);
- }
- };
-
- int _generate_bvh(BVH *p_bvh,int p_len,int p_depth);
-
-public:
-
- virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_CONCAVE_POLYGON; }
-
- virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { /*project_range(p_normal,p_transform,r_min,r_max);*/ }
- virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;
-
- virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;
-
- virtual real_t get_moment_of_inertia(float p_mass) const { return 0; }
-
- virtual void set_data(const Variant& p_data);
- virtual Variant get_data() const;
-
- virtual void cull(const Rect2& p_local_aabb,Callback p_callback,void* p_userdata) const;
-
-};
-
-
-#endif // SHAPE_2D_2DSW_H
+#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<ShapeOwner2DSW*,int> owners; +protected: + + void configure(const Rect2& p_aabb); +public: + + _FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; } + _FORCE_INLINE_ RID get_self() const {return self; } + + virtual Physics2DServer::ShapeType get_type() const=0; + + _FORCE_INLINE_ Rect2 get_aabb() const { return aabb; } + _FORCE_INLINE_ bool is_configured() const { return configured; } + + virtual bool is_concave() const { return false; } + + virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const=0; + virtual 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<ShapeOwner2DSW*,int>& 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;i<r_amount;i++) + r_supports[i]=p_xform.xform(r_supports[i]); + + if (r_amount==1) { + + if (Math::abs( p_normal.dot(p_cast.normalized()) )<(1.0-_SEGMENT_IS_VALID_SUPPORT_TRESHOLD) ) { + //make line because they are parallel + r_amount=2; + r_supports[1]=r_supports[0]+p_cast; + } else if (p_cast.dot(p_normal)>0) { + //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<r_min) { + + SWAP(r_max,r_min); + } + } + + DEFAULT_PROJECT_RANGE_CAST + + + _FORCE_INLINE_ RayShape2DSW() {} + _FORCE_INLINE_ RayShape2DSW(real_t p_length) { length=p_length; } +}; + + +class SegmentShape2DSW : public Shape2DSW { + + + Vector2 a; + Vector2 b; + Vector2 n; + +public: + + + _FORCE_INLINE_ const Vector2& get_a() const { return a; } + _FORCE_INLINE_ const Vector2& get_b() const { return b; } + _FORCE_INLINE_ const Vector2& get_normal() const { return n; } + + virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_SEGMENT; } + + _FORCE_INLINE_ Vector2 get_xformed_normal(const Matrix32& p_xform) const { + + return (p_xform.xform(b) - p_xform.xform(a)).normalized().tangent(); + } + virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); } + virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const; + + virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const; + virtual real_t get_moment_of_inertia(float p_mass) const; + + virtual void set_data(const Variant& p_data); + virtual Variant get_data() const; + + _FORCE_INLINE_ void project_range(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { + //real large + r_max = p_normal.dot(p_transform.xform(a)); + r_min = p_normal.dot(p_transform.xform(b)); + if (r_max<r_min) { + + SWAP(r_max,r_min); + } + } + + DEFAULT_PROJECT_RANGE_CAST + + _FORCE_INLINE_ SegmentShape2DSW() {} + _FORCE_INLINE_ SegmentShape2DSW(const Vector2& p_a,const Vector2& p_b,const Vector2& p_n) { a=p_a; b=p_b; n=p_n; } +}; + + +class CircleShape2DSW : public Shape2DSW { + + + real_t radius; + +public: + + _FORCE_INLINE_ const real_t& get_radius() const { return radius; } + + virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_CIRCLE; } + + virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); } + virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const; + + virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const; + virtual real_t get_moment_of_inertia(float p_mass) const; + + virtual void set_data(const Variant& p_data); + virtual Variant get_data() const; + + _FORCE_INLINE_ void project_range(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { + //real large + real_t d = p_normal.dot( p_transform.get_origin() ); + + // figure out scale at point + Vector2 local_normal = p_transform.basis_xform_inv(p_normal); + real_t scale = local_normal.length(); + + r_min = d - (radius) * scale; + r_max = d + (radius) * scale; + } + + + DEFAULT_PROJECT_RANGE_CAST + +}; + + + +class RectangleShape2DSW : public Shape2DSW { + + + Vector2 half_extents; + +public: + + _FORCE_INLINE_ const Vector2& get_half_extents() const { return half_extents; } + + virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_RECTANGLE; } + + virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); } + virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const; + + virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const; + virtual real_t get_moment_of_inertia(float p_mass) const; + + virtual void set_data(const Variant& p_data); + virtual Variant get_data() const; + + _FORCE_INLINE_ void project_range(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { + // no matter the angle, the box is mirrored anyway + Vector2 local_normal=p_transform.basis_xform_inv(p_normal); + + float length = local_normal.abs().dot(half_extents); + float distance = p_normal.dot( p_transform.get_origin() ); + + r_min = distance - length; + r_max = distance + length; + } + + DEFAULT_PROJECT_RANGE_CAST + +}; + +class CapsuleShape2DSW : public Shape2DSW { + + + real_t radius; + real_t height; + +public: + + _FORCE_INLINE_ const real_t& get_radius() const { return radius; } + _FORCE_INLINE_ const real_t& get_height() const { return height; } + + virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_CAPSULE; } + + virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); } + virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const; + + virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const; + virtual real_t get_moment_of_inertia(float p_mass) const; + + virtual void set_data(const Variant& p_data); + virtual Variant get_data() const; + + _FORCE_INLINE_ void project_range(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { + // no matter the angle, the box is mirrored anyway + Vector2 n=p_transform.basis_xform_inv(p_normal).normalized(); + float h = (n.y > 0) ? height : -height; + + n *= radius; + n.y += h * 0.5; + + r_max = p_normal.dot(p_transform.xform(n)); + r_min = p_normal.dot(p_transform.xform(-n)); + + if (r_max<r_min) { + + SWAP(r_max,r_min); + } + + //ERR_FAIL_COND( r_max < r_min ); + } + + DEFAULT_PROJECT_RANGE_CAST + +}; + + + + +class ConvexPolygonShape2DSW : public Shape2DSW { + + + struct Point { + + Vector2 pos; + Vector2 normal; //normal to next segment + }; + + Point *points; + int point_count; + +public: + + _FORCE_INLINE_ int get_point_count() const { return point_count; } + _FORCE_INLINE_ const Vector2& get_point(int p_idx) const { return points[p_idx].pos; } + _FORCE_INLINE_ const Vector2& get_segment_normal(int p_idx) const { return points[p_idx].normal; } + _FORCE_INLINE_ Vector2 get_xformed_segment_normal(const Matrix32& p_xform, int p_idx) const { + + Vector2 a = points[p_idx].pos; + p_idx++; + Vector2 b = points[p_idx==point_count?0:p_idx].pos; + return (p_xform.xform(b)-p_xform.xform(a)).normalized().tangent(); + } + + virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_CONVEX_POLYGON; } + + virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); } + virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const; + + virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const; + virtual real_t get_moment_of_inertia(float p_mass) const; + + virtual void set_data(const Variant& p_data); + virtual Variant get_data() const; + + _FORCE_INLINE_ void project_range(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { + // no matter the angle, the box is mirrored anyway + + r_min = r_max = p_normal.dot(p_transform.xform(points[0].pos)); + for(int i=1;i<point_count;i++) { + + float d = p_normal.dot(p_transform.xform(points[i].pos)); + if (d>r_max) + r_max=d; + if (d<r_min) + r_min=d; + + } + + } + + DEFAULT_PROJECT_RANGE_CAST + + ConvexPolygonShape2DSW(); + ~ConvexPolygonShape2DSW(); +}; + + +class ConcaveShape2DSW : public Shape2DSW { + +public: + + virtual bool is_concave() const { return true; } + typedef void (*Callback)(void* p_userdata,Shape2DSW *p_convex); + + virtual void cull(const Rect2& p_local_aabb,Callback p_callback,void* p_userdata) const=0; + +}; + +class ConcavePolygonShape2DSW : public ConcaveShape2DSW { + + struct Segment { + + int points[2]; + }; + + Vector<Segment> segments; + Vector<Point2> points; + + struct BVH { + + Rect2 aabb; + int left,right; + }; + + + Vector<BVH> bvh; + int bvh_depth; + + + struct BVH_CompareX { + + _FORCE_INLINE_ bool operator ()(const BVH& a, const BVH& b) const { + + return (a.aabb.pos.x+a.aabb.size.x*0.5) < (b.aabb.pos.x+b.aabb.size.x*0.5); + } + }; + + struct BVH_CompareY { + + _FORCE_INLINE_ bool operator ()(const BVH& a, const BVH& b) const { + + return (a.aabb.pos.y+a.aabb.size.y*0.5) < (b.aabb.pos.y+b.aabb.size.y*0.5); + } + }; + + int _generate_bvh(BVH *p_bvh,int p_len,int p_depth); + +public: + + virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_CONCAVE_POLYGON; } + + virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { /*project_range(p_normal,p_transform,r_min,r_max);*/ } + virtual void 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<RID>& p_exclude,uint32_t p_user_mask) {
-
-
- ERR_FAIL_COND_V(space->locked,false);
-
- Vector2 begin,end;
- Vector2 normal;
- begin=p_from;
- end=p_to;
- normal=(end-begin).normalized();
-
-
- int amount = space->broadphase->cull_segment(begin,end,space->intersection_query_results,Space2DSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results);
-
- //todo, create another array tha references results, compute AABBs and check closest point to ray origin, sort, and stop evaluating results when beyond first collision
-
- bool collided=false;
- Vector2 res_point,res_normal;
- int res_shape;
- const CollisionObject2DSW *res_obj;
- real_t min_d=1e10;
-
-
- for(int i=0;i<amount;i++) {
-
- if (space->intersection_query_results[i]->get_type()==CollisionObject2DSW::TYPE_AREA)
- continue; //ignore area
-
- if (p_exclude.has( space->intersection_query_results[i]->get_self()))
- continue;
-
- const CollisionObject2DSW *col_obj=space->intersection_query_results[i];
-
- int shape_idx=space->intersection_query_subindex_results[i];
- Matrix32 inv_xform = col_obj->get_shape_inv_transform(shape_idx) * col_obj->get_inv_transform();
-
- Vector2 local_from = inv_xform.xform(begin);
- Vector2 local_to = inv_xform.xform(end);
-
- /*local_from = col_obj->get_inv_transform().xform(begin);
- local_from = col_obj->get_shape_inv_transform(shape_idx).xform(local_from);
-
- local_to = col_obj->get_inv_transform().xform(end);
- local_to = col_obj->get_shape_inv_transform(shape_idx).xform(local_to);*/
-
- const Shape2DSW *shape = col_obj->get_shape(shape_idx);
-
- Vector2 shape_point,shape_normal;
-
-
- if (shape->intersect_segment(local_from,local_to,shape_point,shape_normal)) {
-
-
- //print_line("inters sgment!");
- Matrix32 xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
- shape_point=xform.xform(shape_point);
-
- real_t ld = normal.dot(shape_point);
-
-
- if (ld<min_d) {
-
- min_d=ld;
- res_point=shape_point;
- res_normal=inv_xform.basis_xform_inv(shape_normal).normalized();
- res_shape=shape_idx;
- res_obj=col_obj;
- collided=true;
- }
- }
-
- }
-
- if (!collided)
- return false;
-
-
- r_result.collider_id=res_obj->get_instance_id();
- if (r_result.collider_id!=0)
- r_result.collider=ObjectDB::get_instance(r_result.collider_id);
- r_result.normal=res_normal;
- r_result.position=res_point;
- r_result.rid=res_obj->get_self();
- r_result.shape=res_shape;
-
- return true;
-
-}
-
-
-int Physics2DDirectSpaceStateSW::intersect_shape(const RID& p_shape, const Matrix32& p_xform,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude,uint32_t p_user_mask) {
-
- if (p_result_max<=0)
- return 0;
-
- Shape2DSW *shape = static_cast<Physics2DServerSW*>(Physics2DServer::get_singleton())->shape_owner.get(p_shape);
- ERR_FAIL_COND_V(!shape,0);
-
- Rect2 aabb = p_xform.xform(shape->get_aabb());
-
- int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,Space2DSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results);
-
- bool collided=false;
- int cc=0;
-
- for(int i=0;i<amount;i++) {
-
- if (cc>=p_result_max)
- break;
-
- if (space->intersection_query_results[i]->get_type()==CollisionObject2DSW::TYPE_AREA)
- continue; //ignore area
-
- if (p_exclude.has( space->intersection_query_results[i]->get_self()))
- continue;
-
-
- const CollisionObject2DSW *col_obj=space->intersection_query_results[i];
- int shape_idx=space->intersection_query_subindex_results[i];
-
- if (!CollisionSolver2DSW::solve_static(shape,p_xform,p_xform.affine_inverse(),col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), col_obj->get_inv_transform() * col_obj->get_shape_inv_transform(shape_idx),NULL,NULL,NULL))
- continue;
-
- r_results[cc].collider_id=col_obj->get_instance_id();
- if (r_results[cc].collider_id!=0)
- r_results[cc].collider=ObjectDB::get_instance(r_results[cc].collider_id);
- r_results[cc].rid=col_obj->get_self();
- r_results[cc].shape=shape_idx;
-
- cc++;
-
- }
-
- return cc;
-
-}
-
-Physics2DDirectSpaceStateSW::Physics2DDirectSpaceStateSW() {
-
-
- space=NULL;
-}
-
-
-////////////////////////////////////////////////////////////////////////////////////////////////////////////
-
-
-
-
-
-
-
-
-
-
-void* Space2DSW::_broadphase_pair(CollisionObject2DSW *A,int p_subindex_A,CollisionObject2DSW *B,int p_subindex_B,void *p_self) {
-
- CollisionObject2DSW::Type type_A=A->get_type();
- CollisionObject2DSW::Type type_B=B->get_type();
- if (type_A>type_B) {
-
- SWAP(A,B);
- SWAP(p_subindex_A,p_subindex_B);
- SWAP(type_A,type_B);
- }
-
- Space2DSW *self = (Space2DSW*)p_self;
-
- if (type_A==CollisionObject2DSW::TYPE_AREA) {
-
-
- ERR_FAIL_COND_V(type_B!=CollisionObject2DSW::TYPE_BODY,NULL);
- Area2DSW *area=static_cast<Area2DSW*>(A);
- Body2DSW *body=static_cast<Body2DSW*>(B);
-
- AreaPair2DSW *area_pair = memnew(AreaPair2DSW(body,p_subindex_B,area,p_subindex_A) );
-
- return area_pair;
- } else {
-
-
- BodyPair2DSW *b = memnew( BodyPair2DSW((Body2DSW*)A,p_subindex_A,(Body2DSW*)B,p_subindex_B) );
- return b;
-
- }
-
- return NULL;
-}
-
-void Space2DSW::_broadphase_unpair(CollisionObject2DSW *A,int p_subindex_A,CollisionObject2DSW *B,int p_subindex_B,void *p_data,void *p_self) {
-
-
-
- Space2DSW *self = (Space2DSW*)p_self;
- Constraint2DSW *c = (Constraint2DSW*)p_data;
- memdelete(c);
-}
-
-
-const SelfList<Body2DSW>::List& Space2DSW::get_active_body_list() const {
-
- return active_list;
-}
-void Space2DSW::body_add_to_active_list(SelfList<Body2DSW>* p_body) {
-
- active_list.add(p_body);
-}
-void Space2DSW::body_remove_from_active_list(SelfList<Body2DSW>* p_body) {
-
- active_list.remove(p_body);
-
-}
-
-void Space2DSW::body_add_to_inertia_update_list(SelfList<Body2DSW>* p_body) {
-
-
- inertia_update_list.add(p_body);
-}
-
-void Space2DSW::body_remove_from_inertia_update_list(SelfList<Body2DSW>* p_body) {
-
- inertia_update_list.remove(p_body);
-}
-
-BroadPhase2DSW *Space2DSW::get_broadphase() {
-
- return broadphase;
-}
-
-void Space2DSW::add_object(CollisionObject2DSW *p_object) {
-
- ERR_FAIL_COND( objects.has(p_object) );
- objects.insert(p_object);
-}
-
-void Space2DSW::remove_object(CollisionObject2DSW *p_object) {
-
- ERR_FAIL_COND( !objects.has(p_object) );
- objects.erase(p_object);
-}
-
-const Set<CollisionObject2DSW*> &Space2DSW::get_objects() const {
-
- return objects;
-}
-
-void Space2DSW::body_add_to_state_query_list(SelfList<Body2DSW>* p_body) {
-
- state_query_list.add(p_body);
-}
-void Space2DSW::body_remove_from_state_query_list(SelfList<Body2DSW>* p_body) {
-
- state_query_list.remove(p_body);
-}
-
-void Space2DSW::area_add_to_monitor_query_list(SelfList<Area2DSW>* p_area) {
-
- monitor_query_list.add(p_area);
-}
-void Space2DSW::area_remove_from_monitor_query_list(SelfList<Area2DSW>* p_area) {
-
- monitor_query_list.remove(p_area);
-}
-
-void Space2DSW::area_add_to_moved_list(SelfList<Area2DSW>* p_area) {
-
- area_moved_list.add(p_area);
-}
-
-void Space2DSW::area_remove_from_moved_list(SelfList<Area2DSW>* p_area) {
-
- area_moved_list.remove(p_area);
-}
-
-const SelfList<Area2DSW>::List& Space2DSW::get_moved_area_list() const {
-
- return area_moved_list;
-}
-
-
-void Space2DSW::call_queries() {
-
- while(state_query_list.first()) {
-
- Body2DSW * b = state_query_list.first()->self();
- b->call_queries();
- state_query_list.remove(state_query_list.first());
- }
-
- while(monitor_query_list.first()) {
-
- Area2DSW * a = monitor_query_list.first()->self();
- a->call_queries();
- monitor_query_list.remove(monitor_query_list.first());
- }
-
-}
-
-void Space2DSW::setup() {
-
-
- while(inertia_update_list.first()) {
- inertia_update_list.first()->self()->update_inertias();
- inertia_update_list.remove(inertia_update_list.first());
- }
-
-
-}
-
-void Space2DSW::update() {
-
- broadphase->update();
-
-}
-
-
-void Space2DSW::set_param(Physics2DServer::SpaceParameter p_param, real_t p_value) {
-
- switch(p_param) {
-
- case Physics2DServer::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: contact_recycle_radius=p_value; break;
- case Physics2DServer::SPACE_PARAM_CONTACT_MAX_SEPARATION: contact_max_separation=p_value; break;
- case Physics2DServer::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: contact_max_allowed_penetration=p_value; break;
- case Physics2DServer::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_TRESHOLD: body_linear_velocity_sleep_treshold=p_value; break;
- case Physics2DServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_TRESHOLD: body_angular_velocity_sleep_treshold=p_value; break;
- case Physics2DServer::SPACE_PARAM_BODY_TIME_TO_SLEEP: body_time_to_sleep=p_value; break;
- case Physics2DServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO: body_angular_velocity_damp_ratio=p_value; break;
- case Physics2DServer::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: constraint_bias=p_value; break;
- }
-}
-
-real_t Space2DSW::get_param(Physics2DServer::SpaceParameter p_param) const {
-
- switch(p_param) {
-
- case Physics2DServer::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: return contact_recycle_radius;
- case Physics2DServer::SPACE_PARAM_CONTACT_MAX_SEPARATION: return contact_max_separation;
- case Physics2DServer::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: return contact_max_allowed_penetration;
- case Physics2DServer::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_TRESHOLD: return body_linear_velocity_sleep_treshold;
- case Physics2DServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_TRESHOLD: return body_angular_velocity_sleep_treshold;
- case Physics2DServer::SPACE_PARAM_BODY_TIME_TO_SLEEP: return body_time_to_sleep;
- case Physics2DServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO: return body_angular_velocity_damp_ratio;
- case Physics2DServer::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: return constraint_bias;
- }
- return 0;
-}
-
-void Space2DSW::lock() {
-
- locked=true;
-}
-
-void Space2DSW::unlock() {
-
- locked=false;
-}
-
-bool Space2DSW::is_locked() const {
-
- return locked;
-}
-
-Physics2DDirectSpaceStateSW *Space2DSW::get_direct_state() {
-
- return direct_access;
-}
-
-Space2DSW::Space2DSW() {
-
-
- locked=false;
- contact_recycle_radius=0.01;
- contact_max_separation=0.05;
- contact_max_allowed_penetration= 0.01;
-
- constraint_bias = 0.01;
- body_linear_velocity_sleep_treshold=0.01;
- body_angular_velocity_sleep_treshold=(8.0 / 180.0 * Math_PI);
- body_time_to_sleep=0.5;
- body_angular_velocity_damp_ratio=15;
-
-
- broadphase = BroadPhase2DSW::create_func();
- broadphase->set_pair_callback(_broadphase_pair,this);
- broadphase->set_unpair_callback(_broadphase_unpair,this);
- area=NULL;
-
- direct_access = memnew( Physics2DDirectSpaceStateSW );
- direct_access->space=this;
-}
-
-Space2DSW::~Space2DSW() {
-
- memdelete(broadphase);
- memdelete( direct_access );
-}
-
-
-
+#include "space_2d_sw.h" +#include "collision_solver_2d_sw.h" +#include "physics_2d_server_sw.h" + + +bool Physics2DDirectSpaceStateSW::intersect_ray(const Vector2& p_from, const Vector2& p_to,RayResult &r_result,const Set<RID>& p_exclude,uint32_t p_user_mask) { + + + ERR_FAIL_COND_V(space->locked,false); + + Vector2 begin,end; + Vector2 normal; + begin=p_from; + end=p_to; + normal=(end-begin).normalized(); + + int amount = space->broadphase->cull_segment(begin,end,space->intersection_query_results,Space2DSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results); + + //todo, create another array tha references results, compute AABBs and check closest point to ray origin, sort, and stop evaluating results when beyond first collision + + bool collided=false; + Vector2 res_point,res_normal; + int res_shape; + const CollisionObject2DSW *res_obj; + real_t min_d=1e10; + + + for(int i=0;i<amount;i++) { + + if (space->intersection_query_results[i]->get_type()==CollisionObject2DSW::TYPE_AREA) + continue; //ignore area + + if (p_exclude.has( space->intersection_query_results[i]->get_self())) + continue; + + const CollisionObject2DSW *col_obj=space->intersection_query_results[i]; + + int shape_idx=space->intersection_query_subindex_results[i]; + Matrix32 inv_xform = col_obj->get_shape_inv_transform(shape_idx) * col_obj->get_inv_transform(); + + Vector2 local_from = inv_xform.xform(begin); + Vector2 local_to = inv_xform.xform(end); + + /*local_from = col_obj->get_inv_transform().xform(begin); + local_from = col_obj->get_shape_inv_transform(shape_idx).xform(local_from); + + local_to = col_obj->get_inv_transform().xform(end); + local_to = col_obj->get_shape_inv_transform(shape_idx).xform(local_to);*/ + + const Shape2DSW *shape = col_obj->get_shape(shape_idx); + + Vector2 shape_point,shape_normal; + + + if (shape->intersect_segment(local_from,local_to,shape_point,shape_normal)) { + + + //print_line("inters sgment!"); + Matrix32 xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); + shape_point=xform.xform(shape_point); + + real_t ld = normal.dot(shape_point); + + + if (ld<min_d) { + + min_d=ld; + res_point=shape_point; + res_normal=inv_xform.basis_xform_inv(shape_normal).normalized(); + res_shape=shape_idx; + res_obj=col_obj; + collided=true; + } + } + + } + + if (!collided) + return false; + + + r_result.collider_id=res_obj->get_instance_id(); + if (r_result.collider_id!=0) + r_result.collider=ObjectDB::get_instance(r_result.collider_id); + r_result.normal=res_normal; + r_result.position=res_point; + r_result.rid=res_obj->get_self(); + r_result.shape=res_shape; + + return true; + +} + + +int Physics2DDirectSpaceStateSW::intersect_shape(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude,uint32_t p_user_mask) { + + if (p_result_max<=0) + return 0; + + Shape2DSW *shape = static_cast<Physics2DServerSW*>(Physics2DServer::get_singleton())->shape_owner.get(p_shape); + ERR_FAIL_COND_V(!shape,0); + + Rect2 aabb = p_xform.xform(shape->get_aabb()); + + int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,Space2DSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results); + + bool collided=false; + int cc=0; + + for(int i=0;i<amount;i++) { + + if (cc>=p_result_max) + break; + + if (space->intersection_query_results[i]->get_type()==CollisionObject2DSW::TYPE_AREA) + continue; //ignore area + + if (p_exclude.has( space->intersection_query_results[i]->get_self())) + continue; + + + const CollisionObject2DSW *col_obj=space->intersection_query_results[i]; + int shape_idx=space->intersection_query_subindex_results[i]; + + if (!CollisionSolver2DSW::solve(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())<CMP_EPSILON) { + //safe to assume it was a perpendicular collision + n=contact_normal; + p=p_point_B; + } else { + //entered in a different angle + Vector2 to = p_point_A+rd->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 (len<rd->best_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<RID>& p_exclude,uint32_t p_user_mask) { + + Shape2DSW *shape = static_cast<Physics2DServerSW*>(Physics2DServer::get_singleton())->shape_owner.get(p_shape); + ERR_FAIL_COND_V(!shape,0); + + Rect2 aabb = p_xform.xform(shape->get_aabb()); + 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;i<amount;i++) { + + + if (space->intersection_query_results[i]->get_type()==CollisionObject2DSW::TYPE_AREA) + continue; //ignore area + + if (p_exclude.has( space->intersection_query_results[i]->get_self())) + continue; //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<Area2DSW*>(A); + Body2DSW *body=static_cast<Body2DSW*>(B); + + AreaPair2DSW *area_pair = memnew(AreaPair2DSW(body,p_subindex_B,area,p_subindex_A) ); + + return area_pair; + } else { + + + BodyPair2DSW *b = memnew( BodyPair2DSW((Body2DSW*)A,p_subindex_A,(Body2DSW*)B,p_subindex_B) ); + return b; + + } + + return NULL; +} + +void Space2DSW::_broadphase_unpair(CollisionObject2DSW *A,int p_subindex_A,CollisionObject2DSW *B,int p_subindex_B,void *p_data,void *p_self) { + + + + Space2DSW *self = (Space2DSW*)p_self; + Constraint2DSW *c = (Constraint2DSW*)p_data; + memdelete(c); +} + + +const SelfList<Body2DSW>::List& Space2DSW::get_active_body_list() const { + + return active_list; +} +void Space2DSW::body_add_to_active_list(SelfList<Body2DSW>* p_body) { + + active_list.add(p_body); +} +void Space2DSW::body_remove_from_active_list(SelfList<Body2DSW>* p_body) { + + active_list.remove(p_body); + +} + +void Space2DSW::body_add_to_inertia_update_list(SelfList<Body2DSW>* p_body) { + + + inertia_update_list.add(p_body); +} + +void Space2DSW::body_remove_from_inertia_update_list(SelfList<Body2DSW>* p_body) { + + inertia_update_list.remove(p_body); +} + +BroadPhase2DSW *Space2DSW::get_broadphase() { + + return broadphase; +} + +void Space2DSW::add_object(CollisionObject2DSW *p_object) { + + ERR_FAIL_COND( objects.has(p_object) ); + objects.insert(p_object); +} + +void Space2DSW::remove_object(CollisionObject2DSW *p_object) { + + ERR_FAIL_COND( !objects.has(p_object) ); + objects.erase(p_object); +} + +const Set<CollisionObject2DSW*> &Space2DSW::get_objects() const { + + return objects; +} + +void Space2DSW::body_add_to_state_query_list(SelfList<Body2DSW>* p_body) { + + state_query_list.add(p_body); +} +void Space2DSW::body_remove_from_state_query_list(SelfList<Body2DSW>* p_body) { + + state_query_list.remove(p_body); +} + +void Space2DSW::area_add_to_monitor_query_list(SelfList<Area2DSW>* p_area) { + + monitor_query_list.add(p_area); +} +void Space2DSW::area_remove_from_monitor_query_list(SelfList<Area2DSW>* p_area) { + + monitor_query_list.remove(p_area); +} + +void Space2DSW::area_add_to_moved_list(SelfList<Area2DSW>* p_area) { + + area_moved_list.add(p_area); +} + +void Space2DSW::area_remove_from_moved_list(SelfList<Area2DSW>* p_area) { + + area_moved_list.remove(p_area); +} + +const SelfList<Area2DSW>::List& Space2DSW::get_moved_area_list() const { + + return area_moved_list; +} + + +void Space2DSW::call_queries() { + + while(state_query_list.first()) { + + Body2DSW * b = state_query_list.first()->self(); + b->call_queries(); + state_query_list.remove(state_query_list.first()); + } + + while(monitor_query_list.first()) { + + Area2DSW * a = monitor_query_list.first()->self(); + a->call_queries(); + monitor_query_list.remove(monitor_query_list.first()); + } + +} + +void Space2DSW::setup() { + + + while(inertia_update_list.first()) { + inertia_update_list.first()->self()->update_inertias(); + inertia_update_list.remove(inertia_update_list.first()); + } + + +} + +void Space2DSW::update() { + + broadphase->update(); + +} + + +void Space2DSW::set_param(Physics2DServer::SpaceParameter p_param, real_t p_value) { + + switch(p_param) { + + case Physics2DServer::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: contact_recycle_radius=p_value; break; + case Physics2DServer::SPACE_PARAM_CONTACT_MAX_SEPARATION: contact_max_separation=p_value; break; + case Physics2DServer::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: contact_max_allowed_penetration=p_value; break; + case Physics2DServer::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_TRESHOLD: body_linear_velocity_sleep_treshold=p_value; break; + case Physics2DServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_TRESHOLD: body_angular_velocity_sleep_treshold=p_value; break; + case Physics2DServer::SPACE_PARAM_BODY_TIME_TO_SLEEP: body_time_to_sleep=p_value; break; + case Physics2DServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO: body_angular_velocity_damp_ratio=p_value; break; + case Physics2DServer::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: constraint_bias=p_value; break; + } +} + +real_t Space2DSW::get_param(Physics2DServer::SpaceParameter p_param) const { + + switch(p_param) { + + case Physics2DServer::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: return contact_recycle_radius; + case Physics2DServer::SPACE_PARAM_CONTACT_MAX_SEPARATION: return contact_max_separation; + case Physics2DServer::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: return contact_max_allowed_penetration; + case Physics2DServer::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_TRESHOLD: return body_linear_velocity_sleep_treshold; + case Physics2DServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_TRESHOLD: return body_angular_velocity_sleep_treshold; + case Physics2DServer::SPACE_PARAM_BODY_TIME_TO_SLEEP: return body_time_to_sleep; + case Physics2DServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO: return body_angular_velocity_damp_ratio; + case Physics2DServer::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: return constraint_bias; + } + return 0; +} + +void Space2DSW::lock() { + + locked=true; +} + +void Space2DSW::unlock() { + + locked=false; +} + +bool Space2DSW::is_locked() const { + + return locked; +} + +Physics2DDirectSpaceStateSW *Space2DSW::get_direct_state() { + + return direct_access; +} + +Space2DSW::Space2DSW() { + + + locked=false; + contact_recycle_radius=0.01; + contact_max_separation=0.05; + contact_max_allowed_penetration= 0.01; + + constraint_bias = 0.01; + body_linear_velocity_sleep_treshold=0.01; + body_angular_velocity_sleep_treshold=(8.0 / 180.0 * Math_PI); + body_time_to_sleep=0.5; + body_angular_velocity_damp_ratio=15; + + + broadphase = BroadPhase2DSW::create_func(); + broadphase->set_pair_callback(_broadphase_pair,this); + broadphase->set_unpair_callback(_broadphase_unpair,this); + area=NULL; + + direct_access = memnew( Physics2DDirectSpaceStateSW ); + direct_access->space=this; +} + +Space2DSW::~Space2DSW() { + + memdelete(broadphase); + memdelete( direct_access ); +} + + + diff --git a/servers/physics_2d/space_2d_sw.h b/servers/physics_2d/space_2d_sw.h 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<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0);
- int intersect_shape(const RID& p_shape, const Matrix32& p_xform,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0);
-
- Physics2DDirectSpaceStateSW();
-};
-
-
-
-class Space2DSW {
-
-
- Physics2DDirectSpaceStateSW *direct_access;
- RID self;
-
- BroadPhase2DSW *broadphase;
- SelfList<Body2DSW>::List active_list;
- SelfList<Body2DSW>::List inertia_update_list;
- SelfList<Body2DSW>::List state_query_list;
- SelfList<Area2DSW>::List monitor_query_list;
- SelfList<Area2DSW>::List area_moved_list;
-
- static void* _broadphase_pair(CollisionObject2DSW *A,int p_subindex_A,CollisionObject2DSW *B,int p_subindex_B,void *p_self);
- static void _broadphase_unpair(CollisionObject2DSW *A,int p_subindex_A,CollisionObject2DSW *B,int p_subindex_B,void *p_data,void *p_self);
-
- Set<CollisionObject2DSW*> objects;
-
- Area2DSW *area;
-
- real_t contact_recycle_radius;
- real_t contact_max_separation;
- real_t contact_max_allowed_penetration;
- real_t constraint_bias;
-
- enum {
-
- INTERSECTION_QUERY_MAX=2048
- };
-
- CollisionObject2DSW *intersection_query_results[INTERSECTION_QUERY_MAX];
- int intersection_query_subindex_results[INTERSECTION_QUERY_MAX];
-
- float body_linear_velocity_sleep_treshold;
- float body_angular_velocity_sleep_treshold;
- float body_time_to_sleep;
- float body_angular_velocity_damp_ratio;
-
- bool locked;
-
-friend class Physics2DDirectSpaceStateSW;
-
-public:
-
- _FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; }
- _FORCE_INLINE_ RID get_self() const { return self; }
-
- void set_default_area(Area2DSW *p_area) { area=p_area; }
- Area2DSW *get_default_area() const { return area; }
-
- const SelfList<Body2DSW>::List& get_active_body_list() const;
- void body_add_to_active_list(SelfList<Body2DSW>* p_body);
- void body_remove_from_active_list(SelfList<Body2DSW>* p_body);
- void body_add_to_inertia_update_list(SelfList<Body2DSW>* p_body);
- void body_remove_from_inertia_update_list(SelfList<Body2DSW>* p_body);
- void area_add_to_moved_list(SelfList<Area2DSW>* p_area);
- void area_remove_from_moved_list(SelfList<Area2DSW>* p_area);
- const SelfList<Area2DSW>::List& get_moved_area_list() const;
-
-
-
-
- void body_add_to_state_query_list(SelfList<Body2DSW>* p_body);
- void body_remove_from_state_query_list(SelfList<Body2DSW>* p_body);
-
- void area_add_to_monitor_query_list(SelfList<Area2DSW>* p_area);
- void area_remove_from_monitor_query_list(SelfList<Area2DSW>* p_area);
-
- BroadPhase2DSW *get_broadphase();
-
- void add_object(CollisionObject2DSW *p_object);
- void remove_object(CollisionObject2DSW *p_object);
- const Set<CollisionObject2DSW*> &get_objects() const;
-
- _FORCE_INLINE_ real_t get_contact_recycle_radius() const { return contact_recycle_radius; }
- _FORCE_INLINE_ real_t get_contact_max_separation() const { return contact_max_separation; }
- _FORCE_INLINE_ real_t get_contact_max_allowed_penetration() const { return contact_max_allowed_penetration; }
- _FORCE_INLINE_ real_t get_constraint_bias() const { return constraint_bias; }
- _FORCE_INLINE_ real_t get_body_linear_velocity_sleep_treshold() const { return body_linear_velocity_sleep_treshold; }
- _FORCE_INLINE_ real_t get_body_angular_velocity_sleep_treshold() const { return body_angular_velocity_sleep_treshold; }
- _FORCE_INLINE_ real_t get_body_time_to_sleep() const { return body_time_to_sleep; }
- _FORCE_INLINE_ real_t get_body_angular_velocity_damp_ratio() const { return body_angular_velocity_damp_ratio; }
-
-
- void update();
- void setup();
- void call_queries();
-
-
- bool is_locked() const;
- void lock();
- void unlock();
-
- void set_param(Physics2DServer::SpaceParameter p_param, real_t p_value);
- real_t get_param(Physics2DServer::SpaceParameter p_param) const;
-
- Physics2DDirectSpaceStateSW *get_direct_state();
-
- Space2DSW();
- ~Space2DSW();
-};
-
-
-#endif // SPACE_2D_SW_H
+#ifndef SPACE_2D_SW_H +#define SPACE_2D_SW_H + +#include "typedefs.h" +#include "hash_map.h" +#include "body_2d_sw.h" +#include "area_2d_sw.h" +#include "body_pair_2d_sw.h" +#include "area_pair_2d_sw.h" +#include "broad_phase_2d_sw.h" +#include "collision_object_2d_sw.h" + + +class Physics2DDirectSpaceStateSW : public Physics2DDirectSpaceState { + + OBJ_TYPE( Physics2DDirectSpaceStateSW, Physics2DDirectSpaceState ); +public: + + Space2DSW *space; + + bool intersect_ray(const Vector2& p_from, const Vector2& p_to,RayResult &r_result,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0); + int intersect_shape(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude=Set<RID>(),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<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0); + + Physics2DDirectSpaceStateSW(); +}; + + + +class Space2DSW { + + + Physics2DDirectSpaceStateSW *direct_access; + RID self; + + BroadPhase2DSW *broadphase; + SelfList<Body2DSW>::List active_list; + SelfList<Body2DSW>::List inertia_update_list; + SelfList<Body2DSW>::List state_query_list; + SelfList<Area2DSW>::List monitor_query_list; + SelfList<Area2DSW>::List area_moved_list; + + static void* _broadphase_pair(CollisionObject2DSW *A,int p_subindex_A,CollisionObject2DSW *B,int p_subindex_B,void *p_self); + static void _broadphase_unpair(CollisionObject2DSW *A,int p_subindex_A,CollisionObject2DSW *B,int p_subindex_B,void *p_data,void *p_self); + + Set<CollisionObject2DSW*> objects; + + Area2DSW *area; + + real_t contact_recycle_radius; + real_t contact_max_separation; + real_t contact_max_allowed_penetration; + real_t constraint_bias; + + enum { + + INTERSECTION_QUERY_MAX=2048 + }; + + CollisionObject2DSW *intersection_query_results[INTERSECTION_QUERY_MAX]; + int intersection_query_subindex_results[INTERSECTION_QUERY_MAX]; + + float body_linear_velocity_sleep_treshold; + float body_angular_velocity_sleep_treshold; + float body_time_to_sleep; + float body_angular_velocity_damp_ratio; + + bool locked; + +friend class Physics2DDirectSpaceStateSW; + +public: + + _FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; } + _FORCE_INLINE_ RID get_self() const { return self; } + + void set_default_area(Area2DSW *p_area) { area=p_area; } + Area2DSW *get_default_area() const { return area; } + + const SelfList<Body2DSW>::List& get_active_body_list() const; + void body_add_to_active_list(SelfList<Body2DSW>* p_body); + void body_remove_from_active_list(SelfList<Body2DSW>* p_body); + void body_add_to_inertia_update_list(SelfList<Body2DSW>* p_body); + void body_remove_from_inertia_update_list(SelfList<Body2DSW>* p_body); + void area_add_to_moved_list(SelfList<Area2DSW>* p_area); + void area_remove_from_moved_list(SelfList<Area2DSW>* p_area); + const SelfList<Area2DSW>::List& get_moved_area_list() const; + + + + + void body_add_to_state_query_list(SelfList<Body2DSW>* p_body); + void body_remove_from_state_query_list(SelfList<Body2DSW>* p_body); + + void area_add_to_monitor_query_list(SelfList<Area2DSW>* p_area); + void area_remove_from_monitor_query_list(SelfList<Area2DSW>* p_area); + + BroadPhase2DSW *get_broadphase(); + + void add_object(CollisionObject2DSW *p_object); + void remove_object(CollisionObject2DSW *p_object); + const Set<CollisionObject2DSW*> &get_objects() const; + + _FORCE_INLINE_ real_t get_contact_recycle_radius() const { return contact_recycle_radius; } + _FORCE_INLINE_ real_t get_contact_max_separation() const { return contact_max_separation; } + _FORCE_INLINE_ real_t get_contact_max_allowed_penetration() const { return contact_max_allowed_penetration; } + _FORCE_INLINE_ real_t get_constraint_bias() const { return constraint_bias; } + _FORCE_INLINE_ real_t get_body_linear_velocity_sleep_treshold() const { return body_linear_velocity_sleep_treshold; } + _FORCE_INLINE_ real_t get_body_angular_velocity_sleep_treshold() const { return body_angular_velocity_sleep_treshold; } + _FORCE_INLINE_ real_t get_body_time_to_sleep() const { return body_time_to_sleep; } + _FORCE_INLINE_ real_t get_body_angular_velocity_damp_ratio() const { return body_angular_velocity_damp_ratio; } + + + void update(); + void setup(); + void call_queries(); + + + bool is_locked() const; + void lock(); + void unlock(); + + void set_param(Physics2DServer::SpaceParameter p_param, real_t p_value); + real_t get_param(Physics2DServer::SpaceParameter p_param) const; + + Physics2DDirectSpaceStateSW *get_direct_state(); + + Space2DSW(); + ~Space2DSW(); +}; + + +#endif // SPACE_2D_SW_H diff --git a/servers/physics_2d/step_2d_sw.cpp b/servers/physics_2d/step_2d_sw.cpp 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<Body2DSW>*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<RID>& p_exclude,uint32_t p_user_mask) { + + + Set<RID> exclude; + for(int i=0;i<p_exclude.size();i++) + exclude.insert(p_exclude[i]); + + + + MotionCastCollision mcc; + + bool result = cast_motion(p_shape,p_xform,p_motion,mcc,exclude,p_user_mask); + + if (!result) + return Variant(); + + Dictionary d(true); + d["point"]=mcc.point; + d["normal"]=mcc.normal; + d["rid"]=mcc.rid; + d["collider_id"]=mcc.collider_id; + d["collider"]=mcc.collider; + d["shape"]=mcc.shape; + + return d; + + +} @@ -175,6 +203,7 @@ void Physics2DDirectSpaceState::_bind_methods() { ObjectTypeDB::bind_method(_MD("intersect_ray:Dictionary","from","to","exclude","umask"),&Physics2DDirectSpaceState::_intersect_ray,DEFVAL(Array()),DEFVAL(0)); ObjectTypeDB::bind_method(_MD("intersect_shape:Physics2DShapeQueryResult","shape","xform","result_max","exclude","umask"),&Physics2DDirectSpaceState::_intersect_shape,DEFVAL(Array()),DEFVAL(0)); + ObjectTypeDB::bind_method(_MD("cast_motion","shape","xform","motion","exclude","umask"),&Physics2DDirectSpaceState::_intersect_shape,DEFVAL(Array()),DEFVAL(0)); } @@ -297,8 +326,8 @@ void Physics2DServer::_bind_methods() { ObjectTypeDB::bind_method(_MD("body_get_object_instance_ID","body"),&Physics2DServer::body_get_object_instance_ID); - ObjectTypeDB::bind_method(_MD("body_set_enable_continuous_collision_detection","body","enable"),&Physics2DServer::body_set_enable_continuous_collision_detection); - ObjectTypeDB::bind_method(_MD("body_is_continuous_collision_detection_enabled","body"),&Physics2DServer::body_is_continuous_collision_detection_enabled); + ObjectTypeDB::bind_method(_MD("body_set_continuous_collision_detection_mode","body","mode"),&Physics2DServer::body_set_continuous_collision_detection_mode); + ObjectTypeDB::bind_method(_MD("body_get_continuous_collision_detection_mode","body"),&Physics2DServer::body_get_continuous_collision_detection_mode); //ObjectTypeDB::bind_method(_MD("body_set_user_flags","flags""),&Physics2DServer::body_set_shape,DEFVAL(Matrix32)); @@ -307,8 +336,6 @@ void Physics2DServer::_bind_methods() { ObjectTypeDB::bind_method(_MD("body_set_param","body","param","value"),&Physics2DServer::body_set_param); ObjectTypeDB::bind_method(_MD("body_get_param","body","param"),&Physics2DServer::body_get_param); - ObjectTypeDB::bind_method(_MD("body_static_simulate_motion","body","new_xform"),&Physics2DServer::body_static_simulate_motion); - ObjectTypeDB::bind_method(_MD("body_set_state","body","state","value"),&Physics2DServer::body_set_state); ObjectTypeDB::bind_method(_MD("body_get_state","body","state"),&Physics2DServer::body_get_state); @@ -371,7 +398,7 @@ void Physics2DServer::_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 ); @@ -394,6 +421,10 @@ void Physics2DServer::_bind_methods() { BIND_CONSTANT( DAMPED_STRING_STIFFNESS ); BIND_CONSTANT( DAMPED_STRING_DAMPING ); + BIND_CONSTANT( CCD_MODE_DISABLED ); + BIND_CONSTANT( CCD_MODE_CAST_RAY ); + BIND_CONSTANT( CCD_MODE_CAST_SHAPE ); + // BIND_CONSTANT( TYPE_BODY ); // BIND_CONSTANT( TYPE_AREA ); diff --git a/servers/physics_2d_server.h b/servers/physics_2d_server.h index 8ace6282fd..5242ec0e2a 100644 --- a/servers/physics_2d_server.h +++ b/servers/physics_2d_server.h @@ -90,6 +90,7 @@ class Physics2DDirectSpaceState : public Object { Variant _intersect_ray(const Vector2& p_from, const Vector2& p_to,const Vector<RID>& p_exclude=Vector<RID>(),uint32_t p_user_mask=0); Variant _intersect_shape(const RID& p_shape, const Matrix32& p_xform,int p_result_max=64,const Vector<RID>& p_exclude=Vector<RID>(),uint32_t p_user_mask=0); + Variant _cast_motion(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,const Vector<RID>& p_exclude=Vector<RID>(),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<RID>& p_exclude=Set<RID>(),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<RID>& p_exclude=Set<RID>(),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<RID>& p_exclude=Set<RID>(),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 |