diff options
Diffstat (limited to 'servers/physics/collision_solver_sw.cpp')
| -rw-r--r-- | servers/physics/collision_solver_sw.cpp | 131 |
1 files changed, 124 insertions, 7 deletions
diff --git a/servers/physics/collision_solver_sw.cpp b/servers/physics/collision_solver_sw.cpp index da28a4934f..673f2d4385 100644 --- a/servers/physics/collision_solver_sw.cpp +++ b/servers/physics/collision_solver_sw.cpp @@ -114,6 +114,10 @@ struct _ConcaveCollisionInfo { bool collided; int aabb_tests; int collisions; + bool tested; + float margin_A; + float margin_B; + Vector3 close_A,close_B; }; @@ -123,7 +127,7 @@ void CollisionSolverSW::concave_callback(void *p_userdata, ShapeSW *p_convex) { _ConcaveCollisionInfo &cinfo = *(_ConcaveCollisionInfo*)(p_userdata); cinfo.aabb_tests++; - bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, p_convex,*cinfo.transform_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result ); + bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, p_convex,*cinfo.transform_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result,NULL,cinfo.margin_A,cinfo.margin_B); if (!collided) return; @@ -132,7 +136,7 @@ void CollisionSolverSW::concave_callback(void *p_userdata, ShapeSW *p_convex) { } -bool CollisionSolverSW::solve_concave(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result) { +bool CollisionSolverSW::solve_concave(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,float p_margin_A,float p_margin_B) { const ConcaveShapeSW *concave_B=static_cast<const ConcaveShapeSW*>(p_shape_B); @@ -146,6 +150,8 @@ bool CollisionSolverSW::solve_concave(const ShapeSW *p_shape_A,const Transform& cinfo.swap_result=p_swap_result; cinfo.collided=false; cinfo.collisions=0; + cinfo.margin_A=p_margin_A; + cinfo.margin_B=p_margin_B; cinfo.aabb_tests=0; @@ -163,21 +169,23 @@ bool CollisionSolverSW::solve_concave(const ShapeSW *p_shape_A,const Transform& float smin,smax; p_shape_A->project_range(axis,rel_transform,smin,smax); + smin-=p_margin_A; + smax+=p_margin_A; smin*=axis_scale; smax*=axis_scale; + local_aabb.pos[i]=smin; local_aabb.size[i]=smax-smin; } concave_B->cull(local_aabb,concave_callback,&cinfo); - return cinfo.collided; } -bool CollisionSolverSW::solve_static(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,Vector3 *r_sep_axis) { +bool CollisionSolverSW::solve_static(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,Vector3 *r_sep_axis,float p_margin_A,float p_margin_B) { PhysicsServer::ShapeType type_A=p_shape_A->get_type(); @@ -225,17 +233,126 @@ bool CollisionSolverSW::solve_static(const ShapeSW *p_shape_A,const Transform& p return false; if (!swap) - return solve_concave(p_shape_A,p_transform_A,p_shape_B,p_transform_B,p_result_callback,p_userdata,false); + return solve_concave(p_shape_A,p_transform_A,p_shape_B,p_transform_B,p_result_callback,p_userdata,false,p_margin_A,p_margin_B); else - return solve_concave(p_shape_B,p_transform_B,p_shape_A,p_transform_A,p_result_callback,p_userdata,true); + return solve_concave(p_shape_B,p_transform_B,p_shape_A,p_transform_A,p_result_callback,p_userdata,true,p_margin_A,p_margin_B); + + + + } else { + + return collision_solver(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback,p_userdata,false,r_sep_axis,p_margin_A,p_margin_B); + } + + + return false; +} + + +void CollisionSolverSW::concave_distance_callback(void *p_userdata, ShapeSW *p_convex) { + + + _ConcaveCollisionInfo &cinfo = *(_ConcaveCollisionInfo*)(p_userdata); + cinfo.aabb_tests++; + if (cinfo.collided) + return; + + Vector3 close_A,close_B; + cinfo.collided = !gjk_epa_calculate_distance(cinfo.shape_A,*cinfo.transform_A,p_convex,*cinfo.transform_B,close_A,close_B); + + if (cinfo.collided) + return; + if (!cinfo.tested || close_A.distance_squared_to(close_B) < cinfo.close_A.distance_squared_to(cinfo.close_B)) { + + cinfo.close_A=close_A; + cinfo.close_B=close_B; + cinfo.tested=true; + } + + cinfo.collisions++; + +} + + +bool CollisionSolverSW::solve_distance(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,Vector3& r_point_A,Vector3& r_point_B,const AABB& p_concave_hint,Vector3 *r_sep_axis) { + + if (p_shape_A->is_concave()) + return false; + + if (p_shape_B->get_type()==PhysicsServer::SHAPE_PLANE) { + + return false; //unsupported + } else if (p_shape_B->is_concave()) { + if (p_shape_A->is_concave()) + return false; + + + const ConcaveShapeSW *concave_B=static_cast<const ConcaveShapeSW*>(p_shape_B); + + _ConcaveCollisionInfo cinfo; + cinfo.transform_A=&p_transform_A; + cinfo.shape_A=p_shape_A; + cinfo.transform_B=&p_transform_B; + cinfo.result_callback=NULL; + cinfo.userdata=NULL; + cinfo.swap_result=false; + cinfo.collided=false; + cinfo.collisions=0; + cinfo.aabb_tests=0; + cinfo.tested=false; + Transform rel_transform = p_transform_A; + rel_transform.origin-=p_transform_B.origin; + + //quickly compute a local AABB + + bool use_cc_hint=p_concave_hint!=AABB(); + AABB cc_hint_aabb; + if (use_cc_hint) { + cc_hint_aabb=p_concave_hint; + cc_hint_aabb.pos-=p_transform_B.origin; + } + + AABB local_aabb; + for(int i=0;i<3;i++) { + + Vector3 axis( p_transform_B.basis.get_axis(i) ); + float axis_scale = 1.0/axis.length(); + axis*=axis_scale; + + float smin,smax; + + if (use_cc_hint) { + cc_hint_aabb.project_range_in_plane(Plane(axis,0),smin,smax); + } else { + p_shape_A->project_range(axis,rel_transform,smin,smax); + } + + smin*=axis_scale; + smax*=axis_scale; + + local_aabb.pos[i]=smin; + local_aabb.size[i]=smax-smin; + } + + + concave_B->cull(local_aabb,concave_distance_callback,&cinfo); + if (!cinfo.collided) { +// print_line(itos(cinfo.tested)); + r_point_A=cinfo.close_A; + r_point_B=cinfo.close_B; + + } + + return !cinfo.collided; } else { - return collision_solver(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback,p_userdata,false,r_sep_axis); + return gjk_epa_calculate_distance(p_shape_A,p_transform_A,p_shape_B,p_transform_B,r_point_A,r_point_B); //should pass sepaxis.. } return false; } + |