diff options
Diffstat (limited to 'servers/physics_2d/space_2d_sw.cpp')
| -rw-r--r-- | servers/physics_2d/space_2d_sw.cpp | 518 |
1 files changed, 517 insertions, 1 deletions
diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp index 5aaf9a7613..767ad9038d 100644 --- a/servers/physics_2d/space_2d_sw.cpp +++ b/servers/physics_2d/space_2d_sw.cpp @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -555,8 +555,524 @@ Physics2DDirectSpaceStateSW::Physics2DDirectSpaceStateSW() { +int Space2DSW::_cull_aabb_for_body(Body2DSW *p_body,const Rect2& p_aabb) { + int amount = broadphase->cull_aabb(p_aabb,intersection_query_results,INTERSECTION_QUERY_MAX,intersection_query_subindex_results); + + for(int i=0;i<amount;i++) { + + bool keep=true; + + if (intersection_query_results[i]==p_body) + keep=false; + else if (intersection_query_results[i]->get_type()==CollisionObject2DSW::TYPE_AREA) + keep=false; + else if ((static_cast<Body2DSW*>(intersection_query_results[i])->test_collision_mask(p_body))==0) + keep=false; + else if (static_cast<Body2DSW*>(intersection_query_results[i])->has_exception(p_body->get_self()) || p_body->has_exception(intersection_query_results[i]->get_self())) + keep=false; + else if (static_cast<Body2DSW*>(intersection_query_results[i])->is_shape_set_as_trigger(intersection_query_subindex_results[i])) + keep=false; + + if (!keep) { + + if (i<amount-1) { + SWAP(intersection_query_results[i],intersection_query_results[amount-1]); + SWAP(intersection_query_subindex_results[i],intersection_query_subindex_results[amount-1]); + + } + + amount--; + i--; + + } + } + + return amount; +} + +bool Space2DSW::test_body_motion(Body2DSW *p_body,const Vector2&p_motion,float p_margin,Physics2DServer::MotionResult *r_result) { + + //give me back regular physics engine logic + //this is madness + //and most people using this function will think + //what it does is simpler than using physics + //this took about a week to get right.. + //but is it right? who knows at this point.. + + Rect2 body_aabb; + + for(int i=0;i<p_body->get_shape_count();i++) { + + if (i==0) + body_aabb=p_body->get_shape_aabb(i); + else + body_aabb=body_aabb.merge(p_body->get_shape_aabb(i)); + } + + body_aabb=body_aabb.grow(p_margin); + + + Matrix32 body_transform = p_body->get_transform(); + + { + //STEP 1, FREE BODY IF STUCK + + const int max_results = 32; + int recover_attempts=4; + Vector2 sr[max_results*2]; + + do { + + Physics2DServerSW::CollCbkData cbk; + cbk.max=max_results; + cbk.amount=0; + cbk.ptr=sr; + + + CollisionSolver2DSW::CallbackResult cbkres=NULL; + + Physics2DServerSW::CollCbkData *cbkptr=NULL; + cbkptr=&cbk; + cbkres=Physics2DServerSW::_shape_col_cbk; + + bool collided=false; + + int amount = _cull_aabb_for_body(p_body,body_aabb); + + for(int j=0;j<p_body->get_shape_count();j++) { + if (p_body->is_shape_set_as_trigger(j)) + continue; + + Matrix32 body_shape_xform = body_transform * p_body->get_shape_transform(j); + Shape2DSW *body_shape = p_body->get_shape(j); + for(int i=0;i<amount;i++) { + + const CollisionObject2DSW *col_obj=intersection_query_results[i]; + int shape_idx=intersection_query_subindex_results[i]; + + if (col_obj->get_type()==CollisionObject2DSW::TYPE_BODY) { + + const Body2DSW *body=static_cast<const Body2DSW*>(col_obj); + + Vector2 cdir = body->get_one_way_collision_direction(); + //if (cdir!=Vector2() && p_motion.dot(cdir)<0) + // continue; + + cbk.valid_dir=cdir; + cbk.valid_depth=body->get_one_way_collision_max_depth(); + } else { + cbk.valid_dir=Vector2(); + cbk.valid_depth=0; + } + + if (CollisionSolver2DSW::solve(body_shape,body_shape_xform,Vector2(),col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx),Vector2(),cbkres,cbkptr,NULL,p_margin)) { + collided=cbk.amount>0; + } + } + } + + + if (!collided) + break; + + Vector2 recover_motion; + + for(int i=0;i<cbk.amount;i++) { + + Vector2 a = sr[i*2+0]; + Vector2 b = sr[i*2+1]; + + // float d = a.distance_to(b); + + //if (d<margin) + /// continue; + recover_motion+=(b-a)*0.4; + } + + if (recover_motion==Vector2()) { + collided=false; + break; + } + + body_transform.elements[2]+=recover_motion; + body_aabb.pos+=recover_motion; + + recover_attempts--; + + } while (recover_attempts); + } + + + + float safe = 1.0; + float unsafe = 1.0; + int best_shape=-1; + + { + // STEP 2 ATTEMPT MOTION + + Rect2 motion_aabb=body_aabb; + motion_aabb.pos+=p_motion; + motion_aabb=motion_aabb.merge(body_aabb); + + int amount = _cull_aabb_for_body(p_body,motion_aabb); + + for(int j=0;j<p_body->get_shape_count();j++) { + + if (p_body->is_shape_set_as_trigger(j)) + continue; + + Matrix32 body_shape_xform = body_transform * p_body->get_shape_transform(j); + Shape2DSW *body_shape = p_body->get_shape(j); + + bool stuck=false; + + float best_safe=1; + float best_unsafe=1; + + for(int i=0;i<amount;i++) { + + const CollisionObject2DSW *col_obj=intersection_query_results[i]; + int shape_idx=intersection_query_subindex_results[i]; + + + Matrix32 col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); + //test initial overlap, does it collide if going all the way? + if (!CollisionSolver2DSW::solve(body_shape,body_shape_xform,p_motion,col_obj->get_shape(shape_idx),col_obj_xform,Vector2() ,NULL,NULL,NULL,0)) { + continue; + } + + + //test initial overlap + if (CollisionSolver2DSW::solve(body_shape,body_shape_xform,Vector2(),col_obj->get_shape(shape_idx),col_obj_xform,Vector2() ,NULL,NULL,NULL,0)) { + + if (col_obj->get_type()==CollisionObject2DSW::TYPE_BODY) { + //if one way collision direction ignore initial overlap + const Body2DSW *body=static_cast<const Body2DSW*>(col_obj); + if (body->get_one_way_collision_direction()!=Vector2()) { + continue; + } + } + + stuck=true; + break; + } + + + //just do kinematic solving + float low=0; + float hi=1; + Vector2 mnormal=p_motion.normalized(); + + for(int i=0;i<8;i++) { //steps should be customizable.. + + //Matrix32 xfa = p_xform; + float ofs = (low+hi)*0.5; + + Vector2 sep=mnormal; //important optimization for this to work fast enough + bool collided = CollisionSolver2DSW::solve(body_shape,body_shape_xform,p_motion*ofs,col_obj->get_shape(shape_idx),col_obj_xform,Vector2(),NULL,NULL,&sep,0); + + if (collided) { + + hi=ofs; + } else { + + low=ofs; + } + } + + if (col_obj->get_type()==CollisionObject2DSW::TYPE_BODY) { + + const Body2DSW *body=static_cast<const Body2DSW*>(col_obj); + if (body->get_one_way_collision_direction()!=Vector2()) { + + Vector2 cd[2]; + Physics2DServerSW::CollCbkData cbk; + cbk.max=1; + cbk.amount=0; + cbk.ptr=cd; + cbk.valid_dir=body->get_one_way_collision_direction(); + cbk.valid_depth=body->get_one_way_collision_max_depth(); + + Vector2 sep=mnormal; //important optimization for this to work fast enough + bool collided = CollisionSolver2DSW::solve(body_shape,body_shape_xform,p_motion*(hi+contact_max_allowed_penetration),col_obj->get_shape(shape_idx),col_obj_xform,Vector2(),Physics2DServerSW::_shape_col_cbk,&cbk,&sep,0); + if (!collided || cbk.amount==0) { + continue; + } + + } + } + + + if (low<best_safe) { + best_safe=low; + best_unsafe=hi; + } + } + + if (stuck) { + + safe=0; + unsafe=0; + best_shape=j; //sadly it's the best + break; + } + if (best_safe==1.0) { + continue; + } + if (best_safe < safe) { + + safe=best_safe; + unsafe=best_unsafe; + best_shape=j; + } + } + } + + bool collided=false; + if (safe>=1) { + //not collided + collided=false; + if (r_result) { + + r_result->motion=p_motion+(body_transform.elements[2]-p_body->get_transform().elements[2]); + r_result->remainder=Vector2(); + } + + } else { + + //it collided, let's get the rest info in unsafe advance + Matrix32 ugt = body_transform; + ugt.elements[2]+=p_motion*unsafe; + + _RestCallbackData2D rcd; + rcd.best_len=0; + rcd.best_object=NULL; + rcd.best_shape=0; + + Matrix32 body_shape_xform = ugt * p_body->get_shape_transform(best_shape); + Shape2DSW *body_shape = p_body->get_shape(best_shape); + + body_aabb.pos+=p_motion*unsafe; + + int amount = _cull_aabb_for_body(p_body,body_aabb); + + + for(int i=0;i<amount;i++) { + + + const CollisionObject2DSW *col_obj=intersection_query_results[i]; + int shape_idx=intersection_query_subindex_results[i]; + + if (col_obj->get_type()==CollisionObject2DSW::TYPE_BODY) { + + const Body2DSW *body=static_cast<const Body2DSW*>(col_obj); + rcd.valid_dir=body->get_one_way_collision_direction(); + rcd.valid_depth=body->get_one_way_collision_max_depth(); + } else { + rcd.valid_dir=Vector2(); + rcd.valid_depth=0; + } + + + rcd.object=col_obj; + rcd.shape=shape_idx; + bool sc = CollisionSolver2DSW::solve(body_shape,body_shape_xform,Vector2(),col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx),Vector2() ,_rest_cbk_result,&rcd,NULL,p_margin); + if (!sc) + continue; + + } + + if (rcd.best_len!=0) { + + if (r_result) { + r_result->collider=rcd.best_object->get_self(); + r_result->collider_id=rcd.best_object->get_instance_id(); + r_result->collider_shape=rcd.best_shape; + r_result->collision_normal=rcd.best_normal; + r_result->collision_point=rcd.best_contact; + r_result->collider_metadata=rcd.best_object->get_shape_metadata(rcd.best_shape); + + const Body2DSW *body = static_cast<const Body2DSW*>(rcd.best_object); + Vector2 rel_vec = r_result->collision_point-body->get_transform().get_origin(); + r_result->collider_velocity = Vector2(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity(); + + r_result->motion=safe*p_motion+(body_transform.elements[2]-p_body->get_transform().elements[2]); + r_result->remainder=p_motion - safe * p_motion; + } + + collided=true; + } else { + if (r_result) { + + r_result->motion=p_motion+(body_transform.elements[2]-p_body->get_transform().elements[2]); + r_result->remainder=Vector2(); + } + + collided=false; + + } + } + + return collided; + + +#if 0 + //give me back regular physics engine logic + //this is madness + //and most people using this function will think + //what it does is simpler than using physics + //this took about a week to get right.. + //but is it right? who knows at this point.. + + + colliding=false; + ERR_FAIL_COND_V(!is_inside_tree(),Vector2()); + Physics2DDirectSpaceState *dss = Physics2DServer::get_singleton()->space_get_direct_state(get_world_2d()->get_space()); + ERR_FAIL_COND_V(!dss,Vector2()); + const int max_shapes=32; + Vector2 sr[max_shapes*2]; + int res_shapes; + + Set<RID> exclude; + exclude.insert(get_rid()); + + + //recover first + int recover_attempts=4; + + bool collided=false; + uint32_t mask=0; + if (collide_static) + mask|=Physics2DDirectSpaceState::TYPE_MASK_STATIC_BODY; + if (collide_kinematic) + mask|=Physics2DDirectSpaceState::TYPE_MASK_KINEMATIC_BODY; + if (collide_rigid) + mask|=Physics2DDirectSpaceState::TYPE_MASK_RIGID_BODY; + if (collide_character) + mask|=Physics2DDirectSpaceState::TYPE_MASK_CHARACTER_BODY; + +// print_line("motion: "+p_motion+" margin: "+rtos(margin)); + + //print_line("margin: "+rtos(margin)); + do { + + //motion recover + for(int i=0;i<get_shape_count();i++) { + + if (is_shape_set_as_trigger(i)) + continue; + if (dss->collide_shape(get_shape(i)->get_rid(), get_global_transform() * get_shape_transform(i),Vector2(),margin,sr,max_shapes,res_shapes,exclude,get_layer_mask(),mask)) + collided=true; + + } + + if (!collided) + break; + + Vector2 recover_motion; + + for(int i=0;i<res_shapes;i++) { + + Vector2 a = sr[i*2+0]; + Vector2 b = sr[i*2+1]; + + float d = a.distance_to(b); + + //if (d<margin) + /// continue; + recover_motion+=(b-a)*0.4; + } + + if (recover_motion==Vector2()) { + collided=false; + break; + } + + Matrix32 gt = get_global_transform(); + gt.elements[2]+=recover_motion; + set_global_transform(gt); + + recover_attempts--; + + } while (recover_attempts); + + + //move second + float safe = 1.0; + float unsafe = 1.0; + int best_shape=-1; + + for(int i=0;i<get_shape_count();i++) { + + if (is_shape_set_as_trigger(i)) + continue; + + float lsafe,lunsafe; + bool valid = dss->cast_motion(get_shape(i)->get_rid(), get_global_transform() * get_shape_transform(i), p_motion, 0,lsafe,lunsafe,exclude,get_layer_mask(),mask); + //print_line("shape: "+itos(i)+" travel:"+rtos(ltravel)); + if (!valid) { + + safe=0; + unsafe=0; + best_shape=i; //sadly it's the best + break; + } + if (lsafe==1.0) { + continue; + } + if (lsafe < safe) { + + safe=lsafe; + unsafe=lunsafe; + best_shape=i; + } + } + + + //print_line("best shape: "+itos(best_shape)+" motion "+p_motion); + + if (safe>=1) { + //not collided + colliding=false; + } else { + + //it collided, let's get the rest info in unsafe advance + Matrix32 ugt = get_global_transform(); + ugt.elements[2]+=p_motion*unsafe; + Physics2DDirectSpaceState::ShapeRestInfo rest_info; + bool c2 = dss->rest_info(get_shape(best_shape)->get_rid(), ugt*get_shape_transform(best_shape), Vector2(), margin,&rest_info,exclude,get_layer_mask(),mask); + if (!c2) { + //should not happen, but floating point precision is so weird.. + + colliding=false; + } else { + + + //print_line("Travel: "+rtos(travel)); + colliding=true; + collision=rest_info.point; + normal=rest_info.normal; + collider=rest_info.collider_id; + collider_vel=rest_info.linear_velocity; + collider_shape=rest_info.shape; + collider_metadata=rest_info.metadata; + } + + } + + Vector2 motion=p_motion*safe; + Matrix32 gt = get_global_transform(); + gt.elements[2]+=motion; + set_global_transform(gt); + + return p_motion-motion; + +#endif + return false; +} |