diff options
Diffstat (limited to 'servers/physics/collision_solver_sw.cpp')
| -rw-r--r-- | servers/physics/collision_solver_sw.cpp | 294 |
1 files changed, 133 insertions, 161 deletions
diff --git a/servers/physics/collision_solver_sw.cpp b/servers/physics/collision_solver_sw.cpp index f0ddde3a76..0f6e964359 100644 --- a/servers/physics/collision_solver_sw.cpp +++ b/servers/physics/collision_solver_sw.cpp @@ -29,18 +29,16 @@ #include "collision_solver_sw.h" #include "collision_solver_sat.h" -#include "gjk_epa.h" #include "collision_solver_sat.h" - +#include "gjk_epa.h" #define collision_solver sat_calculate_penetration //#define collision_solver gjk_epa_calculate_penetration +bool CollisionSolverSW::solve_static_plane(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_static_plane(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) { - - const PlaneShapeSW *plane = static_cast<const PlaneShapeSW*>(p_shape_A); - if (p_shape_B->get_type()==PhysicsServer::SHAPE_PLANE) + const PlaneShapeSW *plane = static_cast<const PlaneShapeSW *>(p_shape_A); + if (p_shape_B->get_type() == PhysicsServer::SHAPE_PLANE) return false; Plane p = p_transform_A.xform(plane->get_plane()); @@ -48,57 +46,54 @@ bool CollisionSolverSW::solve_static_plane(const ShapeSW *p_shape_A,const Transf Vector3 supports[max_supports]; int support_count; - p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(),max_supports,supports,support_count); + p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(), max_supports, supports, support_count); - bool found=false; + bool found = false; - for(int i=0;i<support_count;i++) { + for (int i = 0; i < support_count; i++) { - supports[i] = p_transform_B.xform( supports[i] ); - if (p.distance_to(supports[i])>=0) + supports[i] = p_transform_B.xform(supports[i]); + if (p.distance_to(supports[i]) >= 0) continue; - found=true; + found = true; Vector3 support_A = p.project(supports[i]); if (p_result_callback) { if (p_swap_result) - p_result_callback(supports[i],support_A,p_userdata); + p_result_callback(supports[i], support_A, p_userdata); else - p_result_callback(support_A,supports[i],p_userdata); + p_result_callback(support_A, supports[i], p_userdata); } - } - return found; } -bool CollisionSolverSW::solve_ray(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_ray(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) { - - const RayShapeSW *ray = static_cast<const RayShapeSW*>(p_shape_A); + const RayShapeSW *ray = static_cast<const RayShapeSW *>(p_shape_A); Vector3 from = p_transform_A.origin; - Vector3 to = from+p_transform_A.basis.get_axis(2)*ray->get_length(); - Vector3 support_A=to; + Vector3 to = from + p_transform_A.basis.get_axis(2) * ray->get_length(); + Vector3 support_A = to; Transform ai = p_transform_B.affine_inverse(); from = ai.xform(from); to = ai.xform(to); - Vector3 p,n; - if (!p_shape_B->intersect_segment(from,to,p,n)) + Vector3 p, n; + if (!p_shape_B->intersect_segment(from, to, p, n)) return false; - Vector3 support_B=p_transform_B.xform(p); + Vector3 support_B = p_transform_B.xform(p); if (p_result_callback) { if (p_swap_result) - p_result_callback(support_B,support_A,p_userdata); + p_result_callback(support_B, support_A, p_userdata); else - p_result_callback(support_A,support_B,p_userdata); + p_result_callback(support_A, support_B, p_userdata); } return true; } @@ -117,169 +112,153 @@ struct _ConcaveCollisionInfo { bool tested; real_t margin_A; real_t margin_B; - Vector3 close_A,close_B; - + Vector3 close_A, close_B; }; void CollisionSolverSW::concave_callback(void *p_userdata, ShapeSW *p_convex) { - - _ConcaveCollisionInfo &cinfo = *(_ConcaveCollisionInfo*)(p_userdata); + _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,NULL,cinfo.margin_A,cinfo.margin_B); + 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; - cinfo.collided=true; + cinfo.collided = true; cinfo.collisions++; - } -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,real_t p_margin_A,real_t p_margin_B) { +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, real_t p_margin_A, real_t p_margin_B) { - - const ConcaveShapeSW *concave_B=static_cast<const ConcaveShapeSW*>(p_shape_B); + 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=p_result_callback; - cinfo.userdata=p_userdata; - 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; + cinfo.transform_A = &p_transform_A; + cinfo.shape_A = p_shape_A; + cinfo.transform_B = &p_transform_B; + cinfo.result_callback = p_result_callback; + cinfo.userdata = p_userdata; + 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; Transform rel_transform = p_transform_A; - rel_transform.origin-=p_transform_B.origin; + rel_transform.origin -= p_transform_B.origin; //quickly compute a local AABB Rect3 local_aabb; - for(int i=0;i<3;i++) { - - Vector3 axis( p_transform_B.basis.get_axis(i) ); - real_t axis_scale = 1.0/axis.length(); - axis*=axis_scale; + for (int i = 0; i < 3; i++) { - real_t 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; + Vector3 axis(p_transform_B.basis.get_axis(i)); + real_t axis_scale = 1.0 / axis.length(); + axis *= axis_scale; + real_t 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; + local_aabb.pos[i] = smin; + local_aabb.size[i] = smax - smin; } - concave_B->cull(local_aabb,concave_callback,&cinfo); + concave_B->cull(local_aabb, concave_callback, &cinfo); //print_line("COL AABB TESTS: "+itos(cinfo.aabb_tests)); 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, real_t p_margin_A, real_t p_margin_B) { -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,real_t p_margin_A,real_t p_margin_B) { - - - PhysicsServer::ShapeType type_A=p_shape_A->get_type(); - PhysicsServer::ShapeType type_B=p_shape_B->get_type(); - bool concave_A=p_shape_A->is_concave(); - bool concave_B=p_shape_B->is_concave(); + PhysicsServer::ShapeType type_A = p_shape_A->get_type(); + PhysicsServer::ShapeType type_B = p_shape_B->get_type(); + bool concave_A = p_shape_A->is_concave(); + bool concave_B = p_shape_B->is_concave(); bool swap = false; - if (type_A>type_B) { - SWAP(type_A,type_B); - SWAP(concave_A,concave_B); - swap=true; + if (type_A > type_B) { + SWAP(type_A, type_B); + SWAP(concave_A, concave_B); + swap = true; } - if (type_A==PhysicsServer::SHAPE_PLANE) { + if (type_A == PhysicsServer::SHAPE_PLANE) { - if (type_B==PhysicsServer::SHAPE_PLANE) + if (type_B == PhysicsServer::SHAPE_PLANE) return false; - if (type_B==PhysicsServer::SHAPE_RAY) { + if (type_B == PhysicsServer::SHAPE_RAY) { return false; } if (swap) { - return solve_static_plane(p_shape_B,p_transform_B,p_shape_A,p_transform_A,p_result_callback,p_userdata,true); + return solve_static_plane(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true); } else { - return solve_static_plane(p_shape_A,p_transform_A,p_shape_B,p_transform_B,p_result_callback,p_userdata,false); + return solve_static_plane(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false); } - } else if (type_A==PhysicsServer::SHAPE_RAY) { + } else if (type_A == PhysicsServer::SHAPE_RAY) { - if (type_B==PhysicsServer::SHAPE_RAY) + if (type_B == PhysicsServer::SHAPE_RAY) return false; if (swap) { - return solve_ray(p_shape_B,p_transform_B,p_shape_A,p_transform_A,p_result_callback,p_userdata,true); + return solve_ray(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true); } else { - return solve_ray(p_shape_A,p_transform_A,p_shape_B,p_transform_B,p_result_callback,p_userdata,false); + return solve_ray(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false); } } else if (concave_B) { - if (concave_A) 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,p_margin_A,p_margin_B); + 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,p_margin_A,p_margin_B); - - + 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 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); + _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); + 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.close_A = close_A; + cinfo.close_B = close_B; + cinfo.tested = true; } cinfo.collisions++; - } +bool CollisionSolverSW::solve_distance_plane(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) { - -bool CollisionSolverSW::solve_distance_plane(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 PlaneShapeSW *plane = static_cast<const PlaneShapeSW*>(p_shape_A); - if (p_shape_B->get_type()==PhysicsServer::SHAPE_PLANE) + const PlaneShapeSW *plane = static_cast<const PlaneShapeSW *>(p_shape_A); + if (p_shape_B->get_type() == PhysicsServer::SHAPE_PLANE) return false; Plane p = p_transform_A.xform(plane->get_plane()); @@ -287,43 +266,41 @@ bool CollisionSolverSW::solve_distance_plane(const ShapeSW *p_shape_A,const Tran Vector3 supports[max_supports]; int support_count; - p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(),max_supports,supports,support_count); + p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(), max_supports, supports, support_count); - bool collided=false; + bool collided = false; Vector3 closest; real_t closest_d; + for (int i = 0; i < support_count; i++) { - for(int i=0;i<support_count;i++) { - - supports[i] = p_transform_B.xform( supports[i] ); + supports[i] = p_transform_B.xform(supports[i]); real_t d = p.distance_to(supports[i]); - if (i==0 || d<closest_d) { - closest=supports[i]; - closest_d=d; - if (d<=0) - collided=true; + if (i == 0 || d < closest_d) { + closest = supports[i]; + closest_d = d; + if (d <= 0) + collided = true; } - } - r_point_A=p.project(closest); - r_point_B=closest; + r_point_A = p.project(closest); + r_point_B = closest; return collided; } -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 Rect3& p_concave_hint,Vector3 *r_sep_axis) { +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 Rect3 &p_concave_hint, Vector3 *r_sep_axis) { if (p_shape_A->is_concave()) return false; - if (p_shape_B->get_type()==PhysicsServer::SHAPE_PLANE) { + if (p_shape_B->get_type() == PhysicsServer::SHAPE_PLANE) { - Vector3 a,b; - bool col = solve_distance_plane(p_shape_B,p_transform_B,p_shape_A,p_transform_A,a,b); - r_point_A=b; - r_point_B=a; + Vector3 a, b; + bool col = solve_distance_plane(p_shape_B, p_transform_B, p_shape_A, p_transform_A, a, b); + r_point_A = b; + r_point_B = a; return !col; } else if (p_shape_B->is_concave()) { @@ -331,62 +308,59 @@ bool CollisionSolverSW::solve_distance(const ShapeSW *p_shape_A,const Transform& if (p_shape_A->is_concave()) return false; - - const ConcaveShapeSW *concave_B=static_cast<const ConcaveShapeSW*>(p_shape_B); + 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; + 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; + rel_transform.origin -= p_transform_B.origin; //quickly compute a local AABB - bool use_cc_hint=p_concave_hint!=Rect3(); + bool use_cc_hint = p_concave_hint != Rect3(); Rect3 cc_hint_aabb; if (use_cc_hint) { - cc_hint_aabb=p_concave_hint; - cc_hint_aabb.pos-=p_transform_B.origin; + cc_hint_aabb = p_concave_hint; + cc_hint_aabb.pos -= p_transform_B.origin; } Rect3 local_aabb; - for(int i=0;i<3;i++) { + for (int i = 0; i < 3; i++) { - Vector3 axis( p_transform_B.basis.get_axis(i) ); - real_t axis_scale = ((real_t)1.0)/axis.length(); - axis*=axis_scale; + Vector3 axis(p_transform_B.basis.get_axis(i)); + real_t axis_scale = ((real_t)1.0) / axis.length(); + axis *= axis_scale; - real_t smin,smax; + real_t 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); - } + 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; + smin *= axis_scale; + smax *= axis_scale; - local_aabb.pos[i]=smin; - local_aabb.size[i]=smax-smin; + local_aabb.pos[i] = smin; + local_aabb.size[i] = smax - smin; } - - concave_B->cull(local_aabb,concave_distance_callback,&cinfo); + 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; - + r_point_A = cinfo.close_A; + r_point_B = cinfo.close_B; } //print_line("DIST AABB TESTS: "+itos(cinfo.aabb_tests)); @@ -394,10 +368,8 @@ bool CollisionSolverSW::solve_distance(const ShapeSW *p_shape_A,const Transform& return !cinfo.collided; } else { - 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 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; } - |