diff --git a/scene/2d/physics_body_2d.cpp b/scene/2d/physics_body_2d.cpp index 90b8553645..6c1ff64770 100644 --- a/scene/2d/physics_body_2d.cpp +++ b/scene/2d/physics_body_2d.cpp @@ -29,6 +29,8 @@ #include "physics_body_2d.h" #include "scene/scene_string_names.h" +bool PhysicsBody2D::motion_fix_enabled=false; + void PhysicsBody2D::_notification(int p_what) { /* @@ -436,7 +438,11 @@ bool RigidBody2D::_test_motion(const Vector2& p_motion,float p_margin,const Ref< Physics2DServer::MotionResult *r=NULL; if (p_result.is_valid()) r=p_result->get_result_ptr(); - return Physics2DServer::get_singleton()->body_test_motion(get_rid(),p_motion,p_margin,r); + if (motion_fix_enabled) { + return Physics2DServer::get_singleton()->body_test_motion_from(get_rid(),get_global_transform(),p_motion,p_margin,r); + } else { + return Physics2DServer::get_singleton()->body_test_motion(get_rid(),p_motion,p_margin,r); + } } @@ -1022,7 +1028,6 @@ RigidBody2D::~RigidBody2D() { ////////////////////////// - Variant KinematicBody2D::_get_collider() const { ObjectID oid=get_collider(); @@ -1044,9 +1049,8 @@ void KinematicBody2D::revert_motion() { Matrix32 gt = get_global_transform(); gt.elements[2]-=travel; - travel=Vector2(); set_global_transform(gt); - + travel=Vector2(); } Vector2 KinematicBody2D::get_travel() const { @@ -1057,8 +1061,14 @@ Vector2 KinematicBody2D::get_travel() const { Vector2 KinematicBody2D::move(const Vector2& p_motion) { #if 1 + + Matrix32 gt = get_global_transform(); Physics2DServer::MotionResult result; - colliding = Physics2DServer::get_singleton()->body_test_motion(get_rid(),p_motion,margin,&result); + if (motion_fix_enabled) { + colliding = Physics2DServer::get_singleton()->body_test_motion_from(get_rid(),gt,p_motion,margin,&result); + } else { + colliding = Physics2DServer::get_singleton()->body_test_motion(get_rid(),p_motion,margin,&result); + } collider_metadata=result.collider_metadata; collider_shape=result.collider_shape; @@ -1067,10 +1077,12 @@ Vector2 KinematicBody2D::move(const Vector2& p_motion) { normal=result.collision_normal; collider=result.collider_id; - Matrix32 gt = get_global_transform(); + gt.elements[2]+=result.motion; set_global_transform(gt); travel=result.motion; + + return result.remainder; #else @@ -1081,7 +1093,6 @@ Vector2 KinematicBody2D::move(const Vector2& p_motion) { //this took about a week to get right.. //but is it right? who knows at this point.. - colliding=false; ERR_FAIL_COND_V(!is_inside_tree(),Vector2()); Physics2DDirectSpaceState *dss = Physics2DServer::get_singleton()->space_get_direct_state(get_world_2d()->get_space()); @@ -1099,17 +1110,15 @@ Vector2 KinematicBody2D::move(const Vector2& p_motion) { bool collided=false; uint32_t mask=0; - if (collide_static) + if (true) mask|=Physics2DDirectSpaceState::TYPE_MASK_STATIC_BODY; - if (collide_kinematic) + if (true) mask|=Physics2DDirectSpaceState::TYPE_MASK_KINEMATIC_BODY; - if (collide_rigid) + if (true) mask|=Physics2DDirectSpaceState::TYPE_MASK_RIGID_BODY; - if (collide_character) + if (true) mask|=Physics2DDirectSpaceState::TYPE_MASK_CHARACTER_BODY; -// print_line("motion: "+p_motion+" margin: "+rtos(margin)); - //print_line("margin: "+rtos(margin)); do { @@ -1145,6 +1154,8 @@ Vector2 KinematicBody2D::move(const Vector2& p_motion) { break; } + + Matrix32 gt = get_global_transform(); gt.elements[2]+=recover_motion; set_global_transform(gt); @@ -1191,6 +1202,8 @@ Vector2 KinematicBody2D::move(const Vector2& p_motion) { if (safe>=1) { //not collided colliding=false; + + } else { //it collided, let's get the rest info in unsafe advance @@ -1235,9 +1248,18 @@ bool KinematicBody2D::test_move(const Vector2& p_motion) { ERR_FAIL_COND_V(!is_inside_tree(),false); - return Physics2DServer::get_singleton()->body_test_motion(get_rid(),p_motion,margin); + if (motion_fix_enabled) { + return Physics2DServer::get_singleton()->body_test_motion_from(get_rid(),get_global_transform(),p_motion,margin); + } else { + return Physics2DServer::get_singleton()->body_test_motion(get_rid(),p_motion,margin); + } +} +bool KinematicBody2D::test_move_from(const Matrix32& p_from,const Vector2& p_motion) { + ERR_FAIL_COND_V(!is_inside_tree(),false); + + return Physics2DServer::get_singleton()->body_test_motion_from(get_rid(),p_from,p_motion,margin); } Vector2 KinematicBody2D::get_collision_pos() const { @@ -1302,6 +1324,7 @@ void KinematicBody2D::_bind_methods() { ObjectTypeDB::bind_method(_MD("move_to","position"),&KinematicBody2D::move_to); ObjectTypeDB::bind_method(_MD("test_move","rel_vec"),&KinematicBody2D::test_move); + ObjectTypeDB::bind_method(_MD("test_move_from","from","rel_vec"),&KinematicBody2D::test_move_from); ObjectTypeDB::bind_method(_MD("get_travel"),&KinematicBody2D::get_travel); ObjectTypeDB::bind_method(_MD("revert_motion"),&KinematicBody2D::revert_motion); diff --git a/scene/2d/physics_body_2d.h b/scene/2d/physics_body_2d.h index 37871219d7..c86bb031a7 100644 --- a/scene/2d/physics_body_2d.h +++ b/scene/2d/physics_body_2d.h @@ -33,7 +33,6 @@ #include "servers/physics_2d_server.h" #include "vset.h" - class PhysicsBody2D : public CollisionObject2D { OBJ_TYPE(PhysicsBody2D,CollisionObject2D); @@ -49,6 +48,10 @@ class PhysicsBody2D : public CollisionObject2D { protected: + // So this flag can be set at startup and cached for every body + friend void register_scene_types(); + static bool motion_fix_enabled; + void _notification(int p_what); PhysicsBody2D(Physics2DServer::BodyMode p_mode); @@ -314,6 +317,7 @@ public: Vector2 move_to(const Vector2& p_position); bool test_move(const Vector2& p_motion); + bool test_move_from(const Matrix32 &p_from, const Vector2& p_motion); bool is_colliding() const; Vector2 get_travel() const; diff --git a/scene/register_scene_types.cpp b/scene/register_scene_types.cpp index 0dda569464..2a525de2c7 100644 --- a/scene/register_scene_types.cpp +++ b/scene/register_scene_types.cpp @@ -497,6 +497,7 @@ void register_scene_types() { ObjectTypeDB::register_type(); ObjectTypeDB::register_type(); ObjectTypeDB::register_type(); + KinematicBody2D::motion_fix_enabled=bool(GLOBAL_DEF("physics_2d/motion_fix_enabled",false)); ObjectTypeDB::register_type(); ObjectTypeDB::register_type(); ObjectTypeDB::register_type(); diff --git a/servers/physics_2d/physics_2d_server_sw.cpp b/servers/physics_2d/physics_2d_server_sw.cpp index 0a16fedbeb..baa2381564 100644 --- a/servers/physics_2d/physics_2d_server_sw.cpp +++ b/servers/physics_2d/physics_2d_server_sw.cpp @@ -1016,14 +1016,27 @@ void Physics2DServerSW::body_set_pickable(RID p_body,bool p_pickable) { } -bool Physics2DServerSW::body_test_motion(RID p_body,const Vector2& p_motion,float p_margin,MotionResult *r_result) { +bool Physics2DServerSW::body_test_motion(RID p_body, const Vector2& p_motion, float p_margin, MotionResult *r_result) { Body2DSW *body = body_owner.get(p_body); ERR_FAIL_COND_V(!body,false); ERR_FAIL_COND_V(!body->get_space(),false); ERR_FAIL_COND_V(body->get_space()->is_locked(),false); - return body->get_space()->test_body_motion(body,p_motion,p_margin,r_result); + // Since this is the old-style, broken version, the transform to be used + // is that the physics server is aware of + return body->get_space()->test_body_motion(body,body->get_transform(),p_motion,p_margin,r_result); + +} + +bool Physics2DServerSW::body_test_motion_from(RID p_body, const Matrix32 &p_from, const Vector2& p_motion, float p_margin, MotionResult *r_result) { + + Body2DSW *body = body_owner.get(p_body); + ERR_FAIL_COND_V(!body,false); + ERR_FAIL_COND_V(!body->get_space(),false); + ERR_FAIL_COND_V(body->get_space()->is_locked(),false); + + return body->get_space()->test_body_motion(body,p_from,p_motion,p_margin,r_result); } diff --git a/servers/physics_2d/physics_2d_server_sw.h b/servers/physics_2d/physics_2d_server_sw.h index 217d85377f..99ab99996e 100644 --- a/servers/physics_2d/physics_2d_server_sw.h +++ b/servers/physics_2d/physics_2d_server_sw.h @@ -237,6 +237,7 @@ public: virtual void body_set_pickable(RID p_body,bool p_pickable); virtual bool body_test_motion(RID p_body,const Vector2& p_motion,float p_margin=0.001,MotionResult *r_result=NULL); + virtual bool body_test_motion_from(RID p_body,const Matrix32& p_from,const Vector2& p_motion,float p_margin=0.001,MotionResult *r_result=NULL); /* JOINT API */ diff --git a/servers/physics_2d/physics_2d_server_wrap_mt.h b/servers/physics_2d/physics_2d_server_wrap_mt.h index 0cb9965884..1609919676 100644 --- a/servers/physics_2d/physics_2d_server_wrap_mt.h +++ b/servers/physics_2d/physics_2d_server_wrap_mt.h @@ -272,6 +272,12 @@ public: return physics_2d_server->body_test_motion(p_body,p_motion,p_margin,r_result); } + bool body_test_motion_from(RID p_body,const Matrix32& p_from,const Vector2& p_motion,float p_margin=0.001,MotionResult *r_result=NULL) { + + ERR_FAIL_COND_V(main_thread!=Thread::get_caller_ID(),false); + return physics_2d_server->body_test_motion_from(p_body,p_from,p_motion,p_margin,r_result); + } + /* JOINT API */ diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp index 89030f91bc..8a08e8160a 100644 --- a/servers/physics_2d/space_2d_sw.cpp +++ b/servers/physics_2d/space_2d_sw.cpp @@ -592,7 +592,7 @@ int Space2DSW::_cull_aabb_for_body(Body2DSW *p_body,const Rect2& p_aabb) { return amount; } -bool Space2DSW::test_body_motion(Body2DSW *p_body,const Vector2&p_motion,float p_margin,Physics2DServer::MotionResult *r_result) { +bool Space2DSW::test_body_motion(Body2DSW *p_body, const Matrix32 &p_from, const Vector2&p_motion, float p_margin, Physics2DServer::MotionResult *r_result) { //give me back regular physics engine logic //this is madness @@ -601,6 +601,11 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body,const Vector2&p_motion,float p //this took about a week to get right.. //but is it right? who knows at this point.. + if (r_result) { + r_result->collider_id=0; + r_result->collider_shape=0; + + } Rect2 body_aabb; for(int i=0;iget_shape_count();i++) { @@ -611,10 +616,12 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body,const Vector2&p_motion,float p body_aabb=body_aabb.merge(p_body->get_shape_aabb(i)); } + // Undo the currently transform the physics server is aware of and apply the provided one + body_aabb=p_from.xform(p_body->get_inv_transform().xform(body_aabb)); + body_aabb=body_aabb.grow(p_margin); - - Matrix32 body_transform = p_body->get_transform(); + Matrix32 body_transform = p_from; { //STEP 1, FREE BODY IF STUCK @@ -684,6 +691,17 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body,const Vector2&p_motion,float p Vector2 a = sr[i*2+0]; Vector2 b = sr[i*2+1]; +#if 0 + Vector2 rel = b-a; + float d = rel.length(); + if (d==0) + continue; + + Vector2 n = rel/d; + float traveled = n.dot(recover_motion); + a+=n*traveled; + +#endif // float d = a.distance_to(b); //if (dmotion=p_motion+(body_transform.elements[2]-p_body->get_transform().elements[2]); - r_result->remainder=Vector2(); + r_result->motion=p_motion; + r_result->remainder=Vector2(); + r_result->motion+=(body_transform.elements[2]-p_from.elements[2]); } } else { @@ -898,16 +917,19 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body,const Vector2&p_motion,float p Vector2 rel_vec = r_result->collision_point-body->get_transform().get_origin(); r_result->collider_velocity = Vector2(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity(); - r_result->motion=safe*p_motion+(body_transform.elements[2]-p_body->get_transform().elements[2]); + r_result->motion=safe*p_motion; r_result->remainder=p_motion - safe * p_motion; + r_result->motion+=(body_transform.elements[2]-p_from.elements[2]); + } collided=true; } else { if (r_result) { - r_result->motion=p_motion+(body_transform.elements[2]-p_body->get_transform().elements[2]); + r_result->motion=p_motion; r_result->remainder=Vector2(); + r_result->motion+=(body_transform.elements[2]-p_from.elements[2]); } collided=false; diff --git a/servers/physics_2d/space_2d_sw.h b/servers/physics_2d/space_2d_sw.h index 5861653b47..b7976c589c 100644 --- a/servers/physics_2d/space_2d_sw.h +++ b/servers/physics_2d/space_2d_sw.h @@ -185,7 +185,7 @@ public: int get_collision_pairs() const { return collision_pairs; } - bool test_body_motion(Body2DSW *p_body, const Vector2&p_motion, float p_margin, Physics2DServer::MotionResult *r_result); + bool test_body_motion(Body2DSW *p_body, const Matrix32 &p_from, const Vector2&p_motion, float p_margin, Physics2DServer::MotionResult *r_result); void set_debug_contacts(int p_amount) { contact_debug.resize(p_amount); } diff --git a/servers/physics_2d_server.cpp b/servers/physics_2d_server.cpp index 551edf56bc..dda3cc913d 100644 --- a/servers/physics_2d_server.cpp +++ b/servers/physics_2d_server.cpp @@ -501,6 +501,14 @@ bool Physics2DServer::_body_test_motion(RID p_body,const Vector2& p_motion,float return body_test_motion(p_body,p_motion,p_margin,r); } +bool Physics2DServer::_body_test_motion_from(RID p_body,const Matrix32& p_from,const Vector2& p_motion,float p_margin,const Ref& p_result) { + + MotionResult *r=NULL; + if (p_result.is_valid()) + r=p_result->get_result_ptr(); + return body_test_motion_from(p_body,p_from,p_motion,p_margin,r); +} + void Physics2DServer::_bind_methods() { @@ -620,6 +628,7 @@ void Physics2DServer::_bind_methods() { ObjectTypeDB::bind_method(_MD("body_set_force_integration_callback","body","receiver","method","userdata"),&Physics2DServer::body_set_force_integration_callback,DEFVAL(Variant())); ObjectTypeDB::bind_method(_MD("body_test_motion","body","motion","margin","result:Physics2DTestMotionResult"),&Physics2DServer::_body_test_motion,DEFVAL(0.08),DEFVAL(Variant())); + ObjectTypeDB::bind_method(_MD("body_test_motion_from","body","from","motion","margin","result:Physics2DTestMotionResult"),&Physics2DServer::_body_test_motion_from,DEFVAL(0.08),DEFVAL(Variant())); /* JOINT API */ diff --git a/servers/physics_2d_server.h b/servers/physics_2d_server.h index f44cd3fbd0..cd65c01b0d 100644 --- a/servers/physics_2d_server.h +++ b/servers/physics_2d_server.h @@ -238,7 +238,8 @@ class Physics2DServer : public Object { static Physics2DServer * singleton; - virtual bool _body_test_motion(RID p_body,const Vector2& p_motion,float p_margin=0.08,const Ref& p_result=Ref()); + virtual bool _body_test_motion(RID p_body, const Vector2& p_motion, float p_margin=0.08, const Ref& p_result=Ref()); + virtual bool _body_test_motion_from(RID p_body, const Matrix32 &p_from, const Vector2& p_motion, float p_margin=0.08, const Ref& p_result=Ref()); protected: static void _bind_methods(); @@ -498,6 +499,7 @@ public: }; virtual bool body_test_motion(RID p_body,const Vector2& p_motion,float p_margin=0.001,MotionResult *r_result=NULL)=0; + virtual bool body_test_motion_from(RID p_body,const Matrix32& p_from,const Vector2& p_motion,float p_margin=0.001,MotionResult *r_result=NULL)=0; /* JOINT API */