diff options
Diffstat (limited to 'servers/physics_2d/collision_solver_2d_sat.cpp')
| -rw-r--r-- | servers/physics_2d/collision_solver_2d_sat.cpp | 1167 |
1 files changed, 540 insertions, 627 deletions
diff --git a/servers/physics_2d/collision_solver_2d_sat.cpp b/servers/physics_2d/collision_solver_2d_sat.cpp index 4c116cee3..f72a7ef0e 100644 --- a/servers/physics_2d/collision_solver_2d_sat.cpp +++ b/servers/physics_2d/collision_solver_2d_sat.cpp @@ -38,61 +38,57 @@ struct _CollectorCallback2D { Vector2 normal; Vector2 *sep_axis; - _FORCE_INLINE_ void call(const Vector2& p_point_A, const Vector2& p_point_B) { + _FORCE_INLINE_ void call(const Vector2 &p_point_A, const Vector2 &p_point_B) { /* if (normal.dot(p_point_A) >= normal.dot(p_point_B)) return; */ if (swap) - callback(p_point_B,p_point_A,userdata); + callback(p_point_B, p_point_A, userdata); else - callback(p_point_A,p_point_B,userdata); + callback(p_point_A, p_point_B, userdata); } - }; -typedef void (*GenerateContactsFunc)(const Vector2 *,int, const Vector2 *,int ,_CollectorCallback2D *); - +typedef void (*GenerateContactsFunc)(const Vector2 *, int, const Vector2 *, int, _CollectorCallback2D *); -_FORCE_INLINE_ static void _generate_contacts_point_point(const Vector2 * p_points_A,int p_point_count_A, const Vector2 * p_points_B,int p_point_count_B,_CollectorCallback2D *p_collector) { +_FORCE_INLINE_ static void _generate_contacts_point_point(const Vector2 *p_points_A, int p_point_count_A, const Vector2 *p_points_B, int p_point_count_B, _CollectorCallback2D *p_collector) { #ifdef DEBUG_ENABLED - ERR_FAIL_COND( p_point_count_A != 1 ); - ERR_FAIL_COND( p_point_count_B != 1 ); + ERR_FAIL_COND(p_point_count_A != 1); + ERR_FAIL_COND(p_point_count_B != 1); #endif - p_collector->call(*p_points_A,*p_points_B); + p_collector->call(*p_points_A, *p_points_B); } -_FORCE_INLINE_ static void _generate_contacts_point_edge(const Vector2 * p_points_A,int p_point_count_A, const Vector2 * p_points_B,int p_point_count_B,_CollectorCallback2D *p_collector) { +_FORCE_INLINE_ static void _generate_contacts_point_edge(const Vector2 *p_points_A, int p_point_count_A, const Vector2 *p_points_B, int p_point_count_B, _CollectorCallback2D *p_collector) { #ifdef DEBUG_ENABLED - ERR_FAIL_COND( p_point_count_A != 1 ); - ERR_FAIL_COND( p_point_count_B != 2 ); + ERR_FAIL_COND(p_point_count_A != 1); + ERR_FAIL_COND(p_point_count_B != 2); #endif - Vector2 closest_B = Geometry::get_closest_point_to_segment_uncapped_2d(*p_points_A, p_points_B ); - p_collector->call(*p_points_A,closest_B); - + Vector2 closest_B = Geometry::get_closest_point_to_segment_uncapped_2d(*p_points_A, p_points_B); + p_collector->call(*p_points_A, closest_B); } - struct _generate_contacts_Pair { bool a; int idx; real_t d; - _FORCE_INLINE_ bool operator <(const _generate_contacts_Pair& l) const { return d< l.d; } + _FORCE_INLINE_ bool operator<(const _generate_contacts_Pair &l) const { return d < l.d; } }; -_FORCE_INLINE_ static void _generate_contacts_edge_edge(const Vector2 * p_points_A,int p_point_count_A, const Vector2 * p_points_B,int p_point_count_B,_CollectorCallback2D *p_collector) { +_FORCE_INLINE_ static void _generate_contacts_edge_edge(const Vector2 *p_points_A, int p_point_count_A, const Vector2 *p_points_B, int p_point_count_B, _CollectorCallback2D *p_collector) { #ifdef DEBUG_ENABLED - ERR_FAIL_COND( p_point_count_A != 2 ); - ERR_FAIL_COND( p_point_count_B != 2 ); // circle is actually a 4x3 matrix + ERR_FAIL_COND(p_point_count_A != 2); + ERR_FAIL_COND(p_point_count_B != 2); // circle is actually a 4x3 matrix #endif -# if 0 +#if 0 Vector2 rel_A=p_points_A[1]-p_points_A[0]; Vector2 rel_B=p_points_B[1]-p_points_B[0]; @@ -210,9 +206,6 @@ _FORCE_INLINE_ static void _generate_contacts_edge_edge(const Vector2 * p_points #if 1 - - - Vector2 n = p_collector->normal; Vector2 t = n.tangent(); real_t dA = n.dot(p_points_A[0]); @@ -220,90 +213,87 @@ _FORCE_INLINE_ static void _generate_contacts_edge_edge(const Vector2 * p_points _generate_contacts_Pair dvec[4]; - dvec[0].d=t.dot(p_points_A[0]); - dvec[0].a=true; - dvec[0].idx=0; - dvec[1].d=t.dot(p_points_A[1]); - dvec[1].a=true; - dvec[1].idx=1; - dvec[2].d=t.dot(p_points_B[0]); - dvec[2].a=false; - dvec[2].idx=0; - dvec[3].d=t.dot(p_points_B[1]); - dvec[3].a=false; - dvec[3].idx=1; + dvec[0].d = t.dot(p_points_A[0]); + dvec[0].a = true; + dvec[0].idx = 0; + dvec[1].d = t.dot(p_points_A[1]); + dvec[1].a = true; + dvec[1].idx = 1; + dvec[2].d = t.dot(p_points_B[0]); + dvec[2].a = false; + dvec[2].idx = 0; + dvec[3].d = t.dot(p_points_B[1]); + dvec[3].a = false; + dvec[3].idx = 1; SortArray<_generate_contacts_Pair> sa; - sa.sort(dvec,4); + sa.sort(dvec, 4); - for(int i=1;i<=2;i++) { + for (int i = 1; i <= 2; i++) { if (dvec[i].a) { Vector2 a = p_points_A[dvec[i].idx]; - Vector2 b = n.plane_project(dB,a); - if (n.dot(a) > n.dot(b)-CMP_EPSILON) + Vector2 b = n.plane_project(dB, a); + if (n.dot(a) > n.dot(b) - CMP_EPSILON) continue; - p_collector->call(a,b); + p_collector->call(a, b); } else { Vector2 b = p_points_B[dvec[i].idx]; - Vector2 a = n.plane_project(dA,b); - if (n.dot(a) > n.dot(b)-CMP_EPSILON) + Vector2 a = n.plane_project(dA, b); + if (n.dot(a) > n.dot(b) - CMP_EPSILON) continue; - p_collector->call(a,b); + p_collector->call(a, b); } } - - #elif 0 Vector2 axis = rel_A.normalized(); //make an axis Vector2 axis_B = rel_B.normalized(); - if (axis.dot(axis_B)<0) - axis_B=-axis_B; - axis=(axis+axis_B)*0.5; + if (axis.dot(axis_B) < 0) + axis_B = -axis_B; + axis = (axis + axis_B) * 0.5; Vector2 base_A = p_points_A[0] - axis * axis.dot(p_points_A[0]); Vector2 base_B = p_points_B[0] - axis * axis.dot(p_points_B[0]); //sort all 4 points in axis - real_t dvec[4]={ axis.dot(p_points_A[0]), axis.dot(p_points_A[1]), axis.dot(p_points_B[0]), axis.dot(p_points_B[1]) }; + real_t dvec[4] = { axis.dot(p_points_A[0]), axis.dot(p_points_A[1]), axis.dot(p_points_B[0]), axis.dot(p_points_B[1]) }; //todo , find max/min and then use 2 central points SortArray<real_t> sa; - sa.sort(dvec,4); + sa.sort(dvec, 4); //use the middle ones as contacts - for (int i=1;i<=2;i++) { + for (int i = 1; i <= 2; i++) { - Vector2 a = base_A+axis*dvec[i]; - Vector2 b = base_B+axis*dvec[i]; - if (p_collector->normal.dot(a) > p_collector->normal.dot(b)-0.01) { - print_line("fail a: "+a); - print_line("fail b: "+b); + Vector2 a = base_A + axis * dvec[i]; + Vector2 b = base_B + axis * dvec[i]; + if (p_collector->normal.dot(a) > p_collector->normal.dot(b) - 0.01) { + print_line("fail a: " + a); + print_line("fail b: " + b); continue; } - print_line("res a: "+a); - print_line("res b: "+b); - p_collector->call(a,b); + print_line("res a: " + a); + print_line("res b: " + b); + p_collector->call(a, b); } #endif } -static void _generate_contacts_from_supports(const Vector2 * p_points_A,int p_point_count_A, const Vector2 * p_points_B,int p_point_count_B,_CollectorCallback2D *p_collector) { - +static void _generate_contacts_from_supports(const Vector2 *p_points_A, int p_point_count_A, const Vector2 *p_points_B, int p_point_count_B, _CollectorCallback2D *p_collector) { #ifdef DEBUG_ENABLED - ERR_FAIL_COND( p_point_count_A <1 ); - ERR_FAIL_COND( p_point_count_B <1 ); + ERR_FAIL_COND(p_point_count_A < 1); + ERR_FAIL_COND(p_point_count_B < 1); #endif - - static const GenerateContactsFunc generate_contacts_func_table[2][2]={ + static const GenerateContactsFunc generate_contacts_func_table[2][2] = { + { + _generate_contacts_point_point, + _generate_contacts_point_edge, + }, { - _generate_contacts_point_point, - _generate_contacts_point_edge, - },{ - 0, - _generate_contacts_edge_edge, + 0, + _generate_contacts_edge_edge, } }; @@ -319,28 +309,25 @@ static void _generate_contacts_from_supports(const Vector2 * p_points_A,int p_po pointcount_B = p_point_count_A; pointcount_A = p_point_count_B; - points_A=p_points_B; - points_B=p_points_A; + points_A = p_points_B; + points_B = p_points_A; } else { pointcount_B = p_point_count_B; pointcount_A = p_point_count_A; - points_A=p_points_A; - points_B=p_points_B; + points_A = p_points_A; + points_B = p_points_B; } - int version_A = (pointcount_A > 3 ? 3 : pointcount_A) -1; - int version_B = (pointcount_B > 3 ? 3 : pointcount_B) -1; + int version_A = (pointcount_A > 3 ? 3 : pointcount_A) - 1; + int version_B = (pointcount_B > 3 ? 3 : pointcount_B) - 1; GenerateContactsFunc contacts_func = generate_contacts_func_table[version_A][version_B]; ERR_FAIL_COND(!contacts_func); - contacts_func(points_A,pointcount_A,points_B,pointcount_B,p_collector); - + contacts_func(points_A, pointcount_A, points_B, pointcount_B, p_collector); } - - -template<class ShapeA, class ShapeB,bool castA=false,bool castB=false, bool withMargin=false> +template <class ShapeA, class ShapeB, bool castA = false, bool castB = false, bool withMargin = false> class SeparatorAxisTest2D { const ShapeA *shape_A; @@ -358,16 +345,14 @@ class SeparatorAxisTest2D { _CollectorCallback2D *callback; public: - _FORCE_INLINE_ bool test_previous_axis() { - if (callback && callback->sep_axis && *callback->sep_axis!=Vector2()) { + if (callback && callback->sep_axis && *callback->sep_axis != Vector2()) { return test_axis(*callback->sep_axis); } else { #ifdef DEBUG_ENABLED best_axis_count++; #endif - } return true; } @@ -395,45 +380,44 @@ public: return true; } - _FORCE_INLINE_ bool test_axis(const Vector2& p_axis) { - - Vector2 axis=p_axis; + _FORCE_INLINE_ bool test_axis(const Vector2 &p_axis) { + Vector2 axis = p_axis; - if ( Math::abs(axis.x)<CMP_EPSILON && - Math::abs(axis.y)<CMP_EPSILON) { + if (Math::abs(axis.x) < CMP_EPSILON && + Math::abs(axis.y) < CMP_EPSILON) { // strange case, try an upwards separator - axis=Vector2(0.0,1.0); + axis = Vector2(0.0, 1.0); } - real_t min_A,max_A,min_B,max_B; + real_t min_A, max_A, min_B, max_B; if (castA) - shape_A->project_range_cast(motion_A,axis,*transform_A,min_A,max_A); + shape_A->project_range_cast(motion_A, axis, *transform_A, min_A, max_A); else - shape_A->project_range(axis,*transform_A,min_A,max_A); + shape_A->project_range(axis, *transform_A, min_A, max_A); if (castB) - shape_B->project_range_cast(motion_B,axis,*transform_B,min_B,max_B); + shape_B->project_range_cast(motion_B, axis, *transform_B, min_B, max_B); else - shape_B->project_range(axis,*transform_B,min_B,max_B); + shape_B->project_range(axis, *transform_B, min_B, max_B); if (withMargin) { - min_A-=margin_A; - max_A+=margin_A; - min_B-=margin_B; - max_B+=margin_B; + min_A -= margin_A; + max_A += margin_A; + min_B -= margin_B; + max_B += margin_B; } - min_B -= ( max_A - min_A ) * 0.5; - max_B += ( max_A - min_A ) * 0.5; + min_B -= (max_A - min_A) * 0.5; + max_B += (max_A - min_A) * 0.5; - real_t dmin = min_B - ( min_A + max_A ) * 0.5; - real_t dmax = max_B - ( min_A + max_A ) * 0.5; + real_t dmin = min_B - (min_A + max_A) * 0.5; + real_t dmax = max_B - (min_A + max_A) * 0.5; if (dmin > 0.0 || dmax < 0.0) { if (callback && callback->sep_axis) - *callback->sep_axis=axis; + *callback->sep_axis = axis; #ifdef DEBUG_ENABLED best_axis_count++; #endif @@ -445,26 +429,25 @@ public: dmin = Math::abs(dmin); - if ( dmax < dmin ) { - if ( dmax < best_depth ) { - best_depth=dmax; - best_axis=axis; + if (dmax < dmin) { + if (dmax < best_depth) { + best_depth = dmax; + best_axis = axis; #ifdef DEBUG_ENABLED - best_axis_index=best_axis_count; + best_axis_index = best_axis_count; #endif - } } else { - if ( dmin < best_depth ) { - best_depth=dmin; - best_axis=-axis; // keep it as A axis + if (dmin < best_depth) { + best_depth = dmin; + best_axis = -axis; // keep it as A axis #ifdef DEBUG_ENABLED - best_axis_index=best_axis_count; + best_axis_index = best_axis_count; #endif } } - //print_line("test axis: "+p_axis+" depth: "+rtos(best_depth)); +//print_line("test axis: "+p_axis+" depth: "+rtos(best_depth)); #ifdef DEBUG_ENABLED best_axis_count++; #endif @@ -472,60 +455,55 @@ public: return true; } - _FORCE_INLINE_ void generate_contacts() { // nothing to do, don't generate - if (best_axis==Vector2(0.0,0.0)) + if (best_axis == Vector2(0.0, 0.0)) return; - callback->collided=true; + callback->collided = true; if (!callback->callback) return; //only collide, no callback - static const int max_supports=2; + static const int max_supports = 2; Vector2 supports_A[max_supports]; int support_count_A; if (castA) { - shape_A->get_supports_transformed_cast(motion_A,-best_axis,*transform_A,supports_A,support_count_A); + shape_A->get_supports_transformed_cast(motion_A, -best_axis, *transform_A, supports_A, support_count_A); } else { - shape_A->get_supports(transform_A->basis_xform_inv(-best_axis).normalized(),supports_A,support_count_A); - for(int i=0;i<support_count_A;i++) { + shape_A->get_supports(transform_A->basis_xform_inv(-best_axis).normalized(), supports_A, support_count_A); + for (int i = 0; i < support_count_A; i++) { supports_A[i] = transform_A->xform(supports_A[i]); } } if (withMargin) { - for(int i=0;i<support_count_A;i++) { - supports_A[i]+=-best_axis*margin_A; + for (int i = 0; i < support_count_A; i++) { + supports_A[i] += -best_axis * margin_A; } - } - - Vector2 supports_B[max_supports]; int support_count_B; if (castB) { - shape_B->get_supports_transformed_cast(motion_B,best_axis,*transform_B,supports_B,support_count_B); + shape_B->get_supports_transformed_cast(motion_B, best_axis, *transform_B, supports_B, support_count_B); } else { - shape_B->get_supports(transform_B->basis_xform_inv(best_axis).normalized(),supports_B,support_count_B); - for(int i=0;i<support_count_B;i++) { + shape_B->get_supports(transform_B->basis_xform_inv(best_axis).normalized(), supports_B, support_count_B); + for (int i = 0; i < support_count_B; i++) { supports_B[i] = transform_B->xform(supports_B[i]); } } if (withMargin) { - for(int i=0;i<support_count_B;i++) { - supports_B[i]+=best_axis*margin_B; + for (int i = 0; i < support_count_B; i++) { + supports_B[i] += best_axis * margin_B; } - } -/* + /* print_line("**************************"); @@ -549,38 +527,33 @@ public: } //*/ + callback->normal = best_axis; + _generate_contacts_from_supports(supports_A, support_count_A, supports_B, support_count_B, callback); - - - callback->normal=best_axis; - _generate_contacts_from_supports(supports_A,support_count_A,supports_B,support_count_B,callback); - - if (callback && callback->sep_axis && *callback->sep_axis!=Vector2()) - *callback->sep_axis=Vector2(); //invalidate previous axis (no test) + if (callback && callback->sep_axis && *callback->sep_axis != Vector2()) + *callback->sep_axis = Vector2(); //invalidate previous axis (no test) //CollisionSolver2DSW::CallbackResult cbk=NULL; //cbk(Vector2(),Vector2(),NULL); - } - _FORCE_INLINE_ SeparatorAxisTest2D(const ShapeA *p_shape_A,const Transform2D& p_transform_a, const ShapeB *p_shape_B,const Transform2D& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_A=Vector2(), const Vector2& p_motion_B=Vector2(),real_t p_margin_A=0,real_t p_margin_B=0) { + _FORCE_INLINE_ SeparatorAxisTest2D(const ShapeA *p_shape_A, const Transform2D &p_transform_a, const ShapeB *p_shape_B, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_A = Vector2(), const Vector2 &p_motion_B = Vector2(), real_t p_margin_A = 0, real_t p_margin_B = 0) { - margin_A=p_margin_A; - margin_B=p_margin_B; - best_depth=1e15; - shape_A=p_shape_A; - shape_B=p_shape_B; - transform_A=&p_transform_a; - transform_B=&p_transform_b; - motion_A=p_motion_A; - motion_B=p_motion_B; + margin_A = p_margin_A; + margin_B = p_margin_B; + best_depth = 1e15; + shape_A = p_shape_A; + shape_B = p_shape_B; + transform_A = &p_transform_a; + transform_B = &p_transform_b; + motion_A = p_motion_A; + motion_B = p_motion_B; - callback=p_collector; + callback = p_collector; #ifdef DEBUG_ENABLED - best_axis_count=0; - best_axis_index=-1; + best_axis_count = 0; + best_axis_index = -1; #endif } - }; /****** SAT TESTS *******/ @@ -588,30 +561,26 @@ public: /****** SAT TESTS *******/ /****** SAT TESTS *******/ +#define TEST_POINT(m_a, m_b) \ + ((!separator.test_axis(((m_a) - (m_b)).normalized())) || \ + (castA && !separator.test_axis(((m_a) + p_motion_a - (m_b)).normalized())) || \ + (castB && !separator.test_axis(((m_a) - ((m_b) + p_motion_b)).normalized())) || \ + (castA && castB && !separator.test_axis(((m_a) + p_motion_a - ((m_b) + p_motion_b)).normalized()))) -#define TEST_POINT(m_a,m_b) \ - ( (!separator.test_axis(((m_a)-(m_b)).normalized())) ||\ - (castA && !separator.test_axis(((m_a)+p_motion_a-(m_b)).normalized())) ||\ - (castB && !separator.test_axis(((m_a)-((m_b)+p_motion_b)).normalized())) ||\ - (castA && castB && !separator.test_axis(((m_a)+p_motion_a-((m_b)+p_motion_b)).normalized())) ) - - -typedef void (*CollisionFunc)(const Shape2DSW*,const Transform2D&,const Shape2DSW*,const Transform2D&,_CollectorCallback2D *p_collector,const Vector2&,const Vector2&,real_t,real_t); +typedef void (*CollisionFunc)(const Shape2DSW *, const Transform2D &, const Shape2DSW *, const Transform2D &, _CollectorCallback2D *p_collector, const Vector2 &, const Vector2 &, real_t, real_t); +template <bool castA, bool castB, bool withMargin> +static void _collision_segment_segment(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { -template<bool castA, bool castB,bool withMargin> -static void _collision_segment_segment(const Shape2DSW* p_a,const Transform2D& p_transform_a,const Shape2DSW* p_b,const Transform2D& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,real_t p_margin_A,real_t p_margin_B) { + const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW *>(p_a); + const SegmentShape2DSW *segment_B = static_cast<const SegmentShape2DSW *>(p_b); - const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a); - const SegmentShape2DSW *segment_B = static_cast<const SegmentShape2DSW*>(p_b); - - SeparatorAxisTest2D<SegmentShape2DSW,SegmentShape2DSW,castA,castB,withMargin> separator(segment_A,p_transform_a,segment_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B); + SeparatorAxisTest2D<SegmentShape2DSW, SegmentShape2DSW, castA, castB, withMargin> separator(segment_A, p_transform_a, segment_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); if (!separator.test_previous_axis()) return; //this collision is kind of pointless - /* if (!separator.test_previous_axis()) return; @@ -620,7 +589,6 @@ static void _collision_segment_segment(const Shape2DSW* p_a,const Transform2D& p if (!separator.test_cast()) return; - if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a))) return; if (!separator.test_axis(segment_B->get_xformed_normal(p_transform_b))) @@ -629,30 +597,26 @@ static void _collision_segment_segment(const Shape2DSW* p_a,const Transform2D& p if (withMargin) { //points grow to circles - - if (TEST_POINT( p_transform_a.xform(segment_A->get_a()),p_transform_b.xform(segment_B->get_a())) ) + if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), p_transform_b.xform(segment_B->get_a()))) return; - if (TEST_POINT( p_transform_a.xform(segment_A->get_a()),p_transform_b.xform(segment_B->get_b())) ) + if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), p_transform_b.xform(segment_B->get_b()))) return; - if (TEST_POINT( p_transform_a.xform(segment_A->get_b()),p_transform_b.xform(segment_B->get_a())) ) + if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), p_transform_b.xform(segment_B->get_a()))) return; - if (TEST_POINT( p_transform_a.xform(segment_A->get_b()),p_transform_b.xform(segment_B->get_b())) ) + if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), p_transform_b.xform(segment_B->get_b()))) return; } separator.generate_contacts(); - } -template<bool castA, bool castB,bool withMargin> -static void _collision_segment_circle(const Shape2DSW* p_a,const Transform2D& p_transform_a,const Shape2DSW* p_b,const Transform2D& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,real_t p_margin_A,real_t p_margin_B) { +template <bool castA, bool castB, bool withMargin> +static void _collision_segment_circle(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW *>(p_a); + const CircleShape2DSW *circle_B = static_cast<const CircleShape2DSW *>(p_b); - const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a); - const CircleShape2DSW *circle_B = static_cast<const CircleShape2DSW*>(p_b); - - - SeparatorAxisTest2D<SegmentShape2DSW,CircleShape2DSW,castA,castB,withMargin> separator(segment_A,p_transform_a,circle_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B); + SeparatorAxisTest2D<SegmentShape2DSW, CircleShape2DSW, castA, castB, withMargin> separator(segment_A, p_transform_a, circle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); if (!separator.test_previous_axis()) return; @@ -662,28 +626,26 @@ static void _collision_segment_circle(const Shape2DSW* p_a,const Transform2D& p_ //segment normal if (!separator.test_axis( - (p_transform_a.xform(segment_A->get_b())-p_transform_a.xform(segment_A->get_a())).normalized().tangent() - )) + (p_transform_a.xform(segment_A->get_b()) - p_transform_a.xform(segment_A->get_a())).normalized().tangent())) return; //endpoint a vs circle - if (TEST_POINT(p_transform_a.xform(segment_A->get_a()),p_transform_b.get_origin())) + if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), p_transform_b.get_origin())) return; //endpoint b vs circle - if (TEST_POINT(p_transform_a.xform(segment_A->get_b()),p_transform_b.get_origin())) + if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), p_transform_b.get_origin())) return; - separator.generate_contacts(); } -template<bool castA, bool castB,bool withMargin> -static void _collision_segment_rectangle(const Shape2DSW* p_a,const Transform2D& p_transform_a,const Shape2DSW* p_b,const Transform2D& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,real_t p_margin_A,real_t p_margin_B) { +template <bool castA, bool castB, bool withMargin> +static void _collision_segment_rectangle(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a); - const RectangleShape2DSW *rectangle_B = static_cast<const RectangleShape2DSW*>(p_b); + const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW *>(p_a); + const RectangleShape2DSW *rectangle_B = static_cast<const RectangleShape2DSW *>(p_b); - SeparatorAxisTest2D<SegmentShape2DSW,RectangleShape2DSW,castA,castB,withMargin> separator(segment_A,p_transform_a,rectangle_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B); + SeparatorAxisTest2D<SegmentShape2DSW, RectangleShape2DSW, castA, castB, withMargin> separator(segment_A, p_transform_a, rectangle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); if (!separator.test_previous_axis()) return; @@ -707,48 +669,46 @@ static void _collision_segment_rectangle(const Shape2DSW* p_a,const Transform2D& Vector2 a = p_transform_a.xform(segment_A->get_a()); Vector2 b = p_transform_a.xform(segment_A->get_b()); - if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,inv,a))) + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a))) return; - if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,inv,b))) + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, b))) return; if (castA) { - if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,inv,a+p_motion_a))) + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a + p_motion_a))) return; - if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,inv,b+p_motion_a))) + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, b + p_motion_a))) return; } if (castB) { - if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,inv,a-p_motion_b))) + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a - p_motion_b))) return; - if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,inv,b-p_motion_b))) + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, b - p_motion_b))) return; } if (castA && castB) { - if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,inv,a-p_motion_b+p_motion_a))) + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a - p_motion_b + p_motion_a))) return; - if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,inv,b-p_motion_b+p_motion_a))) + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, b - p_motion_b + p_motion_a))) return; } - } separator.generate_contacts(); - } -template<bool castA, bool castB,bool withMargin> -static void _collision_segment_capsule(const Shape2DSW* p_a,const Transform2D& p_transform_a,const Shape2DSW* p_b,const Transform2D& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,real_t p_margin_A,real_t p_margin_B) { +template <bool castA, bool castB, bool withMargin> +static void _collision_segment_capsule(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a); - const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b); + const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW *>(p_a); + const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW *>(p_b); - SeparatorAxisTest2D<SegmentShape2DSW,CapsuleShape2DSW,castA,castB,withMargin> separator(segment_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B); + SeparatorAxisTest2D<SegmentShape2DSW, CapsuleShape2DSW, castA, castB, withMargin> separator(segment_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); if (!separator.test_previous_axis()) return; @@ -762,25 +722,25 @@ static void _collision_segment_capsule(const Shape2DSW* p_a,const Transform2D& p if (!separator.test_axis(p_transform_b.elements[0].normalized())) return; - if (TEST_POINT(p_transform_a.xform(segment_A->get_a()),(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*0.5))) + if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * 0.5))) return; - if (TEST_POINT(p_transform_a.xform(segment_A->get_a()),(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*-0.5))) + if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * -0.5))) return; - if (TEST_POINT(p_transform_a.xform(segment_A->get_b()),(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*0.5))) + if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * 0.5))) return; - if (TEST_POINT(p_transform_a.xform(segment_A->get_b()),(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*-0.5))) + if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * -0.5))) return; separator.generate_contacts(); } -template<bool castA, bool castB,bool withMargin> -static void _collision_segment_convex_polygon(const Shape2DSW* p_a,const Transform2D& p_transform_a,const Shape2DSW* p_b,const Transform2D& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,real_t p_margin_A,real_t p_margin_B) { +template <bool castA, bool castB, bool withMargin> +static void _collision_segment_convex_polygon(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a); - const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b); + const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW *>(p_a); + const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW *>(p_b); - SeparatorAxisTest2D<SegmentShape2DSW,ConvexPolygonShape2DSW,castA,castB,withMargin> separator(segment_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B); + SeparatorAxisTest2D<SegmentShape2DSW, ConvexPolygonShape2DSW, castA, castB, withMargin> separator(segment_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); if (!separator.test_previous_axis()) return; @@ -791,37 +751,32 @@ static void _collision_segment_convex_polygon(const Shape2DSW* p_a,const Transfo if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a))) return; - for(int i=0;i<convex_B->get_point_count();i++) { + for (int i = 0; i < convex_B->get_point_count(); i++) { - if (!separator.test_axis( convex_B->get_xformed_segment_normal(p_transform_b,i))) + if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) return; if (withMargin) { - if (TEST_POINT(p_transform_a.xform(segment_A->get_a()),p_transform_b.xform(convex_B->get_point(i) ))) + if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), p_transform_b.xform(convex_B->get_point(i)))) return; - if (TEST_POINT(p_transform_a.xform(segment_A->get_b()),p_transform_b.xform(convex_B->get_point(i) ))) + if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), p_transform_b.xform(convex_B->get_point(i)))) return; - } - } separator.generate_contacts(); - } - ///////// -template<bool castA, bool castB,bool withMargin> -static void _collision_circle_circle(const Shape2DSW* p_a,const Transform2D& p_transform_a,const Shape2DSW* p_b,const Transform2D& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,real_t p_margin_A,real_t p_margin_B) { - - const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW*>(p_a); - const CircleShape2DSW *circle_B = static_cast<const CircleShape2DSW*>(p_b); +template <bool castA, bool castB, bool withMargin> +static void _collision_circle_circle(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW *>(p_a); + const CircleShape2DSW *circle_B = static_cast<const CircleShape2DSW *>(p_b); - SeparatorAxisTest2D<CircleShape2DSW,CircleShape2DSW,castA,castB,withMargin> separator(circle_A,p_transform_a,circle_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B); + SeparatorAxisTest2D<CircleShape2DSW, CircleShape2DSW, castA, castB, withMargin> separator(circle_A, p_transform_a, circle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); if (!separator.test_previous_axis()) return; @@ -829,22 +784,19 @@ static void _collision_circle_circle(const Shape2DSW* p_a,const Transform2D& p_t if (!separator.test_cast()) return; - if (TEST_POINT(p_transform_a.get_origin(),p_transform_b.get_origin())) + if (TEST_POINT(p_transform_a.get_origin(), p_transform_b.get_origin())) return; - separator.generate_contacts(); - } -template<bool castA, bool castB,bool withMargin> -static void _collision_circle_rectangle(const Shape2DSW* p_a,const Transform2D& p_transform_a,const Shape2DSW* p_b,const Transform2D& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,real_t p_margin_A,real_t p_margin_B) { - - const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW*>(p_a); - const RectangleShape2DSW *rectangle_B = static_cast<const RectangleShape2DSW*>(p_b); +template <bool castA, bool castB, bool withMargin> +static void _collision_circle_rectangle(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW *>(p_a); + const RectangleShape2DSW *rectangle_B = static_cast<const RectangleShape2DSW *>(p_b); - SeparatorAxisTest2D<CircleShape2DSW,RectangleShape2DSW,castA,castB,withMargin> separator(circle_A,p_transform_a,rectangle_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B); + SeparatorAxisTest2D<CircleShape2DSW, RectangleShape2DSW, castA, castB, withMargin> separator(circle_A, p_transform_a, rectangle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); if (!separator.test_previous_axis()) return; @@ -852,8 +804,8 @@ static void _collision_circle_rectangle(const Shape2DSW* p_a,const Transform2D& if (!separator.test_cast()) return; - const Vector2 &sphere=p_transform_a.elements[2]; - const Vector2 *axis=&p_transform_b.elements[0]; + const Vector2 &sphere = p_transform_a.elements[2]; + const Vector2 *axis = &p_transform_b.elements[0]; //const Vector2& half_extents = rectangle_B->get_half_extents(); if (!separator.test_axis(axis[0].normalized())) @@ -865,42 +817,41 @@ static void _collision_circle_rectangle(const Shape2DSW* p_a,const Transform2D& Transform2D binv = p_transform_b.affine_inverse(); { - if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,binv,sphere ) ) ) + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphere))) return; } if (castA) { Vector2 sphereofs = sphere + p_motion_a; - if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,binv, sphereofs) ) ) + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphereofs))) return; } if (castB) { Vector2 sphereofs = sphere - p_motion_b; - if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,binv, sphereofs) ) ) + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphereofs))) return; } if (castA && castB) { Vector2 sphereofs = sphere - p_motion_b + p_motion_a; - if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,binv, sphereofs) ) ) + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphereofs))) return; } separator.generate_contacts(); } -template<bool castA, bool castB,bool withMargin> -static void _collision_circle_capsule(const Shape2DSW* p_a,const Transform2D& p_transform_a,const Shape2DSW* p_b,const Transform2D& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,real_t p_margin_A,real_t p_margin_B) { - - const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW*>(p_a); - const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b); +template <bool castA, bool castB, bool withMargin> +static void _collision_circle_capsule(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW *>(p_a); + const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW *>(p_b); - SeparatorAxisTest2D<CircleShape2DSW,CapsuleShape2DSW,castA,castB,withMargin> separator(circle_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B); + SeparatorAxisTest2D<CircleShape2DSW, CapsuleShape2DSW, castA, castB, withMargin> separator(circle_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); if (!separator.test_previous_axis()) return; @@ -913,24 +864,21 @@ static void _collision_circle_capsule(const Shape2DSW* p_a,const Transform2D& p_ return; //capsule endpoints - if (TEST_POINT(p_transform_a.get_origin(),(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*0.5))) + if (TEST_POINT(p_transform_a.get_origin(), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * 0.5))) return; - if (TEST_POINT(p_transform_a.get_origin(),(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*-0.5))) + if (TEST_POINT(p_transform_a.get_origin(), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * -0.5))) return; separator.generate_contacts(); - - } -template<bool castA, bool castB,bool withMargin> -static void _collision_circle_convex_polygon(const Shape2DSW* p_a,const Transform2D& p_transform_a,const Shape2DSW* p_b,const Transform2D& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,real_t p_margin_A,real_t p_margin_B) { - - const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW*>(p_a); - const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b); +template <bool castA, bool castB, bool withMargin> +static void _collision_circle_convex_polygon(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW *>(p_a); + const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW *>(p_b); - SeparatorAxisTest2D<CircleShape2DSW,ConvexPolygonShape2DSW,castA,castB,withMargin> separator(circle_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B); + SeparatorAxisTest2D<CircleShape2DSW, ConvexPolygonShape2DSW, castA, castB, withMargin> separator(circle_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); if (!separator.test_previous_axis()) return; @@ -938,31 +886,28 @@ static void _collision_circle_convex_polygon(const Shape2DSW* p_a,const Transfor if (!separator.test_cast()) return; - //poly faces and poly points vs circle - for(int i=0;i<convex_B->get_point_count();i++) { + for (int i = 0; i < convex_B->get_point_count(); i++) { - if (TEST_POINT( p_transform_a.get_origin(),p_transform_b.xform(convex_B->get_point(i)) )) + if (TEST_POINT(p_transform_a.get_origin(), p_transform_b.xform(convex_B->get_point(i)))) return; - if (!separator.test_axis( convex_B->get_xformed_segment_normal(p_transform_b,i))) + if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) return; } separator.generate_contacts(); } - ///////// -template<bool castA, bool castB,bool withMargin> -static void _collision_rectangle_rectangle(const Shape2DSW* p_a,const Transform2D& p_transform_a,const Shape2DSW* p_b,const Transform2D& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,real_t p_margin_A,real_t p_margin_B) { +template <bool castA, bool castB, bool withMargin> +static void _collision_rectangle_rectangle(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const RectangleShape2DSW *rectangle_A = static_cast<const RectangleShape2DSW*>(p_a); - const RectangleShape2DSW *rectangle_B = static_cast<const RectangleShape2DSW*>(p_b); + const RectangleShape2DSW *rectangle_A = static_cast<const RectangleShape2DSW *>(p_a); + const RectangleShape2DSW *rectangle_B = static_cast<const RectangleShape2DSW *>(p_b); - - SeparatorAxisTest2D<RectangleShape2DSW,RectangleShape2DSW,castA,castB,withMargin> separator(rectangle_A,p_transform_a,rectangle_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B); + SeparatorAxisTest2D<RectangleShape2DSW, RectangleShape2DSW, castA, castB, withMargin> separator(rectangle_A, p_transform_a, rectangle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); if (!separator.test_previous_axis()) return; @@ -986,38 +931,38 @@ static void _collision_rectangle_rectangle(const Shape2DSW* p_a,const Transform2 if (withMargin) { - Transform2D invA=p_transform_a.affine_inverse(); - Transform2D invB=p_transform_b.affine_inverse(); + Transform2D invA = p_transform_a.affine_inverse(); + Transform2D invB = p_transform_b.affine_inverse(); - if (!separator.test_axis( rectangle_A->get_box_axis(p_transform_a,invA,rectangle_B,p_transform_b,invB) ) ) + if (!separator.test_axis(rectangle_A->get_box_axis(p_transform_a, invA, rectangle_B, p_transform_b, invB))) return; if (castA || castB) { Transform2D aofs = p_transform_a; - aofs.elements[2]+=p_motion_a; + aofs.elements[2] += p_motion_a; Transform2D bofs = p_transform_b; - bofs.elements[2]+=p_motion_b; + bofs.elements[2] += p_motion_b; Transform2D aofsinv = aofs.affine_inverse(); Transform2D bofsinv = bofs.affine_inverse(); if (castA) { - if (!separator.test_axis( rectangle_A->get_box_axis(aofs,aofsinv,rectangle_B,p_transform_b,invB) ) ) + if (!separator.test_axis(rectangle_A->get_box_axis(aofs, aofsinv, rectangle_B, p_transform_b, invB))) return; } if (castB) { - if (!separator.test_axis( rectangle_A->get_box_axis(p_transform_a,invA,rectangle_B,bofs,bofsinv) ) ) + if (!separator.test_axis(rectangle_A->get_box_axis(p_transform_a, invA, rectangle_B, bofs, bofsinv))) return; } if (castA && castB) { - if (!separator.test_axis( rectangle_A->get_box_axis(aofs,aofsinv,rectangle_B,bofs,bofsinv) ) ) + if (!separator.test_axis(rectangle_A->get_box_axis(aofs, aofsinv, rectangle_B, bofs, bofsinv))) return; } } @@ -1026,14 +971,13 @@ static void _collision_rectangle_rectangle(const Shape2DSW* p_a,const Transform2 separator.generate_contacts(); } -template<bool castA, bool castB,bool withMargin> -static void _collision_rectangle_capsule(const Shape2DSW* p_a,const Transform2D& p_transform_a,const Shape2DSW* p_b,const Transform2D& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,real_t p_margin_A,real_t p_margin_B) { - - const RectangleShape2DSW *rectangle_A = static_cast<const RectangleShape2DSW*>(p_a); - const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b); +template <bool castA, bool castB, bool withMargin> +static void _collision_rectangle_capsule(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const RectangleShape2DSW *rectangle_A = static_cast<const RectangleShape2DSW *>(p_a); + const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW *>(p_b); - SeparatorAxisTest2D<RectangleShape2DSW,CapsuleShape2DSW,castA,castB,withMargin> separator(rectangle_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B); + SeparatorAxisTest2D<RectangleShape2DSW, CapsuleShape2DSW, castA, castB, withMargin> separator(rectangle_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); if (!separator.test_previous_axis()) return; @@ -1052,61 +996,55 @@ static void _collision_rectangle_capsule(const Shape2DSW* p_a,const Transform2D& if (!separator.test_axis(p_transform_b.elements[0].normalized())) return; - //box endpoints to capsule circles Transform2D boxinv = p_transform_a.affine_inverse(); - for(int i=0;i<2;i++) { + for (int i = 0; i < 2; i++) { { - Vector2 capsule_endpoint = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(i==0?0.5:-0.5); + Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * (i == 0 ? 0.5 : -0.5); - if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,capsule_endpoint))) + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) return; } - if (castA) { - Vector2 capsule_endpoint = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(i==0?0.5:-0.5); - capsule_endpoint-=p_motion_a; + Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * (i == 0 ? 0.5 : -0.5); + capsule_endpoint -= p_motion_a; - if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,capsule_endpoint))) + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) return; } if (castB) { - Vector2 capsule_endpoint = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(i==0?0.5:-0.5); - capsule_endpoint+=p_motion_b; + Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * (i == 0 ? 0.5 : -0.5); + capsule_endpoint += p_motion_b; - if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,capsule_endpoint))) + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) return; } if (castA && castB) { - Vector2 capsule_endpoint = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(i==0?0.5:-0.5); - capsule_endpoint-=p_motion_a; - capsule_endpoint+=p_motion_b; - + Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * (i == 0 ? 0.5 : -0.5); + capsule_endpoint -= p_motion_a; + capsule_endpoint += p_motion_b; - if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,capsule_endpoint))) + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) return; } - } - separator.generate_contacts(); } -template<bool castA, bool castB,bool withMargin> -static void _collision_rectangle_convex_polygon(const Shape2DSW* p_a,const Transform2D& p_transform_a,const Shape2DSW* p_b,const Transform2D& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,real_t p_margin_A,real_t p_margin_B) { - - const RectangleShape2DSW *rectangle_A = static_cast<const RectangleShape2DSW*>(p_a); - const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b); +template <bool castA, bool castB, bool withMargin> +static void _collision_rectangle_convex_polygon(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - SeparatorAxisTest2D<RectangleShape2DSW,ConvexPolygonShape2DSW,castA,castB,withMargin> separator(rectangle_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B); + const RectangleShape2DSW *rectangle_A = static_cast<const RectangleShape2DSW *>(p_a); + const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW *>(p_b); + SeparatorAxisTest2D<RectangleShape2DSW, ConvexPolygonShape2DSW, castA, castB, withMargin> separator(rectangle_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); if (!separator.test_previous_axis()) return; @@ -1124,51 +1062,47 @@ static void _collision_rectangle_convex_polygon(const Shape2DSW* p_a,const Trans //convex faces Transform2D boxinv; if (withMargin) { - boxinv=p_transform_a.affine_inverse(); + boxinv = p_transform_a.affine_inverse(); } - for(int i=0;i<convex_B->get_point_count();i++) { + for (int i = 0; i < convex_B->get_point_count(); i++) { - if (!separator.test_axis( convex_B->get_xformed_segment_normal(p_transform_b,i))) + if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) return; if (withMargin) { //all points vs all points need to be tested if margin exist - if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,p_transform_b.xform(convex_B->get_point(i))))) + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, p_transform_b.xform(convex_B->get_point(i))))) return; if (castA) { - if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,p_transform_b.xform(convex_B->get_point(i))-p_motion_a))) + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, p_transform_b.xform(convex_B->get_point(i)) - p_motion_a))) return; } if (castB) { - if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,p_transform_b.xform(convex_B->get_point(i))+p_motion_b))) + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, p_transform_b.xform(convex_B->get_point(i)) + p_motion_b))) return; } if (castA && castB) { - if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,p_transform_b.xform(convex_B->get_point(i))+p_motion_b-p_motion_a))) + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, p_transform_b.xform(convex_B->get_point(i)) + p_motion_b - p_motion_a))) return; } - } } separator.generate_contacts(); - } - ///////// -template<bool castA, bool castB,bool withMargin> -static void _collision_capsule_capsule(const Shape2DSW* p_a,const Transform2D& p_transform_a,const Shape2DSW* p_b,const Transform2D& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,real_t p_margin_A,real_t p_margin_B) { +template <bool castA, bool castB, bool withMargin> +static void _collision_capsule_capsule(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const CapsuleShape2DSW *capsule_A = static_cast<const CapsuleShape2DSW*>(p_a); - const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b); + const CapsuleShape2DSW *capsule_A = static_cast<const CapsuleShape2DSW *>(p_a); + const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW *>(p_b); - - SeparatorAxisTest2D<CapsuleShape2DSW,CapsuleShape2DSW,castA,castB,withMargin> separator(capsule_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B); + SeparatorAxisTest2D<CapsuleShape2DSW, CapsuleShape2DSW, castA, castB, withMargin> separator(capsule_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); if (!separator.test_previous_axis()) return; @@ -1186,32 +1120,29 @@ static void _collision_capsule_capsule(const Shape2DSW* p_a,const Transform2D& p //capsule endpoints - for(int i=0;i<2;i++) { + for (int i = 0; i < 2; i++) { - Vector2 capsule_endpoint_A = p_transform_a.get_origin()+p_transform_a.elements[1]*capsule_A->get_height()*(i==0?0.5:-0.5); + Vector2 capsule_endpoint_A = p_transform_a.get_origin() + p_transform_a.elements[1] * capsule_A->get_height() * (i == 0 ? 0.5 : -0.5); - for(int j=0;j<2;j++) { + for (int j = 0; j < 2; j++) { - Vector2 capsule_endpoint_B = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(j==0?0.5:-0.5); + Vector2 capsule_endpoint_B = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * (j == 0 ? 0.5 : -0.5); - if (TEST_POINT(capsule_endpoint_A,capsule_endpoint_B) ) + if (TEST_POINT(capsule_endpoint_A, capsule_endpoint_B)) return; - } } separator.generate_contacts(); - } -template<bool castA, bool castB,bool withMargin> -static void _collision_capsule_convex_polygon(const Shape2DSW* p_a,const Transform2D& p_transform_a,const Shape2DSW* p_b,const Transform2D& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,real_t p_margin_A,real_t p_margin_B) { +template <bool castA, bool castB, bool withMargin> +static void _collision_capsule_convex_polygon(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const CapsuleShape2DSW *capsule_A = static_cast<const CapsuleShape2DSW*>(p_a); - const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b); + const CapsuleShape2DSW *capsule_A = static_cast<const CapsuleShape2DSW *>(p_a); + const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW *>(p_b); - - SeparatorAxisTest2D<CapsuleShape2DSW,ConvexPolygonShape2DSW,castA,castB,withMargin> separator(capsule_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B); + SeparatorAxisTest2D<CapsuleShape2DSW, ConvexPolygonShape2DSW, castA, castB, withMargin> separator(capsule_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); if (!separator.test_previous_axis()) return; @@ -1224,40 +1155,35 @@ static void _collision_capsule_convex_polygon(const Shape2DSW* p_a,const Transfo if (!separator.test_axis(p_transform_a.elements[0].normalized())) return; - //poly vs capsule - for(int i=0;i<convex_B->get_point_count();i++) { + for (int i = 0; i < convex_B->get_point_count(); i++) { Vector2 cpoint = p_transform_b.xform(convex_B->get_point(i)); - for(int j=0;j<2;j++) { + for (int j = 0; j < 2; j++) { - Vector2 capsule_endpoint_A = p_transform_a.get_origin()+p_transform_a.elements[1]*capsule_A->get_height()*(j==0?0.5:-0.5); + Vector2 capsule_endpoint_A = p_transform_a.get_origin() + p_transform_a.elements[1] * capsule_A->get_height() * (j == 0 ? 0.5 : -0.5); - if (TEST_POINT(capsule_endpoint_A,cpoint )) + if (TEST_POINT(capsule_endpoint_A, cpoint)) return; - } - if (!separator.test_axis( convex_B->get_xformed_segment_normal(p_transform_b,i))) + if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) return; } separator.generate_contacts(); } - ///////// +template <bool castA, bool castB, bool withMargin> +static void _collision_convex_polygon_convex_polygon(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { -template<bool castA, bool castB,bool withMargin> -static void _collision_convex_polygon_convex_polygon(const Shape2DSW* p_a,const Transform2D& p_transform_a,const Shape2DSW* p_b,const Transform2D& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,real_t p_margin_A,real_t p_margin_B) { - - - const ConvexPolygonShape2DSW *convex_A = static_cast<const ConvexPolygonShape2DSW*>(p_a); - const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b); + const ConvexPolygonShape2DSW *convex_A = static_cast<const ConvexPolygonShape2DSW *>(p_a); + const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW *>(p_b); - SeparatorAxisTest2D<ConvexPolygonShape2DSW,ConvexPolygonShape2DSW,castA,castB,withMargin> separator(convex_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B); + SeparatorAxisTest2D<ConvexPolygonShape2DSW, ConvexPolygonShape2DSW, castA, castB, withMargin> separator(convex_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); if (!separator.test_previous_axis()) return; @@ -1265,345 +1191,332 @@ static void _collision_convex_polygon_convex_polygon(const Shape2DSW* p_a,const if (!separator.test_cast()) return; + for (int i = 0; i < convex_A->get_point_count(); i++) { - for(int i=0;i<convex_A->get_point_count();i++) { - - if (!separator.test_axis( convex_A->get_xformed_segment_normal(p_transform_a,i))) + if (!separator.test_axis(convex_A->get_xformed_segment_normal(p_transform_a, i))) return; } - for(int i=0;i<convex_B->get_point_count();i++) { + for (int i = 0; i < convex_B->get_point_count(); i++) { - if (!separator.test_axis( convex_B->get_xformed_segment_normal(p_transform_b,i))) + if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) return; - } if (withMargin) { - for(int i=0;i<convex_A->get_point_count();i++) { - for(int j=0;j<convex_B->get_point_count();j++) { + for (int i = 0; i < convex_A->get_point_count(); i++) { + for (int j = 0; j < convex_B->get_point_count(); j++) { - if (TEST_POINT(p_transform_a.xform(convex_A->get_point(i)) , p_transform_b.xform(convex_B->get_point(j)))) + if (TEST_POINT(p_transform_a.xform(convex_A->get_point(i)), p_transform_b.xform(convex_B->get_point(j)))) return; } } - } separator.generate_contacts(); - } - //////// -bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Transform2D& p_transform_A, const Vector2& p_motion_A, const Shape2DSW *p_shape_B, const Transform2D& p_transform_B,const Vector2& p_motion_B, CollisionSolver2DSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap,Vector2 *sep_axis,real_t p_margin_A,real_t p_margin_B) { +bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CollisionSolver2DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap, Vector2 *sep_axis, real_t p_margin_A, real_t p_margin_B) { - Physics2DServer::ShapeType type_A=p_shape_A->get_type(); + Physics2DServer::ShapeType type_A = p_shape_A->get_type(); - ERR_FAIL_COND_V(type_A==Physics2DServer::SHAPE_LINE,false); + ERR_FAIL_COND_V(type_A == Physics2DServer::SHAPE_LINE, false); //ERR_FAIL_COND_V(type_A==Physics2DServer::SHAPE_RAY,false); - ERR_FAIL_COND_V(p_shape_A->is_concave(),false); + ERR_FAIL_COND_V(p_shape_A->is_concave(), false); - Physics2DServer::ShapeType type_B=p_shape_B->get_type(); + Physics2DServer::ShapeType type_B = p_shape_B->get_type(); - ERR_FAIL_COND_V(type_B==Physics2DServer::SHAPE_LINE,false); + ERR_FAIL_COND_V(type_B == Physics2DServer::SHAPE_LINE, false); //ERR_FAIL_COND_V(type_B==Physics2DServer::SHAPE_RAY,false); - ERR_FAIL_COND_V(p_shape_B->is_concave(),false); + ERR_FAIL_COND_V(p_shape_B->is_concave(), false); - - static const CollisionFunc collision_table[5][5]={ - {_collision_segment_segment<false,false,false>, - _collision_segment_circle<false,false,false>, - _collision_segment_rectangle<false,false,false>, - _collision_segment_capsule<false,false,false>, - _collision_segment_convex_polygon<false,false,false>}, - {0, - _collision_circle_circle<false,false,false>, - _collision_circle_rectangle<false,false,false>, - _collision_circle_capsule<false,false,false>, - _collision_circle_convex_polygon<false,false,false>}, - {0, - 0, - _collision_rectangle_rectangle<false,false,false>, - _collision_rectangle_capsule<false,false,false>, - _collision_rectangle_convex_polygon<false,false,false>}, - {0, - 0, - 0, - _collision_capsule_capsule<false,false,false>, - _collision_capsule_convex_polygon<false,false,false>}, - {0, - 0, - 0, - 0, - _collision_convex_polygon_convex_polygon<false,false,false>} + static const CollisionFunc collision_table[5][5] = { + { _collision_segment_segment<false, false, false>, + _collision_segment_circle<false, false, false>, + _collision_segment_rectangle<false, false, false>, + _collision_segment_capsule<false, false, false>, + _collision_segment_convex_polygon<false, false, false> }, + { 0, + _collision_circle_circle<false, false, false>, + _collision_circle_rectangle<false, false, false>, + _collision_circle_capsule<false, false, false>, + _collision_circle_convex_polygon<false, false, false> }, + { 0, + 0, + _collision_rectangle_rectangle<false, false, false>, + _collision_rectangle_capsule<false, false, false>, + _collision_rectangle_convex_polygon<false, false, false> }, + { 0, + 0, + 0, + _collision_capsule_capsule<false, false, false>, + _collision_capsule_convex_polygon<false, false, false> }, + { 0, + 0, + 0, + 0, + _collision_convex_polygon_convex_polygon<false, false, false> } }; - static const CollisionFunc collision_table_castA[5][5]={ - {_collision_segment_segment<true,false,false>, - _collision_segment_circle<true,false,false>, - _collision_segment_rectangle<true,false,false>, - _collision_segment_capsule<true,false,false>, - _collision_segment_convex_polygon<true,false,false>}, - {0, - _collision_circle_circle<true,false,false>, - _collision_circle_rectangle<true,false,false>, - _collision_circle_capsule<true,false,false>, - _collision_circle_convex_polygon<true,false,false>}, - {0, - 0, - _collision_rectangle_rectangle<true,false,false>, - _collision_rectangle_capsule<true,false,false>, - _collision_rectangle_convex_polygon<true,false,false>}, - {0, - 0, - 0, - _collision_capsule_capsule<true,false,false>, - _collision_capsule_convex_polygon<true,false,false>}, - {0, - 0, - 0, - 0, - _collision_convex_polygon_convex_polygon<true,false,false>} + static const CollisionFunc collision_table_castA[5][5] = { + { _collision_segment_segment<true, false, false>, + _collision_segment_circle<true, false, false>, + _collision_segment_rectangle<true, false, false>, + _collision_segment_capsule<true, false, false>, + _collision_segment_convex_polygon<true, false, false> }, + { 0, + _collision_circle_circle<true, false, false>, + _collision_circle_rectangle<true, false, false>, + _collision_circle_capsule<true, false, false>, + _collision_circle_convex_polygon<true, false, false> }, + { 0, + 0, + _collision_rectangle_rectangle<true, false, false>, + _collision_rectangle_capsule<true, false, false>, + _collision_rectangle_convex_polygon<true, false, false> }, + { 0, + 0, + 0, + _collision_capsule_capsule<true, false, false>, + _collision_capsule_convex_polygon<true, false, false> }, + { 0, + 0, + 0, + 0, + _collision_convex_polygon_convex_polygon<true, false, false> } }; - static const CollisionFunc collision_table_castB[5][5]={ - {_collision_segment_segment<false,true,false>, - _collision_segment_circle<false,true,false>, - _collision_segment_rectangle<false,true,false>, - _collision_segment_capsule<false,true,false>, - _collision_segment_convex_polygon<false,true,false>}, - {0, - _collision_circle_circle<false,true,false>, - _collision_circle_rectangle<false,true,false>, - _collision_circle_capsule<false,true,false>, - _collision_circle_convex_polygon<false,true,false>}, - {0, - 0, - _collision_rectangle_rectangle<false,true,false>, - _collision_rectangle_capsule<false,true,false>, - _collision_rectangle_convex_polygon<false,true,false>}, - {0, - 0, - 0, - _collision_capsule_capsule<false,true,false>, - _collision_capsule_convex_polygon<false,true,false>}, - {0, - 0, - 0, - 0, - _collision_convex_polygon_convex_polygon<false,true,false>} + static const CollisionFunc collision_table_castB[5][5] = { + { _collision_segment_segment<false, true, false>, + _collision_segment_circle<false, true, false>, + _collision_segment_rectangle<false, true, false>, + _collision_segment_capsule<false, true, false>, + _collision_segment_convex_polygon<false, true, false> }, + { 0, + _collision_circle_circle<false, true, false>, + _collision_circle_rectangle<false, true, false>, + _collision_circle_capsule<false, true, false>, + _collision_circle_convex_polygon<false, true, false> }, + { 0, + 0, + _collision_rectangle_rectangle<false, true, false>, + _collision_rectangle_capsule<false, true, false>, + _collision_rectangle_convex_polygon<false, true, false> }, + { 0, + 0, + 0, + _collision_capsule_capsule<false, true, false>, + _collision_capsule_convex_polygon<false, true, false> }, + { 0, + 0, + 0, + 0, + _collision_convex_polygon_convex_polygon<false, true, false> } }; - static const CollisionFunc collision_table_castA_castB[5][5]={ - {_collision_segment_segment<true,true,false>, - _collision_segment_circle<true,true,false>, - _collision_segment_rectangle<true,true,false>, - _collision_segment_capsule<true,true,false>, - _collision_segment_convex_polygon<true,true,false>}, - {0, - _collision_circle_circle<true,true,false>, - _collision_circle_rectangle<true,true,false>, - _collision_circle_capsule<true,true,false>, - _collision_circle_convex_polygon<true,true,false>}, - {0, - 0, - _collision_rectangle_rectangle<true,true,false>, - _collision_rectangle_capsule<true,true,false>, - _collision_rectangle_convex_polygon<true,true,false>}, - {0, - 0, - 0, - _collision_capsule_capsule<true,true,false>, - _collision_capsule_convex_polygon<true,true,false>}, - {0, - 0, - 0, - 0, - _collision_convex_polygon_convex_polygon<true,true,false>} + static const CollisionFunc collision_table_castA_castB[5][5] = { + { _collision_segment_segment<true, true, false>, + _collision_segment_circle<true, true, false>, + _collision_segment_rectangle<true, true, false>, + _collision_segment_capsule<true, true, false>, + _collision_segment_convex_polygon<true, true, false> }, + { 0, + _collision_circle_circle<true, true, false>, + _collision_circle_rectangle<true, true, false>, + _collision_circle_capsule<true, true, false>, + _collision_circle_convex_polygon<true, true, false> }, + { 0, + 0, + _collision_rectangle_rectangle<true, true, false>, + _collision_rectangle_capsule<true, true, false>, + _collision_rectangle_convex_polygon<true, true, false> }, + { 0, + 0, + 0, + _collision_capsule_capsule<true, true, false>, + _collision_capsule_convex_polygon<true, true, false> }, + { 0, + 0, + 0, + 0, + _collision_convex_polygon_convex_polygon<true, true, false> } }; - static const CollisionFunc collision_table_margin[5][5]={ - {_collision_segment_segment<false,false,true>, - _collision_segment_circle<false,false,true>, - _collision_segment_rectangle<false,false,true>, - _collision_segment_capsule<false,false,true>, - _collision_segment_convex_polygon<false,false,true>}, - {0, - _collision_circle_circle<false,false,true>, - _collision_circle_rectangle<false,false,true>, - _collision_circle_capsule<false,false,true>, - _collision_circle_convex_polygon<false,false,true>}, - {0, - 0, - _collision_rectangle_rectangle<false,false,true>, - _collision_rectangle_capsule<false,false,true>, - _collision_rectangle_convex_polygon<false,false,true>}, - {0, - 0, - 0, - _collision_capsule_capsule<false,false,true>, - _collision_capsule_convex_polygon<false,false,true>}, - {0, - 0, - 0, - 0, - _collision_convex_polygon_convex_polygon<false,false,true>} + static const CollisionFunc collision_table_margin[5][5] = { + { _collision_segment_segment<false, false, true>, + _collision_segment_circle<false, false, true>, + _collision_segment_rectangle<false, false, true>, + _collision_segment_capsule<false, false, true>, + _collision_segment_convex_polygon<false, false, true> }, + { 0, + _collision_circle_circle<false, false, true>, + _collision_circle_rectangle<false, false, true>, + _collision_circle_capsule<false, false, true>, + _collision_circle_convex_polygon<false, false, true> }, + { 0, + 0, + _collision_rectangle_rectangle<false, false, true>, + _collision_rectangle_capsule<false, false, true>, + _collision_rectangle_convex_polygon<false, false, true> }, + { 0, + 0, + 0, + _collision_capsule_capsule<false, false, true>, + _collision_capsule_convex_polygon<false, false, true> }, + { 0, + 0, + 0, + 0, + _collision_convex_polygon_convex_polygon<false, false, true> } }; - static const CollisionFunc collision_table_castA_margin[5][5]={ - {_collision_segment_segment<true,false,true>, - _collision_segment_circle<true,false,true>, - _collision_segment_rectangle<true,false,true>, - _collision_segment_capsule<true,false,true>, - _collision_segment_convex_polygon<true,false,true>}, - {0, - _collision_circle_circle<true,false,true>, - _collision_circle_rectangle<true,false,true>, - _collision_circle_capsule<true,false,true>, - _collision_circle_convex_polygon<true,false,true>}, - {0, - 0, - _collision_rectangle_rectangle<true,false,true>, - _collision_rectangle_capsule<true,false,true>, - _collision_rectangle_convex_polygon<true,false,true>}, - {0, - 0, - 0, - _collision_capsule_capsule<true,false,true>, - _collision_capsule_convex_polygon<true,false,true>}, - {0, - 0, - 0, - 0, - _collision_convex_polygon_convex_polygon<true,false,true>} + static const CollisionFunc collision_table_castA_margin[5][5] = { + { _collision_segment_segment<true, false, true>, + _collision_segment_circle<true, false, true>, + _collision_segment_rectangle<true, false, true>, + _collision_segment_capsule<true, false, true>, + _collision_segment_convex_polygon<true, false, true> }, + { 0, + _collision_circle_circle<true, false, true>, + _collision_circle_rectangle<true, false, true>, + _collision_circle_capsule<true, false, true>, + _collision_circle_convex_polygon<true, false, true> }, + { 0, + 0, + _collision_rectangle_rectangle<true, false, true>, + _collision_rectangle_capsule<true, false, true>, + _collision_rectangle_convex_polygon<true, false, true> }, + { 0, + 0, + 0, + _collision_capsule_capsule<true, false, true>, + _collision_capsule_convex_polygon<true, false, true> }, + { 0, + 0, + 0, + 0, + _collision_convex_polygon_convex_polygon<true, false, true> } }; - static const CollisionFunc collision_table_castB_margin[5][5]={ - {_collision_segment_segment<false,true,true>, - _collision_segment_circle<false,true,true>, - _collision_segment_rectangle<false,true,true>, - _collision_segment_capsule<false,true,true>, - _collision_segment_convex_polygon<false,true,true>}, - {0, - _collision_circle_circle<false,true,true>, - _collision_circle_rectangle<false,true,true>, - _collision_circle_capsule<false,true,true>, - _collision_circle_convex_polygon<false,true,true>}, - {0, - 0, - _collision_rectangle_rectangle<false,true,true>, - _collision_rectangle_capsule<false,true,true>, - _collision_rectangle_convex_polygon<false,true,true>}, - {0, - 0, - 0, - _collision_capsule_capsule<false,true,true>, - _collision_capsule_convex_polygon<false,true,true>}, - {0, - 0, - 0, - 0, - _collision_convex_polygon_convex_polygon<false,true,true>} + static const CollisionFunc collision_table_castB_margin[5][5] = { + { _collision_segment_segment<false, true, true>, + _collision_segment_circle<false, true, true>, + _collision_segment_rectangle<false, true, true>, + _collision_segment_capsule<false, true, true>, + _collision_segment_convex_polygon<false, true, true> }, + { 0, + _collision_circle_circle<false, true, true>, + _collision_circle_rectangle<false, true, true>, + _collision_circle_capsule<false, true, true>, + _collision_circle_convex_polygon<false, true, true> }, + { 0, + 0, + _collision_rectangle_rectangle<false, true, true>, + _collision_rectangle_capsule<false, true, true>, + _collision_rectangle_convex_polygon<false, true, true> }, + { 0, + 0, + 0, + _collision_capsule_capsule<false, true, true>, + _collision_capsule_convex_polygon<false, true, true> }, + { 0, + 0, + 0, + 0, + _collision_convex_polygon_convex_polygon<false, true, true> } }; - static const CollisionFunc collision_table_castA_castB_margin[5][5]={ - {_collision_segment_segment<true,true,true>, - _collision_segment_circle<true,true,true>, - _collision_segment_rectangle<true,true,true>, - _collision_segment_capsule<true,true,true>, - _collision_segment_convex_polygon<true,true,true>}, - {0, - _collision_circle_circle<true,true,true>, - _collision_circle_rectangle<true,true,true>, - _collision_circle_capsule<true,true,true>, - _collision_circle_convex_polygon<true,true,true>}, - {0, - 0, - _collision_rectangle_rectangle<true,true,true>, - _collision_rectangle_capsule<true,true,true>, - _collision_rectangle_convex_polygon<true,true,true>}, - {0, - 0, - 0, - _collision_capsule_capsule<true,true,true>, - _collision_capsule_convex_polygon<true,true,true>}, - {0, - 0, - 0, - 0, - _collision_convex_polygon_convex_polygon<true,true,true>} + static const CollisionFunc collision_table_castA_castB_margin[5][5] = { + { _collision_segment_segment<true, true, true>, + _collision_segment_circle<true, true, true>, + _collision_segment_rectangle<true, true, true>, + _collision_segment_capsule<true, true, true>, + _collision_segment_convex_polygon<true, true, true> }, + { 0, + _collision_circle_circle<true, true, true>, + _collision_circle_rectangle<true, true, true>, + _collision_circle_capsule<true, true, true>, + _collision_circle_convex_polygon<true, true, true> }, + { 0, + 0, + _collision_rectangle_rectangle<true, true, true>, + _collision_rectangle_capsule<true, true, true>, + _collision_rectangle_convex_polygon<true, true, true> }, + { 0, + 0, + 0, + _collision_capsule_capsule<true, true, true>, + _collision_capsule_convex_polygon<true, true, true> }, + { 0, + 0, + 0, + 0, + _collision_convex_polygon_convex_polygon<true, true, true> } }; - _CollectorCallback2D callback; - callback.callback=p_result_callback; - callback.swap=p_swap; - callback.userdata=p_userdata; - callback.collided=false; - callback.sep_axis=sep_axis; + callback.callback = p_result_callback; + callback.swap = p_swap; + callback.userdata = p_userdata; + callback.collided = false; + callback.sep_axis = sep_axis; - const Shape2DSW *A=p_shape_A; - const Shape2DSW *B=p_shape_B; - const Transform2D *transform_A=&p_transform_A; - const Transform2D *transform_B=&p_transform_B; - const Vector2 *motion_A=&p_motion_A; - const Vector2 *motion_B=&p_motion_B; - real_t margin_A=p_margin_A,margin_B=p_margin_B; + const Shape2DSW *A = p_shape_A; + const Shape2DSW *B = p_shape_B; + const Transform2D *transform_A = &p_transform_A; + const Transform2D *transform_B = &p_transform_B; + const Vector2 *motion_A = &p_motion_A; + const Vector2 *motion_B = &p_motion_B; + real_t margin_A = p_margin_A, margin_B = p_margin_B; if (type_A > type_B) { - SWAP(A,B); - SWAP(transform_A,transform_B); - SWAP(type_A,type_B); - SWAP(motion_A,motion_B); - SWAP(margin_A,margin_B); + SWAP(A, B); + SWAP(transform_A, transform_B); + SWAP(type_A, type_B); + SWAP(motion_A, motion_B); + SWAP(margin_A, margin_B); callback.swap = !callback.swap; } - CollisionFunc collision_func; if (p_margin_A || p_margin_B) { - if (*motion_A==Vector2() && *motion_B==Vector2()) { - collision_func = collision_table_margin[type_A-2][type_B-2]; - } else if (*motion_A!=Vector2() && *motion_B==Vector2()) { - collision_func = collision_table_castA_margin[type_A-2][type_B-2]; - } else if (*motion_A==Vector2() && *motion_B!=Vector2()) { - collision_func = collision_table_castB_margin[type_A-2][type_B-2]; + if (*motion_A == Vector2() && *motion_B == Vector2()) { + collision_func = collision_table_margin[type_A - 2][type_B - 2]; + } else if (*motion_A != Vector2() && *motion_B == Vector2()) { + collision_func = collision_table_castA_margin[type_A - 2][type_B - 2]; + } else if (*motion_A == Vector2() && *motion_B != Vector2()) { + collision_func = collision_table_castB_margin[type_A - 2][type_B - 2]; } else { - collision_func = collision_table_castA_castB_margin[type_A-2][type_B-2]; + collision_func = collision_table_castA_castB_margin[type_A - 2][type_B - 2]; } } else { - if (*motion_A==Vector2() && *motion_B==Vector2()) { - collision_func = collision_table[type_A-2][type_B-2]; - } else if (*motion_A!=Vector2() && *motion_B==Vector2()) { - collision_func = collision_table_castA[type_A-2][type_B-2]; - } else if (*motion_A==Vector2() && *motion_B!=Vector2()) { - collision_func = collision_table_castB[type_A-2][type_B-2]; + if (*motion_A == Vector2() && *motion_B == Vector2()) { + collision_func = collision_table[type_A - 2][type_B - 2]; + } else if (*motion_A != Vector2() && *motion_B == Vector2()) { + collision_func = collision_table_castA[type_A - 2][type_B - 2]; + } else if (*motion_A == Vector2() && *motion_B != Vector2()) { + collision_func = collision_table_castB[type_A - 2][type_B - 2]; } else { - collision_func = collision_table_castA_castB[type_A-2][type_B-2]; + collision_func = collision_table_castA_castB[type_A - 2][type_B - 2]; } - } + ERR_FAIL_COND_V(!collision_func, false); - - ERR_FAIL_COND_V(!collision_func,false); - - collision_func(A,*transform_A,B,*transform_B,&callback,*motion_A,*motion_B,margin_A,margin_B); + collision_func(A, *transform_A, B, *transform_B, &callback, *motion_A, *motion_B, margin_A, margin_B); return callback.collided; - - } |