-Rewritten KinematicBody2D::move to MUCH more efficient code.
-KinematicBody2D::move now properly recognizes collision exceptions and masks, fixes #1649 -Removed object type masking for KinematicBody2D -Added a test_motion() function to RigidBody2D, allowing simlar behavior to KinematicBody2D::move there.
This commit is contained in:
Juan Linietsky
parent
1de1a04b78
commit
28c4afeb57
|
|
@ -14,4 +14,5 @@ func _ready():
|
|||
|
||||
func _on_princess_body_enter( body ):
|
||||
#the name of this editor-generated callback is unfortunate
|
||||
if (body.get_name()=="player"):
|
||||
get_node("youwin").show()
|
||||
|
|
|
|||
Binary file not shown.
|
|
@ -341,6 +341,16 @@ struct _RigidBody2DInOut {
|
|||
int local_shape;
|
||||
};
|
||||
|
||||
|
||||
bool RigidBody2D::_test_motion(const Vector2& p_motion,float p_margin,const Ref<Physics2DTestMotionResult>& p_result) {
|
||||
|
||||
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);
|
||||
|
||||
}
|
||||
|
||||
void RigidBody2D::_direct_state_changed(Object *p_state) {
|
||||
|
||||
//eh.. fuck
|
||||
|
|
@ -791,6 +801,8 @@ void RigidBody2D::_bind_methods() {
|
|||
ObjectTypeDB::bind_method(_MD("set_can_sleep","able_to_sleep"),&RigidBody2D::set_can_sleep);
|
||||
ObjectTypeDB::bind_method(_MD("is_able_to_sleep"),&RigidBody2D::is_able_to_sleep);
|
||||
|
||||
ObjectTypeDB::bind_method(_MD("test_motion","motion","margin","result:Physics2DTestMotionResult"),&RigidBody2D::_test_motion,DEFVAL(0.08),DEFVAL(Variant()));
|
||||
|
||||
ObjectTypeDB::bind_method(_MD("_direct_state_changed"),&RigidBody2D::_direct_state_changed);
|
||||
ObjectTypeDB::bind_method(_MD("_body_enter_tree"),&RigidBody2D::_body_enter_tree);
|
||||
ObjectTypeDB::bind_method(_MD("_body_exit_tree"),&RigidBody2D::_body_exit_tree);
|
||||
|
|
@ -888,20 +900,25 @@ Variant KinematicBody2D::_get_collider() const {
|
|||
}
|
||||
|
||||
|
||||
bool KinematicBody2D::_ignores_mode(Physics2DServer::BodyMode p_mode) const {
|
||||
|
||||
switch(p_mode) {
|
||||
case Physics2DServer::BODY_MODE_STATIC: return !collide_static;
|
||||
case Physics2DServer::BODY_MODE_KINEMATIC: return !collide_kinematic;
|
||||
case Physics2DServer::BODY_MODE_RIGID: return !collide_rigid;
|
||||
case Physics2DServer::BODY_MODE_CHARACTER: return !collide_character;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
Vector2 KinematicBody2D::move(const Vector2& p_motion) {
|
||||
|
||||
#if 1
|
||||
Physics2DServer::MotionResult result;
|
||||
colliding = Physics2DServer::get_singleton()->body_test_motion(get_rid(),p_motion,margin,&result);
|
||||
|
||||
collider_metadata=result.collider_metadata;
|
||||
collider_shape=result.collider_shape;
|
||||
collider_vel=result.collider_velocity;
|
||||
collision=result.collision_point;
|
||||
normal=result.collision_normal;
|
||||
collider=result.collider_id;
|
||||
|
||||
Matrix32 gt = get_global_transform();
|
||||
gt.elements[2]+=result.motion;
|
||||
set_global_transform(gt);
|
||||
return result.remainder;
|
||||
|
||||
#else
|
||||
//give me back regular physics engine logic
|
||||
//this is madness
|
||||
//and most people using this function will think
|
||||
|
|
@ -1051,7 +1068,7 @@ Vector2 KinematicBody2D::move(const Vector2& p_motion) {
|
|||
set_global_transform(gt);
|
||||
|
||||
return p_motion-motion;
|
||||
|
||||
#endif
|
||||
}
|
||||
|
||||
Vector2 KinematicBody2D::move_to(const Vector2& p_position) {
|
||||
|
|
@ -1059,58 +1076,22 @@ Vector2 KinematicBody2D::move_to(const Vector2& p_position) {
|
|||
return move(p_position-get_global_pos());
|
||||
}
|
||||
|
||||
bool KinematicBody2D::can_move_to(const Vector2& p_position, bool p_discrete) {
|
||||
|
||||
ERR_FAIL_COND_V(!is_inside_tree(),false);
|
||||
Physics2DDirectSpaceState *dss = Physics2DServer::get_singleton()->space_get_direct_state(get_world_2d()->get_space());
|
||||
ERR_FAIL_COND_V(!dss,false);
|
||||
|
||||
uint32_t mask=0;
|
||||
if (collide_static)
|
||||
mask|=Physics2DDirectSpaceState::TYPE_MASK_STATIC_BODY;
|
||||
if (collide_kinematic)
|
||||
mask|=Physics2DDirectSpaceState::TYPE_MASK_KINEMATIC_BODY;
|
||||
if (collide_rigid)
|
||||
mask|=Physics2DDirectSpaceState::TYPE_MASK_RIGID_BODY;
|
||||
if (collide_character)
|
||||
mask|=Physics2DDirectSpaceState::TYPE_MASK_CHARACTER_BODY;
|
||||
|
||||
Vector2 motion = p_position-get_global_pos();
|
||||
Matrix32 xform=get_global_transform();
|
||||
|
||||
if (p_discrete) {
|
||||
|
||||
xform.elements[2]+=motion;
|
||||
motion=Vector2();
|
||||
}
|
||||
|
||||
Set<RID> exclude;
|
||||
exclude.insert(get_rid());
|
||||
|
||||
//fill exclude list..
|
||||
for(int i=0;i<get_shape_count();i++) {
|
||||
|
||||
|
||||
bool col = dss->intersect_shape(get_shape(i)->get_rid(), xform * get_shape_transform(i),motion,0,NULL,0,exclude,get_layer_mask(),mask);
|
||||
if (col)
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool KinematicBody2D::is_colliding() const {
|
||||
bool KinematicBody2D::test_move(const Vector2& p_motion) {
|
||||
|
||||
ERR_FAIL_COND_V(!is_inside_tree(),false);
|
||||
|
||||
return colliding;
|
||||
return Physics2DServer::get_singleton()->body_test_motion(get_rid(),p_motion,margin);
|
||||
|
||||
|
||||
}
|
||||
|
||||
Vector2 KinematicBody2D::get_collision_pos() const {
|
||||
|
||||
ERR_FAIL_COND_V(!colliding,Vector2());
|
||||
return collision;
|
||||
|
||||
}
|
||||
|
||||
Vector2 KinematicBody2D::get_collision_normal() const {
|
||||
|
||||
ERR_FAIL_COND_V(!colliding,Vector2());
|
||||
|
|
@ -1143,43 +1124,10 @@ Variant KinematicBody2D::get_collider_metadata() const {
|
|||
|
||||
}
|
||||
|
||||
void KinematicBody2D::set_collide_with_static_bodies(bool p_enable) {
|
||||
|
||||
collide_static=p_enable;
|
||||
}
|
||||
bool KinematicBody2D::can_collide_with_static_bodies() const {
|
||||
bool KinematicBody2D::is_colliding() const{
|
||||
|
||||
return collide_static;
|
||||
}
|
||||
|
||||
void KinematicBody2D::set_collide_with_rigid_bodies(bool p_enable) {
|
||||
|
||||
collide_rigid=p_enable;
|
||||
|
||||
}
|
||||
bool KinematicBody2D::can_collide_with_rigid_bodies() const {
|
||||
|
||||
|
||||
return collide_rigid;
|
||||
}
|
||||
|
||||
void KinematicBody2D::set_collide_with_kinematic_bodies(bool p_enable) {
|
||||
|
||||
collide_kinematic=p_enable;
|
||||
|
||||
}
|
||||
bool KinematicBody2D::can_collide_with_kinematic_bodies() const {
|
||||
|
||||
return collide_kinematic;
|
||||
}
|
||||
|
||||
void KinematicBody2D::set_collide_with_character_bodies(bool p_enable) {
|
||||
|
||||
collide_character=p_enable;
|
||||
}
|
||||
bool KinematicBody2D::can_collide_with_character_bodies() const {
|
||||
|
||||
return collide_character;
|
||||
return colliding;
|
||||
}
|
||||
|
||||
void KinematicBody2D::set_collision_margin(float p_margin) {
|
||||
|
|
@ -1198,7 +1146,7 @@ void KinematicBody2D::_bind_methods() {
|
|||
ObjectTypeDB::bind_method(_MD("move","rel_vec"),&KinematicBody2D::move);
|
||||
ObjectTypeDB::bind_method(_MD("move_to","position"),&KinematicBody2D::move_to);
|
||||
|
||||
ObjectTypeDB::bind_method(_MD("can_move_to","position","discrete"),&KinematicBody2D::can_move_to,DEFVAL(false));
|
||||
ObjectTypeDB::bind_method(_MD("test_move","rel_vec"),&KinematicBody2D::test_move);
|
||||
|
||||
ObjectTypeDB::bind_method(_MD("is_colliding"),&KinematicBody2D::is_colliding);
|
||||
|
||||
|
|
@ -1209,26 +1157,9 @@ void KinematicBody2D::_bind_methods() {
|
|||
ObjectTypeDB::bind_method(_MD("get_collider_shape"),&KinematicBody2D::get_collider_shape);
|
||||
ObjectTypeDB::bind_method(_MD("get_collider_metadata"),&KinematicBody2D::get_collider_metadata);
|
||||
|
||||
|
||||
ObjectTypeDB::bind_method(_MD("set_collide_with_static_bodies","enable"),&KinematicBody2D::set_collide_with_static_bodies);
|
||||
ObjectTypeDB::bind_method(_MD("can_collide_with_static_bodies"),&KinematicBody2D::can_collide_with_static_bodies);
|
||||
|
||||
ObjectTypeDB::bind_method(_MD("set_collide_with_kinematic_bodies","enable"),&KinematicBody2D::set_collide_with_kinematic_bodies);
|
||||
ObjectTypeDB::bind_method(_MD("can_collide_with_kinematic_bodies"),&KinematicBody2D::can_collide_with_kinematic_bodies);
|
||||
|
||||
ObjectTypeDB::bind_method(_MD("set_collide_with_rigid_bodies","enable"),&KinematicBody2D::set_collide_with_rigid_bodies);
|
||||
ObjectTypeDB::bind_method(_MD("can_collide_with_rigid_bodies"),&KinematicBody2D::can_collide_with_rigid_bodies);
|
||||
|
||||
ObjectTypeDB::bind_method(_MD("set_collide_with_character_bodies","enable"),&KinematicBody2D::set_collide_with_character_bodies);
|
||||
ObjectTypeDB::bind_method(_MD("can_collide_with_character_bodies"),&KinematicBody2D::can_collide_with_character_bodies);
|
||||
|
||||
ObjectTypeDB::bind_method(_MD("set_collision_margin","pixels"),&KinematicBody2D::set_collision_margin);
|
||||
ObjectTypeDB::bind_method(_MD("get_collision_margin","pixels"),&KinematicBody2D::get_collision_margin);
|
||||
|
||||
ADD_PROPERTY( PropertyInfo(Variant::BOOL,"collide_with/static"),_SCS("set_collide_with_static_bodies"),_SCS("can_collide_with_static_bodies"));
|
||||
ADD_PROPERTY( PropertyInfo(Variant::BOOL,"collide_with/kinematic"),_SCS("set_collide_with_kinematic_bodies"),_SCS("can_collide_with_kinematic_bodies"));
|
||||
ADD_PROPERTY( PropertyInfo(Variant::BOOL,"collide_with/rigid"),_SCS("set_collide_with_rigid_bodies"),_SCS("can_collide_with_rigid_bodies"));
|
||||
ADD_PROPERTY( PropertyInfo(Variant::BOOL,"collide_with/character"),_SCS("set_collide_with_character_bodies"),_SCS("can_collide_with_character_bodies"));
|
||||
ADD_PROPERTY( PropertyInfo(Variant::REAL,"collision/margin",PROPERTY_HINT_RANGE,"0.001,256,0.001"),_SCS("set_collision_margin"),_SCS("get_collision_margin"));
|
||||
|
||||
|
||||
|
|
@ -1236,11 +1167,6 @@ void KinematicBody2D::_bind_methods() {
|
|||
|
||||
KinematicBody2D::KinematicBody2D() : PhysicsBody2D(Physics2DServer::BODY_MODE_KINEMATIC){
|
||||
|
||||
collide_static=true;
|
||||
collide_rigid=true;
|
||||
collide_kinematic=true;
|
||||
collide_character=true;
|
||||
|
||||
colliding=false;
|
||||
collider=0;
|
||||
|
||||
|
|
|
|||
|
|
@ -188,6 +188,7 @@ private:
|
|||
void _body_inout(int p_status, ObjectID p_instance, int p_body_shape,int p_local_shape);
|
||||
void _direct_state_changed(Object *p_state);
|
||||
|
||||
bool _test_motion(const Vector2& p_motion,float p_margin=0.08,const Ref<Physics2DTestMotionResult>& p_result=Ref<Physics2DTestMotionResult>());
|
||||
|
||||
protected:
|
||||
|
||||
|
|
@ -249,6 +250,8 @@ public:
|
|||
void set_applied_force(const Vector2& p_force);
|
||||
Vector2 get_applied_force() const;
|
||||
|
||||
|
||||
|
||||
Array get_colliding_bodies() const; //function for script
|
||||
|
||||
RigidBody2D();
|
||||
|
|
@ -266,11 +269,6 @@ class KinematicBody2D : public PhysicsBody2D {
|
|||
OBJ_TYPE(KinematicBody2D,PhysicsBody2D);
|
||||
|
||||
float margin;
|
||||
bool collide_static;
|
||||
bool collide_rigid;
|
||||
bool collide_kinematic;
|
||||
bool collide_character;
|
||||
|
||||
bool colliding;
|
||||
Vector2 collision;
|
||||
Vector2 normal;
|
||||
|
|
@ -290,7 +288,7 @@ public:
|
|||
Vector2 move(const Vector2& p_motion);
|
||||
Vector2 move_to(const Vector2& p_position);
|
||||
|
||||
bool can_move_to(const Vector2& p_position,bool p_discrete=false);
|
||||
bool test_move(const Vector2& p_motion);
|
||||
bool is_colliding() const;
|
||||
Vector2 get_collision_pos() const;
|
||||
Vector2 get_collision_normal() const;
|
||||
|
|
@ -299,18 +297,6 @@ public:
|
|||
int get_collider_shape() const;
|
||||
Variant get_collider_metadata() const;
|
||||
|
||||
void set_collide_with_static_bodies(bool p_enable);
|
||||
bool can_collide_with_static_bodies() const;
|
||||
|
||||
void set_collide_with_rigid_bodies(bool p_enable);
|
||||
bool can_collide_with_rigid_bodies() const;
|
||||
|
||||
void set_collide_with_kinematic_bodies(bool p_enable);
|
||||
bool can_collide_with_kinematic_bodies() const;
|
||||
|
||||
void set_collide_with_character_bodies(bool p_enable);
|
||||
bool can_collide_with_character_bodies() const;
|
||||
|
||||
void set_collision_margin(float p_margin);
|
||||
float get_collision_margin() const;
|
||||
|
||||
|
|
|
|||
|
|
@ -959,6 +959,18 @@ void Physics2DServerSW::body_set_pickable(RID p_body,bool p_pickable) {
|
|||
|
||||
}
|
||||
|
||||
bool Physics2DServerSW::body_test_motion(RID p_body,const Vector2& p_motion,float p_margin,MotionResult *r_result) {
|
||||
|
||||
Body2DSW *body = body_owner.get(p_body);
|
||||
ERR_FAIL_COND_V(!body,false);
|
||||
ERR_FAIL_COND_V(!body->get_space(),false);
|
||||
ERR_FAIL_COND_V(body->get_space()->is_locked(),false);
|
||||
|
||||
return body->get_space()->test_body_motion(body,p_motion,p_margin,r_result);
|
||||
|
||||
}
|
||||
|
||||
|
||||
/* JOINT API */
|
||||
|
||||
void Physics2DServerSW::joint_set_param(RID p_joint, JointParam p_param, real_t p_value) {
|
||||
|
|
|
|||
|
|
@ -223,6 +223,9 @@ public:
|
|||
|
||||
virtual void body_set_pickable(RID p_body,bool p_pickable);
|
||||
|
||||
virtual bool body_test_motion(RID p_body,const Vector2& p_motion,float p_margin=0.001,MotionResult *r_result=NULL);
|
||||
|
||||
|
||||
/* JOINT API */
|
||||
|
||||
virtual void joint_set_param(RID p_joint, JointParam p_param, real_t p_value);
|
||||
|
|
|
|||
|
|
@ -556,7 +556,511 @@ Physics2DDirectSpaceStateSW::Physics2DDirectSpaceStateSW() {
|
|||
|
||||
|
||||
|
||||
bool Space2DSW::test_body_motion(Body2DSW *p_body,const Vector2&p_motion,float p_margin,Physics2DServer::MotionResult *r_result) {
|
||||
|
||||
//give me back regular physics engine logic
|
||||
//this is madness
|
||||
//and most people using this function will think
|
||||
//what it does is simpler than using physics
|
||||
//this took about a week to get right..
|
||||
//but is it right? who knows at this point..
|
||||
|
||||
Rect2 body_aabb;
|
||||
|
||||
for(int i=0;i<p_body->get_shape_count();i++) {
|
||||
|
||||
if (i==0)
|
||||
body_aabb=p_body->get_shape_aabb(i);
|
||||
else
|
||||
body_aabb=body_aabb.merge(p_body->get_shape_aabb(i));
|
||||
}
|
||||
|
||||
body_aabb=body_aabb.grow(p_margin);
|
||||
|
||||
{
|
||||
//add motion
|
||||
|
||||
Rect2 motion_aabb=body_aabb;
|
||||
motion_aabb.pos+=p_motion;
|
||||
body_aabb=body_aabb.merge(motion_aabb);
|
||||
}
|
||||
|
||||
|
||||
int amount = broadphase->cull_aabb(body_aabb,intersection_query_results,INTERSECTION_QUERY_MAX,intersection_query_subindex_results);
|
||||
|
||||
for(int i=0;i<amount;i++) {
|
||||
|
||||
bool keep=true;
|
||||
|
||||
if (intersection_query_results[i]==p_body)
|
||||
keep=false;
|
||||
else if (intersection_query_results[i]->get_type()==CollisionObject2DSW::TYPE_AREA)
|
||||
keep=false;
|
||||
else if ((static_cast<Body2DSW*>(intersection_query_results[i])->get_layer_mask()&p_body->get_layer_mask())==0)
|
||||
keep=false;
|
||||
else if (static_cast<Body2DSW*>(intersection_query_results[i])->has_exception(p_body->get_self()) || p_body->has_exception(intersection_query_results[i]->get_self()))
|
||||
keep=false;
|
||||
else if (static_cast<Body2DSW*>(intersection_query_results[i])->is_shape_set_as_trigger(intersection_query_subindex_results[i]))
|
||||
keep=false;
|
||||
|
||||
if (!keep) {
|
||||
|
||||
if (i<amount-1) {
|
||||
SWAP(intersection_query_results[i],intersection_query_results[amount-1]);
|
||||
SWAP(intersection_query_subindex_results[i],intersection_query_subindex_results[amount-1]);
|
||||
|
||||
}
|
||||
|
||||
amount--;
|
||||
i--;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
Matrix32 body_transform = p_body->get_transform();
|
||||
|
||||
{
|
||||
//STEP 1, FREE BODY IF STUCK
|
||||
|
||||
const int max_results = 32;
|
||||
int recover_attempts=4;
|
||||
Vector2 sr[max_results*2];
|
||||
|
||||
do {
|
||||
|
||||
Physics2DServerSW::CollCbkData cbk;
|
||||
cbk.max=max_results;
|
||||
cbk.amount=0;
|
||||
cbk.ptr=sr;
|
||||
|
||||
|
||||
CollisionSolver2DSW::CallbackResult cbkres=NULL;
|
||||
|
||||
Physics2DServerSW::CollCbkData *cbkptr=NULL;
|
||||
cbkptr=&cbk;
|
||||
cbkres=Physics2DServerSW::_shape_col_cbk;
|
||||
|
||||
bool collided=false;
|
||||
|
||||
|
||||
for(int j=0;j<p_body->get_shape_count();j++) {
|
||||
if (p_body->is_shape_set_as_trigger(j))
|
||||
continue;
|
||||
|
||||
Matrix32 body_shape_xform = body_transform * p_body->get_shape_transform(j);
|
||||
Shape2DSW *body_shape = p_body->get_shape(j);
|
||||
for(int i=0;i<amount;i++) {
|
||||
|
||||
const CollisionObject2DSW *col_obj=intersection_query_results[i];
|
||||
int shape_idx=intersection_query_subindex_results[i];
|
||||
|
||||
if (col_obj->get_type()==CollisionObject2DSW::TYPE_BODY) {
|
||||
|
||||
const Body2DSW *body=static_cast<const Body2DSW*>(col_obj);
|
||||
cbk.valid_dir=body->get_one_way_collision_direction();
|
||||
cbk.valid_depth=body->get_one_way_collision_max_depth();
|
||||
} else {
|
||||
cbk.valid_dir=Vector2();
|
||||
cbk.valid_depth=0;
|
||||
}
|
||||
|
||||
if (CollisionSolver2DSW::solve(body_shape,body_shape_xform,Vector2(),col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx),Vector2(),cbkres,cbkptr,NULL,p_margin)) {
|
||||
collided=cbk.amount>0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
if (!collided)
|
||||
break;
|
||||
|
||||
Vector2 recover_motion;
|
||||
|
||||
for(int i=0;i<cbk.amount;i++) {
|
||||
|
||||
Vector2 a = sr[i*2+0];
|
||||
Vector2 b = sr[i*2+1];
|
||||
|
||||
// float d = a.distance_to(b);
|
||||
|
||||
//if (d<margin)
|
||||
/// continue;
|
||||
recover_motion+=(b-a)*0.4;
|
||||
}
|
||||
|
||||
if (recover_motion==Vector2()) {
|
||||
collided=false;
|
||||
break;
|
||||
}
|
||||
|
||||
body_transform.elements[2]+=recover_motion;
|
||||
|
||||
recover_attempts--;
|
||||
|
||||
} while (recover_attempts);
|
||||
}
|
||||
|
||||
|
||||
|
||||
float safe = 1.0;
|
||||
float unsafe = 1.0;
|
||||
int best_shape=-1;
|
||||
|
||||
{
|
||||
// STEP 2 ATTEMPT MOTION
|
||||
|
||||
|
||||
|
||||
for(int j=0;j<p_body->get_shape_count();j++) {
|
||||
|
||||
if (p_body->is_shape_set_as_trigger(j))
|
||||
continue;
|
||||
|
||||
Matrix32 body_shape_xform = body_transform * p_body->get_shape_transform(j);
|
||||
Shape2DSW *body_shape = p_body->get_shape(j);
|
||||
|
||||
bool stuck=false;
|
||||
|
||||
float best_safe=1;
|
||||
float best_unsafe=1;
|
||||
|
||||
for(int i=0;i<amount;i++) {
|
||||
|
||||
const CollisionObject2DSW *col_obj=intersection_query_results[i];
|
||||
int shape_idx=intersection_query_subindex_results[i];
|
||||
|
||||
|
||||
Matrix32 col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
|
||||
//test initial overlap, does it collide if going all the way?
|
||||
if (!CollisionSolver2DSW::solve(body_shape,body_shape_xform,p_motion,col_obj->get_shape(shape_idx),col_obj_xform,Vector2() ,NULL,NULL,NULL,0)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
|
||||
//test initial overlap
|
||||
if (CollisionSolver2DSW::solve(body_shape,body_shape_xform,Vector2(),col_obj->get_shape(shape_idx),col_obj_xform,Vector2() ,NULL,NULL,NULL,0)) {
|
||||
|
||||
if (col_obj->get_type()==CollisionObject2DSW::TYPE_BODY) {
|
||||
//if one way collision direction ignore initial overlap
|
||||
const Body2DSW *body=static_cast<const Body2DSW*>(col_obj);
|
||||
if (body->get_one_way_collision_direction()!=Vector2()) {
|
||||
continue;
|
||||
}
|
||||
}
|
||||
|
||||
stuck=true;
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
//just do kinematic solving
|
||||
float low=0;
|
||||
float hi=1;
|
||||
Vector2 mnormal=p_motion.normalized();
|
||||
|
||||
for(int i=0;i<8;i++) { //steps should be customizable..
|
||||
|
||||
//Matrix32 xfa = p_xform;
|
||||
float ofs = (low+hi)*0.5;
|
||||
|
||||
Vector2 sep=mnormal; //important optimization for this to work fast enough
|
||||
bool collided = CollisionSolver2DSW::solve(body_shape,body_shape_xform,p_motion*ofs,col_obj->get_shape(shape_idx),col_obj_xform,Vector2(),NULL,NULL,&sep,0);
|
||||
|
||||
if (collided) {
|
||||
|
||||
hi=ofs;
|
||||
} else {
|
||||
|
||||
low=ofs;
|
||||
}
|
||||
}
|
||||
|
||||
if (col_obj->get_type()==CollisionObject2DSW::TYPE_BODY) {
|
||||
|
||||
const Body2DSW *body=static_cast<const Body2DSW*>(col_obj);
|
||||
if (body->get_one_way_collision_direction()!=Vector2()) {
|
||||
|
||||
Vector2 cd[2];
|
||||
Physics2DServerSW::CollCbkData cbk;
|
||||
cbk.max=1;
|
||||
cbk.amount=0;
|
||||
cbk.ptr=cd;
|
||||
cbk.valid_dir=body->get_one_way_collision_direction();
|
||||
cbk.valid_depth=body->get_one_way_collision_max_depth();
|
||||
|
||||
Vector2 sep=mnormal; //important optimization for this to work fast enough
|
||||
bool collided = CollisionSolver2DSW::solve(body_shape,body_shape_xform,p_motion*(hi+contact_max_allowed_penetration),col_obj->get_shape(shape_idx),col_obj_xform,Vector2(),Physics2DServerSW::_shape_col_cbk,&cbk,&sep,0);
|
||||
if (!collided || cbk.amount==0) {
|
||||
continue;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
if (low<best_safe) {
|
||||
best_safe=low;
|
||||
best_unsafe=hi;
|
||||
}
|
||||
}
|
||||
|
||||
if (stuck) {
|
||||
|
||||
safe=0;
|
||||
unsafe=0;
|
||||
best_shape=j; //sadly it's the best
|
||||
break;
|
||||
}
|
||||
if (best_safe==1.0) {
|
||||
continue;
|
||||
}
|
||||
if (best_safe < safe) {
|
||||
|
||||
safe=best_safe;
|
||||
unsafe=best_unsafe;
|
||||
best_shape=j;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool collided=false;
|
||||
if (safe>=1) {
|
||||
//not collided
|
||||
collided=false;
|
||||
if (r_result) {
|
||||
|
||||
r_result->motion=p_motion+(body_transform.elements[2]-p_body->get_transform().elements[2]);
|
||||
r_result->remainder=Vector2();
|
||||
}
|
||||
|
||||
} else {
|
||||
|
||||
//it collided, let's get the rest info in unsafe advance
|
||||
Matrix32 ugt = body_transform;
|
||||
ugt.elements[2]+=p_motion*unsafe;
|
||||
|
||||
_RestCallbackData2D rcd;
|
||||
rcd.best_len=0;
|
||||
rcd.best_object=NULL;
|
||||
rcd.best_shape=0;
|
||||
|
||||
Matrix32 body_shape_xform = ugt * p_body->get_shape_transform(best_shape);
|
||||
Shape2DSW *body_shape = p_body->get_shape(best_shape);
|
||||
|
||||
|
||||
for(int i=0;i<amount;i++) {
|
||||
|
||||
|
||||
const CollisionObject2DSW *col_obj=intersection_query_results[i];
|
||||
int shape_idx=intersection_query_subindex_results[i];
|
||||
|
||||
if (col_obj->get_type()==CollisionObject2DSW::TYPE_BODY) {
|
||||
|
||||
const Body2DSW *body=static_cast<const Body2DSW*>(col_obj);
|
||||
rcd.valid_dir=body->get_one_way_collision_direction();
|
||||
rcd.valid_depth=body->get_one_way_collision_max_depth();
|
||||
} else {
|
||||
rcd.valid_dir=Vector2();
|
||||
rcd.valid_depth=0;
|
||||
}
|
||||
|
||||
|
||||
rcd.object=col_obj;
|
||||
rcd.shape=shape_idx;
|
||||
bool sc = CollisionSolver2DSW::solve(body_shape,body_shape_xform,Vector2(),col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx),Vector2() ,_rest_cbk_result,&rcd,NULL,p_margin);
|
||||
if (!sc)
|
||||
continue;
|
||||
|
||||
}
|
||||
|
||||
if (rcd.best_len!=0) {
|
||||
|
||||
if (r_result) {
|
||||
r_result->collider=rcd.best_object->get_self();
|
||||
r_result->collider_id=rcd.best_object->get_instance_id();
|
||||
r_result->collider_shape=rcd.best_shape;
|
||||
r_result->collision_normal=rcd.best_normal;
|
||||
r_result->collision_point=rcd.best_contact;
|
||||
r_result->collider_metadata=rcd.best_object->get_shape_metadata(rcd.best_shape);
|
||||
|
||||
const Body2DSW *body = static_cast<const Body2DSW*>(rcd.best_object);
|
||||
Vector2 rel_vec = r_result->collision_point-body->get_transform().get_origin();
|
||||
r_result->collider_velocity = Vector2(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity();
|
||||
|
||||
r_result->motion=safe*p_motion+(body_transform.elements[2]-p_body->get_transform().elements[2]);
|
||||
r_result->remainder=p_motion - safe * p_motion;
|
||||
}
|
||||
|
||||
collided=true;
|
||||
} else {
|
||||
if (r_result) {
|
||||
|
||||
r_result->motion=p_motion+(body_transform.elements[2]-p_body->get_transform().elements[2]);
|
||||
r_result->remainder=Vector2();
|
||||
}
|
||||
|
||||
collided=false;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
return collided;
|
||||
|
||||
|
||||
#if 0
|
||||
//give me back regular physics engine logic
|
||||
//this is madness
|
||||
//and most people using this function will think
|
||||
//what it does is simpler than using physics
|
||||
//this took about a week to get right..
|
||||
//but is it right? who knows at this point..
|
||||
|
||||
|
||||
colliding=false;
|
||||
ERR_FAIL_COND_V(!is_inside_tree(),Vector2());
|
||||
Physics2DDirectSpaceState *dss = Physics2DServer::get_singleton()->space_get_direct_state(get_world_2d()->get_space());
|
||||
ERR_FAIL_COND_V(!dss,Vector2());
|
||||
const int max_shapes=32;
|
||||
Vector2 sr[max_shapes*2];
|
||||
int res_shapes;
|
||||
|
||||
Set<RID> exclude;
|
||||
exclude.insert(get_rid());
|
||||
|
||||
|
||||
//recover first
|
||||
int recover_attempts=4;
|
||||
|
||||
bool collided=false;
|
||||
uint32_t mask=0;
|
||||
if (collide_static)
|
||||
mask|=Physics2DDirectSpaceState::TYPE_MASK_STATIC_BODY;
|
||||
if (collide_kinematic)
|
||||
mask|=Physics2DDirectSpaceState::TYPE_MASK_KINEMATIC_BODY;
|
||||
if (collide_rigid)
|
||||
mask|=Physics2DDirectSpaceState::TYPE_MASK_RIGID_BODY;
|
||||
if (collide_character)
|
||||
mask|=Physics2DDirectSpaceState::TYPE_MASK_CHARACTER_BODY;
|
||||
|
||||
// print_line("motion: "+p_motion+" margin: "+rtos(margin));
|
||||
|
||||
//print_line("margin: "+rtos(margin));
|
||||
do {
|
||||
|
||||
//motion recover
|
||||
for(int i=0;i<get_shape_count();i++) {
|
||||
|
||||
if (is_shape_set_as_trigger(i))
|
||||
continue;
|
||||
if (dss->collide_shape(get_shape(i)->get_rid(), get_global_transform() * get_shape_transform(i),Vector2(),margin,sr,max_shapes,res_shapes,exclude,get_layer_mask(),mask))
|
||||
collided=true;
|
||||
|
||||
}
|
||||
|
||||
if (!collided)
|
||||
break;
|
||||
|
||||
Vector2 recover_motion;
|
||||
|
||||
for(int i=0;i<res_shapes;i++) {
|
||||
|
||||
Vector2 a = sr[i*2+0];
|
||||
Vector2 b = sr[i*2+1];
|
||||
|
||||
float d = a.distance_to(b);
|
||||
|
||||
//if (d<margin)
|
||||
/// continue;
|
||||
recover_motion+=(b-a)*0.4;
|
||||
}
|
||||
|
||||
if (recover_motion==Vector2()) {
|
||||
collided=false;
|
||||
break;
|
||||
}
|
||||
|
||||
Matrix32 gt = get_global_transform();
|
||||
gt.elements[2]+=recover_motion;
|
||||
set_global_transform(gt);
|
||||
|
||||
recover_attempts--;
|
||||
|
||||
} while (recover_attempts);
|
||||
|
||||
|
||||
//move second
|
||||
float safe = 1.0;
|
||||
float unsafe = 1.0;
|
||||
int best_shape=-1;
|
||||
|
||||
for(int i=0;i<get_shape_count();i++) {
|
||||
|
||||
if (is_shape_set_as_trigger(i))
|
||||
continue;
|
||||
|
||||
float lsafe,lunsafe;
|
||||
bool valid = dss->cast_motion(get_shape(i)->get_rid(), get_global_transform() * get_shape_transform(i), p_motion, 0,lsafe,lunsafe,exclude,get_layer_mask(),mask);
|
||||
//print_line("shape: "+itos(i)+" travel:"+rtos(ltravel));
|
||||
if (!valid) {
|
||||
|
||||
safe=0;
|
||||
unsafe=0;
|
||||
best_shape=i; //sadly it's the best
|
||||
break;
|
||||
}
|
||||
if (lsafe==1.0) {
|
||||
continue;
|
||||
}
|
||||
if (lsafe < safe) {
|
||||
|
||||
safe=lsafe;
|
||||
unsafe=lunsafe;
|
||||
best_shape=i;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
//print_line("best shape: "+itos(best_shape)+" motion "+p_motion);
|
||||
|
||||
if (safe>=1) {
|
||||
//not collided
|
||||
colliding=false;
|
||||
} else {
|
||||
|
||||
//it collided, let's get the rest info in unsafe advance
|
||||
Matrix32 ugt = get_global_transform();
|
||||
ugt.elements[2]+=p_motion*unsafe;
|
||||
Physics2DDirectSpaceState::ShapeRestInfo rest_info;
|
||||
bool c2 = dss->rest_info(get_shape(best_shape)->get_rid(), ugt*get_shape_transform(best_shape), Vector2(), margin,&rest_info,exclude,get_layer_mask(),mask);
|
||||
if (!c2) {
|
||||
//should not happen, but floating point precision is so weird..
|
||||
|
||||
colliding=false;
|
||||
} else {
|
||||
|
||||
|
||||
//print_line("Travel: "+rtos(travel));
|
||||
colliding=true;
|
||||
collision=rest_info.point;
|
||||
normal=rest_info.normal;
|
||||
collider=rest_info.collider_id;
|
||||
collider_vel=rest_info.linear_velocity;
|
||||
collider_shape=rest_info.shape;
|
||||
collider_metadata=rest_info.metadata;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
Vector2 motion=p_motion*safe;
|
||||
Matrix32 gt = get_global_transform();
|
||||
gt.elements[2]+=motion;
|
||||
set_global_transform(gt);
|
||||
|
||||
return p_motion-motion;
|
||||
|
||||
#endif
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -165,6 +165,8 @@ public:
|
|||
|
||||
int get_collision_pairs() const { return collision_pairs; }
|
||||
|
||||
bool test_body_motion(Body2DSW *p_body, const Vector2&p_motion, float p_margin, Physics2DServer::MotionResult *r_result);
|
||||
|
||||
Physics2DDirectSpaceStateSW *get_direct_state();
|
||||
|
||||
Space2DSW();
|
||||
|
|
|
|||
|
|
@ -421,13 +421,86 @@ void Physics2DShapeQueryResult::_bind_methods() {
|
|||
|
||||
}
|
||||
|
||||
///////////////////////////////
|
||||
|
||||
/*bool Physics2DTestMotionResult::is_colliding() const {
|
||||
|
||||
return colliding;
|
||||
}*/
|
||||
Vector2 Physics2DTestMotionResult::get_motion() const{
|
||||
|
||||
return result.motion;
|
||||
}
|
||||
Vector2 Physics2DTestMotionResult::get_motion_remainder() const{
|
||||
|
||||
return result.remainder;
|
||||
}
|
||||
|
||||
Vector2 Physics2DTestMotionResult::get_collision_point() const{
|
||||
|
||||
return result.collision_point;
|
||||
}
|
||||
Vector2 Physics2DTestMotionResult::get_collision_normal() const{
|
||||
|
||||
return result.collision_normal;
|
||||
}
|
||||
Vector2 Physics2DTestMotionResult::get_collider_velocity() const{
|
||||
|
||||
return result.collider_velocity;
|
||||
}
|
||||
ObjectID Physics2DTestMotionResult::get_collider_id() const{
|
||||
|
||||
return result.collider_id;
|
||||
}
|
||||
RID Physics2DTestMotionResult::get_collider_rid() const{
|
||||
|
||||
return result.collider;
|
||||
}
|
||||
|
||||
Object* Physics2DTestMotionResult::get_collider() const {
|
||||
return ObjectDB::get_instance(result.collider_id);
|
||||
}
|
||||
|
||||
int Physics2DTestMotionResult::get_collider_shape() const{
|
||||
|
||||
return result.collider_shape;
|
||||
}
|
||||
|
||||
void Physics2DTestMotionResult::_bind_methods() {
|
||||
|
||||
//ObjectTypeDB::bind_method(_MD("is_colliding"),&Physics2DTestMotionResult::is_colliding);
|
||||
ObjectTypeDB::bind_method(_MD("get_motion"),&Physics2DTestMotionResult::get_motion);
|
||||
ObjectTypeDB::bind_method(_MD("get_motion_remainder"),&Physics2DTestMotionResult::get_motion_remainder);
|
||||
ObjectTypeDB::bind_method(_MD("get_collision_point"),&Physics2DTestMotionResult::get_collision_point);
|
||||
ObjectTypeDB::bind_method(_MD("get_collision_normal"),&Physics2DTestMotionResult::get_collision_normal);
|
||||
ObjectTypeDB::bind_method(_MD("get_collider_velocity"),&Physics2DTestMotionResult::get_collider_velocity);
|
||||
ObjectTypeDB::bind_method(_MD("get_collider_id"),&Physics2DTestMotionResult::get_collider_id);
|
||||
ObjectTypeDB::bind_method(_MD("get_collider_rid"),&Physics2DTestMotionResult::get_collider_rid);
|
||||
ObjectTypeDB::bind_method(_MD("get_collider"),&Physics2DTestMotionResult::get_collider);
|
||||
ObjectTypeDB::bind_method(_MD("get_collider_shape"),&Physics2DTestMotionResult::get_collider_shape);
|
||||
|
||||
}
|
||||
|
||||
Physics2DTestMotionResult::Physics2DTestMotionResult(){
|
||||
|
||||
colliding=false;
|
||||
result.collider_id=0;
|
||||
result.collider_shape=0;
|
||||
}
|
||||
|
||||
|
||||
///////////////////////////////////////
|
||||
|
||||
|
||||
|
||||
bool Physics2DServer::_body_test_motion(RID p_body,const Vector2& p_motion,float p_margin,const Ref<Physics2DTestMotionResult>& p_result) {
|
||||
|
||||
MotionResult *r=NULL;
|
||||
if (p_result.is_valid())
|
||||
r=p_result->get_result_ptr();
|
||||
return body_test_motion(p_body,p_motion,p_margin,r);
|
||||
}
|
||||
|
||||
void Physics2DServer::_bind_methods() {
|
||||
|
||||
|
||||
|
|
@ -543,6 +616,8 @@ void Physics2DServer::_bind_methods() {
|
|||
|
||||
ObjectTypeDB::bind_method(_MD("body_set_force_integration_callback","body","receiver","method"),&Physics2DServer::body_set_force_integration_callback);
|
||||
|
||||
ObjectTypeDB::bind_method(_MD("body_test_motion","body","motion","margin","result:Physics2DTestMotionResult"),&Physics2DServer::_body_test_motion,DEFVAL(0.08),DEFVAL(Variant()));
|
||||
|
||||
/* JOINT API */
|
||||
|
||||
ObjectTypeDB::bind_method(_MD("joint_set_param","joint","param","value"),&Physics2DServer::joint_set_param);
|
||||
|
|
|
|||
|
|
@ -230,6 +230,7 @@ public:
|
|||
Physics2DShapeQueryResult();
|
||||
};
|
||||
|
||||
class Physics2DTestMotionResult;
|
||||
|
||||
class Physics2DServer : public Object {
|
||||
|
||||
|
|
@ -237,6 +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<Physics2DTestMotionResult>& p_result=Ref<Physics2DTestMotionResult>());
|
||||
|
||||
protected:
|
||||
static void _bind_methods();
|
||||
|
||||
|
|
@ -468,6 +471,22 @@ public:
|
|||
|
||||
virtual void body_set_pickable(RID p_body,bool p_pickable)=0;
|
||||
|
||||
struct MotionResult {
|
||||
|
||||
Vector2 motion;
|
||||
Vector2 remainder;
|
||||
|
||||
Vector2 collision_point;
|
||||
Vector2 collision_normal;
|
||||
Vector2 collider_velocity;
|
||||
ObjectID collider_id;
|
||||
RID collider;
|
||||
int collider_shape;
|
||||
Variant collider_metadata;
|
||||
};
|
||||
|
||||
virtual bool body_test_motion(RID p_body,const Vector2& p_motion,float p_margin=0.001,MotionResult *r_result=NULL)=0;
|
||||
|
||||
/* JOINT API */
|
||||
|
||||
enum JointType {
|
||||
|
|
@ -532,6 +551,37 @@ public:
|
|||
~Physics2DServer();
|
||||
};
|
||||
|
||||
|
||||
class Physics2DTestMotionResult : public Reference {
|
||||
|
||||
OBJ_TYPE( Physics2DTestMotionResult, Reference );
|
||||
|
||||
Physics2DServer::MotionResult result;
|
||||
bool colliding;
|
||||
friend class Physics2DServer;
|
||||
|
||||
protected:
|
||||
static void _bind_methods();
|
||||
public:
|
||||
|
||||
Physics2DServer::MotionResult* get_result_ptr() const { return const_cast<Physics2DServer::MotionResult*>(&result); }
|
||||
|
||||
//bool is_colliding() const;
|
||||
Vector2 get_motion() const;
|
||||
Vector2 get_motion_remainder() const;
|
||||
|
||||
Vector2 get_collision_point() const;
|
||||
Vector2 get_collision_normal() const;
|
||||
Vector2 get_collider_velocity() const;
|
||||
ObjectID get_collider_id() const;
|
||||
RID get_collider_rid() const;
|
||||
Object* get_collider() const;
|
||||
int get_collider_shape() const;
|
||||
|
||||
Physics2DTestMotionResult();
|
||||
};
|
||||
|
||||
|
||||
VARIANT_ENUM_CAST( Physics2DServer::ShapeType );
|
||||
VARIANT_ENUM_CAST( Physics2DServer::SpaceParameter );
|
||||
VARIANT_ENUM_CAST( Physics2DServer::AreaParameter );
|
||||
|
|
|
|||
|
|
@ -55,6 +55,7 @@ void register_server_types() {
|
|||
ObjectTypeDB::register_virtual_type<Physics2DDirectBodyState>();
|
||||
ObjectTypeDB::register_virtual_type<Physics2DDirectSpaceState>();
|
||||
ObjectTypeDB::register_virtual_type<Physics2DShapeQueryResult>();
|
||||
ObjectTypeDB::register_virtual_type<Physics2DTestMotionResult>();
|
||||
ObjectTypeDB::register_type<Physics2DShapeQueryParameters>();
|
||||
|
||||
ObjectTypeDB::register_type<PhysicsShapeQueryParameters>();
|
||||
|
|
|
|||
Loading…
Reference in New Issue